基于視覺-IMU傳感器融合SALM研究

韓樹蘭，楊 旗，王文杰

（沈陽理工大學機械工程學院，沈陽 110043）

0 引言

相機在快速移動、光照改變等情況下容易失效。而慣性測量單元（interial measurement unit，IMU）能夠高頻地獲得機器人內部的運動信息，并且不受周圍環境的影響，從而彌補相機的不足；視覺SLAM（simultaneous localization and mapping），即實時定位與地圖創建對環境的稠密感知，利用視覺傳感器類似于人眼的特點，可以更好地構建出符合人類感知的環境地圖。基于視覺的缺點，本文采用相機與IMU融合，目的是使融合后的信息能夠快速響應，不易丟失。

1 相機與IMU的標定原理

相機與IMU 傳感器標定是使用在線標定的標定方法［1］，即通過Kalibr 工具箱、D415 相機和IMU 來標定，標定的內容主要是相機的內外參數，相機與IMU 之間的外參和相機與IMU 的時間差。標定出的結果如圖1 所示，（Imu0 to cam0）表示IMU 到相機的變換矩陣，（cam0 to imu0）表示其逆矩陣，distortion 表示畸變系數，timeshift 表示相機與 IMU 的時間差為0.00319806137695秒。

圖1 相機與IMU聯合標定結果

2 多傳感器融合SLAM的融合方法

2.1 視覺+IMU融合SLAM

IMU 適合時間短、快速運動，視覺適合時間長、慢速運動，因此將兩者進行融合，利用視覺定位信息來估計IMU 的零偏，減少IMU 由偏置導致的累計誤差［2］；反之，IMU可以為視覺提供快速運動時的定位信息，有效地結合了兩者的優點。如圖2 所示，是相機與IMU 融合的基本框架。

圖2 視覺-IMU融合系統示意圖

2.2 視覺特征點的匹配與剔除

在視覺SLAM 中，選用ORB 算法提取圖像的特征點，通過暴力匹配算法對特征點進行匹配，利用RANSAC 算法去除錯誤的匹配，最后推導出重投影誤差［3］。ORB 算法提取的場景特征點能夠很好地識別圖像角點，并且對密集部分的特征點也有很好的提取效果。除此之外，目前較為常用的特征點匹配算法是暴力匹配，暴力匹配是對當前所有特征點與下一幀所有特征點進行一一對比，根據描述子之間的距離進行排序，選擇距離最短的點作為特征點的匹配對，由于暴力匹配算法是采用暴力匹配的方式，在匹配圖像幀的所有特征點時可能出現誤匹配的情況，導致相機定位丟失，所以需要對誤匹配的特征點進行剔除［4］，留下與正向匹配有相同映射關系的匹配對，再使用RANSAC 算法進一步迭代，去除多余的錯誤特征點匹配。

2.3 視覺殘差項

視覺殘差是指視覺重投影誤差，即當前估計的世界坐標系下的三維點與投影位置之間的距離差［5］，因為種種原因計算得到的值和實際情況不會完全相符，也就是這個差值不可能恰好為零，此時就需要將這些差值的和最小化，以獲取最優的相機位姿參數及三維空間點的坐標。

如圖3所示，假設左右兩個相機為視覺所要獲取的圖像幀，兩幀分別為第i幀和第j幀，兩幀觀測到的歸一化平面［5］的點分別為p1(uci,vci)和p2(ucj,vcj)，根據特征點在第i幀下的深度值為λ，將p1(uci,vci)反投影，得到其在第i幀中的3D空間點P，根據上文標定外參矩陣Tbc1得到它在世界坐標系下的坐標，再通過第j幀在世界坐標系下的位姿得到其在第j幀中的3D 坐標P=(Xcj,Ycj,Zcj)，將P在第j幀成像上投影，與p2j做差，可以得到視覺殘差rij，即估計值與觀測值的差，也即視覺殘差項，如圖3所示。

圖3 視覺殘差示意圖

待估計的狀態量為特征點的三維空間坐標(x,y,z)T，觀測值(u,v)T為特征在相機歸一化平面的坐標。

3 IMU殘差項

3.1 IMU預積分模型

視覺與IMU 在融合的過程中，相機與IMU輸出頻率不一致，通常情況下IMU 頻率較快，相機的輸出頻率低于IMU 的頻率，若在相機位姿估算時將IMU 所有采樣時刻所對應的全部位姿插入每幀間進行優化，數據太過于繁瑣并且降低了運行效率［6］，所以需要對IMU 進行一個預積分處理，IMU 在連續時間狀態下的數學模型方程形式如下：p、v、q分別表示IMU 在k時刻到k+1 時刻的位置、速度和旋轉；w表示世界坐標系，b表示IMU 坐標系，將上述IMU 積分模型轉化為預積分模型，表達式如下：

IMU預積分殘差項如下所示：

3.2 變量優化

隨著系統的運行，狀態變量的數目越來越多，使用滑動窗口優化滑窗內的狀態變量［7z-8］，即需要優化的狀態向量是IMU 的相鄰時間的位置、速度、旋轉、偏置、相機相鄰時刻的逆深度點。

即:

其中：xk表示在第K幀圖像下IMU 的位姿、速度、旋轉和對誤差的估計；λ表示視覺觀測到的逆深度信息；為傳感器的外參矩陣，因為已經聯合標定，因此外參不參與優化。最后可得目標函數如下所示：

其中：第一項表示IMU 殘差；第二項表示視覺殘差。

3.3 仿真與結果分析

本仿真實驗使用多種類型仿真數據驗證融合算法的有效性，包括視覺-IMU 融合公開數據集和ubuntu 系統下GAZEBO 環境搭建的仿真模型，通過對比VINS-FUSION 以及融合算法［9］，評估在相同條件下視覺-IMU 融合SLAM 算法的實時性。其中視覺-IMU 融合方法的指標［10］中最為重要的是絕對位姿誤差APE 的均方根誤差，以此來對視覺-IMU 融合的SLAM 系統的精度進行分析。如圖4所示。

表1給出了圖4 的具體數據。從圖4 和表1可知，相機-IMU 未融合時的均方根誤差為0.03590742，融合后均方根誤差為0.02476373，融合優化之后的均方根誤差為0.01568627，由此可知，融合后的均方根誤差相較于未融合的均方根誤差減少了31.03%，優化之后比未融合誤差減少了56.31%

圖4 相機與IMU融合

表1 視覺與IMU融合APE統計學數據對比

4 結語

本文提出相機-IMU 聯合標定的方法來標定IMU 與相機，使相機與IMU 的數據在時間上進行對齊，在GAZEBO中建立了仿真模型，運用圖優化融合的方法證明了融合之后絕對位姿誤差比未融合的絕對位姿誤差要小，但總體來看精度并不高。下一步工作會將激光雷達-視覺-IMU進行融合，采用圖優化的融合方法，會使位姿精度進一步提高。