基于圖優(yōu)化的移動(dòng)機(jī)器人視覺(jué)SLAM

張毅，沙建松

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

1 系統(tǒng)結(jié)構(gòu)

本文提出的室內(nèi)環(huán)境下移動(dòng)機(jī)器人三維視覺(jué)SLAM算法的總體框架如圖1所示，主要分為前端的關(guān)鍵幀篩選、幀間配準(zhǔn)、閉環(huán)檢測(cè)和后端的圖優(yōu)化、點(diǎn)云融合、點(diǎn)云優(yōu)化幾個(gè)部分。

圖1 基于圖優(yōu)化的視覺(jué)SLAM系統(tǒng)框架Fig. 1 Visual-SLAM system framework based on graph optimization

2 特征點(diǎn)的檢測(cè)與配準(zhǔn)

2.1 特征點(diǎn)的檢測(cè)

特征點(diǎn)的檢測(cè)與匹配是視覺(jué)SLAM問(wèn)題中“圖像前端”的部分，也是視覺(jué)SLAM中占用計(jì)算資源最多，最影響系統(tǒng)實(shí)時(shí)性的部分。基于FAST特征檢測(cè)改進(jìn)的ORB特征[7]具有旋轉(zhuǎn)不變和尺度不變特性的二進(jìn)制特征，它是一種高效的具有良好針對(duì)視圖不變的特征，比SIFT特征檢測(cè)快兩個(gè)數(shù)量級(jí)，同時(shí)特征點(diǎn)的質(zhì)量較高、魯棒性好。考慮到SLAM問(wèn)題對(duì)魯棒性、實(shí)時(shí)性的要求，本文采用了ORB特征提取算法。

2.2 特征點(diǎn)的配準(zhǔn)

圖像產(chǎn)生的本質(zhì)是光線通過(guò)相機(jī)透鏡投影到圖像傳感器上，這個(gè)過(guò)程是3-D場(chǎng)景到2-D平面的投影，因此場(chǎng)景與它的成像之間存在重要的聯(lián)系，相同場(chǎng)景的不同成像之間也有聯(lián)系。而通過(guò)這些聯(lián)系，可以實(shí)現(xiàn)特征點(diǎn)的配準(zhǔn)。

特征點(diǎn)配準(zhǔn)算法可以分為以下5個(gè)步驟：

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

2) 提取描述子：對(duì)于檢測(cè)出的角點(diǎn)，用一些數(shù)學(xué)上的特征對(duì)其進(jìn)行描述，這里通過(guò)cv::FeatureDetectorExtractor接口設(shè)置特征描述子提取器。

3) 匹配：通過(guò)各個(gè)角點(diǎn)的描述子來(lái)判斷它們?cè)趦蓮垐D像中的對(duì)應(yīng)關(guān)系。這里采用的方法為快速最近鄰算法FLANN。

4) 去外點(diǎn)：利用RANSAC算法去除錯(cuò)誤匹配的外點(diǎn)，保留正確的內(nèi)點(diǎn)。

5) 位姿求解：調(diào)用Opencv的PnPsolver()函數(shù)，求解相機(jī)的位姿運(yùn)動(dòng)(R,t)。

3 位姿優(yōu)化

針對(duì)傳統(tǒng)的利用濾波方法估計(jì)位姿存在線性化誤差、更新效率低下等問(wèn)題，本文依據(jù)SLAM研究中的圖優(yōu)化[8]思想，設(shè)計(jì)了一種基于非線性優(yōu)化的位姿優(yōu)化方法，同時(shí)實(shí)現(xiàn)局部位姿優(yōu)化和全局位姿優(yōu)化。

3.1 模型的建立

SLAM的經(jīng)典數(shù)學(xué)模型通常分為3部分：狀態(tài)變量、運(yùn)動(dòng)方程、觀測(cè)方程。

本文主要以Kinect作為主要的傳感器輸入，通過(guò)對(duì)視覺(jué)信號(hào)的處理，構(gòu)建視覺(jué)里程計(jì)，用于位姿估計(jì)。所以這里只考慮狀態(tài)變量和觀測(cè)方程，構(gòu)建出目標(biāo)函數(shù)：

