移動機器人RGB-D視覺SLAM算法

陳 劭 郭宇翔 高天嘯 宮清源 張軍國

(1.北京林業大學工學院， 北京 100083； 2.林業裝備與自動化國家林業和草原局重點實驗室， 北京 100083)

0 引言

移動機器人涉及控制理論、人工智能技術、多傳感器數據融合技術、電子信息與計算機技術等多學科交叉，在很多領域得到了廣泛應用。移動機器人通常在無全局定位信息的未知環境中作業，如林地勘測、災害救援等。如何在未知環境中利用自身攜帶的傳感器，通過感知自身狀態和周圍環境完成自主導航是其面臨的主要挑戰之一。同步定位與地圖構建技術可以獲取精確的位置信息與地圖環境信息，成為解決上述難題的重要手段與當前機器人領域的研究熱點。

在同步定位與地圖構建技術中，根據采用的傳感器類型，可以分為激光雷達[1-2]與視覺采集設備[3-4]兩種。微軟Kinect、英特爾RealSense、華碩Xtion Pro Live等一系列RGB-D相機，可同時獲得RGB圖像及與之匹配的深度圖像，節省了大量的計算時間，較好地滿足了移動機器人視覺同步定位與地圖構建(Visual simultaneous localization and mapping, SLAM)過程中對實時性的要求，且設備價格低廉，因而得到廣泛應用[5-9]。

2012年，基于Kinect相機，NEWCOMBE等[10]提出了Kinect Fusion算法，借助GPU工具使算法速度達到30 Hz，滿足了實時性處理要求，但對硬件設備要求較高且算法中沒有閉環檢測環節，算法精確度較低、魯棒性較差。隨后，HENRY等[11]利用隨機抽樣一致性(Random sample consensus, RANSAC)的方法獲得相鄰兩幀圖像之間的6D變換矩陣，再結合深度數據與迭代最近點(Iterative closest point, ICP)算法優化變換矩陣，獲得相機位姿，然后利用稀疏捆綁調整算法對得到的機器人位姿進行優化，得到全局一致的地圖。2014年，ENDRES等[12]在上述基礎上開發出一套較為完善的RGB-D SLAM系統，將整個SLAM過程分為前端處理與后端優化兩部分，在HENRY等的研究基礎上使用非線性誤差函數的框架G2O優化方法進行全局位姿圖優化，生成全局彩色稠密點云地圖。但該方法的定位精度及算法實時性均有待改善。

因此，針對移動機器人視覺SLAM算法中存在的精確度較低、實時性較差等問題，本文提出一種用于移動機器人的RGB-D視覺SLAM算法，對傳統的RANSAC算法進行改進，將改進后的RE-RANSAC算法與GICP算法相結合來解決上述問題，并利用FR1數據集對本文算法的性能進行測試。

1 算法框架

本文提出的基于RGB-D數據的視覺SLAM算法的總體框架如圖1所示。首先提取出RGB圖像中的特征點并完成匹配，結合深度圖像數據將2D平面特征點轉換為3D點云，根據三維數據估計優化相機位姿，然后對圖像進行篩選得到關鍵幀序列并完成幀間配準，再通過閉環檢測及圖優化得到全局最優相機位姿，最終經過點云拼接構建三維點云地圖。

圖1 基于RGB-D數據的實時視覺SLAM算法總體框架Fig.1 Overall framework of real-time visual SLAM algorithm based on RGB-D data

2 特征點提取與匹配

每幀RGB-D數據約有30萬個點，若直接用整幀RGB-D數據進行定位及地圖構建，會因數據量過大導致算法執行速度過慢，實用性降低。因此在獲取RGB-D數據后，需提取RGB圖像中的特征點，以減少數據量，并獲得二維匹配點對，再結合深度數據，進而得到特征點相對于相機坐標系的三維坐標及三維匹配點對。

2.1 特征點的提取

特征點的提取包括特征檢測與描述符提取，目前常用的特征點提取算法有SIFT、SURF、ORB 3種。尺度不變特征變換(Scale invariant feature transform, SIFT)算法[13]與加速魯棒特征(Speeded up robust features, SURF)算法[14]計算量大、耗時長，無法滿足移動機器人在定位建圖過程中對實時性的要求。因此，本文采用基于ORB的特征檢測與描述符提取方法[15]對特征點進行提取，結果如圖2所示。該方法利用oFAST特征檢測與rBRIEF描述符提取，可保證特征點的旋轉不變性，而且運算速度快。

圖2 ORB特征點提取結果Fig.2 Key feature points extraction results based on ORB algorithm

