基于滑窗非線性優(yōu)化的雙目視覺SLAM算法

毛家寧，趙龍

北京航空航天大學(xué) 自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191

視覺SLAM(simultaneous localization and mapping, SLAM)是指移動(dòng)載體在未知環(huán)境中，通過獲取自身攜帶的視覺傳感器信息，創(chuàng)建與環(huán)境一致的地圖，同時(shí)確定自身在地圖中的位置[1-3]。該技術(shù)不僅在機(jī)器人導(dǎo)航、任務(wù)規(guī)劃和遠(yuǎn)程操作等領(lǐng)域具有重要研究意義，也在計(jì)算機(jī)游戲、虛擬現(xiàn)實(shí)和增強(qiáng)現(xiàn)實(shí)等領(lǐng)域具有廣闊的應(yīng)用前景[4-7]。研究視覺SLAM問題常用的相機(jī)主要有深度相機(jī)[8]、單目相機(jī)[9]和雙目相機(jī)[10]，其中深度相機(jī)受光照條件影響苛刻，難以在戶外大場景中應(yīng)用；單目相機(jī)無法恢復(fù)尺度信息，應(yīng)用價(jià)值有限；雙目相機(jī)可計(jì)算像素深度，因此在學(xué)術(shù)和工業(yè)領(lǐng)域得到了廣泛應(yīng)用。

視覺SLAM可通過是否使用圖像特征分為特征法和直接法兩類[11]，且上述2種方法在實(shí)際應(yīng)用中各有優(yōu)劣。特征法[12-13]通過匹配圖像特征，構(gòu)建相鄰幀之間的對應(yīng)關(guān)系，并通過最小化重投影誤差(re-projection error)對相機(jī)運(yùn)動(dòng)和地圖結(jié)構(gòu)進(jìn)行優(yōu)化。基于特征法的視覺SLAM系統(tǒng)魯棒性強(qiáng)，即使在較大幀間運(yùn)動(dòng)時(shí)也可正常工作；但特征匹配操作需消耗大量時(shí)間，且特征法僅針對圖像中少量特征點(diǎn)進(jìn)行運(yùn)算，忽略了其他像素信息，無法實(shí)現(xiàn)稠密建圖。直接法[14-15]僅考慮像素灰度信息，通過最小化光度誤差(photometricerror)恢復(fù)相機(jī)運(yùn)動(dòng)和地圖結(jié)構(gòu)?；谥苯臃ǖ囊曈XSLAM系統(tǒng)無需進(jìn)行特征匹配操作，計(jì)算效率高，可使用圖像中的所有像素信息進(jìn)行稠密或半稠密建圖；但直接法基于灰度不變假設(shè)，魯棒性較差，且稠密或半稠密建圖需對大量像素進(jìn)行運(yùn)算，CPU無法實(shí)時(shí)運(yùn)行，需借助GPU加速實(shí)現(xiàn)。

本文以設(shè)計(jì)高精度、高適應(yīng)性的雙目視覺SLAM系統(tǒng)為目的，選擇基于特征法的雙目視覺SLAM系統(tǒng)作為研究對象。在眾多基于特征法的雙目視覺SLAM系統(tǒng)中，雙目ORB-SLAM[16]是目前最完善易用的實(shí)時(shí)視覺SLAM系統(tǒng)之一[2]。然而，雙目ORB-SLAM算法跟蹤模塊僅解算最新一幀圖像對應(yīng)的狀態(tài)量，且精度有限，需通過回環(huán)檢測模塊對其進(jìn)行修正。若實(shí)際運(yùn)動(dòng)不包含回環(huán)路徑，狀態(tài)估計(jì)結(jié)果將產(chǎn)生漂移，且隨系統(tǒng)運(yùn)行時(shí)間緩慢增大。本文對雙目ORB-SLAM算法進(jìn)行了改進(jìn)，提出了一種基于滑窗非線性優(yōu)化的雙目視覺SLAM算法，通過引入滑窗思想，解算最新多幀圖像對應(yīng)的系統(tǒng)狀態(tài)，同時(shí)采用了邊緣化策略將其余狀態(tài)量轉(zhuǎn)換為先驗(yàn)測量信息，從而在保證算法實(shí)時(shí)性的基礎(chǔ)上提高了不包含回環(huán)路徑時(shí)系統(tǒng)的精確性。

1 算法框架

