一種采用圖像特征匹配技術的RGB-D SLAM算法

許曉東，陳國良，李曉園，周文振，杜珊珊

(中國礦業大學環境與測繪學院，江蘇 徐州 221116)

一種采用圖像特征匹配技術的RGB-DSLAM算法

許曉東，陳國良，李曉園，周文振，杜珊珊

(中國礦業大學環境與測繪學院，江蘇 徐州 221116)

針對復雜環境下運動機器人自主運行的任務要求，提出了一種采用圖像特征匹配技術的視覺SlAM算法。相比于傳統濾波器方法在機器人長時間運動過程中產生的誤差積累，采用了基于圖優化的SlAM方法，本文算法分為兩個部分：前端和后端。前端負責處理圖像數據提取機器人位姿幾何關系，首先提取彩色RGB圖像的特征點，創建特征點的高維特征描述子，建立特征點的位置對應關系；后端負責表達各時刻機器人的位姿并最大化地消除軌跡漂移問題，根據前端處理所產生的信息，構建一個代表機器人幾何位置關系及其不確定性的關系圖，通過對圖的優化將測量軌跡最大化地逼近真實軌跡，最后生成稀疏點云地圖和高精度的機器人運行軌跡。試驗表明本文所提出的方法實用性強，具有較高的魯棒性。

同時定位與地圖構建；RGB-D；圖優化；特征提取匹配；閉環檢測

機器人同步定位和制圖(simultaneous localization and mapping，SLAM)是實現機器人自主導航定位的關鍵技術，其主要解決的問題可簡略描述為：機器人于未知的環境中移動，利用自身所攜帶的傳感器對周圍的環境進行感知，根據所感知到的信息描繪出周圍環境大概的地圖樣貌并估計出自身在地圖中所處的位置。根據所使用傳感器的類型，SLAM技術可分為：基于激光的SLAM、基于聲吶的SLAM、基于GPS的SLAM和基于視覺的SLAM[1]。相較于其他的傳感器，視覺傳感器價格便宜，不太依賴于硬件性能，還能獲取周圍環境的深度信息，隨著科技的進步和計算機技術的飛速發展，圖像處理獲得了極大的關注，目前基于視覺的SLAM技術越來越受到研究人員的青睞。

根據使用視覺傳感器的數量和類型的不同，視覺SLAM系統主要可分為3類：單目視覺SLAM(MonoSLAM)、多目視覺SLAM和RGB-D SLAM。MonoSLAM雖然成本很低，但是無法測得實物的絕對深度，需要借助其他傳感器的幫助估計機器人的軌跡和地圖；相較于MonoSLAM，多目SLAM可以在運行過程中估計深度，但是計算量依然繁雜；RGB-D SLAM是一種新興的相機，擁有3個“感官”：紅外線投影儀、彩色攝像頭、紅外深度攝像頭，它通過紅外結構光原理可以直接獲取圖像中各像素實物與相機的真實距離。

SLAM本質上是一個機器人運行狀態的估計問題，根據求解方法可將其分為兩類：一類是基于濾波器[2]的方法，另一類是基于圖優化[3]的方法。基于濾波器的方法將各個時刻機器人的姿態看作一系列獨立的時間狀態，是對最新時刻狀態的估計和預測，是一種不斷迭代的過程?；趫D優化的方法和其存在本質的不同，它所關注的并不是當前時刻的某一狀態，而是整個姿態圖，需要考慮過去所有的信息，對全部時刻的姿態及各時刻間的約束關系進行優化。文獻[4]提出一種實時SLAM算法，算法結合顏色信息和粒子采樣步驟提高了定位精度，但是采用了點云配準ICP算法，導致該算法適用性不高；文獻[5]引入了關鍵幀選取機制，借以提高地圖成圖效率，但是沒有后端優化步驟，魯棒性較低；文獻[6]將RANSAC算法和ICP算法相結合，用以求取最優變換；文獻[7]提出利用一種環境測量模型來驗證運動估計的有效性，后端采用姿態圖優化軟件G2O[8]來進行軌跡優化，但是由于算法限制導致環境適用性相對不高。

本文提出一種采用圖像特征匹配的圖優化SLAM方法，將整個過程描述為3個階段：準備階段、處理階段和結束階段。如圖1所示，準備階段主要處理平臺的搭建與硬件的校準問題，本文采用了通用的機器人平臺，該平臺主要包括底盤，RGB-D像機和支架3個部分，搭建完平臺，將RGB攝像頭和深度攝像頭進行對準，分別對兩個攝像頭進行標定，確定攝像機的數學模型，記錄標定參數用于下個階段；處理階段分為前端和后端，首先提取相鄰兩幀RGB圖像的特征，計算其描述子并進行匹配，然后根據特征點的深度信息結合RANSAC[9]算法思想進行相鄰兩幀圖像間的運動估計[10-11]，為了獲得更好的魯棒性和精度，本文將閾值的思想運用到了運動估計中，設置最小特征匹配數量等閾值，用于檢驗運動估計的有效性，后端主要通過關鍵幀選取機制檢測回環，結合圖優化方法解決軌跡漂移問題；結束階段用以表達高精度的點云地圖和軌跡。

