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

(中國礦業(yè)大學(xué)環(huán)境與測繪學(xué)院，江蘇 徐州 221116)

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

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

(中國礦業(yè)大學(xué)環(huán)境與測繪學(xué)院，江蘇 徐州 221116)

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

同時(shí)定位與地圖構(gòu)建；RGB-D；圖優(yōu)化；特征提取匹配；閉環(huán)檢測

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

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

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

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

圖1 SLAM系統(tǒng)框架

1 算法原理及系統(tǒng)框架

1.1 RGB-D相機(jī)標(biāo)定

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

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

(1)

結(jié)合相機(jī)標(biāo)定工具包將這些參數(shù)一一結(jié)算，進(jìn)而確定出空間點(diǎn)到二維圖像點(diǎn)的投影關(guān)系，對圖像某一處像素坐標(biāo)(u,v)，其深度值為depth(u,v)，則有

(2)

由式(2)即可確定該像素點(diǎn)的空間坐標(biāo)

(3)

相機(jī)的外參表現(xiàn)為旋轉(zhuǎn)平移關(guān)系

T=[R,t]

(4)

式中，R為旋轉(zhuǎn)矩陣；t為平移矩陣。

1.2 視覺里程計(jì)

里程計(jì)是通過分析處理移動(dòng)傳感器所獲取的信息進(jìn)而推算出其位置姿態(tài)隨著時(shí)間變化而改變的一種方法。在計(jì)算機(jī)領(lǐng)域中，將通過處理圖像數(shù)據(jù)進(jìn)行運(yùn)動(dòng)估計(jì)的里程計(jì)稱為視覺里程計(jì)，基于圖像特征的視覺里程計(jì)包括特征檢測、特征匹配和運(yùn)動(dòng)估計(jì)3個(gè)部分。

1.2.1 特征檢測與匹配

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

提取出特征點(diǎn)后，根據(jù)特征點(diǎn)周圍的信息生成特征描述子進(jìn)而完成特征匹配，根據(jù)特征相似性建立起兩幀圖像間特征點(diǎn)的對應(yīng)關(guān)系。

1.2.2 運(yùn)動(dòng)估計(jì)

傳統(tǒng)ICP(迭代最近點(diǎn))算法對于初值的選擇非常敏感，當(dāng)初值選取不能滿足求解需要時(shí)，算法就會(huì)陷入局部最優(yōu)解的缺陷，為了提高算法的適用性及魯棒性，本文提出采用RANSAC(隨機(jī)采樣一致性)算法來求取兩幀圖像間運(yùn)動(dòng)轉(zhuǎn)換的最優(yōu)解。

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

1.3 閉環(huán)檢測與圖優(yōu)化

1.3.1 閉環(huán)檢測

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

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

圖2 RANSAC流程

1.3.2 圖優(yōu)化

通過運(yùn)動(dòng)估計(jì)及閉環(huán)檢測完成姿態(tài)圖的構(gòu)建后，SLAM問題就轉(zhuǎn)化為圖優(yōu)化的問題，由于估計(jì)誤差的存在，姿態(tài)圖并不具備全局一致性軌跡，針對該問題，本文在G2O框架下進(jìn)行姿態(tài)圖優(yōu)化。為了計(jì)算全局一致性軌跡，將估計(jì)誤差函數(shù)F(X)最小化，其誤差公式為

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

(5)

2 仿真試驗(yàn)

為了驗(yàn)證算法的有效性，文本使用了TUM數(shù)據(jù)中的rgbd_dataset_freiburg1_d Esk數(shù)據(jù)集進(jìn)行仿真試驗(yàn)，數(shù)據(jù)集中提供了機(jī)器人的真實(shí)軌跡，為了能更形象地驗(yàn)證算法的可行性，本文計(jì)算了一個(gè)均方根函數(shù)RMSE來表達(dá)絕對軌跡誤差A(yù)TE，其具體函數(shù)公式為

(6)

圖3 真實(shí)軌跡與估計(jì)軌跡的對比

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

3 結(jié) 論

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

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

[2] 李超.基于信息濾波器的水下機(jī)器人SLAM算法研究[D].青島：中國海洋大學(xué),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數(shù)據(jù)的實(shí)時(shí)SLAM算法[J].機(jī)器人,2015,37(6):683-692.

[5] 賈松敏，王可，郭兵，等.基于RGB-D相機(jī)的移動(dòng)機(jī)器人三維SLAM[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版),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后端優(yōu)化算法研究[D].西安：西安電子科技大學(xué),2014.

[9] 閆陽陽,李永強(qiáng),王英杰,等.三維激光點(diǎn)云聯(lián)合無人機(jī)影像的三維場景重建研究[J].測繪通報(bào),2016(1):84-87.

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

[11] 閆利,譚駿祥,楊容浩,等.一種閉合條件約束的全局最優(yōu)多視點(diǎn)云配準(zhǔn)方法[J].測繪學(xué)報(bào),2016,45(4):418-424.

[12] 路紅亮.機(jī)器視覺中相機(jī)標(biāo)定方法的研究[D].沈陽：沈陽工業(yè)大學(xué),2013.

[13] 胡衡,梁嵐珍.基于SURF特征的單目視覺SLAM方法研究[J].新疆大學(xué)學(xué)報(bào)(自然科學(xué)版),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ì)的機(jī)器人定位[J].儀器儀表學(xué)報(bào),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

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

10.13474/j.cnki.11-2246.2017.0315.

2017-02-27；

2017-05-19

國家重點(diǎn)研發(fā)計(jì)劃(2016YFB0502105)；國家自然科學(xué)基金(41371423)；江蘇省自然科學(xué)基金(BK20161181)

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

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

P23

A

0494-0911(2017)10-0048-04