基于激光信息的移動機(jī)器人SLAM研究

柳俊城，李 迪，翁瀟文

（華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣州 510640）

隨著移動機(jī)器人的發(fā)展，傳統(tǒng)的導(dǎo)航方式如電磁導(dǎo)航、磁帶導(dǎo)航和慣性導(dǎo)航等，存在著靈活性差，易受干擾等不足。在移動機(jī)器人自主導(dǎo)航研究方面，實現(xiàn)機(jī)器人自主導(dǎo)航和靈活避障的關(guān)鍵是，解決移動機(jī)器人運動過程中的同時定位與地圖構(gòu)建SLAM（simultaneous localization and mapping）。 SLAM 問題可以描述為：機(jī)器人在未知環(huán)境中從一個未知位置開始運動，在運動過程中依據(jù)位置估量和傳感器數(shù)據(jù)進(jìn)行自身定位，同時構(gòu)建增量式地圖[1-2]。目前，針對SLAM的實現(xiàn)過程，研究學(xué)者們根據(jù)不同的傳感信息提出了多種算法來實現(xiàn)，如基于里程計及二維傳感信息實現(xiàn)的算法有卡爾曼濾波、擴(kuò)展卡爾曼濾波、粒子濾波等[3-4]，基于視覺等三維傳感信息實現(xiàn)的算法有RGBD SLAM等[5]。

激光導(dǎo)航，作為新型的移動機(jī)器人導(dǎo)航方式，具有高度靈活性、測量速度快等特點，且具有較強(qiáng)有抗干擾能力[6-7]。目前，應(yīng)用在移動機(jī)器人導(dǎo)航且能獲得激光信息的傳感器有2類，一類是激光雷達(dá)，這類通過激光測距原理能直接取得周圍環(huán)境的距離信息。另一類是三維傳感器如Kinect，雙目視覺傳感器等，此類傳感器能獲得空間的任意位置的深度信息，通過信息轉(zhuǎn)化可以轉(zhuǎn)成相應(yīng)激光信息。在此，首先介紹激光雷達(dá)和Kinect的激光信息獲取方法，進(jìn)而提出一種基于激光信息的SLAM實施方法。

1 傳感器信息提取

1.1 Kinect激光信息提取

微軟Kinect 1.0視覺傳感器可以同時獲得彩色圖像和深度圖像，其水平視角為57°，垂直視角為43°，檢測范圍為 0.8～4.0 m[8]。 Kinect獲得的深度信息包含攝像機(jī)視野內(nèi)任意點的空間信息，所以利用Kinect獲取的深度信息進(jìn)行轉(zhuǎn)換時，可以將垂直平面上相應(yīng)空間的信息整合而成二維激光信息，將Kinect所獲取的三維信息有效地融合轉(zhuǎn)換。

首先，Kinect在環(huán)境中測量獲取到深度信息，而后根據(jù)式（1）和（2）計算X，Y坐標(biāo)軸上對應(yīng)的像素深度（即Z軸）為

式中：i和j分別為Z陣列中像素的行和列的數(shù)值；w和h分別為Z陣列的寬度和高度；M為Kinect NUI相機(jī)的骨架倍增系數(shù)。

而每個空間點在X軸上投影的點到相機(jī)光心O的距離r（如圖1所示）為

為了將深度圖像中的三維數(shù)據(jù)壓縮成二維的激光數(shù)據(jù)，可以取Z陣列中每一列最小元素作為該列的標(biāo)識數(shù)據(jù)，表示在前方縱向上最近障礙物的距離。

圖1 空間點到相機(jī)光心距離Fig.1 Optical center distance from spatial point to camera

由式（4）可知，矩陣Z′包含了Z軸上每次水平掃描中最近障礙物的距離信息，因此根據(jù)能更為完整的構(gòu)造出地圖中障礙物的距離信息，成功避免發(fā)生碰撞。

1.2 激光雷達(dá)信息提取

激光雷達(dá)的測距原理有2類。一類是運用TOF（time of flight）原理，將激光束平行發(fā)射出去，測量發(fā)射到接收到反射回來的激光的時間差來計算目標(biāo)距離，從而得到環(huán)境地圖的平面圖。另一類是基于激光三角測距法原理，被測物體表面被一束特定入射角度的激光照射，激光在物體表面發(fā)生反射和散射，利用透鏡在另一角度對反射激光匯聚成像，光斑成像在CCD位置傳感器上。當(dāng)位置傳感器上的光斑發(fā)生移動時，說明被測物體沿激光方向發(fā)生移動，而被測物體的移動距離對應(yīng)了光斑的位移大小。據(jù)此可通過光斑位移的距離，結(jié)合入射光角度和反射光角度構(gòu)成的三角形，利用幾何三角定理[9]原理設(shè)計算法計算出被測物體與基線的距離值。