2.2 特征點的匹配

提取到相鄰兩幀圖像的特征點后，通常采用暴力匹配 (BruteForce)方法進行匹配[12]，但當處理特征點數量較多或需匹配一幀圖像和整張地圖時，該方法匹配時間長，且誤匹配較多，如圖3a所示。針對上述問題，本文利用基于FLANN算法的雙向KNN特征匹配方法以減少誤匹配點，并采用多重隨機k-d樹方法，提高快速最近鄰搜索的速度，特征匹配效果如圖3b所示。

圖3 BruteForce與本文方法匹配對比 Fig.3 Matching effect comparison between BruteForce and proposed algorithm

為進一步提高特征點的匹配準確度，需剔除匹配中的誤匹配點。本文在RANSAC算法[16]的基礎上進行改進，利用改進后的RE-RANSAC算法剔除誤匹配點，算法原理如圖4所示。

圖4 RE-RANSAC算法原理圖Fig.4 Schematic of RE-RANSAC algorithm

本算法基于迭代思想，在每次迭代過程中隨機從三維坐標匹配點對中少量采樣，估計得出變換模型，然后使用整個三維坐標匹配點對集合對該模型進行評估，得到滿足該模型的局內點集合，根據局內點集合中的匹配點對重新求得新的變換模型。與原算法不同，RE-RANSAC算法在重新求得新的變換模型后，再次利用整個三維坐標匹配點對集合對該模型進行評估，獲得新的局內點集合與變換模型，再與當前最優模型進行比較篩選。當隨機采樣k個點時，有

1-p=(1-uk)N

(1)

式中p——迭代N次后得到的采樣集合中不包含局外點的概率

u——單次采樣得到局內點的概率

令v=1-u代表單次采樣得到局外點的概率，則迭代次數N為

(2)

在迭代結束得到最優變換模型、局內點集合及誤差后，若局內點數目大于閾值，誤差小于閾值，則利用該局內點集合再次計算運動變換模型，并使用上述方法獲得最終的局內點集合。

改進后的RE-RANSAC算法在每一次迭代過程中均對運動模型進行二次評估篩選，以提高算法精確度，并通過減少迭代次數提高運算速度。RE-RANSAC算法剔除誤匹配的效果如圖5所示。

圖5 RE-RANSAC算法剔除誤匹配后的效果Fig.5 Effect after removal of mis-match by using RE-RANSAC algorithm

3 位姿估計優化與地圖構建

完成相鄰幀的匹配后，需通過相鄰幀間的運動變換矩陣求解相機位姿并對其進行優化，進而得到全局最優相機位姿和相機運動軌跡，并經點云拼接，構建出三維點云地圖。

3.1 位姿估計與優化

相機位姿估計與優化的過程是對相鄰兩幀圖像間運動變換矩陣求解的過程。該矩陣由一個旋轉矩陣R和一個三維平移向量(tx,ty,tz)組成

(3)

若已知點P=(x,y,z,1)和對應的運動變換矩陣T，則點P在運動變換矩陣T的投影P′為

P′=T×P

(4)

基于ICP算法[17]求解相鄰兩幀間的運動變換矩陣，從而得到相機的估計位姿是一種較為經典的方法。但當初始變化選取不當或匹配點對數量較多時，該算法的誤差較大。因此本文提出先使用2.2節提出的改進后的RE-RANSAC算法迭代篩選獲得最優局內點集合估計的運動變換矩陣T，將其作為位姿優化過程中的初始條件。并在匹配點數量較多時采用精度更高的GICP算法對估計的運動變換矩陣進行優化。

利用GICP算法[18]優化運動變換矩陣T，實質是通過概率模型進行求解。在概率性模型中，假設存在2個集合，={i}和={i}，若、集合完全對應，則存在運動變換T*使得

i=T*i(i=1,2,…,N)

(5)

依據概率模型

ai～N(i,Ci,A)

(6)

bi～N(i,Ci,B)

(7)

式中Ci,A——集合A待觀測點的協方差矩陣

Ci,B——集合B待觀測點的協方差矩陣

產生實際的點云集合A={ai}和B={bi}。

(8)

結合極大似然估計(MLE)可求得最佳運動變換T為

(9)

式(9)可以簡化為

(10)

由此可得到優化后的運動變換矩陣T。

3.2 隨機閉環檢測

由于僅考慮了相鄰幀間的運動變換，導致上一幀所產生的誤差會傳遞到下一幀。經過多幀圖像累積，累積誤差最終使得計算出的軌跡出現嚴重偏移。本節通過增加閉環檢測環節來減少累計誤差。

