基于圖優化的移動機器人視覺SLAM

張毅，沙建松

同時定位與地圖創建(SLAM)技術已經成為目前機器人研究領域的熱點，隨著SLAM問題研究的不斷深入，對機器人位姿監測和環境地圖創建的精確度、實時性也都有了更高的要求。因此，如何在精確度與實時性之間尋求一種平衡是目前眾多SLAM算法中亟待解決的一個問題。而新型RGBD攝像機Kinect[1]，具有豐富3維空間信息與顏色紋理信息，為上述問題的解決提供了的可能。文獻[2]提出的基于RGB-D相機的室內SLAM方法可實現對室內環境的稠密化建圖，但論文中的SIFT特征提取算法較為費時，會占用較多的運算資源，因此其實時性較差。ORB-SLAM[3]采用了基于單目攝像機的SLAM方案，以高效的ORB特征貫穿于系統中的特征處理、匹配、回環檢測、重定位等方面，更側重于系統處理的實時性，但因為單目相機的限制，ORB-SLAM算法在稠密化建圖方面還有所欠缺。針對目前移動機器人視覺SLAM研究中存在的實時性差、精確度不高、無法稠密化建圖等問題，本文提出了一種基于Kinect深度攝像機的移動機器人三維視覺SLAM算法，以實現對環境進行實時的、精確的稠密化建圖。在本算法前端處理中，通過分析多種特征點檢測算法的優劣，采用了魯棒性與實時性更好的ORB特征檢測。利用 RANSAC算法[4]對可能存在的誤匹配點進行剔除完成初始匹配，以PNP(perspective-N-point)[5]問題求解的方式實現了圖像的快速配準。在后端優化中，通過對SLAM問題建模分析，遵循圖優化思想，設計了一種非線性優化方法，實現對機器人位姿的優化。并與閉環檢測[6]機制相結合，提出了一種基于局部閉環與全局閉環約束的點云優化算法，同時完成對位姿與稠密化點云的優化，可有效地抑制系統的累積誤差，進一步提升整個系統的精確性。最后通過實驗，驗證了本文SLAM方法的可行性和有效性。

1 系統結構

本文提出的室內環境下移動機器人三維視覺SLAM算法的總體框架如圖1所示，主要分為前端的關鍵幀篩選、幀間配準、閉環檢測和后端的圖優化、點云融合、點云優化幾個部分。

圖1 基于圖優化的視覺SLAM系統框架Fig. 1 Visual-SLAM system framework based on graph optimization

2 特征點的檢測與配準

2.1 特征點的檢測

特征點的檢測與匹配是視覺SLAM問題中“圖像前端”的部分，也是視覺SLAM中占用計算資源最多，最影響系統實時性的部分?；贔AST特征檢測改進的ORB特征[7]具有旋轉不變和尺度不變特性的二進制特征，它是一種高效的具有良好針對視圖不變的特征，比SIFT特征檢測快兩個數量級，同時特征點的質量較高、魯棒性好?？紤]到SLAM問題對魯棒性、實時性的要求，本文采用了ORB特征提取算法。

2.2 特征點的配準

圖像產生的本質是光線通過相機透鏡投影到圖像傳感器上，這個過程是3-D場景到2-D平面的投影，因此場景與它的成像之間存在重要的聯系，相同場景的不同成像之間也有聯系。而通過這些聯系，可以實現特征點的配準。

特征點配準算法可以分為以下5個步驟：

1) 提取特征點：Opencv提供了通用接口cv::FeatureDetector，這里將其設定為ORB特征。

2) 提取描述子：對于檢測出的角點，用一些數學上的特征對其進行描述，這里通過cv::FeatureDetectorExtractor接口設置特征描述子提取器。

3) 匹配：通過各個角點的描述子來判斷它們在兩張圖像中的對應關系。這里采用的方法為快速最近鄰算法FLANN。

4) 去外點：利用RANSAC算法去除錯誤匹配的外點，保留正確的內點。

5) 位姿求解：調用Opencv的PnPsolver()函數，求解相機的位姿運動(R,t)。

3 位姿優化

針對傳統的利用濾波方法估計位姿存在線性化誤差、更新效率低下等問題，本文依據SLAM研究中的圖優化[8]思想，設計了一種基于非線性優化的位姿優化方法，同時實現局部位姿優化和全局位姿優化。

3.1 模型的建立

SLAM的經典數學模型通常分為3部分：狀態變量、運動方程、觀測方程。

本文主要以Kinect作為主要的傳感器輸入，通過對視覺信號的處理，構建視覺里程計，用于位姿估計。所以這里只考慮狀態變量和觀測方程，構建出目標函數：

目標函數的物理意義：這里假設了誤差函數e(·)符合高斯分布，則SLAM問題的貝葉斯概率描述為，求解適合的，使系統最有可能產生當前的觀測數據Z，即最大似然(MLE)：