本文算法運(yùn)行原理圖如圖1所示，主要分為3個(gè)并行線程：1) 特征提取和跟蹤線程，主要負(fù)責(zé)提取和匹配圖像特征，如圖1中紅框部分所示；2) 狀態(tài)量估計(jì)線程，主要負(fù)責(zé)視覺特征點(diǎn)重投影誤差計(jì)算，狀態(tài)量非線性優(yōu)化以及邊緣化處理，如圖1中藍(lán)框部分所示；3) 建圖線程，主要負(fù)責(zé)采用線性三角化和光束平差優(yōu)化構(gòu)建增量式地圖，如圖1中綠框部分所示。

2 圖像特征跟蹤算法

雙目視覺SLAM需對左右目圖像以及相鄰幀圖像進(jìn)行特征點(diǎn)檢測與跟蹤，由于圖像間畸變微小，本文采用導(dǎo)向性FAST特征加旋轉(zhuǎn)BRIEF描述子(oriented FAST and rotated BRIEF, ORB)[17]特征實(shí)現(xiàn)特征跟蹤，結(jié)果如圖2所示。

本文圖像特征跟蹤具體實(shí)現(xiàn)步驟如下：首先，以原始圖像為基準(zhǔn)構(gòu)建尺度因子為1.2的8層圖像金字塔，在金字塔各層圖像上提取基于加速分割測試的特征(features from accelerated sagment test, FAST)[18]；其次，計(jì)算各特征點(diǎn)的方向，構(gòu)建ORB特征描述子；最后，將左右目圖像與相鄰幀圖像進(jìn)行匹配，構(gòu)建特征點(diǎn)間的匹配對應(yīng)關(guān)系。

圖2 左右目以及前后幀圖像特征跟蹤效果

3 滑窗雙目視覺SLAM系統(tǒng)狀態(tài)估計(jì)

基于特征點(diǎn)的SLAM算法通常把狀態(tài)估計(jì)問題構(gòu)建為以視覺特征點(diǎn)重投影誤差為代價(jià)函數(shù)的非線性優(yōu)化模型，通過引入滑窗思想，本文構(gòu)建了新的代價(jià)函數(shù)，并采用高斯牛頓法迭代地求解最優(yōu)的狀態(tài)變量。如圖3所示，滑窗雙目視覺SLAM系統(tǒng)代價(jià)函數(shù)可表示為

圖3 滑窗雙目視覺SLAM狀態(tài)估計(jì)示意

3.1 視覺特征點(diǎn)重投影誤差模型

圖4 視覺特征點(diǎn)重投影誤差模型示意

3.2 狀態(tài)量邊緣化

在視覺SLAM系統(tǒng)中，隨著運(yùn)行時(shí)間推移，由視覺特征點(diǎn)三維坐標(biāo)與相機(jī)位姿構(gòu)成的狀態(tài)變量數(shù)目將顯著增加，算法復(fù)雜度將隨狀態(tài)量數(shù)目增加而增大。如果求解全狀態(tài)SLAM問題將無法滿足實(shí)時(shí)性需求；如果僅求解最新一幀圖像對應(yīng)的狀態(tài)量將忽略各狀態(tài)量間的關(guān)聯(lián)，顯著降低了SLAM系統(tǒng)的精度。本文引入了滑窗思想，有效限制了待求解狀態(tài)量數(shù)目，采用狀態(tài)量邊緣化策略，且將其余狀態(tài)量轉(zhuǎn)換為待求解狀態(tài)量的先驗(yàn)觀測信息，有效實(shí)現(xiàn)了算法復(fù)雜度和精確性間的平衡。

3.2.1 邊緣化原理

采用高斯牛頓法求解狀態(tài)增量的公式為

采用舒爾補(bǔ)(schur complement)[19]原理簡化式(2)，得

3.2.2 邊緣化在滑窗雙目視覺SLAM中的應(yīng)用

固定滑窗內(nèi)圖像幀數(shù)目為n，本文邊緣化策略的如下：如果滑窗內(nèi)第n-1幀為關(guān)鍵幀，將邊緣化滑窗內(nèi)第1個(gè)關(guān)鍵幀對應(yīng)狀態(tài)量及與其相關(guān)的視覺測量；如果第n-1幀為非關(guān)鍵幀，將直接刪除第n-1幀對應(yīng)狀態(tài)量及與其相關(guān)的視覺測量，此時(shí)采用了刪除策略，并未進(jìn)行邊緣化處理，其目的是為了保持滑窗內(nèi)系統(tǒng)的稀疏性。采用何種策略取決于滑窗內(nèi)第n-1幀是否為關(guān)鍵幀，如滿足下述條件之一即為關(guān)鍵幀：

