侯榮波，魏 武，黃 婷，鄧超鋒

(華南理工大學(xué) 自動化科學(xué)與工程學(xué)院，廣州 510640)

基于ORB-SLAM的室內(nèi)機(jī)器人定位和三維稠密地圖構(gòu)建

侯榮波，魏 武*，黃 婷，鄧超鋒

(華南理工大學(xué) 自動化科學(xué)與工程學(xué)院，廣州 510640)

(*通信作者電子郵箱weiwu@scut.edu.cn)

針對在室內(nèi)機(jī)器人定位和三維稠密地圖構(gòu)建系統(tǒng)中，現(xiàn)有方法無法同時滿足高精度定位、大范圍和快速性要求的問題，應(yīng)用具有跟蹤、地圖構(gòu)建和重定位三平行線程的ORB-SLAM算法估計(jì)機(jī)器人三維位姿; 然后拼接深度攝像頭KINECT獲得的三維稠密點(diǎn)云，提出空間域上的關(guān)鍵幀提取方法剔除冗余的視頻幀; 接著提出子地圖法進(jìn)一步減少地圖構(gòu)建的時間, 最終提高算法的整體速度。實(shí)驗(yàn)結(jié)果表明，所提系統(tǒng)能夠在大范圍環(huán)境中準(zhǔn)確定位機(jī)器人位置，在運(yùn)動軌跡為50 m的大范圍中，機(jī)器人的均方根誤差為1.04 m，即誤差為2%，同時整體速度為11幀/秒，其中定位速度達(dá)到17幀/秒，可以滿足室內(nèi)機(jī)器人定位和三維稠密地圖構(gòu)建的精度、大范圍和快速性的要求。

同時定位和地圖構(gòu)建；室內(nèi)機(jī)器人；ORB-SLAM；關(guān)鍵幀提取；KINECT；圖優(yōu)化

0 引言

在過去的十年里，室外定位在以衛(wèi)星定位為基礎(chǔ)的技術(shù)上獲得了快速的發(fā)展和廣泛的應(yīng)用，如，全球定位系統(tǒng)(Global Positioning System, GPS)。但是我們超過70%的時間都是在室內(nèi)，因此室內(nèi)定位技術(shù)具有巨大的研究價值和應(yīng)用價值。由于室內(nèi)環(huán)境復(fù)雜，需要從一系列的測量數(shù)據(jù)中估計(jì)移動節(jié)點(diǎn)的實(shí)時位置，所以到目前為止沒有出現(xiàn)可以應(yīng)用的解決方法[1]。

在機(jī)器人的定位領(lǐng)域，基于激光和視覺的同時定位和地圖構(gòu)建(Simultaneous Localization And Mapping, SLAM)系統(tǒng)得到了快速的發(fā)展。SLAM是指搭載傳感器的機(jī)器人在移動時利用傳感器測量數(shù)據(jù)建立環(huán)境地圖，同時估計(jì)機(jī)器人自身的位姿[2]。SLAM 同時包含定位與地圖構(gòu)建技術(shù)，被認(rèn)為是實(shí)現(xiàn)機(jī)器人自主性的關(guān)鍵技術(shù)之一，對機(jī)器人的控制、導(dǎo)航、任務(wù)規(guī)劃等領(lǐng)域有重要的研究意義[3]。根據(jù)SLAM的求解方法可以將其分為擴(kuò)展卡爾曼濾波類(Extended Kalman Filter, EKF) SLAM、粒子濾波類(Particle Filtering，PF) SLAM 和圖優(yōu)化類(Graph Optimization) SLAM。

EKF-SLAM最早由Smith等[4]提出，在假設(shè)噪聲為高斯噪聲的基礎(chǔ)上，通過運(yùn)動模型和觀測模型來估計(jì)機(jī)器人自身及路標(biāo)點(diǎn)的位置。之后許多專家學(xué)者在其方法上進(jìn)一步改進(jìn)，針對EKF-SLAM的估計(jì)不一致性和非線性函數(shù)的線性化問題，Julier 等[5]提出無跡卡爾曼濾波(Unscented Kalman Filter, UKF)方法,利用樣本加權(quán)求和直接逼近隨機(jī)分布，從而避免了對非線性函數(shù)雅各比矩陣的計(jì)算。張毅等[6]提出了迭代擴(kuò)展卡爾曼濾波(Iterated Extended Kalman Filter, IEKF)通過多次迭代更新獲得更高的后驗(yàn)概率，降低了估計(jì)誤差。此外，EKF-SLAM依賴于正確的數(shù)據(jù)關(guān)聯(lián)，一旦數(shù)據(jù)關(guān)聯(lián)出錯則會導(dǎo)致構(gòu)建地圖失??；而且EKF-SLAM的計(jì)算成本高，計(jì)算量正比于地圖規(guī)模的平方，因此只能對較小范圍的環(huán)境構(gòu)建地圖。在 EKF-SLAM 框架上，一些研究人員在不同機(jī)器人系統(tǒng)上嘗試了許多開發(fā)工作[7]。

