基于激光信息的移動機器人SLAM研究

柳俊城，李 迪，翁瀟文

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

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

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

1 傳感器信息提取

1.1 Kinect激光信息提取

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

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

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

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

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

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

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

1.2 激光雷達信息提取

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

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

2 移動機器人控制平臺

2.1 機器人操作系統

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

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

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

2.2 移動機器人平臺控制

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

圖2 ROS節點運行Fig.2 ROS node operation

3 SLAM問題的解決方案

3.1 二維SLAM模型

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

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

圖3SLAM模型Fig.3 SLAM model

3.2 SLAM算法

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

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

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

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

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

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

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

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

對于采樣分布函數，結合觀測數據和里程計數據，采樣的結果要盡可能分布在觀測數據的似然函數可行區域內，則對每個粒子建議分布為

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

其中

為歸一化因子。

3.3 SLAM算法流程

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

4 試驗結果

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

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

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

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

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

5 結語

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

[1] 滿增光．基于激光雷達的室內AGV地圖創建與定位方法研究[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]．系統工程與電子技術，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]．自動化與儀表，2012，27（5）：5-9．

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

[10] 章志誠，杜昌平．基于激光雷達的多旋翼飛行器實時避障系統[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的移動機器人地圖創建方法與應用研究[D]．綿陽：西南科技大學，2016．

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