融合IMU的視覺(jué)SLAM系統(tǒng)研究

趙永達(dá)

摘要??? 基于特征點(diǎn)法的視覺(jué)SLAM系統(tǒng)在運(yùn)行的過(guò)程中，運(yùn)動(dòng)過(guò)大，環(huán)境特征過(guò)少都會(huì)影響SLAM系統(tǒng)的運(yùn)行效果。針對(duì)這個(gè)問(wèn)題，提出了一種融合IMU的視覺(jué)SLAM系統(tǒng)。首先，提取關(guān)鍵點(diǎn)，并結(jié)合PNP與G2O方法估計(jì)相機(jī)的最優(yōu)位姿，并構(gòu)建地圖。然后，統(tǒng)計(jì)當(dāng)前幀關(guān)鍵點(diǎn)數(shù)量，若特征點(diǎn)足夠少，特征點(diǎn)法無(wú)法估計(jì)相機(jī)的位姿變化，則結(jié)合陀螺儀與輪式里程計(jì)估計(jì)位姿;最后，如果特征點(diǎn)數(shù)量增加，提取關(guān)鍵特征，結(jié)合PNP與G2O的方法估計(jì)相機(jī)位姿的變化。通過(guò)實(shí)驗(yàn)可以看出融合IMU的視覺(jué)SLAM系統(tǒng)，可以準(zhǔn)確的估計(jì)相機(jī)相機(jī)的位姿變化。

【關(guān)鍵詞】特征點(diǎn)法 IMU 陀螺儀 輪式里程計(jì)

近年來(lái)，同時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）發(fā)展迅速涌現(xiàn)了大量?jī)?yōu)秀成果，如基于直接法SVO，以及基于特征點(diǎn)法的ORB-SLAM.CNN也被引入到了SLAM系統(tǒng)中，但以上的方法過(guò)于依賴圖像數(shù)據(jù)，如果獲取的圖像中關(guān)鍵點(diǎn)不容易提取，將嚴(yán)重影響算法的準(zhǔn)確性。

因此，本文提出一種融合IMU的SLAM算法。本方法首先按照特征點(diǎn)法估計(jì)位姿，當(dāng)特征點(diǎn)變少時(shí)，結(jié)合輪式里程計(jì)以及陀螺儀估計(jì)位姿的變化，并構(gòu)建地圖。

1 算法簡(jiǎn)介

融合IMU的視覺(jué)SLAM算法首先獲得一幀幀描述環(huán)境信息的圖像，包括彩色圖像和深度圖像，并對(duì)彩色圖像提取關(guān)鍵特征，通過(guò)PNP與G2O結(jié)合的方法估計(jì)相機(jī)的位姿變化。如果提取關(guān)鍵特點(diǎn)數(shù)量少時(shí)，采用IMU方法計(jì)算位姿。通過(guò)點(diǎn)云拼接構(gòu)建地圖。

2 關(guān)鍵特征法估計(jì)位姿

使用ORB算法提取圖像特征，提取關(guān)鍵特征的同時(shí)，統(tǒng)計(jì)關(guān)鍵點(diǎn)的個(gè)數(shù)，如果某一幀圖像提取關(guān)鍵點(diǎn)的個(gè)數(shù)小于50，則使用IMU計(jì)算位姿。在提取關(guān)鍵特征的同時(shí)，對(duì)關(guān)鍵特征進(jìn)行簡(jiǎn)單的優(yōu)化，ORB特征的提取分為提取Fast關(guān)鍵點(diǎn)和計(jì)算Brief描述子兩個(gè)部分，在獲取相鄰兩幀圖像的匹配點(diǎn)的同時(shí)，統(tǒng)計(jì)所有匹配點(diǎn)中最小的海明距離d。對(duì)匹配點(diǎn)進(jìn)行簡(jiǎn)單的優(yōu)化，如果一對(duì)匹配點(diǎn)的海明距離大max（3d，40），認(rèn)為該匹配點(diǎn)對(duì)不合格，從匹配結(jié)果中刪除該匹配點(diǎn)對(duì)。按照該方法驗(yàn)證所有的匹配點(diǎn)對(duì)。

