，，

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

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

文生平，舒凱翔，洪華鋒

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

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

移動機器人；Kinect相機；同時定位與地圖構建；特征匹配；點云配準

0 引言

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

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

1 系統(tǒng)總體結構

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

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

2 特征提取與匹配

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

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

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

3 點云生成與點云配準

3.1 Kinect相機標定

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

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

3.2 三維點云的生成

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

(1)

(2)

(3)

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

以上參數皆可由相機標定得到。

3.3 點云配準

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

3.3.1 ICP算法

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

(4)

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

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

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

(5)

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

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

(6)

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

(7)

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

3.3.2 PNP算法

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

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

圖2 P3P問題

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

3.4 位姿優(yōu)化

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

3.4.1 RANSAC優(yōu)化位姿

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

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

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

(8)

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

3.4.2 圖優(yōu)化

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

(9)

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

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

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

Hk△x=-bk

(5)

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

4 實驗結果及分析

4.1 實驗平臺介紹

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

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

圖3實驗裝置

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

4.2.1 特征提取與特征匹配

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

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

表1 特征點檢測結果對比

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

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

4.2.2 三維地圖的構建

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

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

圖6 三維建圖結果

圖7 位姿估計結果

5 結論

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

[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-)，男，湖北荊州人，教授，碩士研究生導師，主要從事工業(yè)裝備的智能控制與機器視覺方向的研究。

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

1671-4598(2017)09-0246-04

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

TP24

A