基于RGB-D相機的室內(nèi)移動機器人自定位方法

，，，

(國防科技大學(xué)，長沙 410073)

0 引言

移動機器人發(fā)展研究的挑戰(zhàn)在于如何讓機器人處在復(fù)雜變動的環(huán)境情形下高效率地完成預(yù)定的任務(wù)[1]。隨著現(xiàn)代化社會的發(fā)展，人們有了減輕家務(wù)負擔(dān)的需求，這使得室內(nèi)服務(wù)機器人有很大的市場空間與前景。室內(nèi)服務(wù)機器人作為移動機器人的典型代表，要求能夠?qū)崟r進行自定位，并且有效地避開各種靜態(tài)和動態(tài)障礙物。自定位問題是移動機器人系統(tǒng)研究中的重難點之一，高精度可靠的定位是保證移動機器人自主地完成預(yù)定任務(wù)的關(guān)鍵[2]，根據(jù)環(huán)境模型是否已知可分為基于環(huán)境模型的定位、未知環(huán)境模型的定位以及同時定位與建圖，其中基于環(huán)境模型的定位方法可分為相對定位、絕對定位與組合定位三類。絕對定位技術(shù)通常借助于外部傳感器獲得機器人的絕對位置，常用的方法有陸標定位、視覺定位以及衛(wèi)星導(dǎo)航定位等。相對定位又稱航跡推算技術(shù)，該方法僅根據(jù)編碼器便能在短距離內(nèi)獲得較高的定位精度，但是長時間工作時存在漂移。

本文針對室內(nèi)環(huán)境的特點提出了一種基于RGB-D相機的移動機器人自定位方法，該方法基于環(huán)境模型利用匹配優(yōu)化定位算法可以高效率地實現(xiàn)精度較高自定位又能夠確保系統(tǒng)的魯棒性，本文最后通過開展實驗研究和實驗結(jié)果分析，驗證所提方法的有效性。

1 研究現(xiàn)狀

精準可靠的自定位是保證移動機器人完成各項自主性任務(wù)的關(guān)鍵，不同的定位算法各有優(yōu)缺點，一些學(xué)者將各種理論應(yīng)用于移動機器人的定位研究，其中Monte Carlo定位[3-5]是一種比較常用的方法，也稱為粒子濾波方法，該算法的主要思想是使用帶有權(quán)重的粒子點的集合來表示機器人的定位值在狀態(tài)空間中的分布。初始化時，通常假設(shè)粒子點在場地中均勻分布，定位過程中，基于權(quán)重高低的重采樣、基于機器人的運動模型更新粒子點的位姿和基于機器人的感知模型更新粒子點的權(quán)重等三個步驟循環(huán)進行，最后將所有的粒子點加權(quán)求和即可得到機器人自定位的結(jié)果，但是傳統(tǒng)的粒子濾波定位算法將過多的時間用于處理權(quán)重不高的粒子，因此有一定的盲目性。文獻[6----7]提出了一種基于超寬帶的室內(nèi)定位系統(tǒng)，該系統(tǒng)使用傳輸速度較高、穿透能力較強以及發(fā)射功率低的無線技術(shù)，基于超寬帶的室內(nèi)移動機器人自定位系統(tǒng)響應(yīng)速度較快，可達到定位精度的要求，但是其成本較高，不適合應(yīng)用于低成本的服務(wù)機器人。文獻[8]設(shè)計了一種基于路標和三角測距法的移動機器人自定位系統(tǒng)并通過實物驗證定位系統(tǒng)的有效性，但該系統(tǒng)嚴重依賴預(yù)先設(shè)定的環(huán)境模型，并且機器人上需要安置特定的傳感器用于觀測環(huán)境中的信標，其安裝與維護信標的費用較高。文獻[9]采用一種利用Hough變換的移動機器人定位算法，該方法利用建立全局地圖的Hough能量譜函數(shù)估計機器人在全局地圖中的位姿分布。針對RoboCup中型組足球機器人的結(jié)構(gòu)化場地環(huán)境的特殊性，文獻[10]提出了基于匹配優(yōu)化定位的中型組足球機器人自定位算法，其核心思想是將機器人所觀測到的特征點與環(huán)境信息相匹配，預(yù)先設(shè)定誤差函數(shù)，并利用優(yōu)化算法求出機器人自定位的最優(yōu)解從而最小化誤差函數(shù)。理論上而言，匹配優(yōu)化定位方法的定位精度只由優(yōu)化算法本身的計算精度和視覺測量精度所決定，并且優(yōu)化算法可以在較短的時間內(nèi)對一幀圖像完成定位解算，所以匹配優(yōu)化定位方法具有高效率與高精度的特點，但該定位算法需要使用機器人的初始定位值進行優(yōu)化，也就是需要獲得已知的機器定位初值。

