韓樹蘭，楊 旗，王文杰

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

0 引言

相機(jī)在快速移動(dòng)、光照改變等情況下容易失效。而慣性測量單元（interial measurement unit，IMU）能夠高頻地獲得機(jī)器人內(nèi)部的運(yùn)動(dòng)信息，并且不受周圍環(huán)境的影響，從而彌補(bǔ)相機(jī)的不足；視覺SLAM（simultaneous localization and mapping），即實(shí)時(shí)定位與地圖創(chuàng)建對(duì)環(huán)境的稠密感知，利用視覺傳感器類似于人眼的特點(diǎn)，可以更好地構(gòu)建出符合人類感知的環(huán)境地圖。基于視覺的缺點(diǎn)，本文采用相機(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 的時(shí)間差。標(biāo)定出的結(jié)果如圖1 所示，（Imu0 to cam0）表示IMU 到相機(jī)的變換矩陣，（cam0 to imu0）表示其逆矩陣，distortion 表示畸變系數(shù)，timeshift 表示相機(jī)與 IMU 的時(shí)間差為0.00319806137695秒。

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

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

2.1 視覺+IMU融合SLAM

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

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

2.2 視覺特征點(diǎn)的匹配與剔除

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

2.3 視覺殘差項(xiàng)

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

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

圖3 視覺殘差示意圖

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

3 IMU殘差項(xiàng)

3.1 IMU預(yù)積分模型

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

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

3.2 變量優(yōu)化

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

即:

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

其中：第一項(xiàng)表示IMU 殘差；第二項(xiàng)表示視覺殘差。

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

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

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

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

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

4 結(jié)語

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