基于滑窗非線性優化的雙目視覺SLAM算法

毛家寧，趙龍

北京航空航天大學 自動化科學與電氣工程學院，北京 100191

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

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

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

1 算法框架

本文算法運行原理圖如圖1所示，主要分為3個并行線程：1) 特征提取和跟蹤線程，主要負責提取和匹配圖像特征，如圖1中紅框部分所示；2) 狀態量估計線程，主要負責視覺特征點重投影誤差計算，狀態量非線性優化以及邊緣化處理，如圖1中藍框部分所示；3) 建圖線程，主要負責采用線性三角化和光束平差優化構建增量式地圖，如圖1中綠框部分所示。

2 圖像特征跟蹤算法

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

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

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

3 滑窗雙目視覺SLAM系統狀態估計

基于特征點的SLAM算法通常把狀態估計問題構建為以視覺特征點重投影誤差為代價函數的非線性優化模型，通過引入滑窗思想，本文構建了新的代價函數，并采用高斯牛頓法迭代地求解最優的狀態變量。如圖3所示，滑窗雙目視覺SLAM系統代價函數可表示為

圖3 滑窗雙目視覺SLAM狀態估計示意

3.1 視覺特征點重投影誤差模型

圖4 視覺特征點重投影誤差模型示意

3.2 狀態量邊緣化

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

3.2.1 邊緣化原理

采用高斯牛頓法求解狀態增量的公式為

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

3.2.2 邊緣化在滑窗雙目視覺SLAM中的應用

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

1) 第n-1幀跟蹤到的特征點數目大于60個；

2) 跟蹤到的特征點中50%為未知三維坐標；

3) 地圖構建線程處于空閑狀態。

4 滑窗雙目視覺SLAM系統地圖構建

視覺SLAM系統需根據相機運動狀態以及觀測到的特征點信息構建增量式地圖。由于建圖線程不受相機幀率限制，本文僅選用關鍵幀圖像觀測到的特征點構建地圖。當滑窗內倒數第一幀圖像為關鍵幀時，對該幀圖像中未知三維坐標的匹配點，采用線性三角化方法，進行三維重建。記特征點在參考幀相機坐標系中的齊次坐標可得

式中：分別為當前幀相機坐標系到參考幀相機坐標系的旋轉矩陣和平移向量分別為特征點在當前幀和參考幀像素坐標系中的齊次坐標；表示相機的逆投影變換，即

5 實驗測試與結果分析

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

圖5 運動軌跡

圖6 稀疏三維地圖

圖7 橫滾角絕對誤差

圖8 俯仰角絕對誤差

圖9 航向角絕對誤差

圖10 x軸位置絕對誤差

圖11 y軸位置絕對誤差

圖12 z軸位置絕對誤差

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

表1 位姿誤差統計結果

6 結論

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

1）在雙目ORB-SLAM算法框架基礎上，引入了滑窗思想，解算最新多幀圖像對應的狀態。

2）采用了邊緣化策略將其余狀態量轉換為先驗測量信息，從而保證了算法的精確性和實時性。

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