文中采用的激光雷達(dá)是一款基于三角測距原理的掃描式傳感器，每次測距會發(fā)射已調(diào)制好的紅外激光，激光信號在被照射物上產(chǎn)生反光被紅外感受器所接收。經(jīng)過雷達(dá)內(nèi)置的處理器實時解算出與被照射物與激光雷達(dá)之間的距離及夾角信息[10]。在雷達(dá)內(nèi)置的電機(jī)驅(qū)動下，測距模塊能實現(xiàn)360°全方位環(huán)境掃描測距檢測。該激光雷達(dá)主要技術(shù)參數(shù)見表1。

2 移動機(jī)器人控制平臺

2.1 機(jī)器人操作系統(tǒng)

激光導(dǎo)航移動機(jī)器人的控制系統(tǒng)使用基于Linux操作系統(tǒng)開發(fā)的機(jī)器人操作系統(tǒng)ROS（robotoperating system）。開源性是ROS的一個重要特點，可以為操作者提供硬件抽象層，使其軟件能夠方便地在多種平臺進(jìn)行移植[11]；能夠為機(jī)器人操作程序提供通訊功能。ROS還自帶一些工具軟件和庫函數(shù)，能更方便地服務(wù)與編寫高兼容性的程序。ROS框架中包含節(jié)點、數(shù)據(jù)包、堆棧以及監(jiān)測工具，是一種分布處理框架，每個節(jié)點可以單獨被設(shè)計，而后可以封裝到數(shù)據(jù)包或者堆棧中，監(jiān)測工具則用來識別主題發(fā)布的內(nèi)容[12]。在這樣的框架下，每個執(zhí)行程序都能被各自獨立地設(shè)計，使用時可以實時組合實現(xiàn)響應(yīng)的功能。

表1 激光雷達(dá)主要技術(shù)參數(shù)Tab.1 Main technical parameters of laser radar

ROS從圖級分析可以分為4部分：節(jié)點、消息、主題、節(jié)點管理器。每一個節(jié)點都是一個數(shù)據(jù)處理點，可通過主題與其他節(jié)點通信；通信的內(nèi)容即為消息，是一種自定義的ROS數(shù)據(jù)類型；節(jié)點管理器對每個節(jié)點進(jìn)行查找及注冊，方便開發(fā)人員查看并維護(hù)。

2.2 移動機(jī)器人平臺控制

文中所使用的移動機(jī)器人為兩輪差動驅(qū)動的機(jī)器人Powerbot，其能在最高達(dá)到100 kg的負(fù)載下能以正常速度1.6 m/s行駛，是一個理想的可用于研究運輸、導(dǎo)航和操控等問題的實驗平臺。在機(jī)器人操作系統(tǒng)ROS中，節(jié)點rosaria為大多數(shù)移動機(jī)器人平臺提供了一個ros接口，利用開源庫Aria來控制電機(jī)、檢測電池和獲取編碼器數(shù)據(jù)[13]。文中試驗均在Powerbot機(jī)器人上完成，其節(jié)點控制如圖2所示。

圖2 ROS節(jié)點運行Fig.2 ROS node operation

3 SLAM問題的解決方案

3.1 二維SLAM模型

在運動過程中移動機(jī)器人位姿可以用一個三維向量（x，y，θ）表示。 假設(shè)，已經(jīng)觀測到的地圖為mt，SLAM的觀測模型如圖3所示。圖中，zt為t時刻傳感器的觀測量；ut為t時刻給機(jī)器人的控制信息。隨著傳感器的移動，附近的路標(biāo)被感知到，依據(jù)輸入信息從而估計出機(jī)器人運動路徑x1∶t以及地圖m。采用遞歸貝葉斯估計，可得

式中：η為歸一化常量。由此可知t時刻后驗證概率能夠表示成t-1時刻的后驗概率、感知模型和運動模型的迭代形式。

圖3SLAM模型Fig.3 SLAM model

3.2 SLAM算法

根據(jù)式（5）給定的分布 p（zt∣xt，mt）和 p（xt，mt∣xt-1，mt-1，ut-1），可以得到 SLAM 系統(tǒng)中的 2 個模型，分別為機(jī)器人的感知模型和運動模型。前者表示在移動機(jī)器人地圖mt與機(jī)器人位姿xt給定的條件下，傳感器獲取環(huán)境的不確定性；后者表示在上一時刻移動機(jī)器人軌跡xt-1和控制命令ut-1下，機(jī)器人新位姿xt的概率密度。使用Rao-Blackwellised粒子濾波器（RBPF），可以將貝葉斯濾波器狀態(tài)空間分解為

