基于特征點(diǎn)法的自適應(yīng)SLAM改進(jìn)方法

陳繼清, 桂海寧, 王志奎, 龍騰, 黃春林

( 1.廣西大學(xué) 機(jī)械工程學(xué)院, 廣西 南寧 530004；2.南寧正騰農(nóng)機(jī)有限公司, 廣西 南寧 530007)

0 引言

近些年，即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)作為自動(dòng)駕駛領(lǐng)域的關(guān)鍵技術(shù)[1-5]，已經(jīng)開始廣泛的應(yīng)用于航空航天、無人化駕駛和掃地機(jī)器人等領(lǐng)域。

對(duì)于SLAM[6]系統(tǒng)而言，高精度和低計(jì)算成本是視覺SLAM的核心的要求。目前主流的方法有基于光度不變性的直接方法DSO[7]和LSD(large scale direct monocular， SLAM )[8]，以及基于特征的間接方法。直接方法通過最小化光度誤差[7-8]來恢復(fù)相機(jī)姿態(tài)位置，而間接方法則利用最小化重投影誤差[9]來判別相機(jī)姿態(tài)位置，并使用回環(huán)檢測(cè)實(shí)現(xiàn)重定位來消除全局基于特征點(diǎn)的漂移。這種連續(xù)跟蹤相鄰幀的關(guān)鍵點(diǎn)來恢復(fù)相機(jī)運(yùn)動(dòng)軌跡的方法具有魯棒性，因?yàn)檫@種辨別出的關(guān)鍵點(diǎn)相對(duì)而言是不受視點(diǎn)和光照變化的影響的。最近，很多學(xué)者提出了許多用于實(shí)時(shí)的間接SLAM方法的應(yīng)用。其中，ORB-SLAM2[10]被認(rèn)為是一種先進(jìn)的SLAM方法[11-14]，是在總結(jié)前人的基礎(chǔ)上進(jìn)一步開發(fā)的SLAM系統(tǒng)，是一種結(jié)合了實(shí)時(shí)視覺SLAM系統(tǒng)(PTAM[15])與一種快速位置的識(shí)別方法(Bow2[16])，以及一種高效的基于圖像處理的光束法平算法的SLAM系統(tǒng)，因此，與現(xiàn)有的其他解決方案相比ORB-SLAM2有更好的精度和穩(wěn)定性。在實(shí)驗(yàn)研究中發(fā)現(xiàn)，在一些低紋理場(chǎng)合中，ORB-SLAM2無法得到足夠的描述子數(shù)量，使得ORB-SLAM2的跟蹤失敗，致使ORB-SLAM2無法正常運(yùn)作。而LSD-SLAM因?yàn)槠渲苯臃ㄊ褂玫氖窍袼靥荻榷槐厥墙屈c(diǎn)，所以可以在特征缺失的環(huán)境下運(yùn)行得到了定位信息。

為了應(yīng)對(duì)在低紋理環(huán)境下ORB-SLAM2失效的情況，本文中提出了一種對(duì)于環(huán)境進(jìn)行預(yù)處理的檢測(cè)方法，使得系統(tǒng)可以自主分辨出環(huán)境是否為低紋理環(huán)境，對(duì)于分辨出的低紋理環(huán)境由LSD-SLAM進(jìn)行處理，而對(duì)于非低紋理環(huán)境的圖片由ORB-SLAM2進(jìn)行處理。最后把從LSD-SLAM和ORB-SLAM2中得到的位姿信息與慣性測(cè)量單元(inertial measurement unit，IMU)[17]相結(jié)合，使得整個(gè)算法得到更為準(zhǔn)確的尺度信息，從而提高整個(gè)SLAM算法[18-20]的精度。該算法首先基于傳統(tǒng)的ORB-SLAM2和LSD-SLAM算法，不僅使得跟蹤可以在低紋理環(huán)境下運(yùn)行，且保持著更快的速度比之于傳統(tǒng)的SLAM算法。其次，對(duì)于針對(duì)單目相機(jī)對(duì)于尺度判斷的不確定性，通過IMU數(shù)據(jù)[21]增強(qiáng)系統(tǒng)的魯棒性，增加系統(tǒng)的精度。

1 圖像預(yù)處理設(shè)計(jì)