2 基于RGB-D相機的匹配優(yōu)化自定位算法

機器人自定位問題的核心就是求出機器人在世界坐標系中的位置與姿態(tài)(x,y,θ)，本文針對室內(nèi)環(huán)境的特點提出一種基于RGB-D相機的匹配優(yōu)化自定位方法，該方法首先建立機器人工作環(huán)境的地圖模型，接著基于建立的地圖模型生成環(huán)境誤差查找表，最后通過匹配優(yōu)化自定位算法完成機器人的自定位，利用匹配優(yōu)化定位算法，既能高效率地實現(xiàn)高精度自定位又可以保證系統(tǒng)的魯棒性。本文所提出的自定位算法的執(zhí)行流程如下：

1)基于gmapping建立室內(nèi)環(huán)境地圖模型；

2)基于Hough變換提取線特征生成環(huán)境模型誤差查找表并儲存；

3)將RGB-D相機獲得的深度圖像轉(zhuǎn)化為等價的激光掃描信息并發(fā)布；

4)機器人訂閱實時的激光掃描信息；

5)機器人基于環(huán)境模型誤差表通過匹配優(yōu)化定位算法解算出機器人的位姿，算法結(jié)束。

2.1 基于gmapping建立未知環(huán)境的二維占據(jù)柵格地圖

gmapping是一種高效率的Rao-Blackwellized粒子濾波算法，在建議分布與自適應(yīng)重采樣兩個方面具有很大的改進，本文基于gmapping將RGB-D相機得到的深度圖像轉(zhuǎn)化為等價的激光掃描信息，以建立未知室內(nèi)環(huán)境的二維占據(jù)柵格地圖，其建圖過程大致可分為三步，首先利用RGB-D相機每秒所產(chǎn)生的30幀深度圖生成局部二維占據(jù)柵格地圖同時估計出粒子(機器人)的位置與姿態(tài)，接著讀取新的激光掃描信息并將其加入到粒子群再利用RBPF算法[11]優(yōu)化粒子數(shù)目，最后選取權(quán)重最大的粒子作為機器人的最佳位姿并將生成的局部二維占據(jù)柵格地圖加入到全局地圖中，這三步驟不斷地循環(huán)直至環(huán)境建圖結(jié)束。

本文以實驗室為研究環(huán)境，首先讓機器人遍歷室內(nèi)環(huán)境，獲得RGB-D相機輸出的等價的激光掃描信息和里程計輸出的機器人位姿數(shù)據(jù)，接著利用gmapping對機器人所采集的數(shù)據(jù)進行處理，生成二維占據(jù)柵格地圖并存儲，圖1為機器人遍歷室內(nèi)長走廊與會議室后所建立的二維占據(jù)柵格地圖，地圖分辨率為每個像素點5 cm，白色，黑色與灰色像素分別代表未被占據(jù)、占據(jù)以及未知3種狀態(tài)，墻壁障礙物已經(jīng)用黑色粗實線標出。

圖1 機器人建立的環(huán)境地圖

2.2 基于霍夫變換線特征提取建立環(huán)境模型誤差查找表

霍夫變換(Hough Transform)方法能夠?qū)Χ祱D像進行直線特征的提取，標準參數(shù)化方式下，式(1)為直線l的表達(定義與性質(zhì)，參考文獻[12])。

r=x1cosθ+y1sinθ

(1)

記N×N二值圖像(xi,yi)像素的灰度值為I(xi,yi)，參數(shù)空間中，[0,π]區(qū)間上，θ均勻地取M個離散值，r的采樣個數(shù)為Q，標準霍夫變換(standrad hough transform , SHT)在直線檢測中由式(2)表示：

(2)

Hough變換可以有效地提取線特征，本文基于Hough變換線特征提取來構(gòu)建環(huán)境模型誤差查找表。環(huán)境模型誤差查找表表示世界坐標系下所有環(huán)境內(nèi)的點到離它最近的線特征點之間的實際距離。

