基于RGB-D的移動機器人三維視覺SLAM

，，

(華南理工大學(xué) 聚合物新型成型裝備國家工程研究中心 聚合物成型加工工程教育部重點實驗室,廣州 510640)

基于RGB-D的移動機器人三維視覺SLAM

文生平，舒凱翔，洪華鋒

(華南理工大學(xué)聚合物新型成型裝備國家工程研究中心聚合物成型加工工程教育部重點實驗室,廣州510640)

針對移動機器人三維視覺SLAM中存在的實時性不高，匹配誤差較大的問題，提出了一種改進的特征匹配算法，提取ORB特征進行特征匹配，并采用基于最小距離與RANSAC結(jié)合的方法剔除誤匹配;針對位姿估計不準(zhǔn)確的問題，將傳統(tǒng)的ICP算法與PNP算法結(jié)合提高配準(zhǔn)成功率，并通過RANSAC與優(yōu)化位姿估計;實驗結(jié)果表明該方法能有效提高SLAM系統(tǒng)的實時性和位姿估計的準(zhǔn)確性。

移動機器人；Kinect相機；同時定位與地圖構(gòu)建；特征匹配；點云配準(zhǔn)

0 引言

智能移動機器人要求能夠在環(huán)境中自我導(dǎo)航與定位,而同步定位和建圖(SLAM)被認(rèn)為是實現(xiàn)移動機器人完全自主移動的關(guān)鍵。隨著傳感器技術(shù)的發(fā)展，基于二維激光傳感器的自定位技術(shù)已經(jīng)逐漸成熟，然而高精度的激光傳感器價格昂貴，性價比很低。相對于二維激光傳感器，相機具有更高性價比的同時能夠獲得更多的信息。2010年6月微軟發(fā)布的Kinect為三維視覺SLAM提供了一種很好的思路。Kinect價格低廉，結(jié)構(gòu)緊湊，具有一個紅外相機與一個普通相機，分別用于采集深度(Depth)信息與彩色(RGB)信息，因而十分適合運用于三維SLAM領(lǐng)域。

傳統(tǒng)的SLAM通常是基于濾波器求解，使用卡爾曼濾波，擴展卡爾曼濾波和粒子濾波等作為數(shù)學(xué)模型。如果在視覺SLAM中采用基于濾波器的方法，隨著時間推移地圖擴大，內(nèi)存消耗和計算量都很大，難以用于大規(guī)模建圖。基于圖優(yōu)化的SLAM把優(yōu)化問題表現(xiàn)成圖的形式，相機的位姿節(jié)點和路標(biāo)點表示圖的頂點，相機的運動模型和觀測模型表示為圖的邊，用圖模型來表示一個非線性最小二乘問題。圖優(yōu)化能夠利用更多的信息，相對于濾波器方法，圖優(yōu)化在精度和效率上都有更好的表現(xiàn)。2010年Henry等人提出了一種室內(nèi)三維V-SLAM方法[1]，其方法通過從Kinect獲取的RGB圖像中提取SIFT特征點，結(jié)合深度數(shù)據(jù)與迭代最近點算法(ICP)[2]估計出手持?jǐn)z像機的位姿，同時建立環(huán)境的三維模型。雖然SIFT特征具有尺度不變性，但是帶來了巨大地計算量，目前為止計算機還無法實時地計算SIFT特征。針對上述研究現(xiàn)狀，本文提出了一種室內(nèi)環(huán)境下移動機器人三維視覺SLAM方法。該方法利用Kinect獲取圖像信息，提取ORB特征點進行特征匹配[3]；針對圖像特征匹配精度不高影響位姿估計精度地問題，利用隨機采樣一致性(RANSAC)與最小距離法相結(jié)合消除誤匹配特征點；在位姿估計階段，采用ICP與PNP算法結(jié)合提高配準(zhǔn)成功率，并采用RANSAC與優(yōu)化當(dāng)前位姿。實驗驗證本文方法可以有效提高系統(tǒng)的實時性和位姿估計的準(zhǔn)確性。

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