針對EKF-SLAM對正確數(shù)據(jù)關(guān)聯(lián)十分敏感的問題，Sim等[8]引入粒子濾波器(PF)來解決，其思路是將機(jī)器人和路標(biāo)點(diǎn)的位置用粒子的分布來表達(dá)，由于沒有了EKF-SLAM對噪聲的高斯假設(shè)，同時粒子數(shù)量較多，對機(jī)器人和路標(biāo)點(diǎn)位置具有多個假設(shè)，因此對正確的數(shù)據(jù)關(guān)聯(lián)不敏感。但是PF-SLAM仍面臨著由于粒子數(shù)量大導(dǎo)致計(jì)算成本高，以及由于重采樣導(dǎo)致的粒子耗盡問題。為了減少粒子數(shù)量，Blanco等[9]使用KL距離(Kullback-Leibler Distance， KLD)方法自適應(yīng)確定粒子數(shù)量,從而剔除了冗余的粒子。

除了濾波器求解SLAM問題之外，最近基于圖優(yōu)化SLAM成為研究的熱點(diǎn)。其起源于基于捆集調(diào)整(Bundle Adjustment，BA)的運(yùn)動構(gòu)建(Structure From Motion, SFM)領(lǐng)域，SFM把位姿估計(jì)看作是一個全局優(yōu)化問題，約束條件為相機(jī)的運(yùn)動方程，目標(biāo)函數(shù)為觀測誤差，最終通過最小化目標(biāo)函數(shù)獲得相機(jī)位姿。但是由于全局優(yōu)化的計(jì)算量巨大，早期的SFM只能是離線的位姿估計(jì)和地圖構(gòu)建。后來研究者逐步認(rèn)識到了SLAM求解問題的稀疏性，Sibley[10]提出了把SLAM問題看作是一個圖優(yōu)化問題，圖優(yōu)化的節(jié)點(diǎn)描述了需要優(yōu)化的變量，邊則是與優(yōu)化變量相關(guān)的約束條件，最終采用稀疏目標(biāo)優(yōu)化方法計(jì)算獲得優(yōu)化變量，即機(jī)器人的位姿和路標(biāo)點(diǎn)位置。Klein等[11]提出了平行跟蹤和地圖構(gòu)建方法(Parallel Tracking And Mapping, PTAM)，把運(yùn)動跟蹤和地圖構(gòu)建分為兩個任務(wù), 并在兩個平行的線程同時進(jìn)行處理，在線實(shí)時地實(shí)現(xiàn)了SLAM算法，雖然PTAM存在著缺少足夠的回環(huán)檢測、重定位不變性低、需要人工初始化地圖等問題，但是其代表了視覺SLAM的一個重要突破。繼PTAM之后，Mur-Artal等[12]提出了ORB-SLAM(Oriented FAST and Rotated BRIEF-Simultaneous Localization And Mapping)算法，該算法具有跟蹤、地圖構(gòu)建、重定位三個平行線程，有效地解決了回環(huán)檢測、重定位和地圖初始化等問題，能夠在小范圍和大范圍的未知環(huán)境中在線實(shí)時地實(shí)現(xiàn)高精度定位，但算法最終只構(gòu)建基于特征點(diǎn)的稀疏地圖，無法應(yīng)用于機(jī)器人導(dǎo)航、路徑規(guī)劃等實(shí)際領(lǐng)域。

針對ORB-SLAM算法的不足，引入了在線快速構(gòu)建未知環(huán)境三維稠密地圖的方法，算法的整體步驟如圖1所示。

圖1 算法流程Fig. 1 Algorithm flow