對(duì)于環(huán)境的分析較為常見的是特征點(diǎn)的直接統(tǒng)計(jì)，但是在實(shí)驗(yàn)的過程中發(fā)現(xiàn)直接統(tǒng)計(jì)特征點(diǎn)的方法不能非常有效的區(qū)分低紋理環(huán)境。雖然有些環(huán)境下的圖片的特征點(diǎn)分布較為集中，導(dǎo)致其特征點(diǎn)數(shù)量達(dá)到了要求[22]，但是分布的問題導(dǎo)致其并不能滿足特征點(diǎn)法的需求。本文提出一種圖像預(yù)處理方法來幫助SLAM系統(tǒng)可以識(shí)別當(dāng)前環(huán)境。圖像預(yù)處理流程圖如圖1所示。

首先，圖1中的圖像處理器使用Fast特征點(diǎn)[5]提取的方法對(duì)于圖片的特征進(jìn)行處理。而傳統(tǒng)的對(duì)于特征點(diǎn)的判斷是基于特征點(diǎn)的數(shù)量，圖片中的特征點(diǎn)非常集中且為遠(yuǎn)點(diǎn)不利于特征點(diǎn)法的定位，而相對(duì)于特征點(diǎn)法，LSD-SLAM則體現(xiàn)出了相當(dāng)強(qiáng)的捕捉能力。

圖1 圖像預(yù)處理流程圖

系統(tǒng)首先用Bresenham四分法[23]對(duì)于整個(gè)圖像進(jìn)行處理。去除其不含特征點(diǎn)的區(qū)域，以此方法對(duì)此迭代5次，得到256個(gè)分割后的圖像，對(duì)于保有特征點(diǎn)的圖像進(jìn)行保留并記錄，并記錄保留圖像總數(shù)記為Num，K為計(jì)算所得當(dāng)前圖像的評(píng)價(jià)系數(shù)，n為圖像的評(píng)價(jià)系數(shù)。

K=Num/256。

經(jīng)過多次實(shí)驗(yàn)驗(yàn)證n取0.23時(shí)系統(tǒng)表現(xiàn)最好。具體流程如圖2所示。

圖2 圖像處理器進(jìn)程圖

初始化處理20張圖像后，通過記錄K值的大小，分辨出圖片是否為低紋理圖片[24]，再流向不同的SLAM系統(tǒng)對(duì)于圖像信息進(jìn)行進(jìn)一步的處理。與此同時(shí)，F(xiàn)AST提取特征點(diǎn)ORB-SLAM2的Tracking的一個(gè)進(jìn)程，所以對(duì)于處理后的結(jié)果無需重復(fù)進(jìn)行，直接將圖導(dǎo)入ORB-SLAM2中，由于沒有進(jìn)行匹配，因此運(yùn)行時(shí)間僅為20 ms。特征點(diǎn)處理時(shí)間為150 ms。當(dāng)圖片導(dǎo)入到LSD-SLAM算法中，由于直接法使得算法的運(yùn)行速度大大快于原有的特征點(diǎn)法，因此整個(gè)運(yùn)行速度提升。同時(shí)，低紋理狀態(tài)下的圖片導(dǎo)入到了LSD-SLAM中，使得整個(gè)系統(tǒng)得以對(duì)低紋理環(huán)境繼續(xù)進(jìn)行追蹤，彌補(bǔ)了ORB-SLAM的失效問題。

自適應(yīng)改進(jìn)SLAM整體框架如圖3所示，圖3中的系統(tǒng)A與系統(tǒng)B分別為ORB-SLAM2與LSD-SLAM系統(tǒng)。深色的方框是自適應(yīng)改進(jìn)SLAM的貢獻(xiàn)。

為了實(shí)現(xiàn)運(yùn)動(dòng)的連貫性，采用了速度滑冰接力賽的方法：預(yù)留一個(gè)緩沖區(qū)，使得系統(tǒng)的切換變得更加緊湊，由于ORB-SLAM2的處理速度遠(yuǎn)遠(yuǎn)慢于LSD-SLAM的處理速度，因此當(dāng)判定符號(hào)發(fā)出以后，LSD-SLAM的線程繼續(xù)運(yùn)行30張圖片后處理公布出姿態(tài)位置信息，ORB-SLAM2同時(shí)也對(duì)這30張圖進(jìn)行處理，并公布姿態(tài)位置信息，2個(gè)姿態(tài)位置信息融合得到新的姿態(tài)位置信息，隨后LSD-SLAM停止運(yùn)作，在新的姿態(tài)信息的基礎(chǔ)上ORB-SLAM2對(duì)于后面的信息進(jìn)行運(yùn)作。為了保持系統(tǒng)的穩(wěn)定性，對(duì)于LSD-SLAM線程切換到ORB-SLAM2線程也是運(yùn)用這種方式。