本文提出的基于RGB-D圖像的移動機器人三維視覺SLAM總體框架如圖1所示，主要包括特征提取與匹配，點云的生成與配準(zhǔn)，位姿優(yōu)化3部分。

圖1 基于RGB-D圖像的移動機器人三維視覺SLAM總體框架

2 特征提取與匹配

視覺SLAM按是否提取特征點，分為特征點法與直接法。直接法無需提取特征點，省去了計算特征點、描述子的時間，對于特征缺失的場景有很好的效果。但其完全依靠梯度搜索且目標(biāo)函數(shù)非凸，使得算法容易陷入極小值，只有在運動很小時才能成功。本文針對于較為一般的場景建圖，因此采用魯棒性更好的基于特征點SLAM。特征匹配常用的特征有SIFT，SURF等，然而這些特征雖然穩(wěn)定，但是較大的計算量難以滿足實時SLAM的需求。ORB特征是2011年提出的一種特征匹配算法，它是對FAST特征點與BREIF特征描述子的一種結(jié)合與改進，他改進了FAST檢測子不具有方向性的問題，并采用速度極快的二進制描述子BRIEF，使整個圖像特征提取的環(huán)節(jié)大大加速，對于實時SLAM來說是一個很好的選擇。

判斷基于二進制描述的特征點是否匹配，采用漢明距離來代替歐氏距離作為度量，兩個特征點之間的漢明距離指的是兩個二進制串對應(yīng)位置不同字符的個數(shù)，計算過程簡單，能有效縮短匹配所需的時間。

關(guān)于匹配算法的選擇，由于視覺SLAM中特征點數(shù)量很大，為了有效提高匹配速度，采用快速近似最近鄰(FLANN)作為匹配算法獲得粗匹配[4]。但是因此將導(dǎo)致粗匹配中存在大量的誤匹配，本文提出了一種基于最小匹配距離和RANSAC相結(jié)合的方法剔除誤匹配。方法如下：首先根據(jù)場景特征點數(shù)量大小設(shè)置判斷條件，當(dāng)場景紋理豐富，特征點數(shù)量較多時，計算匹配點的最小匹配距離，根據(jù)工程經(jīng)驗選取漢明距離大于最小匹配距離兩倍的匹配為誤匹配，并通過RANSAC迭代獲取內(nèi)點，進一步去除誤匹配；對于場景特征單調(diào)，容易引起歧義匹配的環(huán)境，為防止特征匹配過少引起匹配失敗，只使用RANSAC去除誤匹配。通過上述方法，能有效減少誤匹配的數(shù)量。

3 點云生成與點云配準(zhǔn)

3.1 Kinect相機標(biāo)定

要獲得三維點云的坐標(biāo)需要知道相機的內(nèi)參矩陣。本文采用了文獻[5]的標(biāo)定方法對Kinect的彩色相機與深度相機進行標(biāo)定，在完成了Kinect的標(biāo)定與校準(zhǔn)之后，就可

以通過圖中任意點的深度信息得到空間三維點云的坐標(biāo)信息。

3.2 三維點云的生成

根據(jù)針孔相機的成像模型，給定一個圖像點的像素坐標(biāo)(u,v)和對應(yīng)的深度值，對應(yīng)的三維坐標(biāo)可由如下計算公式得到：

(1)

(2)

(3)

式中，fx,fy分別為水平與垂直方向上的相機焦距；S為縮放系數(shù)；cx，cy為像素坐標(biāo)系與成像平面的平移量。

以上參數(shù)皆可由相機標(biāo)定得到。

3.3 點云配準(zhǔn)

在點云配準(zhǔn)階段，對于RGBD-SLAM[6]來說通常采用ICP算法來估計位姿，但是考慮到Kinect相機精度限制導(dǎo)致特征點的深度可能測量不到，本文通過ICP與PNP結(jié)合的方式進行位姿估計。

3.3.1 ICP算法

通過特征提取與匹配環(huán)節(jié)，兩幀圖像間相同特征點的一一對應(yīng)關(guān)系得以確立。在k時刻，當(dāng)前相機的位姿可以表示為

(4)