采用深度攝像頭KINECT作為傳感器設(shè)備，首先提出空間域上的方法提取關(guān)鍵幀，即當(dāng)機(jī)器人的運(yùn)動超過一定閾值時則把該幀選取為關(guān)鍵幀，并將它的點(diǎn)云拼接到已有的地圖中；然后提出子地圖法，將大規(guī)模的地圖分解為一定規(guī)模的小地圖，從而減少保存地圖的運(yùn)算內(nèi)存和構(gòu)建地圖的時間，由此來提高算法的整體速度。實(shí)驗(yàn)表明，本文方法在標(biāo)準(zhǔn)RGBD數(shù)據(jù)集上能夠取得較好的效果，在定位精度和快速性兩方面都明顯優(yōu)于RGBD-SLAM[13]算法，定位誤差僅為RGBD-SLAM的40%，同時算法速度是RGBD-SLAM的2.6倍；在實(shí)際室內(nèi)機(jī)器人實(shí)驗(yàn)中，本文方法能夠快速準(zhǔn)確地定位機(jī)器人位置，定位速度達(dá)到17幀/秒，能建立高精度的三維稠密地圖。

1 ORB-SLAM概述

ORB-SLAM是一種基于圖像特征和非線性優(yōu)化的單目視覺SLAM系統(tǒng)。它包括基于ORB(Oriented FAST and Rotated BRIEF)圖像特征的詞袋(Bag of Words, BoW)用于位置識別及回環(huán)檢測[14]、信息關(guān)聯(lián)視圖[15]、G2o圖優(yōu)化通用框架[16]。對于大范圍的地圖構(gòu)建，應(yīng)用了尺度感知的回環(huán)檢測。該算法在全部的處理當(dāng)中只用到了ORB來作特征檢測和描述，這樣一來提高了其在位置識別和回環(huán)檢測的效果。文獻(xiàn)[12]完整地介紹了ORB-SLAM算法，在此主要總結(jié)了它的三個主要步驟，包括跟蹤、構(gòu)建特征地圖、回環(huán)檢測，如圖2所示。

圖2 ORB-SLAM系統(tǒng)流程Fig. 2 System overview of ORB-SLAM

1.1 跟蹤

這一部分的主要任務(wù)是通過KINECT獲得視頻幀，并用光流法[17]來跟蹤其位置。光流法是利用圖像序列中像素在時間域上的變化，求解出空間運(yùn)動物體在觀察成像平面上的像素運(yùn)動的瞬時速度s=(u,v)，從而計(jì)算出相鄰幀之間物體的運(yùn)動信息，估計(jì)上一幀跟當(dāng)前幀之間存在的對應(yīng)關(guān)系。假設(shè)像素的光流運(yùn)動微小和亮度恒定，可以得到I(x,y,t)=I(x+dx,y+dy,t+dt)，由一階泰勒展開得：

(1)

(2)

對于N個像素點(diǎn)的光流：

(3)

將式(3)記為A·s=b，那么：

(4)

假設(shè)地圖點(diǎn)的三維位置可用，并且用ORB特征來描述。如果在當(dāng)前圖像幀中跟蹤成功，則通過之前圖像幀的運(yùn)動模型平均值來估計(jì)當(dāng)前KINECT的位姿，然后重映射地圖點(diǎn)估計(jì)其在當(dāng)前幀上的圖像。用ORB特征描述的地圖點(diǎn)與在預(yù)測點(diǎn)附近范圍檢測到的特征進(jìn)行匹配，圖像上最小漢明距離的特征點(diǎn)被選為匹配點(diǎn)。然后利用非線性優(yōu)化最小化重投影誤差來計(jì)算獲得相機(jī)位姿。優(yōu)化之后，匹配點(diǎn)根據(jù)合適的閾值被分為內(nèi)點(diǎn)和外點(diǎn)。如果跟蹤失敗，首先把當(dāng)前圖像幀轉(zhuǎn)換成圖像詞袋，檢索圖像數(shù)據(jù)庫，為全局重定位查找關(guān)鍵幀。然后計(jì)算ORB特征和每個關(guān)鍵幀的地圖云點(diǎn)的對應(yīng)關(guān)系，最后對每個關(guān)鍵幀執(zhí)行隨機(jī)一致性檢驗(yàn)(RANdom Sample And Consensus，RANSAC)迭代計(jì)算，用PnP(Perspective-n-Point)算法估計(jì)KINECT位姿。一旦估計(jì)獲得KINECT的位姿，就可以恢復(fù)跟蹤模塊。

1.2 構(gòu)建局部地圖