目標(biāo)函數(shù)的物理意義：這里假設(shè)了誤差函數(shù)e(·)符合高斯分布，則SLAM問(wèn)題的貝葉斯概率描述為，求解適合的，使系統(tǒng)最有可能產(chǎn)生當(dāng)前的觀測(cè)數(shù)據(jù)Z，即最大似然(MLE)：

圖2 優(yōu)化求解流程Fig. 2 Process of optimizing solution

3.2 圖的建立與優(yōu)化

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

圖3 圖的建立與優(yōu)化Fig. 3 Establishment and optimization of graph

4 閉環(huán)檢測(cè)下的點(diǎn)云優(yōu)化

在基于位姿增量估計(jì)的SLAM建圖中，會(huì)產(chǎn)生誤差的累積，則在長(zhǎng)時(shí)間的大范圍SLAM建圖中，就需要閉環(huán)檢測(cè)對(duì)位姿和生成的點(diǎn)云進(jìn)行優(yōu)化。RGBD-SLAM算法是在局部回環(huán)檢測(cè)后，只使用局部回環(huán)優(yōu)化后的位姿作為三維地圖點(diǎn)云的重建。ORB-SLAM算法則是在運(yùn)動(dòng)跟蹤過(guò)程中，時(shí)刻檢測(cè)并維護(hù)關(guān)鍵幀之間的點(diǎn)云共視關(guān)系，同時(shí)實(shí)現(xiàn)對(duì)點(diǎn)云優(yōu)化。但這種方法只適用于規(guī)模有限的稀疏點(diǎn)云，并不適用于稠密化點(diǎn)云的優(yōu)化。基于此，提出了一種基于局部閉環(huán)與全局閉環(huán)約束的點(diǎn)云優(yōu)化算法，同時(shí)完成對(duì)位姿與稠密化點(diǎn)云的優(yōu)化。算法具體步驟如下：

1) 封裝構(gòu)造數(shù)據(jù)幀F(xiàn)rame，根據(jù)幀間的關(guān)聯(lián)程度，篩選出關(guān)鍵幀序列。

2) 執(zhí)行幀間配準(zhǔn)。完成相機(jī)位姿初始估計(jì)。

3) 在相機(jī)觀測(cè)約束下，直接應(yīng)用式(6)，構(gòu)建如圖3(a)所示的圖優(yōu)化。求得優(yōu)化后的相機(jī)的位姿變化，將原始點(diǎn)云進(jìn)行坐標(biāo)系變換，實(shí)現(xiàn)點(diǎn)云的拼接融合。

4) 執(zhí)行閉環(huán)檢測(cè)。通過(guò)動(dòng)態(tài)計(jì)算閉環(huán)幀篩選閾值，得到匹配質(zhì)量較高的閉環(huán)幀候選序列。

5) 閉環(huán)檢測(cè)結(jié)束后，遍歷閉環(huán)幀候選序列，在當(dāng)前幀與閉環(huán)候選幀之間，進(jìn)行Sim3(相似變換矩陣)求解與優(yōu)化。當(dāng)某一候選幀通過(guò)了Sim3求解與優(yōu)化，則認(rèn)為閉環(huán)檢測(cè)成功，跳出對(duì)閉環(huán)幀候選序列的遍歷。求得對(duì)應(yīng)相似變換矩陣為

6) 位姿優(yōu)化的傳播。當(dāng)前幀坐標(biāo)系與世界坐標(biāo)系之間的變換在5)中已經(jīng)確定并優(yōu)化。通過(guò)一級(jí)相連關(guān)系的傳播，可以確定這些相連的關(guān)鍵幀坐標(biāo)系與世界坐標(biāo)系之間的相似變換矩陣。

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

5.1 實(shí)驗(yàn)平臺(tái)

為了驗(yàn)證算法的有效性，并保證本文實(shí)驗(yàn)的一致可對(duì)比性，故采用了知名的TUM標(biāo)準(zhǔn)數(shù)據(jù)集[12]，并選用了基于Kinect視覺(jué)傳感器采集的室內(nèi)基準(zhǔn)數(shù)據(jù)包。該基準(zhǔn)數(shù)據(jù)包中包含Kinect 產(chǎn)生的彩色圖像和深度圖像序列，以及機(jī)器人的真實(shí)運(yùn)動(dòng)位姿。本文實(shí)驗(yàn)數(shù)據(jù)處理平臺(tái)為一臺(tái)Intel雙核2.4 GHz主頻的PC機(jī)，運(yùn)行Ubuntu 14.04操作系統(tǒng)。程序均設(shè)定以30幀/秒的速度同時(shí)讀取數(shù)據(jù)包中的RGB數(shù)據(jù)和深度數(shù)據(jù)。

