視覺與慣性傳感器融合的室內導航算法研究

楊志芳，潘忠運

（武漢工程大學 電氣信息學院，武漢430200）

目前，室內移動機器人的導航定位算法主要采用的是SLAM（同步定位與建圖）算法[1]，SLAM 算法大致可分為兩大類，激光SLAM 和視覺SLAM[2]。激光SLAM 是通過采集不同時刻的兩片點云信息（一系列分散的、具有準確角度和距離信息的點集）然后將兩片點云圖進行匹配，計算出激光雷達相對運動的距離和姿態變化，也就是對機器人的定位。激光SLAM 需要搭載成本較高的激光雷達，同時激光雷達獲得的環境信息不如視覺傳感器豐富。視覺SLAM 成本低廉、獲得的環境信息豐富，然而為了提高移動機器人定位的準確度和穩定性，靠純視覺SLAM 系統來對環境的感知是不夠的，因為純視覺SLAM 系統往往容易受到光照變化的影響，同時在低紋理和運動速度較快導致圖像信息模糊的情況下定位精度很低，甚至無法使用。融合IMU[3]很好地克服了純視覺SLAM 的缺點，在運動速度過快的情況下，圖像會出現模糊，兩幀圖像之間的特征點匹配極少甚至沒有，在遇到玻璃、白墻等低紋理的情況下同樣特征點匹配很少。在這些情況下，IMU 可以提供數據，繼續估算可靠的位姿。在動態環境中，當環境變化時，純視覺SLAM 可能誤以為自身在動，而IMU 可以感知自身運動，從而避免誤判。

近幾年IMU 與視覺融合的SLAM 研究越來越流行，有一些開源的VIO[4]（視覺慣導里程計）系統。如ROVIO 是基于擴展卡爾曼濾波的緊耦合方法，由Bloesch 等人在2015年提出的一種視覺里程計[5]，具有跟蹤3D 路標點圖像特征匹配的功能，但僅支持單目相機。OKVIS 也是一種基于擴展卡爾曼濾波的緊耦合方法，它是由蘇黎世大學的Stefan Leutenegge 等人在2015年提出的[6]，在前端采用的是特征點法和BRISK 描述子[7]，后端使用滑動窗口模型的視覺慣導SLAM 系統，同時首次在系統中對IMU 進行了預積分處理[8]，解決了IMU 與相機幀率差異問題。支持基于單目和雙目相機的滑動窗口位姿估計。VINS 是香港科技大學沈劭劼團隊于2017年提出的通用單目視覺慣性估計的魯棒方法[9]，它的前端使用Harise 角點檢測，光流法來跟蹤[10]，后端是基于優化和滑動窗口的VIO，該系統主要應用于增強現實領域，也可以在手持式設備或無人機上運行。

1 改進系統框架

本文是在經典的ORB-SLAM2 框架基礎上進行改進的，設計了基于非線性優化的IMU 數據與RGB-D 圖像數據緊耦合的方式來解決純視覺SLAM所面對的問題。改進系統框架如圖1所示，系統的前端包括圖像信息和IMU 傳感器信息，將獲得的圖像信息進行特征提取與匹配，然后進行動態特征點濾除和RANSAC 算法的誤匹配剔除，將動態特征點和誤匹配點濾除掉。IMU 數據進行預計分處理，使其頻率與相機獲取圖像的頻率保持一致，然后系統進行視覺慣導聯合初始化，確定載體的初始狀態（初始位置、初始速度、加速度和陀螺儀的偏差、重力向量、世界坐標與載體坐標系的變換關系等等），后端包括基于滑動窗口的關鍵幀篩選和緊耦合非線性優化，最小化重投影誤差（相機的重投影誤差和IMU 誤差）求出載體的位姿信息。輸出是載體的位姿和運動軌跡。

圖1 本文算法流程Fig.1 Algorithm flow chart of this paper

2 IMU 與RGB-D 融合算法設計

2.1 動態特征點濾除

動態特征點濾除主要是利用圖像幀間的幾何約束方法來濾除圖像中的動態特征點，為后面的RANSAC 算法剔除誤匹配提供更好的數據樣本。從而減少RANSAC 算法的迭代次數，提高系統運行效率，同時使其在動態場景中有更高的定位精度。

引入幾何約束圖像模型，如圖2所示。F1和F2為相鄰兩幀圖像，p1、p2、p3為F1圖像幀的3 個特征點，q1、q2、q3是其在F2圖像幀上對應的投影點，3 個特征點在F1上構成三角形Δ p1p2p3，對應的投影點構成三角形Δq1q2q3，其圖像中特征點間的距離用三角形三邊d 表示。