為了構(gòu)建環(huán)境的三維地圖，系統(tǒng)在KINECT獲得的視頻幀中提取合適的關(guān)鍵幀。利用跟蹤模塊獲得的匹配特征點(diǎn)，估計(jì)關(guān)鍵幀之間的匹配關(guān)系。一旦匹配有效，就采用BA來優(yōu)化估計(jì)三維地圖點(diǎn)位置和關(guān)鍵幀位姿。算法在一個和跟蹤線程平行的線程中以較低的頻率連續(xù)計(jì)算匹配和迭代提高地圖點(diǎn)的精度。BA最小化關(guān)鍵幀匹配點(diǎn)的重映射誤差，Tw,ci為第i個關(guān)鍵幀的位姿，Xw, j為第j個三維地圖點(diǎn):

(5)

其中:ρh為Huber函數(shù)，Ωi, j為協(xié)方差矩陣,ei, j由式(6)、(7)計(jì)算獲得：

ei, j=ui, j-CamProj(Tw,ci,Xw, j)

(6)

CamProj(Tw,ci,Xw, j)：=

(7)

其中：ui, j為第j個地圖點(diǎn)在第i個關(guān)鍵幀上的觀測；CamProj計(jì)算了地圖點(diǎn)在已知相機(jī)位姿上的投影，其定義為式(7)，其中fx、fy、cx、cy為相機(jī)內(nèi)參。當(dāng)KINECT到達(dá)一個新的區(qū)域，那么新的關(guān)鍵幀就會添加到地圖中。添加關(guān)鍵幀之后，當(dāng)前關(guān)鍵幀就會和之前的關(guān)鍵幀建立關(guān)聯(lián)并初始化新的地圖點(diǎn)。

最初，地圖點(diǎn)和關(guān)鍵幀是冗余的，那么需要進(jìn)一步更為嚴(yán)苛地篩選它們。出現(xiàn)以下情況的地圖點(diǎn)就會被剔除：1)這些點(diǎn)在接下來的圖像幀中無法跟蹤和匹配;2)投影光線通過三角化計(jì)算點(diǎn)處于低視差;3)三角測量點(diǎn)會產(chǎn)生的重投影誤差較大。這個嚴(yán)格地圖點(diǎn)篩選過程保證了構(gòu)建地圖的魯棒性。為了保證重構(gòu)地圖的簡潔性，降低BA過程中的復(fù)雜度，檢測冗余關(guān)鍵幀并刪除它們，在關(guān)鍵幀集中，如果一個關(guān)鍵幀的90%的地圖點(diǎn)至少在其他的三個關(guān)鍵幀中被檢測到，那么則剔除該關(guān)鍵幀。

1.3 回環(huán)檢測

為了降低視覺里程計(jì)過程中的積累誤差，利用各關(guān)鍵幀的相互關(guān)聯(lián)性進(jìn)行有效的閉合回環(huán)，在本質(zhì)視圖上優(yōu)化位姿。這樣就可以將累計(jì)的誤差分散到位姿圖中，并通過相似變換矯正尺度偏移。通過回環(huán)檢測優(yōu)化位姿之后，根據(jù)優(yōu)化后的關(guān)鍵幀矯正地圖點(diǎn)云。首先計(jì)算關(guān)鍵幀Ki的詞袋和其數(shù)據(jù)關(guān)聯(lián)視圖附近關(guān)鍵幀的相似度，并提出相似度較低的關(guān)鍵幀，同時刪除和Ki直接連接的關(guān)鍵幀，最終獲得閉合回環(huán)。然后當(dāng)閉合回環(huán)達(dá)到一定程度時，利用通用圖優(yōu)化框架(General Graph optimization, G2o)優(yōu)化回環(huán)位姿圖，如圖3所示。最后更新地圖點(diǎn)，融合重復(fù)的地圖點(diǎn)。位姿圖優(yōu)化公式如下：

(8)

其中ei, j由式(9)計(jì)算獲得:

(9)

其中:Xi, j是位姿Xw, j到位姿Xw,i的相似變換，logsim(3)將轉(zhuǎn)移矩陣的位姿誤差映射到7維歐氏空間R7，Δi, j為邊的信息矩陣。

圖3 回環(huán)檢測位姿圖Fig. 3 Pose graph of loop detection

2 三維稠密地圖快速構(gòu)建

在ORB-SLAM系統(tǒng)中，其構(gòu)建的是三維稀疏特征地圖，分辨率極低，無法在實(shí)際的機(jī)器人領(lǐng)域中應(yīng)用，如機(jī)器人導(dǎo)航、路徑規(guī)劃等。為了克服這一不足之處，本文提出了利用深度攝像頭KINECT作為傳感器設(shè)備來構(gòu)建三維稠密地圖。如果將每一幀的點(diǎn)云都融合到地圖中，那么地圖的容量會很大，從而降低了系統(tǒng)實(shí)時性能。由于機(jī)器人在運(yùn)動過程中相鄰圖像幀具有連續(xù)性，即相鄰幀的位姿在空間上變化較小，提出了空間域上的關(guān)鍵幀提取方法來篩選合適的圖像幀，并引入子地圖法進(jìn)一步減少構(gòu)建地圖的時間，由此來提高算法的速度。

