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

(1.北京林業(yè)大學(xué)工學(xué)院， 北京 100083； 2.林業(yè)裝備與自動(dòng)化國家林業(yè)和草原局重點(diǎn)實(shí)驗(yàn)室， 北京 100083)

0 引言

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

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

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

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

1 算法框架

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

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

2 特征點(diǎn)提取與匹配

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

2.1 特征點(diǎn)的提取

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

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

2.2 特征點(diǎn)的匹配

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

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

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

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

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

1-p=(1-uk)N

(1)

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

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

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

(2)

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

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

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

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

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

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

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

(3)

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

P′=T×P

(4)

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

利用GICP算法[18]優(yōu)化運(yùn)動(dòng)變換矩陣T，實(shí)質(zhì)是通過概率模型進(jìn)行求解。在概率性模型中，假設(shè)存在2個(gè)集合，={i}和={i}，若、集合完全對(duì)應(yīng)，則存在運(yùn)動(dò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待觀測(cè)點(diǎn)的協(xié)方差矩陣

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

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

(8)

結(jié)合極大似然估計(jì)(MLE)可求得最佳運(yùn)動(dòng)變換T為

(9)

式(9)可以簡(jiǎn)化為

(10)

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

3.2 隨機(jī)閉環(huán)檢測(cè)

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

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

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

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

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

(11)

(12)

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

C——時(shí)間序列集合

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

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

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

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

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

(13)

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

3.4 地圖構(gòu)建

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

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

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)平臺(tái)

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

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

4.2.1特征提取對(duì)比

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

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

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

4.2.2RANSAC算法改進(jìn)對(duì)比

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

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

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

4.2.3相機(jī)運(yùn)動(dòng)軌跡估計(jì)

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

圖8 真實(shí)軌跡與估計(jì)軌跡比較Fig.8 Comparison between real trajectory and estimated trajectory

4.2.4定位準(zhǔn)確性及實(shí)時(shí)性對(duì)比

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

從表2可以看出，針對(duì)所測(cè)試的4個(gè)FR1數(shù)據(jù)包，本文方法定位精度及處理速度均優(yōu)于RGB-D SLAM。經(jīng)計(jì)算，本文方法的平均RMSE約為0.024 5 m，平均每幀數(shù)據(jù)處理用時(shí)約為0.032 s，與RGB-D SLAM方法相比，分別降低了40%和90%。

表2 本文方法與RGB-D SLAM方法的性能對(duì)比Tab.2 Comparison between proposed algorithm and RGB-D SLAM algorithm

4.3 地圖構(gòu)建評(píng)估

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

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

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

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

4.3.2實(shí)際場(chǎng)景建圖

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

圖10 實(shí)驗(yàn)室樓道及其3D地圖Fig.10 Laboratory corridor and its 3D map

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

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

圖11 移動(dòng)機(jī)器人軌跡Fig.11 Trajectory of mobile robot

5 結(jié)束語

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