對(duì)于空間中的任意一個(gè)點(diǎn)P，用其次坐標(biāo)表示為（x，y，z，1），P點(diǎn)投影到不同的圖像中表示為

公式

，相機(jī)的位置和姿態(tài)表示為R、t，K為相機(jī)內(nèi)參，得到關(guān)系：，可以看出[R|t]矩陣有12個(gè)未知量，因此取至少六個(gè)點(diǎn)，通過(guò)SVD分解可求得[R|t]。由于存在觀測(cè)噪聲，計(jì)算相機(jī)位姿會(huì)產(chǎn)生重投影誤差。通過(guò)將誤差求和構(gòu)建最小二乘問(wèn)題，誤差項(xiàng)表示如（1）所示，通過(guò)G2O方法，估計(jì)最優(yōu)位姿ξ。

公式

3 IMU方法估計(jì)位姿

采用IMU方法可以將位姿表示為特殊歐式群：

其中T是線性坐標(biāo)，T的左上角為旋轉(zhuǎn)矩

陣R，右側(cè)為平移向量t，SO（3）是特殊正交群，

可以表示為

公式

表示空間中的坐標(biāo)點(diǎn)。

4 實(shí)驗(yàn)結(jié)果分析

運(yùn)行融合IMU的視覺(jué)slam系統(tǒng)，估計(jì)相機(jī)的位姿，并對(duì)相機(jī)的真實(shí)位置進(jìn)行記錄。實(shí)驗(yàn)運(yùn)行在系ubuntu14.04系統(tǒng)上，4核的cpu型號(hào)為Intel（R）Core（TM）i5-5200U，內(nèi)存大小為8GB，顯卡的型號(hào)是Intel（R） HD Graphics 5500。運(yùn)行結(jié)果如圖1所示。其中藍(lán)色曲線為相機(jī)的真是運(yùn)動(dòng)軌跡，紅色曲線為

融合IMU的視覺(jué)SLAM系統(tǒng)的估計(jì)結(jié)果。綠色曲線為ORB特征點(diǎn)法估計(jì)的軌跡。

5 結(jié)束語(yǔ)

通過(guò)實(shí)驗(yàn)可以看出由于相機(jī)運(yùn)動(dòng)到特征點(diǎn)較少的地方，特征點(diǎn)法的位姿估計(jì)，產(chǎn)生了較大的誤差。本文提出的融合IMU的視覺(jué)SLAM系統(tǒng)，可以較準(zhǔn)確的估計(jì)位姿，具有更好的穩(wěn)定性。

參考文獻(xiàn)

[1]Mur-Artal R， Montiel J M M， Tardos J D. ORB-SLAM： a Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics， 2015， 31（5）：1147-1163.

[2]Forster C， Pizzoli M， Davide Scaramuzza*. SVO： Fast Semi-Direct Monocular Visual Odometry[C]// IEEE International Conference on Robotics and Automation （ICRA）， Hong Kong， 2014. IEEE， 2014.

[3]張攀.慣性傳感器輔助的立體視覺(jué)SLAM實(shí)現(xiàn)[D].廈門(mén)大學(xué)，2013.

[4]王澤華，梁冬泰，梁丹等.基于慣性/磁力傳感器與單目視覺(jué)融合的SLAM方法[J].機(jī)器人，2018，40（06）：933-941.

[5]顧照鵬，董秋雷.基于部分慣性傳感器信息的單目視覺(jué)同步定位與地圖創(chuàng)建方法[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，2012，24（02）：155-160.

[6]周紹磊，吳修振，劉剛等.一種單目視覺(jué)ORB-SLAM/INS組合導(dǎo)航方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2016，24（05）：633-637.