圖2 優化求解流程Fig. 2 Process of optimizing solution

3.2 圖的建立與優化

上述SLAM問題全部抽象成節點與邊，節點代表了優化變量，包括pose節點和point節點。邊代表了誤差項約束條件，包括pose-point約束和posepose約束。pose-point約束依賴于相機觀測產生的約束，如圖3(a)所示。pose-pose約束則依賴于系統中閉環檢測產生的約束，如圖3(b)、(c)所示，可有效地抑制系統的累積誤差，從而得到信息一致的地圖。

圖3 圖的建立與優化Fig. 3 Establishment and optimization of graph

4 閉環檢測下的點云優化

在基于位姿增量估計的SLAM建圖中，會產生誤差的累積，則在長時間的大范圍SLAM建圖中，就需要閉環檢測對位姿和生成的點云進行優化。RGBD-SLAM算法是在局部回環檢測后，只使用局部回環優化后的位姿作為三維地圖點云的重建。ORB-SLAM算法則是在運動跟蹤過程中，時刻檢測并維護關鍵幀之間的點云共視關系，同時實現對點云優化。但這種方法只適用于規模有限的稀疏點云，并不適用于稠密化點云的優化。基于此，提出了一種基于局部閉環與全局閉環約束的點云優化算法，同時完成對位姿與稠密化點云的優化。算法具體步驟如下：

1) 封裝構造數據幀Frame，根據幀間的關聯程度，篩選出關鍵幀序列。

2) 執行幀間配準。完成相機位姿初始估計。

3) 在相機觀測約束下，直接應用式(6)，構建如圖3(a)所示的圖優化。求得優化后的相機的位姿變化，將原始點云進行坐標系變換，實現點云的拼接融合。

4) 執行閉環檢測。通過動態計算閉環幀篩選閾值，得到匹配質量較高的閉環幀候選序列。

5) 閉環檢測結束后，遍歷閉環幀候選序列，在當前幀與閉環候選幀之間，進行Sim3(相似變換矩陣)求解與優化。當某一候選幀通過了Sim3求解與優化，則認為閉環檢測成功，跳出對閉環幀候選序列的遍歷。求得對應相似變換矩陣為

6) 位姿優化的傳播。當前幀坐標系與世界坐標系之間的變換在5)中已經確定并優化。通過一級相連關系的傳播，可以確定這些相連的關鍵幀坐標系與世界坐標系之間的相似變換矩陣。

5 實驗結果及分析

5.1 實驗平臺

為了驗證算法的有效性，并保證本文實驗的一致可對比性，故采用了知名的TUM標準數據集[12]，并選用了基于Kinect視覺傳感器采集的室內基準數據包。該基準數據包中包含Kinect 產生的彩色圖像和深度圖像序列，以及機器人的真實運動位姿。本文實驗數據處理平臺為一臺Intel雙核2.4 GHz主頻的PC機，運行Ubuntu 14.04操作系統。程序均設定以30幀/秒的速度同時讀取數據包中的RGB數據和深度數據。

5.2 室內環境下的三維視覺SLAM

5.2.1 特征提取與特征點配準

對兩幀圖像進行ORB特征提取后，在利用特征匹配算法(FLANN)獲得相鄰幀間對應關系的基礎上，利用 RANSAC 算法對點云進行匹配，匹配結果如圖4(c)所示。在圖4(a)中，通過特征匹配算法，一共產生了929個匹配特征點，但誤匹配點過多。因此，通過人工制定規則的篩選，去除了部分誤匹配點，此時的匹配個數為250。為進一步提升匹配精讀，通過RANSAC算法對圖4(b)進行進一步的匹配篩選。最終，產生了97個經篩選后的匹配點，實驗中整個匹配所耗時間為 0.003 5 s。

圖4 特征提取與配準實驗結果Fig. 4 The experimental results of feature extraction and matching

5.2.2 回環檢測約束

通過回環檢測，可以增加幀間的約束，進一步提升系統的魯棒性，進而提升整個SLAM的軌跡與地圖的質量。在本文算法中，通過篩選關鍵幀之后進行回環檢測，并以PNP求解的結果作為對邊的估計，增加到圖優化中，如圖5所示。

圖5 回環檢測產生的約束Fig. 5 Loop closure of constraint

5.2.3 三維環境重構效果對比

圖6(a)為采用了ORB-SLAM方法得到的三維環境重構結果，所構建的三維地圖為稀疏點云。圖6(b)為采用RGBD-SLAM方法得到的三維環境重構結果，物體輪廓可以辨別，但冗余點相對較多。圖6(c)為采用本文的SLAM方法得到的三維環境重構結果，物體輪廓清晰可辨，冗余點明顯減少。本算法實時性也遠優于 RGBD-SLAM，與ORBSLAM的實時性相當。RGBD-SLAM 處理一幀數據平均用時約為350 ms，每秒鐘只能處理 3 幀數據，因此不適于實時 SLAM。而本算法前端處理一幀數據平均約為31 ms，后端經過位姿優化與點云優化，以約5幀/秒的頻率更新點云地圖?？梢詽M足SLAM系統對實時性要求。