圖3 改進(jìn)的SLAM的框架

2 IMU融合設(shè)計(jì)

首先在通常情況下IMU[25]的更新頻率遠(yuǎn)遠(yuǎn)快于系統(tǒng)輸出的位姿估計(jì)，使得對(duì)于IMU融合的時(shí)候要同步二者的更新頻率，保證二者在時(shí)域上進(jìn)行統(tǒng)一，以慣性數(shù)據(jù)為核心, 視覺測(cè)量數(shù)據(jù)修正慣性測(cè)量數(shù)據(jù)的累積誤差，從而得到更準(zhǔn)確的位姿信息。

系統(tǒng)采用的是多傳感器融合(multi-sensor fusion，MSF)[26]的方法，方法的進(jìn)程圖如圖4所示。

圖4 IMU融合進(jìn)程圖

圖4中位姿預(yù)測(cè)信息來源于之前系統(tǒng)中生成的坐標(biāo)信息，IMU信息[27]來自于傳感器和數(shù)據(jù)集中提供的IMU信息。IMU主要是加速度信息和速度信息，位姿信息為系統(tǒng)生成的坐標(biāo)信息，由于單目相機(jī)中生成的圖片信息很容易產(chǎn)生尺度的突變。為了應(yīng)對(duì)這種現(xiàn)象同時(shí)計(jì)算雙方的相對(duì)位置[28]，這樣尺度就可以過濾的比較準(zhǔn)確。然后，再把優(yōu)化后的值加入到原來的狀態(tài)中，整合出新的位姿[29]的均值和擾動(dòng)，進(jìn)而優(yōu)化了整個(gè)IMU系統(tǒng)。具體關(guān)于IMU與姿態(tài)的整合公式說明如下：

IMU初始狀態(tài)表示：

(1)

理想運(yùn)動(dòng)模型：

(2)

(3)

(4)

(5)

(6)

實(shí)際運(yùn)動(dòng)模型：

(7)

(8)

(9)

(10)

(11)

誤差狀態(tài)表示：

(12)

誤差運(yùn)動(dòng)狀態(tài)模型：

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

誤差狀態(tài)導(dǎo)數(shù)的轉(zhuǎn)移矩陣：

(21)

二階泰勒展開(忽略二階以上微小量)：

(22)

誤差狀態(tài)的轉(zhuǎn)移矩陣：

(23)

預(yù)測(cè)誤差狀態(tài)和預(yù)測(cè)誤差協(xié)方差矩陣：

(24)

因?yàn)橄到y(tǒng)與IMU的融合是建立在位姿估算已經(jīng)完成的情況下，所以在這里系統(tǒng)已經(jīng)完成了回環(huán)檢測(cè)以及建圖的工作，因此系統(tǒng)無需與常規(guī)的IMU融合一樣進(jìn)行全局BA。系統(tǒng)以標(biāo)定的位姿估計(jì)為起點(diǎn)，把視覺的相對(duì)測(cè)量值與IMU的相對(duì)測(cè)量值進(jìn)行融合，隨后融合得到相對(duì)的測(cè)量狀態(tài)，再把相對(duì)狀態(tài)的均值和擾動(dòng)合并進(jìn)主狀態(tài)以及主狀態(tài)擾動(dòng)，最后主狀態(tài)再與位姿估計(jì)融合出新的主狀態(tài)。

對(duì)于p的位置進(jìn)行測(cè)算：

(25)

(26)

得到誤差測(cè)量參數(shù)矩陣Hp：

(27)

再對(duì)于q位姿進(jìn)行測(cè)算：

(28)

(29)

得到誤差測(cè)量參數(shù)矩陣Hq:

(30)

最后得到位置糾正矩陣H:

(31)

最后更新位置信息：

(32)

完成IMU融合，同時(shí)更新地圖信息，輸出跟蹤軌跡信息。

3 實(shí)驗(yàn)

3.1 實(shí)驗(yàn)設(shè)計(jì)

為了測(cè)試自適應(yīng)改進(jìn)SLAM的性能，系統(tǒng)將在Ubuntu18.04和ROS Melodic中進(jìn)行運(yùn)行通用數(shù)據(jù)集EuRoC，實(shí)驗(yàn)所用的電腦配置為低功耗的Intel Core i7-10750H CPU@2.60GHz。其中n值取0.23，為提高ORB-SLAM2運(yùn)行速度及提高描述子采集頻率描述子關(guān)聯(lián)數(shù)量取5。