在SLAM過程中，由于圖像幀數較多，因此首先需通過篩選得到關鍵幀以減少算法處理時間。具體方法如下：設關鍵幀序列為Pi(i=0,1，…，N),且第1幀圖像P0為首幀關鍵幀，每采集到一幀新的圖像需計算其與序列Pi中最后一幀的運動變換，得到運動變換矩陣Ti(i=0,1，…，N),若該運動變換量符合所設定的閾值要求，則判定該幀為關鍵幀，并將該幀與Pi中的最后m個關鍵幀進行匹配，最后將該幀與從Pi中隨機取出的n個關鍵幀進行匹配。若匹配成功，則可由此形成圖6所示的局部回環或全局閉環，快速有效地減小累積誤差。

圖6 局部回環Fig.6 Local closed-loop

3.3 G2O位姿圖優化

在位姿圖中增加閉環約束后，為得到全局最優運動變換約束和全局最優相機位姿，采用G2O優化方法對整個位姿圖進行優化。G2O優化方法[19]可將位姿圖優化的問題轉換為非線性最小二乘法問題，在已知運動變換約束與閉環約束情況下，求解相機位姿最優配置x*。其中目標函數F(x)定義為

(11)

(12)

式中xi、xj——第i時刻和第j時刻相機的位姿，即G2O圖中的節點

C——時間序列集合

zij——位姿xi和xj之間的實際約束關系

Ωij——位姿xi和xj之間的信息矩陣，即兩節點間的不確定性

e(xi,xj,zij)——2個節點組成的邊所產生的向量誤差函數

向量誤差函數可表示為

e(xi,xj,zij)=zij-ij(xi,xj)

(13)

該誤差函數表示xi和xj滿足約束條件zij的程度，當xi和xj完全滿足約束條件時，其值為0。

3.4 地圖構建

在得到優化后的相機位姿及相機運動軌跡后，根據不同相機位姿下的觀察結果生成不同的點云，將這些點云全部變換到全局相機位姿下進行累加點云拼接，即可構建當前時刻的三維點云地圖。

然而，由于每幀點云數據量巨大，一幀分辨率為640像素×480像素的圖像，有30多萬個點，導致地圖過于龐大，占用大量存儲空間。因此本文將稠密的三維點云地圖保存為基于八叉樹的OctoMap。OctoMap[20]本質上是一種三維柵格地圖，其基本組成單元是體素，可以通過改變體素的大小來調整該地圖的分辨率。該類型地圖可用于后續移動機器人的路徑規劃及導航。

4 實驗結果與分析

4.1 實驗平臺

為驗證算法的有效性，并保證實驗的一致對比性，本文實驗采用TUM標準數據集[21]中基于Kinect視覺傳感器采集的室內基準數據包。該基準數據包中包含Kinect產生的彩色圖像和深度圖像序列，以及相機的真實運動位姿。本文算法運行在配置四核2.5 GHz主頻、i7處理器的PC機上，使用Ubuntu14.04操作系統。實驗程序中的RGB數據和深度數據均設定以30 f/s的速度同時從數據包中讀取。

4.2 數據集實驗結果分析

4.2.1特征提取對比

本文分別使用SIFT、SURF、ORB 3種算法提取FR1/xyz數據包中全部798幀圖像的特征點，并對3種算法的特征點提取數目以及提取時間進行對比，結果如圖7所示。圖7中μ表示平均值。

圖7 SIFT、SURF、ORB算法對比Fig.7 Comparison between SIFT, SURF and ORB algorithms

從圖7可知，SIFT算法、SURF算法、ORB算法平均每幀圖像提取的特征點數目分別為1 328、1 641、500個，所用時間分別為118.3、131.8、6.2 ms。在滿足特征點匹配數目要求的情況下，ORB算法的運行速度比SIFT算法和SURF算法快2個數量級，可滿足視覺SLAM的實時性要求。

4.2.2RANSAC算法改進對比

為驗證RE-RANSAC算法可提高局內點比例，使得算法精度提高、運行時間減少，本節根據FR1/xyz、FR1/desk、FR1/desk2和FR1/plant 4個數據包對RANSAC算法與RE-RANSAC算法進行對比分析，對比結果如表1所示。

表1 RANSAC算法與RE-RANSAC算法對比Tab.1 Comparison between RANSAC algorithm and RE-RANSAC algorithm