圖6 室內環境三維重構結果Fig. 6 3D reconstruction result of the indoor environment

5.2.4 機器人運動軌跡跟蹤

為了獲得機器人位姿的變化，本文以3D-2D的特征點約束，采用PNP求解的方法對機器人相鄰位姿進行估計，并借助圖優化方法對估計位姿進行優化。為了使運動軌跡更為直觀，實驗給出了本文算法在不同數據集下，機器人運動軌跡在X/Y平面上的投影，如圖7所示。表1則記錄了不同算法在不同數據集下，由計算所得的機器人在三維空間中的運動軌跡的真實位姿之間的均方根誤差(RMSE)。通過均方根誤差的對比，可有效地評估算法對機器人位姿跟蹤的誤差大小?？梢钥闯霰疚姆椒茌^好地實現機器人的運動軌跡跟蹤。

圖7 真實位姿與估計位姿對比圖Fig. 7 Comparison between ground truth and estimated trajectories

表1 不同算法位姿估計的均方根誤差對比Table 1 Pose estimation comparison of different algorithms about RMSE /m

6 結束語

在本文基于圖優化的視覺SLAM算法前端框架中，采用的ORB特征檢測、RANSAC算法等方法，較好地解決了幀間配準過程中匹配誤差較大、效率低下的問題。在三維地圖生成部分，以圖優化后的位姿為基礎，進行點云構建，并引入閉環檢測的機制，實現了對位姿與稠密化點云的雙重優化。實驗驗證了本文所提方法能夠準確、迅速地重構出稠密化的三維環境模型，并能精確估計出移動機器人的運動軌跡。下一步計劃對系統進行兩方面的改進: 1)改進前端的幀間配準方法，考慮采用直接法進行位姿跟蹤，完成位姿估計；2)研究環境三維地圖與語義信息的結合，實現更好的人機交互與導航。

[1]ZHANG Zhengyou. Microsoft kinect sensor and its effect[J]. IEEE multimedia, 2012, 19(2): 4–10.

[2]ENDRES F, HESS J, STURM J, et al. 3-D mapping with an RGB-D camera[J]. IEEE transactions on robotics, 2014,30(1): 177–187.

[3]MUR-ARTAL R, MONTIEL J M M, TARDóS J D. ORBSLAM: a versatile and accurate monocular slam system[J].IEEE transactions on robotics, 2015, 31(5): 1147–1163.

[4]RUSU R B, COUSINS S. 3D is here: point cloud library(PCL)[C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE,2011: 1–4.

[5]HARTLEY R, ZISSERMAN A. 計算機視覺中的多視圖幾何[M]. 韋穗, 楊尚駿, 章權兵, 等, 譯. 合肥: 安徽大學出版社, 2002: 158–256.HARTLEY R, ZISSERMAN A. Multiple view geometry in computer vision[M]. WEI Sui, YANG Shangjun, ZHANG Quanbing, et al, Trans. Hefei: Anhui University Press, 2002:158–256.

[6]LABBé M, MICHAUD F. Online global loop closure detection for large-scale multi-session graph-based slam[C]//Proceedings of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago, IL, USA: IEEE,2014: 2661–2666.

[7]RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: an efficient alternative to SIFT or SURF[C]//Proceedings of 2011 International conference on computer vision. Barcelona, Spain: IEEE, 2011: 2564–2571.

[8]梁明杰, 閔華清, 羅榮華. 基于圖優化的同時定位與地圖創建綜述[J]. 機器人, 2013, 35(4): 500–512.LIANG Mingjie, MIN Huaqing, LUO Ronghua. Graphbased SLAM: a survey[J]. Robot, 2013, 35(4): 500–512.

[9]FADDEEV L D, RESHETIKHIN N Y, TAKHTAJAN L A.Quantization of lie groups and lie algebras[M]//KASHIWARA M, KAWAI T. Algebraic analysis: papers dedicated to professor mikio Sato on the occasion of his sixtieth birthday. Boston, MA: Academic Press, 2014: 129–139.

[10]SHAWASH J, SELVIAH D R. Real-time nonlinear parameter estimation using the Levenberg–Marquardt algorithm on field programmable gate arrays[J]. IEEE transactions on industrial electronics, 2013, 60(1): 170–176.

[11]KüMMERLE R, GRISETTI G, STRASDAT H, et al. G2o:a general framework for graph optimization[C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011: 3607–3613.

[12]STURM J, MAGNENAT S, ENGELHARD N, et al. Towards a benchmark for RGB-D SLAM evaluation[C]//RGB-D Workshop on Advanced Reasoning with Depth Cameras at Robotics: Science and Systems Conference. Los Angeles,United States: RSS, 2011: 1–2.