柳俊城，李 迪，翁瀟文

（華南理工大學 機械與汽車工程學院，廣州 510640）

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

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

1 傳感器信息提取

1.1 Kinect激光信息提取

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

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

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

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

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

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

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

1.2 激光雷達信息提取

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

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

2 移動機器人控制平臺

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

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

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

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

2.2 移動機器人平臺控制

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

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

3 SLAM問題的解決方案

3.1 二維SLAM模型

在運動過程中移動機器人位姿可以用一個三維向量（x，y，θ）表示。 假設，已經(jīng)觀測到的地圖為mt，SLAM的觀測模型如圖3所示。圖中，zt為t時刻傳感器的觀測量；ut為t時刻給機器人的控制信息。隨著傳感器的移動，附近的路標被感知到，依據(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 個模型，分別為機器人的感知模型和運動模型。前者表示在移動機器人地圖mt與機器人位姿xt給定的條件下，傳感器獲取環(huán)境的不確定性；后者表示在上一時刻移動機器人軌跡xt-1和控制命令ut-1下，機器人新位姿xt的概率密度。使用Rao-Blackwellised粒子濾波器（RBPF），可以將貝葉斯濾波器狀態(tài)空間分解為

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

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

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

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

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

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

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

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

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

其中

為歸一化因子。

3.3 SLAM算法流程

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

4 試驗結果

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

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

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

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

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

5 結語

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

[1] 滿增光．基于激光雷達的室內(nèi)AGV地圖創(chuàng)建與定位方法研究[D]．南京：南京航空航天大學，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] 劉丹，段建民，于宏嘯．基于自適應漸消EKF的FastSLAM算法[J]．系統(tǒng)工程與電子技術，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] 肖鵬，欒貽青，郭銳，等．變電站智能巡檢機器人激光導航系統(tǒng)研究[J]．自動化與儀表，2012，27（5）：5-9．

[7] 吳越，藍偉，許大浦，等．一種基于單線激光雷達的無人車輛導航避障系統(tǒng)設計[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] 李昀澤．基于激光雷達的室內(nèi)機器人SLAM研究[D]．廣州：華南理工大學，2016．

[10] 章志誠，杜昌平．基于激光雷達的多旋翼飛行器實時避障系統(tǒng)[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] 王權，胡越黎，張賀．基于ROS與Kinect的移動機器人同時定位與地圖構建[J]．計算機應用研究，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的移動機器人地圖創(chuàng)建方法與應用研究[D]．綿陽：西南科技大學，2016．

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