采用的數(shù)據(jù)集為EuRoC，其中數(shù)據(jù)集名稱中的MH代表工廠(多紋理環(huán)境)，V代表室內(nèi)環(huán)境(多紋理和低紋理共存環(huán)境)，easy代表相機(jī)的移動(dòng)速度為慢，med代表相機(jī)的移動(dòng)速度為中等，dif代表相機(jī)的移動(dòng)速度為快。

為了展示實(shí)驗(yàn)的效果圖片，實(shí)驗(yàn)收集了改進(jìn)的SLAM的位姿信息和ORB-SLAM2位姿信息，并用Evo做出其與真實(shí)軌跡的對(duì)比圖片。

為了評(píng)估系統(tǒng)與其他系統(tǒng)之間的差異性，實(shí)驗(yàn)使用絕對(duì)軌跡誤差(APE)和相對(duì)位姿誤差(RPE)的均方根誤差(RMSE)進(jìn)行評(píng)估。

最后為了評(píng)估系統(tǒng)與其他系統(tǒng)運(yùn)行時(shí)間方面的差異性，實(shí)驗(yàn)統(tǒng)計(jì)了平均執(zhí)行時(shí)間，以此對(duì)于系統(tǒng)的運(yùn)行速度進(jìn)行評(píng)估。

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

首先分別使用ORB-SLAM2和改進(jìn)的SLAM運(yùn)行數(shù)據(jù)集EuRoC 中的數(shù)據(jù)包，使得生成位姿信息，并分別使用Evo把位姿信息與真實(shí)軌跡進(jìn)行對(duì)比，結(jié)果如圖5-16所示。

(a) ORB-SLAM2相對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2相對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置誤差分析表

(a) ORB-SLAM2絕對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2絕對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置誤差分析表

(a) ORB-SLAM2相對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2相對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置誤差分析表

(a) ORB-SLAM2絕對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2絕對(duì)位置誤差分析表

(d)改進(jìn)的SLAM系統(tǒng)絕對(duì)位置誤差分析表

(a) ORB-SLAM2相對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2相對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置誤差分析表

(a) ORB-SLAM2絕對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2絕對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置誤差分析表

(a) ORB-SLAM2相對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2相對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置誤差分析表

(a) ORB-SLAM2絕對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2絕對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置誤差分析表

(a) ORB-SLAM2相對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2相對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置誤差分析表

(a) ORB-SLAM2絕對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2絕對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置誤差分析表

(a) ORB-SLAM2相對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2相對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)相對(duì)位置誤差分析表

(a) ORB-SLAM2絕對(duì)位置軌跡跟蹤圖

(b) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置軌跡跟蹤圖

(c) ORB-SLAM2絕對(duì)位置誤差分析表

(d) 改進(jìn)的SLAM系統(tǒng)絕對(duì)位置誤差分析表

其中圖5-16中的圖(a)與圖(c)是原有的ORB-SLAM2系統(tǒng)運(yùn)行結(jié)果，圖(b)與圖(d)是自適應(yīng)改進(jìn)SLAM的運(yùn)行結(jié)果。

由于與IMU的融合，使得整個(gè)SLAM有了尺度信息，因此自適應(yīng)改進(jìn)SLAM系統(tǒng)的RPE要遠(yuǎn)遠(yuǎn)優(yōu)于ORB-SLAM2系統(tǒng)的RPE。在實(shí)驗(yàn)數(shù)據(jù)中可以發(fā)現(xiàn)MH_01_easy、MH_02_easy、V1_01_easy和V2_01_easy數(shù)據(jù)集中改進(jìn)的SLAM的RPE要比ORB-SLAM2 的RPE小10倍，但是在MH_05_difficult和V2_03_difficult中發(fā)現(xiàn)，改進(jìn)的SLAM的APE反倒不如ORB-SLAM2，經(jīng)研究發(fā)現(xiàn)是因?yàn)檫@2個(gè)數(shù)據(jù)集是在較快運(yùn)動(dòng)速度的情況下運(yùn)行，由于改進(jìn)的SLAM系統(tǒng)的圖片分選，系統(tǒng)的反應(yīng)較慢，因此在這2個(gè)數(shù)據(jù)集中的APE表現(xiàn)不理想，但是數(shù)據(jù)上改進(jìn)的SLAM系統(tǒng)在低速的環(huán)境下運(yùn)行的表現(xiàn)要比現(xiàn)有的ORB-SLAM2系統(tǒng)誤差減少1/2。