KINECT攝像頭由彩色相機(jī)和深度傳感器組成，其中深度傳感器由紅外發(fā)射器和紅外接收器兩部分構(gòu)成，如圖4所示。彩色相機(jī)可以獲得每個像素點(diǎn)的RGB值即彩色圖像，而深度傳感器則可以測量到像素點(diǎn)的距離信息即深度圖像，根據(jù)KINECT成像原理，如圖5所示，融合彩色圖和深度圖像計(jì)算獲得三維點(diǎn)云。

圖4 KINECT結(jié)構(gòu)Fig. 4 Structure of KINECT

從二維彩色圖像和深度圖像計(jì)算三維點(diǎn)云的公式如下：

(10)

其中:fx,fy,cx,cy為相機(jī)內(nèi)參，(u,v)為圖像坐標(biāo)，(x,y,z)為圖像坐標(biāo)系坐標(biāo)，d深度相機(jī)測得像素點(diǎn)的距離，單位毫米(mm)，s為實(shí)際距離和測得距離d的比例系數(shù)，這里取1 000。

從相機(jī)坐標(biāo)系點(diǎn)云到全局坐標(biāo)系點(diǎn)云的變換公式如下：

Xw, j=Tw,ciXci, j

(11)

其中:Tw,ci為第i個關(guān)鍵幀的位姿，Xci, j為在第i個關(guān)鍵幀坐標(biāo)系上的點(diǎn)云，Xw, j是變換后獲得在全局坐標(biāo)系上的點(diǎn)云。

圖5 針孔模型成像原理Fig. 5 Imaging principle of pinhole model

為了避免三維點(diǎn)云的冗余，造成不必要的計(jì)算量，本文采用基于空間域上的方法來提取關(guān)鍵幀，其思想是只有當(dāng)相機(jī)在相鄰幀運(yùn)動了一定大小時才把該幀視為關(guān)鍵幀，并把它的點(diǎn)云疊加到現(xiàn)有的地圖中去。其計(jì)算公式如下：

min_norm≤‖Δt‖+min(2π-‖r‖,‖r‖)≤max_norm

(12)

其中:Δt為相機(jī)在相鄰幀之間的位移矢量;r為相機(jī)在相鄰幀之間的旋轉(zhuǎn)角度，用它們的范數(shù)和來描述相機(jī)運(yùn)動大小;min_norm為相機(jī)的最小運(yùn)動，即當(dāng)相機(jī)在相鄰幀運(yùn)動大于min_norm時則把該幀提取為關(guān)鍵幀;max_norm為相機(jī)的最大運(yùn)動，即當(dāng)其運(yùn)動大于max_norm時，可認(rèn)為是相機(jī)位姿估計(jì)錯誤，剔除該幀。在本文中根據(jù)實(shí)驗(yàn)經(jīng)驗(yàn)，min_norm和max_norm分別取值為0.4、5。

在大范圍環(huán)境中建立三維稠密地圖時，整個地圖會相當(dāng)大，占用大量的計(jì)算機(jī)內(nèi)存，從而降低了系統(tǒng)的整體速度。本文提出了子地圖法，將整個大地圖分成多個包含一定關(guān)鍵幀的子地圖。當(dāng)?shù)貓D規(guī)模達(dá)到了一定的程度即一定關(guān)鍵幀數(shù)量時，將該子地圖從計(jì)算內(nèi)存中釋放出來，保存到計(jì)算機(jī)硬盤中。待系統(tǒng)將要關(guān)閉，則將多個子地圖整合為完整的地圖。本文的子地圖規(guī)模為10個關(guān)鍵幀。

3 實(shí)驗(yàn)結(jié)果和分析

為了驗(yàn)證本文算法的有效性，進(jìn)行了兩組實(shí)驗(yàn)，包括：

1)對比實(shí)驗(yàn)。利用RGBD-SLAM提供的標(biāo)準(zhǔn)數(shù)據(jù)集進(jìn)行相機(jī)定位和地圖構(gòu)建實(shí)驗(yàn)，并和RGBD-SLAM進(jìn)行對比。