環(huán)境模型誤差表的建立描述如下。首先對保存的環(huán)境地圖模型進行邊緣檢測并二值化處理，接著利用累計概率霍夫變換對二值化后的環(huán)境地圖進行特征提取，即對障礙物點進行線特征提取，如圖2(a)所示，將非結(jié)構(gòu)化的室內(nèi)環(huán)境轉(zhuǎn)化為結(jié)構(gòu)化的室內(nèi)環(huán)境，檢測出的障礙物線特征用紅色標記，非結(jié)構(gòu)化環(huán)境向結(jié)構(gòu)化環(huán)境轉(zhuǎn)化的目的是便于使用匹配優(yōu)化算法進行最優(yōu)解的解算。接著遍歷環(huán)境地圖，計算出環(huán)境中每一個點與離該點最近障礙物直線段的實際距離(該距離用于匹配優(yōu)化定位中近似觀測的特征點與實際位置的偏差)，最后利用環(huán)境中每一個點與最近障礙物直線段的實際距離來生成環(huán)境模型的誤差查找表并存儲，圖2(b)為環(huán)境模型誤差查找表示意圖，圖中每一個像素點的灰度值代表該點與最近障礙物直線段之間的距離，灰度值越大意味著距離越大，即誤差越大。

2.3 基于匹配優(yōu)化的自定位

匹配優(yōu)化定位的核心思想是把機器人所觀測到的特征點與環(huán)境信息相匹配，預(yù)先定義誤差函數(shù)，利用優(yōu)化算法尋找使誤差函數(shù)最小化的最優(yōu)解[13]。本文機器人所觀測到的特征點主要有室內(nèi)環(huán)境中的墻壁與桌椅等障礙物。

基于匹配優(yōu)化自定位的實現(xiàn)過程描述如下。機器人首先通過RGB-D相機獲得障礙物特征點在機器人體坐標系下的坐標

圖2 Hough變換提取的結(jié)果和誤差查找表

值，并經(jīng)過坐標轉(zhuǎn)換將機器人體坐標系下的坐標值轉(zhuǎn)換到世界坐標系中。定義

為機器人檢測到的障礙物特征點在機器人體坐標系下的坐標值，機器人在世界坐標系下的定位值為(

x

,

y

,

θ

)，坐標轉(zhuǎn)換關(guān)系如圖3(a)所示，公式(3)將障礙物特征點從機器人體坐標系轉(zhuǎn)換到世界坐標系中，世界坐標系下障礙物特征點的坐標值與環(huán)境中該特征點的真實位置的偏差可定義為

d

(

o

i

)，理想情況下，該偏差值應(yīng)該等于0，因為所觀測的障礙物特征點理論上應(yīng)該在環(huán)境模型中障礙物直線段上。該偏差可用觀測的特征點與離該特征點最近障礙物直線段實際距離來近似，因此機器人的自定位問題可轉(zhuǎn)化為求解式(4)的尋優(yōu)問題，其中

n

為障礙物特征點的數(shù)目，本文將誤差函數(shù)

e

(

t

)定義為

如圖3(b)所示，

c

為常量，取值為250。本文使用了RPROP算法

[14]

用于求解誤差函數(shù)尋優(yōu)的過程中無約束最優(yōu)化問題，RPROP算法最早用于訓(xùn)練神經(jīng)網(wǎng)絡(luò)，優(yōu)化權(quán)值，與常見的BP算法不同，RPROP算法只利用梯度的方向以及方向變化的信息來進行權(quán)值調(diào)整而不是根據(jù)梯度大小調(diào)整優(yōu)化步長。本文中機器人自定位問題需要優(yōu)化的是(

x

,

y

,

θ

)三個參數(shù)，求出誤差函數(shù)的一階導(dǎo)數(shù)，作為梯度，再利用梯度的方向和梯度方向變化的特點來尋找函數(shù)的最優(yōu)解從而完成機器人的自定位，見公式(5)～(7)。

圖3 坐標系轉(zhuǎn)換與誤差函數(shù)

(3)

(4)

(5)

(6)

(7)

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

本文在室內(nèi)非結(jié)構(gòu)化環(huán)境下進行了定位實驗，測試了所提出算法的性能?？偣苍O(shè)計了兩組實驗，第一組實驗中，移動機器人在建有地圖的室內(nèi)環(huán)境中進行定位，記錄定位得到的機器人位姿和定位算法處理時間，對定位算法的準確性與實時性進行分析；第二組實驗在第一組實驗的基礎(chǔ)上，人為地在環(huán)境中添加噪聲干擾，測試定位算法的魯棒性。