圖2 幾何約束模型Fig.2 Geometric constraint model

pi和pj的距離表示為d（pi，pj），qi和qj的距離表示為d（pi，pj），則三角形對應邊的差即為

定義幾何約束函數：

A（i，j）表示兩對應邊的平均距離，即：

假設q1為運動目標上的點，由q1運動到q1′的位置，則構成新的三角形Δq1′q2q3，可知計算出的幾何約束函數q（i，j）會很大，但是由于幾何約束函數涉及的是一對特征點，仍然無法確定qi點是動態點，還是qj點是動態點，因此采用計分的方法來確定最終的特征點，即若q（i，j）大于閾值α，則將特征點qi和qj的得分分別加1，這樣不斷地將不同的特征點構成三角形進行計算，最終動態特征點會比靜止特征點的得分大很多，若某特征點得分大于閾值β，則認為該特征點為動態特征點。

2.2 RANSAC 算法

RANSAC（Random Sample Consensus，隨機抽樣一致）算法用來估算表示幀間圖像的位姿變換關系的基本矩陣H，得到一個最優的基本矩陣H 模型，然后根據該模型剔除掉誤匹配的點，使得特征匹配更加的準確，從而提高系統的定位精度。

RANSAC 算法基本流程如圖3所示。

圖3 RANSAC 算法流程Fig.3 Flow chart of RANSAC algorithm

（1）設樣本集為S，其有N 個數據點，內點的比例為w，從樣本集S 中隨機選取4 個數據點，根據該數據點計算出單應矩陣H。

（2）將數據集中所有的數據按單應矩陣H 進行投影，得到所有數據點的投影誤差，將誤差小于閾值d 的數據點添加到內點集Si中。

（3）如果內點集Si超過了某個閾值m，就用Si重新估計單應矩陣H 并結束，否則重新選取新的4 個數據點，計算單應矩陣H，然后重復步驟2。如果迭代的次數大于T，選取最大的內點集Si，用它來估計單應矩陣H，得到最后的估計模型，然后退出。

由上述的計算流程可以看出，RANSAC 算法的運行效率取決于迭代次數，進一步取決于樣本集的內點比例，因此在進行RANSAC 之前，盡量提高樣本集的內點比例，這樣可以提高RANSAC 的運行效率。在之前進行的動態特征點濾除，有效地濾除了樣本集中的部分外點，在很大程度上降低了RANSAC 的迭代次數，提高了RANSAC 的運行效率。

2.3 IMU 預積分

IMU 傳感器是測量移動機器人自身的姿態角和加速度的裝置，由三軸陀螺儀和三軸加速度計組成。由于IMU 和相機的采樣頻率相差較大，在相同的時間內IMU 采集的數據比相機更多，本文使用的IMU 采樣頻率為200 Hz，相機的采樣頻率為20 Hz。所以需要采用預積分方法來解決IMU 與相機頻率不同的問題。

在兩幀圖像之間有多幀IMU 數據，假設IMU的第i 時刻數據與圖像的第k 幀對齊，IMU 的第j時刻數據與圖像的第k+1 幀對齊，然后連續兩圖像之間的IMU 數據進行預積分處理，如圖4所示。

圖4 IMU 預積分示意圖Fig.4 Schematic diagram of IMU pre-integration

在某一離散時刻t 采集數據，可得到t=i 和t=j兩幀之間的位姿與速度關系：

2.4 非線性優化估計位姿

在系統完成初始化之后，本文采用滑動窗口的方法來控制優化的規模，用緊耦合的方式對相機的位姿、速度以及IMU 的偏置進行估計優化。滑動窗口是增加新的變量進入到最小二乘系統優化，維持窗口關鍵幀的數量，數量達到一定個數就移除舊的關鍵幀。然后不斷地重復這一過程。在傳統的視覺SLAM 中，是通過最小化圖像幀中特征點的重投影誤差來求得相機的位姿以及三維路標點。但增加IMU 傳感器之后，增加了要估計的系統狀態變量，需要同時考慮重投影誤差和IMU 誤差。系統的狀態向量表示如下：

根據相機的重投影誤差和IMU 誤差構建目標函數，通過高斯牛頓法求得目標函數最小時的位姿信息，如圖5所示。

圖5 非線性優化流程Fig.5 Flow chart of nonlinear optimization

目標函數為

式中：V 表示滑動窗口中被觀測到兩次以上的特征點集；I 表示滑動窗口中IMU 預積分增量方程的集合；Ecam（k，j）為地圖點k 在第j 幀圖像時的重投影誤差，其誤差方程表示為