2)實(shí)際環(huán)境實(shí)驗(yàn)。利用Turtlebot2機(jī)器人在實(shí)驗(yàn)室樓道進(jìn)行機(jī)器人定位和稠密地圖構(gòu)建實(shí)驗(yàn)。

下面分別是這兩組實(shí)驗(yàn)的過程和結(jié)果分析。

3.1 對比RGBD-SLAM

本節(jié)主要從算法的準(zhǔn)確性和快速性兩方面，將本文算法和RGBD-SLAM算法進(jìn)行對比。

文獻(xiàn)[13]提供了標(biāo)準(zhǔn)的RGB-D數(shù)據(jù)集，包括由KINECT獲取的彩色圖和深度圖，由高精度動作捕捉系統(tǒng)獲得的KINECT運(yùn)動的真實(shí)位姿以及獲取各個數(shù)據(jù)集的相機(jī)內(nèi)參。除此之外，該文獻(xiàn)還提供了計(jì)算估計(jì)位姿和真實(shí)位姿的均方根誤差(Root Mean Square Error,RMSE)工具，這樣可以有效地評估算法的定位精度。

本文主要利用RGB-D數(shù)據(jù)集中FR1組數(shù)據(jù)進(jìn)行實(shí)驗(yàn)，估計(jì)KINECT相機(jī)運(yùn)行軌跡并構(gòu)建三維稠密地圖。實(shí)驗(yàn)對比結(jié)果如表1所示，其中RGBD-SLAM實(shí)驗(yàn)結(jié)果來自文獻(xiàn)[13]，圖6和圖7分別是本文算法在FR1數(shù)據(jù)集上三維地圖局部圖、估計(jì)軌跡和實(shí)際軌跡對比圖。

圖6 部分FR1數(shù)據(jù)集三維地圖Fig. 6 3D maps of part FR1 dataset

圖7 FR1數(shù)據(jù)集估計(jì)軌跡和實(shí)際軌跡對比Fig. 7 Comparison between ground truth and estimated trajectory of FR1 dataset

從表1可以知道:1)在定位精度方面，本文方法明顯高于RGBD-SLAM算法。RGBD-SLAM在 FR1 room數(shù)據(jù)包上的定位誤差最大，達(dá)到了0.219 m，但是本文方法在這個數(shù)據(jù)包上的定位誤差只有0.045 m。RGBD-SLAM在FR1數(shù)據(jù)集上平均定位誤差是0.097 m，而本文方法的平均定位誤差為0.038 m，僅為RGBD-SLAM的40%；2)在快速性方面，本文方法也明顯優(yōu)于RGBD-SLAM算法。RGBD-SLAM在FR1數(shù)據(jù)集上平均每幀的處理時間是0.349 s，而本文方法僅用了0.132 s，速度約是RGBD-SLAM的2.6倍。

表1 本文算法與RGBD-SLAM性能對比Tab. 1 Perormance comparison between the proposed method and RGBD-SLAM

3.2 實(shí)際環(huán)境實(shí)驗(yàn)

為了進(jìn)一步分析本文方法在實(shí)際室內(nèi)機(jī)器人定位和三維稠密地圖構(gòu)建的效果，利用開源機(jī)器人平臺Turtlebot2在實(shí)驗(yàn)室樓道進(jìn)行了分析實(shí)驗(yàn)。Turtlebot2機(jī)器人主要由KOBUKI移動底座、KINECT深度攝像頭和筆記本組成，其結(jié)構(gòu)如圖8所示。本文的SLAM算法是在i5-3210M，2.5 GHz CPU，8 GB RAM，ubuntu14.04操作系統(tǒng)的筆記本上用C++實(shí)現(xiàn)的。

圖8 Turtlebot2機(jī)器人Fig. 8 Turtlebot2 robot

實(shí)驗(yàn)室樓道由四段直線道組成，如圖9所示，樓道存在大量的相似物體，如門、瓷磚等，并且瓷磚的角點(diǎn)數(shù)量較少，這些給特征匹配和回環(huán)檢測帶來了巨大的挑戰(zhàn)。Turtlebot2機(jī)器人上配備了KINETCT攝像頭，并使得機(jī)器人坐標(biāo)系和KINECT攝像頭坐標(biāo)系重合。實(shí)驗(yàn)過程中，用XBOX遙控手柄控制機(jī)器人以0.4 m/s速度水平運(yùn)動，本文系統(tǒng)在線定位機(jī)器人位姿并構(gòu)建三維稠密地圖，最終獲得機(jī)器人水平二維軌跡圖(圖10)和三維地圖(圖11、圖12)。