1) 第n-1幀跟蹤到的特征點(diǎn)數(shù)目大于60個(gè)；

2) 跟蹤到的特征點(diǎn)中50%為未知三維坐標(biāo)；

3) 地圖構(gòu)建線程處于空閑狀態(tài)。

4 滑窗雙目視覺SLAM系統(tǒng)地圖構(gòu)建

視覺SLAM系統(tǒng)需根據(jù)相機(jī)運(yùn)動(dòng)狀態(tài)以及觀測到的特征點(diǎn)信息構(gòu)建增量式地圖。由于建圖線程不受相機(jī)幀率限制，本文僅選用關(guān)鍵幀圖像觀測到的特征點(diǎn)構(gòu)建地圖。當(dāng)滑窗內(nèi)倒數(shù)第一幀圖像為關(guān)鍵幀時(shí)，對該幀圖像中未知三維坐標(biāo)的匹配點(diǎn)，采用線性三角化方法，進(jìn)行三維重建。記特征點(diǎn)在參考幀相機(jī)坐標(biāo)系中的齊次坐標(biāo)可得

式中：分別為當(dāng)前幀相機(jī)坐標(biāo)系到參考幀相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量分別為特征點(diǎn)在當(dāng)前幀和參考幀像素坐標(biāo)系中的齊次坐標(biāo)；表示相機(jī)的逆投影變換，即

5 實(shí)驗(yàn)測試與結(jié)果分析

選用歐洲機(jī)器人技術(shù)挑戰(zhàn)賽(european robotics challenge, EuRoC)公開數(shù)據(jù)集[21]中的MH_05_difficult數(shù)據(jù)文件，將本文算法和雙目ORB-SLAM進(jìn)行對比測試，評估本文算法的精確性和有效性，其中本文算法滑窗內(nèi)圖像幀數(shù)目設(shè)為6幀，算法運(yùn)行平臺為配備Intel Core i7處理器，內(nèi)存為8 GB的筆記本電腦。實(shí)驗(yàn)測試結(jié)果如圖5～12所示，實(shí)驗(yàn)結(jié)果包括軌跡曲線、三維地圖、位置絕對誤差分量和姿態(tài)絕對誤差分量；位置絕對誤差和姿態(tài)絕對誤差的統(tǒng)計(jì)結(jié)果如表1所示。

圖5 運(yùn)動(dòng)軌跡

圖6 稀疏三維地圖

圖7 橫滾角絕對誤差

圖8 俯仰角絕對誤差

圖9 航向角絕對誤差

圖10 x軸位置絕對誤差

圖11 y軸位置絕對誤差

圖12 z軸位置絕對誤差

從圖5中可以看出，雙目ORB-SLAM和本文算法都能有效估計(jì)載體位姿，且不發(fā)生顯著漂移。從表1中可以看出，雙目ORB-SLAM和本文算法的位置均方根誤差分別為0.269 331和0.175 442 m，姿態(tài)均方根誤差為1.591 809°和0.814 265°。由此可見，本文算法明顯優(yōu)于雙目ORB-SLAM算法，其原因在于雙目ORB-SLAM算法跟蹤模塊僅解算最新一幀圖像對應(yīng)的狀態(tài)量，且精度有限；而本文通過引入滑窗思想，解算最新多幀圖像對應(yīng)的系統(tǒng)狀態(tài)，同時(shí)采用了邊緣化策略將其余狀態(tài)量轉(zhuǎn)換為先驗(yàn)測量信息，從而在保證算法實(shí)時(shí)性的基礎(chǔ)上提高了系統(tǒng)的精確性。

表1 位姿誤差統(tǒng)計(jì)結(jié)果

6 結(jié)論

本文提出了一種基于滑窗非線性優(yōu)化的雙目視覺SLAM算法，主要包括：

1）在雙目ORB-SLAM算法框架基礎(chǔ)上，引入了滑窗思想，解算最新多幀圖像對應(yīng)的狀態(tài)。

2）采用了邊緣化策略將其余狀態(tài)量轉(zhuǎn)換為先驗(yàn)測量信息，從而保證了算法的精確性和實(shí)時(shí)性。

3）本文通過公開數(shù)據(jù)集測試實(shí)驗(yàn)，驗(yàn)證了算法的優(yōu)越性。然而，本文雙目視覺SLAM算法僅在PC(personal computer)端實(shí)現(xiàn)了實(shí)時(shí)定位與建圖功能，未來可以考慮簡化算法，將其移植到嵌入式系統(tǒng)或移動(dòng)端應(yīng)用中。