通過RBPF方法將SLAM問題分解成2個獨立的后驗概率的乘積?？上韧ㄟ^對移動機(jī)器人移動的軌跡進(jìn)行估計，然后再結(jié)合觀測模型對地圖進(jìn)行更新。對軌跡的估計采用一個包含n個粒子的集合來表示系統(tǒng)狀態(tài)的后驗概率，每一個粒子的狀態(tài)表示了機(jī)器人的一條可能運動軌跡。對每個粒子來說，它所表示的機(jī)器人位姿是完全確定的，因此能保證上式正確成立。

RBPF對使用里程計結(jié)合傳感器觀測信息構(gòu)建增量式地圖非常有效[14]，更新地圖和機(jī)器人軌跡的采樣有以下4步：

步驟1 采樣。利用觀測數(shù)據(jù)zt和里程計信息ut-1進(jìn)行最優(yōu)位姿估計，結(jié)果x1∶t-1對應(yīng)粒子分布，根據(jù)建議分布函數(shù)q，獲取機(jī)器人的位姿x1∶t組成下一時刻的粒子集。

步驟2 粒子權(quán)重。為彌補(bǔ)上一步采樣分布與目標(biāo)分布的誤差，需要計算每一獨立粒子的權(quán)重，用于判定粒子的有效性，由重要性采樣公式得

步驟3 重采樣。由于粒子是依照其權(quán)重比例來選取，并且使用了有限個粒子來近似連續(xù)分布。利用地圖中隨機(jī)分布多個粒子，每一個粒子都代表機(jī)器人的一種潛在的運動軌跡，機(jī)器人在運動過程中，粒子一直通過迭代收斂，保留權(quán)重高的粒子同時舍棄權(quán)重低的粒子，最終保留與機(jī)器人軌跡基本吻合的粒子集。重采樣過程中，自適應(yīng)閾值Neff為粒子群與機(jī)器人軌跡的吻合程度，其計算公式為

式中：ωi為粒子i的歸一化權(quán)重。在篩選過程中為避免出現(xiàn)粒子枯竭的情況出現(xiàn)，當(dāng)粒子個數(shù)＜N/2時，需要進(jìn)行重采樣步驟[15]。

步驟4 地圖更新。對于每一個粒子來說，可以用其軌跡和觀測信息來計算相應(yīng)的位姿，從而對地圖更新。

對于采樣分布函數(shù)，結(jié)合觀測數(shù)據(jù)和里程計數(shù)據(jù)，采樣的結(jié)果要盡可能分布在觀測數(shù)據(jù)的似然函數(shù)可行區(qū)域內(nèi)，則對每個粒子建議分布為

而為了獲得更高效的下一代采樣粒子，可以將似然函數(shù)峰值附近的分布近似為高斯分布。這樣，每個粒子的高斯分布參數(shù)由次采樣和里程計數(shù)據(jù)可得

其中

為歸一化因子。

3.3 SLAM算法流程

當(dāng)每一個時刻有更新時，粒子的提議分布會重新計算并更新粒子。具體如下：①依據(jù)上一時刻的位姿xt-1和里程計信息ut-1對機(jī)器人當(dāng)前初始位姿進(jìn)行估計；②獲得地圖mt-1執(zhí)行SLAM匹配算法；③根據(jù)式（12）求取采樣建議分布④在建議分布函數(shù)中采樣粒子；⑤計算并更新粒子權(quán)重；⑥重采樣；⑦根據(jù)機(jī)器人位姿、觀測信息及里程計信息計算新時刻地圖，并更新。

4 試驗結(jié)果

試驗在Powerbot機(jī)器人上進(jìn)行，其上搭載了Kinect、激光雷達(dá)、內(nèi)置編碼器和與這些傳感器通信的處理中心PC。平臺結(jié)構(gòu)如圖4所示。PC系統(tǒng)為Ubuntu 14.04，搭載了 ROS Indigo 操作系統(tǒng)。

圖4 移動機(jī)器人試驗平臺Fig.4 Mobile robot experimental platform

在試驗中，移動機(jī)器人Powerbot的運行速度設(shè)定為 0.3 m/s，加速度為 0.1 m/s2，最大角速度為 1.5 rad/s。將移動機(jī)器人放置于20 m×10 m的室內(nèi)環(huán)境中，并分別利用Kinect和激光雷達(dá)對室內(nèi)環(huán)境進(jìn)行掃描獲取激光信息。Kinect和激光雷達(dá)都在距離地面75 cm的高度上進(jìn)行試驗。同時，通過PC向底盤控制系統(tǒng)發(fā)送控制指令，從而控制移動機(jī)器人的運動。在試驗過程中，通過ROS中的RVIZ三維可視化工具，可以查看機(jī)器人當(dāng)前的位置和在試驗區(qū)域內(nèi)的地圖創(chuàng)建過程。試驗的場景、相應(yīng)的激光掃描地圖如圖5所示。圖中，黑色邊緣點表示障礙物，淺灰色區(qū)域表示可通行區(qū)域。