實驗環(huán)境如圖4(a)所示，實驗軟件平臺基于機器人操作系統(tǒng)(robot operating System，ROS)[15]，該操作系統(tǒng)能夠提供類似傳統(tǒng)操作系統(tǒng)的相關(guān)功能，如硬件抽象、底層設(shè)備控制、常用功能實現(xiàn)、進程間消息的傳遞以及軟件包管理等。實驗硬件平臺選用Willow Garage公司的TurtleBot2機器人開源硬件平臺，如圖4(b)所示，TurtleBot2由Kobuki移動底盤和Kinect組成。Kobuki接口控制板32個內(nèi)置傳感器，兩個驅(qū)動輪，單軸陀螺儀，一個可以擴展的輸入輸出端口和一個后擋板，開放式接口可以直接實現(xiàn)對機器人的移動與輸入傳感器的操作。Kinect是一款具有高精度的深度傳感器，被廣泛應(yīng)用于室內(nèi)移動機器人的研究[16-17]。Kinect的RGB 相機每秒能夠獲取30幀的彩色圖像，左右兩個深度傳感器，基上安裝有電機，可以使Kinect在豎直平面內(nèi)上下傾斜正負27°，深度傳感器利用主動射出的紅外光往返的相位差來測量深度，與傳統(tǒng)的結(jié)構(gòu)光測量技術(shù)不同，Kinect的光編碼所使用的是連續(xù)的照明(非脈沖)，無需特制的感光芯片，普通的CMOS感光芯片即可滿足要求。

3.1 機器人自定位準確性和實時性分析

本實驗通過遙控TurtleBot2遍歷室內(nèi)大廳并記錄定位過程中自定位算法得到的機器人位置和姿態(tài)，得到的定位結(jié)果如圖5所示，誤差統(tǒng)計結(jié)果見表1。本文將TurtleBot2提供的定位算法解算的位姿設(shè)為真值(淺色圓圈點為本文算法的估計值，深色星號點為真值)。

圖4 實驗環(huán)境與硬件平臺

圖5 沿室內(nèi)大廳移動時自定位結(jié)果

MAERMSEX/cm13．315．3Y/cm17．520．2θ/rad0．120．18

從圖5和表1中能夠得出，機器人位置定位的平均絕對誤差都不超過17.5 cm，朝向定位的平均絕對誤差小于7°，表明本文提出的機器人自定位算法能夠滿足室內(nèi)移動機器人的定位要求。

實驗環(huán)境改為長1 400 cm，寬132 cm的長條形走廊，沿走廊A、B兩點往返做對比實驗，將本文提出的自定位算法與粒子濾波定位算法在定位時間上進行對比，結(jié)果如圖6所示，其中上細實線為粒子濾波定位計算所需時間曲線，下粗實線為匹配優(yōu)化定位計算所需時間曲線，經(jīng)過測試，機器人采用本文所提出的算法平均處理一次定位計算需要的時間為13.7 ms，粒子濾波定位算法完成一次定位計算平均所需的時間為21.8 ms。實驗中機器人所攜帶處理器主頻為1.7 GHz，內(nèi)存為4.0 G，故本文所提出的算法在實時性上要優(yōu)于粒子濾波定位算法。

圖6 自定位算法時間對比

3.2 機器人自定位算法的魯棒性

本文通過在機器人工作的室內(nèi)環(huán)境里人為地加入噪聲來模擬環(huán)境的變化情況，進而測試本文所提出的自定位算法的魯棒性，噪聲的設(shè)置有門的開關(guān)、增加桌椅數(shù)目以及人員的隨意走動。實驗通過遙控TurtleBot2遍歷長走廊與會議室記錄定位過程中自定位算法得到的機器人位置和姿態(tài)，得到的定位結(jié)果如圖7所示，圖7(a)為未加入噪聲的結(jié)果，圖7(b)為加入噪聲的結(jié)果，定位誤差統(tǒng)計結(jié)果見表2。從圖7和表2中可以看出，加入噪聲后的機器人位置定位的平均誤差增幅小于5 cm，朝向定位的平均誤差增幅小于6°，環(huán)境的變動并沒有導(dǎo)致定位誤差的明顯增大，所以本文所提出的自定位算法對環(huán)境的變化具有一定的魯棒性。

圖7 室內(nèi)環(huán)境機器人定位結(jié)果軌跡圖