5.2 室內(nèi)環(huán)境下的三維視覺(jué)SLAM

5.2.1 特征提取與特征點(diǎn)配準(zhǔn)

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

圖4 特征提取與配準(zhǔn)實(shí)驗(yàn)結(jié)果Fig. 4 The experimental results of feature extraction and matching

5.2.2 回環(huán)檢測(cè)約束

通過(guò)回環(huán)檢測(cè)，可以增加幀間的約束，進(jìn)一步提升系統(tǒng)的魯棒性，進(jìn)而提升整個(gè)SLAM的軌跡與地圖的質(zhì)量。在本文算法中，通過(guò)篩選關(guān)鍵幀之后進(jìn)行回環(huán)檢測(cè)，并以PNP求解的結(jié)果作為對(duì)邊的估計(jì)，增加到圖優(yōu)化中，如圖5所示。

圖5 回環(huán)檢測(cè)產(chǎn)生的約束Fig. 5 Loop closure of constraint

5.2.3 三維環(huán)境重構(gòu)效果對(duì)比

圖6(a)為采用了ORB-SLAM方法得到的三維環(huán)境重構(gòu)結(jié)果，所構(gòu)建的三維地圖為稀疏點(diǎn)云。圖6(b)為采用RGBD-SLAM方法得到的三維環(huán)境重構(gòu)結(jié)果，物體輪廓可以辨別，但冗余點(diǎn)相對(duì)較多。圖6(c)為采用本文的SLAM方法得到的三維環(huán)境重構(gòu)結(jié)果，物體輪廓清晰可辨，冗余點(diǎn)明顯減少。本算法實(shí)時(shí)性也遠(yuǎn)優(yōu)于 RGBD-SLAM，與ORBSLAM的實(shí)時(shí)性相當(dāng)。RGBD-SLAM 處理一幀數(shù)據(jù)平均用時(shí)約為350 ms，每秒鐘只能處理 3 幀數(shù)據(jù)，因此不適于實(shí)時(shí) SLAM。而本算法前端處理一幀數(shù)據(jù)平均約為31 ms，后端經(jīng)過(guò)位姿優(yōu)化與點(diǎn)云優(yōu)化，以約5幀/秒的頻率更新點(diǎn)云地圖。可以滿足SLAM系統(tǒng)對(duì)實(shí)時(shí)性要求。

圖6 室內(nèi)環(huán)境三維重構(gòu)結(jié)果Fig. 6 3D reconstruction result of the indoor environment

5.2.4 機(jī)器人運(yùn)動(dòng)軌跡跟蹤

為了獲得機(jī)器人位姿的變化，本文以3D-2D的特征點(diǎn)約束，采用PNP求解的方法對(duì)機(jī)器人相鄰位姿進(jìn)行估計(jì)，并借助圖優(yōu)化方法對(duì)估計(jì)位姿進(jìn)行優(yōu)化。為了使運(yùn)動(dòng)軌跡更為直觀，實(shí)驗(yàn)給出了本文算法在不同數(shù)據(jù)集下，機(jī)器人運(yùn)動(dòng)軌跡在X/Y平面上的投影，如圖7所示。表1則記錄了不同算法在不同數(shù)據(jù)集下，由計(jì)算所得的機(jī)器人在三維空間中的運(yùn)動(dòng)軌跡的真實(shí)位姿之間的均方根誤差(RMSE)。通過(guò)均方根誤差的對(duì)比，可有效地評(píng)估算法對(duì)機(jī)器人位姿跟蹤的誤差大小。可以看出本文方法能較好地實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)軌跡跟蹤。

圖7 真實(shí)位姿與估計(jì)位姿對(duì)比圖Fig. 7 Comparison between ground truth and estimated trajectories

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

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

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

[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. 計(jì)算機(jī)視覺(jué)中的多視圖幾何[M]. 韋穗, 楊尚駿, 章權(quán)兵, 等, 譯. 合肥: 安徽大學(xué)出版社, 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]梁明杰, 閔華清, 羅榮華. 基于圖優(yōu)化的同時(shí)定位與地圖創(chuàng)建綜述[J]. 機(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.