從表1中可以看出，針對所測試的4個FR1數據包，改進后的RE-RANSAC算法的平均每幀圖像的局內點比例分別為41.7%、43.4%、42.9%、39.8%，平均每幀圖像的迭代次數分別為243、231、257、389次，平均每幀圖像的處理時間分別為0.005 3、0.005 2、0.005 4、0.007 8 s。經對比可知，改進后的RE-RANSAC算法較RANSAC算法提高了局內點比例，使得算法迭代次數減少，提升了算法的計算速度。

4.2.3相機運動軌跡估計

為使估計軌跡與真實軌跡的對比更加直觀，實驗給出了在本文算法下TUM數據集中FR1/desk、FR1/desk2、FR1/xyz、FR1/plant 4個數據包的相機真實軌跡及估計軌跡在XY平面上的投影，如圖8所示。

圖8 真實軌跡與估計軌跡比較Fig.8 Comparison between real trajectory and estimated trajectory

4.2.4定位準確性及實時性對比

為驗證本文算法定位準確性及實時性，實驗計算出真實位姿與利用本文方法得到的估計位姿之間的均方根誤差(Root mean square error, RMSE)及平均每幀數據的處理時間，并將本文方法獲得的RMSE值及算法處理時間與文獻[12]中提出的RGB-D SLAM方法獲得的數值進行對比，對比結果如表2所示。

從表2可以看出，針對所測試的4個FR1數據包，本文方法定位精度及處理速度均優于RGB-D SLAM。經計算，本文方法的平均RMSE約為0.024 5 m，平均每幀數據處理用時約為0.032 s，與RGB-D SLAM方法相比，分別降低了40%和90%。

表2 本文方法與RGB-D SLAM方法的性能對比Tab.2 Comparison between proposed algorithm and RGB-D SLAM algorithm

4.3 地圖構建評估

以FR1/plant數據包和實驗室樓道實際場景建圖結果為例，說明本文算法構建地圖的準確性。

4.3.1FR1/plant數據包建圖

在FR1/plant數據包中，RGB-D相機圍繞花盆旋轉，從各個角度拍攝花盆，構建好稠密點云地圖后將其保存為OctoMap形式，分辨率為2 cm，如圖9所示。從三維地圖的不同角度可發現花盆、桌子、黑板等物體均得到了較好的重建。

圖9 FR1/plant 3D地圖Fig.9 FR1/plant 3D maps

4.3.2實際場景建圖

為驗證本文算法在實際環境中效果，利用Kinect實時采集得到的實際場景與本文算法得到的3D地圖進行對比，并對視覺傳感器軌跡進行估計。

圖10 實驗室樓道及其3D地圖Fig.10 Laboratory corridor and its 3D map

實驗室樓道實際場景如圖10a所示，共有兩部分，左側圖像為縱向樓道，右側圖像為橫向樓道。實驗過程中，將Kinect傳感器放置于移動機器人上，控制移動機器人從縱向樓道粉點處以直線運動至樓道岔口處，右轉至橫向樓道，轉彎路徑近似為半徑0.6 m的圓弧曲線，隨后行至橫向樓道藍點處并完成180°的轉向，返回至樓道岔路口，最后再行至縱向樓道盡頭。在此過程中平均移動速度為0.3 m/s。

根據本文算法所構建的實驗室樓道3D地圖如圖10b所示，可以看出，3D地圖中的地板、墻壁及直角拐彎處等均得到了較好地還原。移動機器人的位置軌跡如圖11所示。實驗過程中，移動機器人在樓道拐角處的轉向角為90°，在縱向樓道和橫向樓道中的實際直線位移距離分別為7.70、3.00 m。本文算法得到的轉向角為92.2°，角度誤差為2.4%，在縱向、橫向定位結果中移動機器人的位移分別為7.84、3.05 m，縱向、橫向位移定位誤差分別為1.8%、1.7%。

圖11 移動機器人軌跡Fig.11 Trajectory of mobile robot

5 結束語

提出了一種用于移動機器人的RGB-D視覺SLAM算法，該算法采用ORB算法提取特征點，隨后提出一種改進的RE-RANSAC算法，通過與GICP算法的結合獲得了最優相機位姿，最后利用隨機閉環檢測及G2O圖優化方法減小了累積誤差，并拼接構建了三維點云地圖。通過實驗對比SIFT、SURF、ORB 3種算法提取的特征點數目以及提取時間，說明本文采用的ORB算法在處理速度上優勢明顯。通過對FR1數據集的測試，對比了本文算法與RGB-D SLAM算法，驗證了本文算法定位準確性及快速性，并以FR1/plant數據包和實驗室樓道實際場景建圖效果為例說明了算法建圖的準確性。本文算法的最小定位誤差為0.011 m，平均定位誤差為0.024 5 m，平均每幀數據處理時間為0.032 s。