而后在搭建的環(huán)境中，分別運(yùn)行ORB-SLAM2、LSD-SLAM、自適應(yīng)改進(jìn)SLAM，得到各自的實(shí)際軌跡數(shù)據(jù)。對(duì)于實(shí)際軌跡數(shù)據(jù)與真實(shí)軌跡數(shù)據(jù)進(jìn)行RMSE運(yùn)算得到的結(jié)果見表1。

表1 RMSE 誤差Tab.1 RMSE error

由表1可知，僅僅使用特征點(diǎn)法或者僅僅使用直接法的結(jié)果都不如改進(jìn)的SLAM算法，且誤差只有LSD的1/2,精度方面提高了2倍，而ORB-SLAM2因?yàn)殛P(guān)鍵點(diǎn)跟蹤容易丟失在某些序列中失敗。相反，自適應(yīng)改進(jìn)SLAM成功地估計(jì)了所有序列中的攝像機(jī)軌跡。

由于實(shí)時(shí)性為SLAM的重要性能指標(biāo)，因此各系統(tǒng)在不同模塊的執(zhí)行時(shí)間進(jìn)行統(tǒng)計(jì)匯總，得到結(jié)果見表2。

表 2 平均執(zhí)行時(shí)間對(duì)比 ORB-SLAM2,LSD-SLAM 和 自適應(yīng)改進(jìn)SLAM Tab 2 Average execution time comparison of ORB-SLAM2, LSD-SLAM and adaptive improved SLAM

由表2可得ORB-SLAM2、LSD-SLAM和自適應(yīng)改進(jìn)SLAM的比較，其中最佳值為粗體。時(shí)間方面因?yàn)槭侨诤舷到y(tǒng)，所以速度與ORB-SLAM2運(yùn)行速度相似。由于系統(tǒng)融合了IMU使得自適應(yīng)改進(jìn)SLAM的刷新頻率是遠(yuǎn)遠(yuǎn)高于LSD-SLAM和ORB-SLAM2的，因此得到的位姿信息更為豐富準(zhǔn)確。

在低紋理環(huán)境中，僅僅使用特征點(diǎn)法或者僅僅使用直接法的方法產(chǎn)生的結(jié)果都比自適應(yīng)改進(jìn)SLAM差，其中ORB-SLAM因?yàn)殛P(guān)鍵點(diǎn)跟蹤容易丟失，所以容易在某些序列中失敗。相反，自適應(yīng)改進(jìn)SLAM成功地估計(jì)了所有序列中的攝像機(jī)軌跡。由于使用IMU融合，因此低紋理環(huán)境下的自適應(yīng)改進(jìn)SLAM比LSD-SLAM更精確。

綜上所述，自適應(yīng)改進(jìn)SLAM的RPE只有ORB-SLAM2的1/10，且在大部分情況下自適應(yīng)改進(jìn)SLAM的RPE都比ORB-SLAM2的APE要小16%～20%，表明適應(yīng)改進(jìn)SLAM方法是一種更好的定位系統(tǒng)。

4 結(jié)論

本文介紹了自適應(yīng)改進(jìn)SLAM方法，這是第一種基于實(shí)時(shí)優(yōu)化的自適應(yīng)單目VINS方法，它選擇了用ORB算法或LSD算法處理圖像的方法。通過實(shí)驗(yàn)，證明了LSD算法可以用于任何其他與直線對(duì)應(yīng)的姿態(tài)估計(jì)相關(guān)的工作。此外，在系統(tǒng)中有效地利用了位姿信息與IMU信息融合的多傳感器融合(multi-sensor fusion，MSF)方法，因此，在相同的姿態(tài)更新率下，融合自適應(yīng)改進(jìn)SLAM可以比ORB-SLAM2和LSD-SLAM產(chǎn)生更精確的定位。

通過實(shí)驗(yàn)證實(shí)了改進(jìn)的SLAM系統(tǒng)中的分選機(jī)構(gòu)可以成功地分選出低紋理環(huán)境，使得改進(jìn)的SLAM系統(tǒng)可以在ORB-SLAM2失效的環(huán)境下正常運(yùn)作，同時(shí)，對(duì)比于LSD-SLAM在紋理環(huán)境清晰的條件下有更好的精度。最后使用位姿信息與IMU的融合可以提高系統(tǒng)的刷新頻率，并且增強(qiáng)了系統(tǒng)的魯棒性。