Tk表示從k-1時刻到k時刻相機的位姿偏移量：

式中,Rcw為3x3旋轉(zhuǎn)矩陣，tcw為3x1平移向量，cw表示從世界坐標(biāo)系到相機坐標(biāo)系。對于一組匹配好的3D點：

兩幀圖像間的歐式變換滿足

(5)

要求出滿足條件的R,t,通常采用ICP(Iterative Closest Point,迭代最近點)的方法來解決。

ICP求解通常用線性代數(shù)的方法，根據(jù)前面所述的ICP問題，定義第對i點的誤差項：

(6)

然后構(gòu)建最小二乘問題，求使誤差平方和達到極小的R,t:

(7)

這個最小二乘問題可以通過SVD分解來求出。當(dāng)特征匹配準(zhǔn)確，深度信息準(zhǔn)確的時候，根據(jù)參考文獻[7-8]，可以通過SVD求出最優(yōu)的R,t。

3.3.2 PNP算法

由于Kinect深度相機的測量范圍限制，匹配特征點的深度信息有可能測量不到，這時可以通過3D-2D的投影關(guān)系來計算位姿。

P3P問題通過給定的3對3D-2D匹配點之間的幾何關(guān)系計算出相機在世界坐標(biāo)系中的位姿變化。方法原理如圖2所示，已知A,B,C在世界坐標(biāo)系下的坐標(biāo)和對應(yīng)點a,b,c在圖像坐標(biāo)系下的坐標(biāo)，利用相似三角形原理和吳消元法，可以求得a,b,c三點在相機坐標(biāo)系下的3D坐標(biāo)，值得注意的是，這種方法會求出4個可能解，所以還需要一個驗證點D來計算出最可能的解。通過4對3D-2D的匹配點對可以將問題轉(zhuǎn)化為一個3D-3D的位姿估計問題，然后用ICP求解即可。

圖2 P3P問題

因此，對于深度信息已知的特征點，通過ICP算法計算位姿；對于深度信息未知的特征點，通過P3P算法求出相機空間坐標(biāo)，將3D-2D的問題轉(zhuǎn)化為3D-3D的問題，然后再利用ICP求解。

3.4 位姿優(yōu)化

點云配準(zhǔn)環(huán)節(jié)系統(tǒng)存在的噪聲會對位姿估計的結(jié)果產(chǎn)生很大的影響，這些噪聲主要來自兩方面：1.特征匹配環(huán)節(jié)仍然存在著誤匹配；2.由于Kinect相機測量深度信息采用結(jié)構(gòu)光原理，同時特征點往往處于物體邊緣處，在這些位置上的深度信息通常誤差比較大。這些噪聲的存在使得ICP算法在迭代的過程中很容易陷入局部極值，對位姿估計的準(zhǔn)確性帶來很大的影響。本文在這里采用在剔除誤匹配時采用的隨機采樣一致性方法來剔除噪聲。

3.4.1 RANSAC優(yōu)化位姿

通過RANSAC優(yōu)化位姿的算法步驟如下：

1)首先從匹配點集中隨機選取3個匹配對(滿足ICP算法條件，當(dāng)沒有深度信息時改用P3P算法)，利用ICP算法估計變換矩陣T；

2)設(shè)置評價函數(shù)式(8)作為內(nèi)點的篩選條件，為內(nèi)點篩選閾值

(8)

3)當(dāng)匹配點間距離小于則計算為一個內(nèi)點，重復(fù)隨機取點并統(tǒng)計內(nèi)點的數(shù)量，迭代N次后，內(nèi)點數(shù)量最多的T作為位姿估計。

3.4.2 圖優(yōu)化

對于空間點存在的測量誤差，可以將空間點和位姿估計同時作為優(yōu)化變量進一步優(yōu)化。目標(biāo)函數(shù)可以表示為：

(9)

這是一個非線性優(yōu)化問題，可以采用g2o優(yōu)化庫來進行優(yōu)化。將整個問題用圖的形式來表示：

1)節(jié)點：所有特征點的空間位置p，相機的位姿節(jié)點ξ∈s;

