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

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

0 引言

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

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

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

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

1 算法框架

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

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

2 特征點提取與匹配

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

2.1 特征點的提取

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

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

2.2 特征點的匹配

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

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

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

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

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

1-p=(1-uk)N

(1)

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

u——單次采樣得到局內(nèi)點的概率

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

(2)

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

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

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

3 位姿估計優(yōu)化與地圖構(gòu)建

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

3.1 位姿估計與優(yōu)化

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

(3)

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

P′=T×P

(4)

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

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

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

(5)

依據(jù)概率模型

ai～N(i,Ci,A)

(6)

bi～N(i,Ci,B)

(7)

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

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

產(chǎn)生實際的點云集合A={ai}和B={bi}。

(8)

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

(9)

式(9)可以簡化為

(10)

由此可得到優(yōu)化后的運動變換矩陣T。

3.2 隨機閉環(huán)檢測

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

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

圖6 局部回環(huán)Fig.6 Local closed-loop

3.3 G2O位姿圖優(yōu)化

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

(11)

(12)

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

C——時間序列集合

zij——位姿xi和xj之間的實際約束關(guān)系

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

e(xi,xj,zij)——2個節(jié)點組成的邊所產(chǎn)生的向量誤差函數(shù)

向量誤差函數(shù)可表示為

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

(13)

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

3.4 地圖構(gòu)建

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

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

4 實驗結(jié)果與分析

4.1 實驗平臺

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

4.2 數(shù)據(jù)集實驗結(jié)果分析

4.2.1特征提取對比

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

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

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

4.2.2RANSAC算法改進對比

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

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

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

4.2.3相機運動軌跡估計

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

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

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

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

從表2可以看出，針對所測試的4個FR1數(shù)據(jù)包，本文方法定位精度及處理速度均優(yōu)于RGB-D SLAM。經(jīng)計算，本文方法的平均RMSE約為0.024 5 m，平均每幀數(shù)據(jù)處理用時約為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 地圖構(gòu)建評估

以FR1/plant數(shù)據(jù)包和實驗室樓道實際場景建圖結(jié)果為例，說明本文算法構(gòu)建地圖的準確性。

4.3.1FR1/plant數(shù)據(jù)包建圖

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

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

4.3.2實際場景建圖

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

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

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

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

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

5 結(jié)束語

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