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

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

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

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

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

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

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

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

同時定位和地圖構(gòu)建；室內(nèi)機器人；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)境復雜，需要從一系列的測量數(shù)據(jù)中估計移動節(jié)點的實時位置，所以到目前為止沒有出現(xiàn)可以應(yīng)用的解決方法[1]。

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

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

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

除了濾波器求解SLAM問題之外，最近基于圖優(yōu)化SLAM成為研究的熱點。其起源于基于捆集調(diào)整(Bundle Adjustment，BA)的運動構(gòu)建(Structure From Motion, SFM)領(lǐng)域，SFM把位姿估計看作是一個全局優(yōu)化問題，約束條件為相機的運動方程，目標函數(shù)為觀測誤差，最終通過最小化目標函數(shù)獲得相機位姿。但是由于全局優(yōu)化的計算量巨大，早期的SFM只能是離線的位姿估計和地圖構(gòu)建。后來研究者逐步認識到了SLAM求解問題的稀疏性，Sibley[10]提出了把SLAM問題看作是一個圖優(yōu)化問題，圖優(yōu)化的節(jié)點描述了需要優(yōu)化的變量，邊則是與優(yōu)化變量相關(guān)的約束條件，最終采用稀疏目標優(yōu)化方法計算獲得優(yōu)化變量，即機器人的位姿和路標點位置。Klein等[11]提出了平行跟蹤和地圖構(gòu)建方法(Parallel Tracking And Mapping, PTAM)，把運動跟蹤和地圖構(gòu)建分為兩個任務(wù), 并在兩個平行的線程同時進行處理，在線實時地實現(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)境中在線實時地實現(xiàn)高精度定位，但算法最終只構(gòu)建基于特征點的稀疏地圖，無法應(yīng)用于機器人導航、路徑規(guī)劃等實際領(lǐng)域。

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

圖1 算法流程Fig. 1 Algorithm flow

采用深度攝像頭KINECT作為傳感器設(shè)備，首先提出空間域上的方法提取關(guān)鍵幀，即當機器人的運動超過一定閾值時則把該幀選取為關(guān)鍵幀，并將它的點云拼接到已有的地圖中；然后提出子地圖法，將大規(guī)模的地圖分解為一定規(guī)模的小地圖，從而減少保存地圖的運算內(nèi)存和構(gòu)建地圖的時間，由此來提高算法的整體速度。實驗表明，本文方法在標準RGBD數(shù)據(jù)集上能夠取得較好的效果，在定位精度和快速性兩方面都明顯優(yōu)于RGBD-SLAM[13]算法，定位誤差僅為RGBD-SLAM的40%，同時算法速度是RGBD-SLAM的2.6倍；在實際室內(nèi)機器人實驗中，本文方法能夠快速準確地定位機器人位置，定位速度達到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)檢測。該算法在全部的處理當中只用到了ORB來作特征檢測和描述，這樣一來提高了其在位置識別和回環(huá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]來跟蹤其位置。光流法是利用圖像序列中像素在時間域上的變化，求解出空間運動物體在觀察成像平面上的像素運動的瞬時速度s=(u,v)，從而計算出相鄰幀之間物體的運動信息，估計上一幀跟當前幀之間存在的對應(yīng)關(guān)系。假設(shè)像素的光流運動微小和亮度恒定，可以得到I(x,y,t)=I(x+dx,y+dy,t+dt)，由一階泰勒展開得：

(1)

(2)

對于N個像素點的光流：

(3)

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

(4)

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

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

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

(5)

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

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

(6)

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

(7)

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

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

1.3 回環(huán)檢測

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

(8)

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

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

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

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

從二維彩色圖像和深度圖像計算三維點云的公式如下：

(10)

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

從相機坐標系點云到全局坐標系點云的變換公式如下：

Xw, j=Tw,ciXci, j

(11)

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

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

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

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

(12)

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

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

3 實驗結(jié)果和分析

為了驗證本文算法的有效性，進行了兩組實驗，包括：

1)對比實驗。利用RGBD-SLAM提供的標準數(shù)據(jù)集進行相機定位和地圖構(gòu)建實驗，并和RGBD-SLAM進行對比。

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

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

3.1 對比RGBD-SLAM

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

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

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

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

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

從表1可以知道:1)在定位精度方面，本文方法明顯高于RGBD-SLAM算法。RGBD-SLAM在 FR1 room數(shù)據(jù)包上的定位誤差最大，達到了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 實際環(huán)境實驗

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

圖8 Turtlebot2機器人Fig. 8 Turtlebot2 robot

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

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

圖10 機器人水平二維軌跡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 測量點位置估計的結(jié)果Tab. 2 Results on position estimation of the measured points

測量點序號真實值/m估計值/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)圖效果，三維地圖和實際三維環(huán)境相符，沒有出現(xiàn)地圖重疊和變形等情況，并且地圖中樓道的反光瓷磚、地面和雜物都清晰可見。

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

4 結(jié)語

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

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

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

1001-9081(2017)05-1439-06

10.11772/j.issn.1001-9081.2017.05.1439

TP242.6

A