圖5 試驗場景以及相應(yīng)的激光掃描地圖Fig.5 Experimental scene and corresponding laser scanning map

試驗結(jié)果表明，2種傳感器在室內(nèi)環(huán)境下能準(zhǔn)確對機(jī)器人位姿定位和地圖構(gòu)建。但是在實驗室同一個環(huán)境下，使用Kinect構(gòu)建的地圖可以表達(dá)出實驗室中央的生產(chǎn)線位置信息，而激光雷達(dá)僅能表現(xiàn)出生產(chǎn)線中支撐腳的信息。但激光雷達(dá)單次掃描范圍大，可以快速構(gòu)建出環(huán)境地圖。這說明，使用Kinect可以彌補(bǔ)激光雷達(dá)無法采集高度信息的缺陷，能較好地提高地圖的可靠性，使得移動機(jī)器人導(dǎo)航過程中更具有安全性。

5 結(jié)語

文中提出一種基于激光信息的移動機(jī)器人同時定位與地圖構(gòu)建算法，在使用時可以選擇直接輸出激光數(shù)據(jù)的激光雷達(dá)，或者使用三維傳感器轉(zhuǎn)換成二維激光信息，這2種傳感器在室內(nèi)環(huán)境下能準(zhǔn)確對機(jī)器人位姿定位和地圖構(gòu)建。使用Kinect可以彌補(bǔ)二維傳感器無高度信息的不足，能較好地提高地圖的可靠性，使得移動機(jī)器人導(dǎo)航過程中更具有安全性。而激光雷達(dá)的有效感知范圍大，能快速構(gòu)建環(huán)境地圖。通過試驗驗證了所提算法的高效性和創(chuàng)建地圖的準(zhǔn)確性。

[1] 滿增光．基于激光雷達(dá)的室內(nèi)AGV地圖創(chuàng)建與定位方法研究[D]．南京：南京航空航天大學(xué)，2014．

[2] Birk A，Pfingsthorn M．Simultaneous localization and mapping（SLAM）[C]//American Cancer Society，2016．

[3] Santos J M，Portugal D，Rui P R．An evaluation of 2D SLAM techniques available in robot operating system[C]//IEEE International Symposium on Safety，Security，and Rescue Robotics．IEEE，2014：1-6．

[4] 劉丹，段建民，于宏嘯．基于自適應(yīng)漸消EKF的FastSLAM算法[J]．系統(tǒng)工程與電子技術(shù)，2016，38（3）：644-651．

[5] Henry P，Krainin M，Herbst E，et al．RGB-D mapping：using depth cameras for dense 3-D modeling of indoor environments[M]//Khatib O，Kumar V，Pappas G J．Experimental Robotics．Berlin Heidelberg：Springer，2014：647-663．

[6] 肖鵬，欒貽青，郭銳，等．變電站智能巡檢機(jī)器人激光導(dǎo)航系統(tǒng)研究[J]．自動化與儀表，2012，27（5）：5-9．

[7] 吳越，藍(lán)偉，許大浦，等．一種基于單線激光雷達(dá)的無人車輛導(dǎo)航避障系統(tǒng)設(shè)計[J]．計算機(jī)測量與控制，2013，21（7）：1902-1904．

[8] Ghani M F A，Sahari K S M，Kiong L C．Improvement of the 2D SLAM system using Kinect sensor for indoor mapping[C]//International Symposium on Soft Computing and Intelligent Systems．IEEE，2015：776-781．

[9] 李昀澤．基于激光雷達(dá)的室內(nèi)機(jī)器人SLAM研究[D]．廣州：華南理工大學(xué)，2016．

[10] 章志誠，杜昌平．基于激光雷達(dá)的多旋翼飛行器實時避障系統(tǒng)[J]．計算機(jī)測量與控制，2016，24（9）：117-121．

[11] Quigley M，Conley K，Gerkey B P，et al．ROS：an open-source robot operating system[C]//ICRA Workshop on Open ource Software.2009．

[12] 王權(quán)，胡越黎，張賀．基于ROS與Kinect的移動機(jī)器人同時定位與地圖構(gòu)建[J]．計算機(jī)應(yīng)用研究，2017，34（10）：3184-3187．

[13] Fathzadeh R，Mokhtari V，Abdollahi F，et al．Mobile robot in domestic environment[C]//International Conference on Information Science，Electronics and ElectricalEngineering．IEEE，2014：918-922．

[14] 卜燕．基于ROS的移動機(jī)器人地圖創(chuàng)建方法與應(yīng)用研究[D]．綿陽：西南科技大學(xué)，2016．

[15] 仇恒坦，平雪良，高文研，等．構(gòu)建移動機(jī)器人地圖的多傳感信息融合算法研究[J]．機(jī)械制造，2017，55（8）：1-4．■