2)邊：每個空間點投影誤差將點和邊依次加入圖中，使用RANSAC優(yōu)化后得到的位姿估計作為初值，計算對應(yīng)于當(dāng)前估計值的雅可比矩陣和海塞矩陣，求解稀疏線性方程:

Hk△x=-bk

(5)

得到梯度方向,選用Levenberg-Marquardt不斷迭代(LM算法)，最終整體誤差穩(wěn)定不變時算法收斂，即得到相機位姿的最優(yōu)值。

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

4.1 實驗平臺介紹

本文實驗采用的深度相機Kinect，彩色圖像分辨率為640×480，深度圖像分辨率為320×240，最高幀率為30幀每秒，水平方向視角。

本文實驗采用的移動機器人平臺為Turtlebot2，如圖3所示，支持ROS操作系統(tǒng)，上面搭載一臺筆記本電腦作為數(shù)據(jù)發(fā)送端，通過ROS系統(tǒng)采集、傳輸圖像到遠程數(shù)據(jù)處理端PC，處理端PC配備主頻為2.5 GHZ的IntelCore i5處理器，運行Ubuntu 14.04操作系統(tǒng)，為了防止機器人移動太快導(dǎo)致特征點丟失，機器人以的線速度在室內(nèi)環(huán)境移動建圖。

圖3實驗裝置

4.2 真實場景下的三維視覺SLAM

4.2.1 特征提取與特征匹配

圖4對采集的圖像分別采用ORB、SIFT和SURF提取特征，驗證特征提取算法的效果。讓機器人運行一段時間并用Kinect采集圖像，按照時間順序選取15張圖像進行特征提取，計算平均特征點數(shù)目與平均耗時，結(jié)果如表1所示。從表中可知，ORB算法的運行速度遠遠優(yōu)于SIFT算法與SURF算法，后續(xù)的特征匹配實驗也可以證明，ORB算法在匹配準(zhǔn)確率方面的表現(xiàn)也十分優(yōu)異，所以對于實時性要求很高的三維SLAM來說是一個很好的選擇。

圖4特征提取算法效果示意圖

表1 特征點檢測結(jié)果對比

對相鄰幀進行特征匹配，圖5(a)是未經(jīng)篩選的的匹配結(jié)果，圖5(b)是經(jīng)過最小匹配距離與RANSAC結(jié)合篩選后的匹配結(jié)果，由圖可以看出該方法可以有效剔除誤匹配，提高特征匹配的準(zhǔn)確性。

圖5 誤匹配剔除效果示意圖

4.2.2 三維地圖的構(gòu)建

機器人以0.2 m/s的速度在室內(nèi)移動，位姿優(yōu)化的最大迭代次數(shù)為50，建圖流程如前所述：提取ORB特征，特征匹配與誤匹配的剔除，位姿估計與位姿優(yōu)化。實驗中一般認(rèn)為匹配的特征點對大于50為有效匹配，否則匹配點過少可能會引起位姿估計不準(zhǔn)確。由于路面不平或機器人運動不平滑可能引起相機的抖動，使得前后兩幀相差過大，我們認(rèn)為這種情況為異常匹配，這時匹配點對較少，可以設(shè)置一個最小匹配閾值將其濾除，從而與下一幀繼續(xù)匹配。另外，當(dāng)目標(biāo)幀與參考幀位移或轉(zhuǎn)角大于一定閾值的時候?qū)⑵湓O(shè)立為關(guān)鍵幀，這樣可以避免將每一幀的數(shù)據(jù)都用來建圖與更新數(shù)據(jù)所帶來的巨大的計算量。