未加入噪聲加入噪聲MAERMSEMAERMSEX/cm17．321．320．725．7Y/cm17．521．121．626．6θ/rad0．180．260．240．31

4 結(jié)論

本文針對室內(nèi)環(huán)境下移動機器人基于RGB-D傳感器的視覺精確自定位問題開展研究，通過離線建立二維占據(jù)柵格地圖，并建立環(huán)境模型誤差查找表，將非結(jié)構(gòu)化環(huán)境中的自定位問題轉(zhuǎn)化為結(jié)構(gòu)化環(huán)境中的自定位問題，進而采用匹配優(yōu)化定位方法實現(xiàn)了移動機器人在室內(nèi)環(huán)境中的自定位，所提出的自定位算法在室內(nèi)環(huán)境下滿足實時性、精確性和魯棒性要求，并

且基于ROS框架的自定位算法具有很好的可移植性與擴展性等優(yōu)點，為下一步研究路徑規(guī)劃與自主導(dǎo)航等問題奠定了良好的基礎(chǔ)。

[1]Wang Y, Wang D, Mihankhah E. Navigation of multiple mobile robots in unknown environments using a new decentralized navigation function[A].Control, Automation, Robotics and Vision (ICARCV), 2016 14th International Conference on[C]. IEEE, 2016: 1-6.

[2]Patompak P, Jeong S, Chong N Y, et al. Mobile robot navigation for human-robot social interaction[A].Control, Automation and Systems (ICCAS), 2016 16th International Conference on[C]. IEEE, 2016: 1298-1303.

[3]劉開創(chuàng)，施家棟, 王建中, 等. 移動機器人自主返航控制系統(tǒng)設(shè)計與實驗[J]. 計算機測量與控制, 2016, 24(3): 71-75.

[4]Handschin J E. Monte Carlo techniques for prediction and filtering of non-linear stochastic processes[J]. Automatica, 1970, 6(4): 555-563.

[5]Bukhori I, Ismail Z H. Detection strategy for kidnapped robot problem in Monte Carlo Localization based on similarity measure of environment[A].Underwater System Technology: Theory and Applications (USYS), IEEE International Conference on[C]. IEEE, 2016: 55-60.

[6]Kok M, Hol J D, Schon T B. Indoor positioning using ultrawideband and inertial measurements[J]. IEEE Transactions on Vehicular Technology, 2015, 64(4): 1293-1303.

[7]Jeábek J, Zaplatílek L, Pola M. A proposal of radio ultra wide band systems for precision indoor localization[A].Radioelektronika, 2015 25th International Conference[C]. IEEE, 2015: 355-358.

[8]Loevsky I, Shimshoni I. Reliable and efficient landmark-based localization for mobile robots[J]. Robotics and Autonomous Systems, 2010, 58(5): 520-528.

[9]Iocchi L, Nardi D. Hough localization for mobile robots in polygonal environments[J]. Robotics and Autonomous Systems, 2002, 40(1): 43-58.

[10]Xiong D, Xiao J, Lu H, et al. The design of an intelligent soccer-playing robot[J]. Industrial Robot: An International Journal, 2016, 43(1): 91-102.

[11]Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE transactions on Robotics, 2007, 23(1): 34-46.

[12]Zhang W, Li H, Yan X, et al. A Method of Recognizing Curve Direction Based on Hough Transform[A].Computational Intelligence and Design (ISCID), 2016 9th International Symposium on[C]. IEEE, 2016, 2: 3-6.

[13]Lauer M, Lange S, Riedmiller M. Calculating the perfect match: an efficient and accurate approach for robot self-localization, RoboCup 2005[A]. robot Soccer World Cup IX[C]. 2006.

[14]Riedmiller M, Braun H. A direct adaptive method for faster backpropagation learning: The RPROP algorithm[A].Neural Networks, 1993, IEEE International Conference on[C]. IEEE, 1993: 586-591.

[15]Martinez A, Fernández E. Learning ROS for robotics programming[M]. Packt Publishing Ltd, 2013.

[16]Hamzeh O, Elnagar A. A Kinect-based indoor mobile robot localization[A].Mechatronics and its Applications (ISMA), 2015 10th International Symposium on[C]. IEEE, 2015: 1-6.

[17]Kamarudin K, Mamduh S M, Shakaff A Y M, et al. Performance analysis of the microsoft kinect sensor for 2D simultaneous localization and mapping (SLAM) techniques[J]. Sensors, 2014, 14(12): 23365-23387.