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

韓樹蘭，楊 旗，王文杰

（沈陽理工大學(xué)機(jī)械工程學(xué)院，沈陽 110043）

0 引言

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

1 相機(jī)與IMU的標(biāo)定原理

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

圖1 相機(jī)與IMU聯(lián)合標(biāo)定結(jié)果

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

2.1 視覺+IMU融合SLAM

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

圖2 視覺-IMU融合系統(tǒng)示意圖

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

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

2.3 視覺殘差項

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

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

圖3 視覺殘差示意圖

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

3 IMU殘差項

3.1 IMU預(yù)積分模型

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

IMU預(yù)積分殘差項如下所示：

3.2 變量優(yōu)化

隨著系統(tǒng)的運(yùn)行，狀態(tài)變量的數(shù)目越來越多，使用滑動窗口優(yōu)化滑窗內(nèi)的狀態(tài)變量［7z-8］，即需要優(yōu)化的狀態(tài)向量是IMU 的相鄰時間的位置、速度、旋轉(zhuǎn)、偏置、相機(jī)相鄰時刻的逆深度點。

即:

其中：xk表示在第K幀圖像下IMU 的位姿、速度、旋轉(zhuǎn)和對誤差的估計；λ表示視覺觀測到的逆深度信息；為傳感器的外參矩陣，因為已經(jīng)聯(lián)合標(biāo)定，因此外參不參與優(yōu)化。最后可得目標(biāo)函數(shù)如下所示：

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

3.3 仿真與結(jié)果分析

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

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

圖4 相機(jī)與IMU融合

表1 視覺與IMU融合APE統(tǒng)計學(xué)數(shù)據(jù)對比

4 結(jié)語

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