圖9 四段樓道局部圖Fig. 9 Four sections of the corridor

圖10 機(jī)器人水平二維軌跡Fig. 10 Horizontal 2D trajectory of the robot

圖11 重構(gòu)后四段路局部圖Fig. 11 Four sections of the corridor after reconstruction

圖12 構(gòu)造的三維稠密地圖Fig. 12 Constructed 3D dense map表2 測量點(diǎn)位置估計(jì)的結(jié)果Tab. 2 Results on position estimation of the measured points

測量點(diǎn)序號真實(shí)值/m估計(jì)值/m誤差/m1(-0.3,4.96)(-0.3,4.90)0.00362(8.13,5.86)(7.6,5.1)0.85003(12.29,9.34)(12.1,8.5)0.60004(30.7,11.2)(29.8,10.5)1.30005(38.7,11)(37.1,10.6)2.7000

從圖11和圖12中可以看到，本文系統(tǒng)即使在大范圍的環(huán)境下依然能夠取得較好的構(gòu)圖效果，三維地圖和實(shí)際三維環(huán)境相符，沒有出現(xiàn)地圖重疊和變形等情況，并且地圖中樓道的反光瓷磚、地面和雜物都清晰可見。

在整個實(shí)驗(yàn)中，系統(tǒng)共處理了1 548張視頻幀，關(guān)鍵幀為130幀，整體速度為11幀/s,其中定位速度為17幀/s。結(jié)合上文對定位精度和三維稠密地圖的效果分析，系統(tǒng)可以滿足機(jī)器人同時定位和地圖構(gòu)建的精度和速度上的要求。

4 結(jié)語

為了建立高精度定位、大范圍和快速的室內(nèi)機(jī)器人定位和三維稠密地圖構(gòu)建系統(tǒng)，本文利用ORB-SLAM算法估計(jì)機(jī)器人三維位姿，然后采用深度攝像頭KINECT獲得每一幀的稠密點(diǎn)云，為了剔除冗余的視頻幀引入了空間域上的關(guān)鍵幀提取方法，并采用子地圖法進(jìn)一步減少地圖構(gòu)建的時間以提高整體速度。利用機(jī)器人Turtlebot2在實(shí)驗(yàn)室樓道進(jìn)行了分析實(shí)驗(yàn)，結(jié)果表明，本文的方法能夠在大范圍環(huán)境中準(zhǔn)確地定位機(jī)器人位置，在運(yùn)動軌跡為50 m的大范圍中，機(jī)器人的均方根誤差為1.04 m，即誤差為2%; 同時系統(tǒng)的整體速度為11幀/秒，其中定位速度達(dá)到17幀/秒，可以滿足室內(nèi)機(jī)器人在精度、大范圍和快速性的要求。但系統(tǒng)仍存在不足，由于ORB-SLAM算法是基于ORB角點(diǎn)特征來實(shí)現(xiàn)位置識別和回環(huán)檢測的，所以在角點(diǎn)特征較少的環(huán)境中機(jī)器人的定位效果較差，容易定位失敗。今后將會結(jié)合基于圖像像素的直接SLAM進(jìn)一步提高ORB-SLAM方法的定位效果。

References)

[1] DARDARI D， CLOSAS P， DJURIC P M. Indoor tracking theory, methods, and technologies [J]. IEEE Transactions on Vehicular Technology, 2015, 64(4):1263-1278.

[2] CHOI H, YANG K W, KIM E. Simultaneous global localization and mapping[J]. IEEE/ASME Transactions on Mechatronics, 2014, 19(4):1160-1170.

[3] CHEN Z, SAMARABANDU J, RODRIGO R. Recent advances in simultaneous localization and map-building using computer vision [J]. Advanced Robotics, 2007, 21(3/4):233-265.

[4] SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[C]// Autonomous Robot Vehicles. New York: Springer, 1990: 167-193.

[5] JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422.

[6] 張毅,汪龍峰,余佳航. 基于深度信息的移動機(jī)器人室內(nèi)環(huán)境三維地圖創(chuàng)建[J]. 計(jì)算機(jī)應(yīng)用,2014,34(12):3438-3445.(ZHANG Y,WANG L F, YU J H. Depth-image based 3D map reconstruction of indoor environment for mobile robots[J]. Journal of Computer Applications, 2014, 34(12): 3438-3445.)

[7] SCHMIDT A. The EKF-based visual SLAM system with relative map orientation measurements[C]// Proceedings of the 2014 International Conference on Computer Vision and Graphics. Berlin: Springer, 2014: 570-577.