圖1 SLAM系統框架

1 算法原理及系統框架

1.1 RGB-D相機標定

由于深度圖和彩色圖的獲取來源并不相同，因此RGB-D相機需要對紅外攝像頭和彩色攝像頭分別標定[12]。

根據模型求解需要，需要標定的參數有：焦距(fx,fy)、光心(cx,cy)及深度縮放因子ds。參數之間的關系可以表達為相機內部標定參數矩陣

(1)

結合相機標定工具包將這些參數一一結算，進而確定出空間點到二維圖像點的投影關系，對圖像某一處像素坐標(u,v)，其深度值為depth(u,v)，則有

(2)

由式(2)即可確定該像素點的空間坐標

(3)

相機的外參表現為旋轉平移關系

T=[R,t]

(4)

式中，R為旋轉矩陣；t為平移矩陣。

1.2 視覺里程計

里程計是通過分析處理移動傳感器所獲取的信息進而推算出其位置姿態隨著時間變化而改變的一種方法。在計算機領域中，將通過處理圖像數據進行運動估計的里程計稱為視覺里程計，基于圖像特征的視覺里程計包括特征檢測、特征匹配和運動估計3個部分。

1.2.1 特征檢測與匹配

常用的特征檢測方法有SIFT、SURF[13]、ORB[14]等算法，對于SIFT算法，雖然其魯棒性較高，但是計算復雜，耗時過多，不適合系統的實時運行需求，ORB算法雖然速度快但是魯棒性較低，不符合高精度的成果需求，而SURF算法的魯棒性雖稍次于SIFT，但是速度上卻有明顯的提升。本文遵循保證一定程度上的高效性、魯棒性及準確性的原則，暫先采用SURF算法。

提取出特征點后，根據特征點周圍的信息生成特征描述子進而完成特征匹配，根據特征相似性建立起兩幀圖像間特征點的對應關系。

1.2.2 運動估計

傳統ICP(迭代最近點)算法對于初值的選擇非常敏感，當初值選取不能滿足求解需要時，算法就會陷入局部最優解的缺陷，為了提高算法的適用性及魯棒性，本文提出采用RANSAC(隨機采樣一致性)算法來求取兩幀圖像間運動轉換的最優解。

首先在所輸入的觀測數據集中隨機提取出部分數據作為假設inliners(內點)，根據這些inliners所預測出的模型篩選出在一定閾值范圍內適用于該模型的所有inliners，假如所得到的inliners足夠多，則使用所有的inliners來優化該模型，否則重復執行第一步，直到得到inliners足夠多的最優模型。具體步驟如圖2所示。

1.3 閉環檢測與圖優化

1.3.1 閉環檢測

在視覺里程計[15]的執行過程中，機器人的運動估計是一個不斷遞推的過程，由于每一次運動估計都是相互獨立的，并且估計過程在一定程度上存在著噪聲，對于缺少特征的估計，噪聲就更為明顯，這就形成了誤差的傳遞累積，而閉環檢測則可以極大地減少累計誤差，緩和軌跡漂移。

為了提高算法魯棒性，提升成果質量，本文提出了一種閉環檢測方案，首先檢測小閉環，對x幀圖像執行運動估計，根據姿態圖的結構建立一定深度的最小生成樹，為了避免重復，先剔除最鄰近的x幀圖像，隨機采樣y幀偏向于較早時間的圖像幀用于檢測閉環，若檢測到閉環，則在閉環附近采樣z幀再次檢測閉環，然后檢測大回環，創立關鍵幀機制，將無法匹配上一關鍵幀的一幀圖像加入關鍵幀序列，隨機采樣k幀關鍵幀與當前幀匹配，若成功匹配則檢測到一個閉環。試驗表明，通過對大小閉環的檢測減少了軌跡的漂移，在一定程度上提升了算法的適用性。

圖2 RANSAC流程

1.3.2 圖優化

通過運動估計及閉環檢測完成姿態圖的構建后，SLAM問題就轉化為圖優化的問題，由于估計誤差的存在，姿態圖并不具備全局一致性軌跡，針對該問題，本文在G2O框架下進行姿態圖優化。為了計算全局一致性軌跡，將估計誤差函數F(X)最小化，其誤差公式為

F(X)=∑i,je(xi,xj,zij)TΩije(xi,xj,zij)

(5)

2 仿真試驗

為了驗證算法的有效性，文本使用了TUM數據中的rgbd_dataset_freiburg1_d Esk數據集進行仿真試驗，數據集中提供了機器人的真實軌跡，為了能更形象地驗證算法的可行性，本文計算了一個均方根函數RMSE來表達絕對軌跡誤差ATE，其具體函數公式為

(6)

圖3 真實軌跡與估計軌跡的對比

從圖3(b)中可以明顯看出，本文所提出的算法進行的軌跡估算與真實軌跡基本保持一致，其絕對軌跡誤差值ATE穩定在0.024 m左右，絕對軌跡誤差均方差RMSE穩定在0.042 m左右，數據表明本文所提出算法實用性較高。

3 結 論