式中：xk為像素平面坐標；π（·）表示三維空間點投影到像素平面的投影變換；WXk為世界坐標系下的三維坐標點；Eimu（i，i+1）為IMU 預積分中連續兩幀i和i+1 之間的增量方程誤差，其誤差方程表示為

3 實驗結果與分析

本文使用EuRoC 和TUM 數據集對本算法進行了測試，將其測試的結果與ORB-SLAM2 算法進行了對比和分析。EuRoC 數據集是用微型飛行器在1個工業廠房和2 個不同的房間內采集，分為easy、medium 和difficult 3 個不同的等級。TUM 數據集中包含大量動態場景的序列，采集了辦公室中同事聊天互動、手勢和起身等場景。本數據的實驗平臺系統為Ubuntu18.04，CPU 為Intel i5-7200U 處理器，主頻為2.7 GHz，內存為8 GB。

本算法的動態特征濾除效果如圖6所示，圖像為TUM 數據集freiburg3_walking 序列中的一幀，動態目標正走向座椅。由圖6可以看出本算法對動態特征點濾除有很好的效果，將人身上的大部分動態點濾除掉了。

圖6 動態特征點濾除對比圖Fig.6 Comparison diagram of dynamic feature points filtering

本文在評估定位精度時，采用算法跟蹤過程中的位姿估計值與數據集提供的真實軌跡的均方根誤差（RMSE）來對算法的定位精度來進行評估。分別比較本算法和ORB-SLAM2 算法中的雙目模式在各種環境下的均方根誤差，所有結果都為3 次實驗結果的均值。

如表1所示，本算法和ORB_SLAM2 在EuRoC數據集和TUM 數據集實驗軌跡精度比較，可以看出本文提出的慣導與雙目視覺融合算法在各個場景中比雙目視覺模式的ORB_SLAM2 算法的定位精度更高。

表1 在EuRoC、TUM 數據集上實驗的均方根誤差對比Tab.1 Comparison of RMS errors of experiments on EuRoC and TUM datasets

圖7為本算法和ORB_SLAM2 算法在MH_05_difficult 序列的絕對軌跡誤差，從MH_05_difficult 序列可以看出，當相機快速運動導致圖像模糊和弱紋理時，ORB_SLAM2 的定位精度有所下降，這是由于在這些序列中ORB_SLAM2 所提取的特征點少，且比配質量差導致的。本算法在ORB_SLAM2 的基礎上增加了IMU 信息，在各場景中不僅僅只用視覺信息進行定位，在提取特征點較少時，可以結合IMU的運動信息進行計算，這樣在弱紋理，快速運動導致圖像模糊等場景時，本算法同樣有很好的定位精度。同時本算法還增加了動態特征點濾除，可以較好的濾除動態特征點，提高特征點的匹配質量，freiburg3_sitting（低動態）和freiburg3_walking（高動態）是含有動態物體的序列，從上表可以看出在freiburg3_walking 序列中，ORB_SLAM2 算法的定位精度急劇下降，大量在動態物體上提取出來的特征點沒有被有效的剔除，而是參與了匹配和進一步的計算，從而對系統的定位精度產生影響，而本算法的動態特征點濾除模塊能有效剔除動態特征點，降低動態特征點對系統的影響，所以在含有動態目標時本算法的定位精度比ORB_SLAM2 更高。綜上本算法比ORB_SLAM2 算法對場景的適應性更好。

4 結語

根據純視覺的SLAM 系統對環境的適應性不強，本文在ORB-SLAM2 的基礎上進行了改進，增加了IMU 傳感器的數據，采用了基于非線性優化的緊耦合方式，這種融合方式系統的運行效率會更高。同時在特征匹配時增加了動態特征點濾除算法，先將動態特征點濾除后，再進行RANSAC 算法誤匹配剔除，使得特征匹配的更加準確，提高了系統的定位精度，同時使系統能夠更好的適應含有動態目標的場景。EuRoC 數據集和TUM 數據集實驗表明，本文改進的算法在弱紋理、快速運動、圖像模糊和含有動態目標影響的場景中定位精度上比ORBSLAM2 表現更好，在快速運動導致圖像模糊時也有很高的定位精度。因為融合了IMU 傳感器的數據，使其算法更為復雜，整個系統的運行效率上略有降低。由于系統在特征點提取上消耗的時間較多，后續可以采用FPGA 對圖像的特征點提取進行硬件加速，這樣可以提升系統的運行速度，同時采用FPGA可以大幅降低功耗。