分別采用兩種位姿估計方法對比建圖：圖6(a)為采用傳統(tǒng)ICP算法估計位姿得到的三維點云地圖，可以看出由于位姿估計不準(zhǔn)確，圖中存在較多冗余點，部分點云分布重疊，物體輪廓模糊。其位姿估計如圖7(a)所示，X、Y、Z為空間三維坐標(biāo)系，圖中位姿曲線較不平穩(wěn)，存在跳變，說明位姿估計的準(zhǔn)確性不高。圖6(b)為采用ICP與PNP結(jié)合的方式進行位姿估計，并通過兩步位姿優(yōu)化得到的三維點云地圖，地圖中冗余點大大減少，物體輪廓清晰。其位姿估計如圖7(b)所示，此方法位姿曲線較為平穩(wěn)，位姿估計更穩(wěn)定。說明本文采用的位姿估計與優(yōu)化方法能較好提高位姿估計的穩(wěn)定性與準(zhǔn)確性。

圖6 三維建圖結(jié)果

圖7 位姿估計結(jié)果

5 結(jié)論

針對室內(nèi)環(huán)境，提出了一種移動機器人三維視覺SLAM方法。該方法提取ORB特征進行特征匹配，提出了基于最小距離與RANSAC結(jié)合的方法剔除誤匹配，提高了SLAM的實時性和特征匹配的準(zhǔn)確性；在點云配準(zhǔn)方面，將傳統(tǒng)的ICP算法與PNP算法結(jié)合提高配準(zhǔn)成功率，并通過RANSAC與迭代優(yōu)化位姿估計。實驗結(jié)果驗證了方法的可行性與有效性，未來將以進一步提高位姿估計的精度與地圖的全局一致性為主要的研究方向。

[1]Henry P, Krainin M, Herbst E, et al. RGB-D mapping: Using Kinect-style depth cameras for dense 3D modeling of indoor environments[J]. The International Journal of Robotics Research, 2012, 31(5): 647-663.

[2]Besl P J, Mckay N D. A Method for Registration of 3-D Shapes[A].Robotics - DL tentative[C]. International Society for Optics and Photonics, 1992:239-256.

[3]Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[A].International Conference on Computer Vision[C]. IEEE Computer Society, 2011:2564-2571.

[4]Muja M, Lowe D G. Fast approximate nearest neighbors with automatic algorithm configuration[J]. VISAPP (1), 2009, 2(331-340): 2.

[5]Smisek J, Jancosek M, Pajdla T. 3D with Kinect[A]. IEEE International Conference on Computer Vision Workshops[C]. IEEE, 2011:1154-1160.

[6]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.

[7]Arun K S, Huang T S, Blostein S D. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on pattern analysis and machine intelligence, 1987 (5): 698-700.

[8]Pomerleau F, Colas F, Siegwart R. A review of point cloud registration algorithms for mobile robotics[J]. Foundations and Trends? in Robotics, 2015, 4(1): 1-104.

[9]高 翔，張濤等.視覺SLAM十四講：從理論到實踐[M].北京：電子工業(yè)出版社，2017.

ThreeDimensionalVisualSLAMforMobileRobotsBasedonRGB-D

Wen Shengping，Shu Kaixiang，Hong Huafeng

(National Engineering Research Center of Novel Equipment for Polymer Processing, Ministerial Key Laboratory of Polymer Processing Engineering, South China University of Technology, Guangzhou 510640, China)

To solve the problem of low real-time and large matching error in mobile robots 3D visual SLAM,an improved feature matching algorithm is proposed, which is based on the combination of the minimum distance and the RANSAC method to eliminate the false matching of the ORB feature.To solve the problem of inaccurate pose estimation, the ICP algorithm combined with PNP algorithmis is proposed to replace the traditional ICP algorithm, and the pose estimation is optimized by RANSAC.The experimental results show that this method can effectively improve the real-time and the accuracy of pose estimation in SLAM system

mobile robot; Kinect camera; SLAM; feature matching; point cloud registration

2017-05-05；

2017-06-07。

廣州市科技計劃項目(201508010058； 201604010079)。

文生平(1966-)，男，湖北荊州人，教授，碩士研究生導(dǎo)師，主要從事工業(yè)裝備的智能控制與機器視覺方向的研究。

舒凱翔(1993-)，男，江西宜春人，碩士研究生，主要從事工業(yè)裝備的智能控制與計算機視覺方向的研究。

1671-4598(2017)09-0246-04

10.16526/j.cnki.11-4762/tp.2017.09.063

TP24

A