[8] SIM R, ELINAS P, LITTLE J. A study of the Rao-Blackwellised particle filter for efficient and accurate vision-based SLAM[J]. International Journal of Computer Vision, 2007, 74(3):303-318.

[9] BLANCO J L, GONZALEZ J, FERNANDEZ-MADRIGAL J A. An optimal filtering algorithm for non-parametric observation models in robot localization[C]// Proceedings of the 2008 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2008:461-466.

[10] SIBLEY G. Relative bundle adjustment[J]. Electronic Notes in Theoretical Computer Science, 2009, 220(3): 976-982.

[11] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[C]// Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Washington, DC: IEEE Computer Society, 2007: 225-234.

[12] MUR-ARTAL R, MONTIEL J M M， TARDS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.

[13] STURM J, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]// Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2012:573-580.

[15] STRASDAT H, DAVISON A J, MONTIEL J M M, et al. Double window optimization for constant time visual SLAM[C]// Proceedings of the 2011 International Conference on Computer Vision. Washington, DC: IEEE Computer Society, 2011: 2352-2359.

[16] KUEMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: a general framework for graph optimization[C]// Proceedings of the 2011 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2011: 3607-3613.

[17] LUCAS B, KANADE T. An iterative image registration technique with an application to stereo vision[C]// Proceedings of 7th International Joint Conference on Artificial Intelligence. San Francisco: Morgan Kaufmann, 1981: 674-679.

This work is partially supported by the National Natural Science Foundation of China (61573148), the Guangdong Provincial Science and Technology Major Project (2015B010919007).

HOU Rongbo, born in 1993, M. S. candidate. His research interests include simultaneous localization and mapping, three-dimensional reconstruction, computer version.

WEI Wu, born in 1970,Ph. D., associate professor. His research interests include robot control, intelligent control, pattern recognition, artificial intelligence.

HUANG Ting，born in 1991,M.S.candidate.Her research interests include computer version, robotic embedded system.

DENG Chaofeng, born in 1990, M. S. candidate. His research interests include robot control, intelligent control.

Indoor robot localization and 3D dense mapping based on ORB-SLAM

HOU Rongbo，WEI Wu*，HUANG Ting，DENG Chaofeng

(SchoolofAutomationScienceandEngineering,SouthChinaUniversityofTechnology,GuangzhouGuangdong510640,China)

In the indoor robot localization and 3D dense mapping, the existing methods can not satisfy the requirements of high-precision localization, large-scale and rapid mapping. The ORB-SLAM (Oriented FAST and Rotated BRIEF-Simultaneous Localization And Mapping) algorithm, which has three parallel threads including tracking, map building and relocation, was used to estimate the three-dimensional (3D) pose of the robot. And then 3D dense point cloud was obtained by using the depth camera KINECT. The key frame extraction method in spatial domain was introduced to eliminate redundant frames, and the sub-map method was proposed to reduce the cost of mapping, thereby the whole speed of the algorithm was improved. The experiment results show that the proposed method can locate the robot position accurately in a large range. In the range of 50 meters, the root-mean-square error of the robot is 1.04 m, namely the error is 2%, the overall speed is 11 frame/s, and the localization speed is up to 17 frame/s. The proposed method can meet the requirements of indoor robot localization and 3D dense mapping with high precision, large-scale and rapidity.

Simultaneous Localization And Mapping (SLAM); indoor robot; Oriented FAST and Rotated BRIEF-Simultaneous Localization And Mapping (ORB-SLAM); key frame extraction；KINECT; graph optimization

2016-10-14;

2016-12-21。

國家自然科學(xué)基金資助項(xiàng)目(61573148)；廣東省科技重大專項(xiàng)(2015B010919007)。

侯榮波(1993—)，男，廣東肇慶人，碩士研究生，主要研究方向：SLAM算法、三維重構(gòu)、計(jì)算機(jī)視覺; 魏武(1970—)，男，湖南益陽人，副教授，博士，主要研究方向：機(jī)器人控制、智能控制、模式識別、人工智能； 黃婷(1991—)，女，湖北孝感人，碩士研究生，主要研究方向：計(jì)算機(jī)視覺、機(jī)器人嵌入式系統(tǒng)； 鄧超鋒(1990-)，男，江西鄱陽人，碩士研究生，主要研究方向：機(jī)器人控制、智能控制。

1001-9081(2017)05-1439-06

10.11772/j.issn.1001-9081.2017.05.1439

TP242.6

A