本文提出了一種采用圖像特征匹配技術的RGB-D SLAM算法。本算法提取出RGB圖像的特征，計算其描述子并進行匹配，根據特征點的深度信息結合RANSAC算法求取出機器人的轉換矩陣，利用相鄰圖像間的轉換矩陣進行機器人的運動估計并得到機器人的位姿信息，對于所得到的轉換估計，采用了基于閾值的算法思想來驗證轉換的有效性，進而剔除錯誤的估計結果得到最優的估測位姿，后端提出了一種創新的閉環估計策略，極大地提高了算法的適用性，最后結合全局約束信息，在G2O的框架下進行整個姿態圖的優化，生成全局一致性軌跡，試驗證明本文所提出的算法具有較高的魯棒性，實用性較強,為進一步提高算法的適應性，將來會以傳感器的多元融合作為主要研究方向。

[1] 梁明杰,閔華清,羅榮華.基于圖優化的同時定位與地圖創建綜述[J].機器人,2013,35(4):500-512.

[2] 李超.基于信息濾波器的水下機器人SLAM算法研究[D].青島：中國海洋大學,2013.

[3] LU F,MILIOS E.Globally Consistent Range Scan Alignment for Environment Mapping[J].Autonomous Robots,1997,4(4): 333-349.

[4] 付夢印，呂憲偉，劉彤，等.基于RGB-D數據的實時SLAM算法[J].機器人,2015,37(6):683-692.

[5] 賈松敏，王可，郭兵，等.基于RGB-D相機的移動機器人三維SLAM[J].華中科技大學學報(自然科學版),2014(1):103-109.

[6] HENRY P,KRAININ M,HERBST E,et al.RGB-D Mapping:Using Depth Cameras for Dense 3D Modeling of Indoor Environments[C]∥12th International Symposium on Experimental Robotics. Berlin:Springer,2014:477-491.

[7] ENDRES F,HESS J,ENGELHARD N,et al.An Evaluation of the RGB-D SLAM System[C]∥IEEE International Conference on Robotics and Automation.Piscataway.[S.l.]:IEEE,2012:1691-1696.

[8] 張彥珍.基于g2o的SLAM后端優化算法研究[D].西安：西安電子科技大學,2014.

[9] 閆陽陽,李永強,王英杰,等.三維激光點云聯合無人機影像的三維場景重建研究[J].測繪通報,2016(1):84-87.

[10] 李明磊,李廣云,王力,等.3D Hough Transform在激光點云特征提取中的應用[J].測繪通報,2015(2):29-33.

[11] 閆利,譚駿祥,楊容浩,等.一種閉合條件約束的全局最優多視點云配準方法[J].測繪學報,2016,45(4):418-424.

[12] 路紅亮.機器視覺中相機標定方法的研究[D].沈陽：沈陽工業大學,2013.

[13] 胡衡,梁嵐珍.基于SURF特征的單目視覺SLAM方法研究[J].新疆大學學報(自然科學版),2015(3):368-372.

[14] RUBLEE E,RABAUD V,KONOLIGE K,et al.ORB:An Efficiental Ternative to SIFT or SURF[C]∥IEEE International Conference on Computer Vision. Piscataway.[S.l.]:IEEE,2011:2564-2571.

[15] 夏凌楠,張波,王營冠,等.基于慣性傳感器和視覺里程計的機器人定位[J].儀器儀表學報,2013,34(1):166-172.

ResearchonRGB-DSLAMBasedonImageFeature

XU Xiaodong，CHEN Guoliang，LI Xiaoyuan，ZHOU Wenzhen，DU Shanshan
(School of Enviroment Science and Spatial Informatics，China University of Mining and Technology，Xuzhou 221116，China)

Aiming at the requirement of autonomous operation of the mobile robot in complex environment,this paper proposes a RGB-D SLAM based on image feature.Compared with the traditional filter method,the error accumulation during the long time motion of the robot is accumulated,adopt SLAM method based on graph optimization.Proposed algorithm is divided into two parts:frontend and backend.The frontend is responsible for processing the image data and extracting the geometric relationship between the poses of the robot,firstly,the feature points of color RGB image are extracted,high dimensional feature descriptor are created and the location correspondence of feature points is established.The backend is responsible for expressing the position and pose of the robot at each moment and diminishing the drift of the trajectory,constructing a graph of the geometric position relationship and its uncertainty,through the optimization of the graph to get the best trajectory,finally,the sparse point cloud map and trajectory are generated.The experimental results show that the proposed method is practical and robust.

SLAM；RGB-D；graph optimization；feature extraction and matching；loop closure detection

許曉東，陳國良，李曉園，等.一種采用圖像特征匹配技術的RGB-D SLAM算法[J].測繪通報，2017(10)：48-51.

10.13474/j.cnki.11-2246.2017.0315.

2017-02-27；

2017-05-19

國家重點研發計劃(2016YFB0502105)；國家自然科學基金(41371423)；江蘇省自然科學基金(BK20161181)

許曉東(1993—)，男，碩士生，主要從事機器人室內定位研究。E-mail：aqautune@sina.com

陳國良。E-mail:chglcumt@163.com

P23

A

0494-0911(2017)10-0048-04