基于ROS的移動機器人自主建圖與路徑規劃*

溫淑慧， 問澤藤， 劉 鑫， 溫淑煥

(燕山大學 河北省工業計算機控制工程重點實驗室， 河北 秦皇島 066004)

近年來，移動機器人技術發展迅速，已經應用到農業、服務業、物流業等多個領域.導航技術作為移動機器人技術研發的關鍵技術，主要包括SLAM(實時定位與建圖)和路徑規劃部分.SLAM[1]需要完成機器人的定位與建圖任務，而路徑規劃問題實際上就是依據某個或者某些優化準則(如實時性、安全性、最優性等)，為移動機器人規劃出一條成功躲避從初始點到目標點的移動過程中所遇到的動態障礙物的可行路徑.

針對移動機器人SLAM問題的解決方法，王元華等[2]通過對局部地圖和全局地圖的線段特征關系進行掃描匹配，實現機器人的全局定位，完成機器人在未知環境下的SLAM.

針對路徑規劃問題，傳統的路徑規劃算法有常用作全局路徑規劃的Dijkstra法、BFS算法及A*算法，以及常用作局部路徑規劃的動態窗口法(DWA)、人工勢場法[3].Dijkstra算法的優點是即使在存在障礙物的環境下，它仍能保證找到一條從初始節點到目標節點的最短路徑，但是運行速度較慢[3].傳統的人工勢場法結構簡單，便于控制，但容易發生障礙物附近震蕩以及所得路徑并非最優路徑的情況[4].

為了提高移動機器人導航的自主性能，本文設計了基于ROS的移動機器人自主建圖與路徑規劃系統，通過搭載的2D激光雷達獲取周圍環境信息，同時利用姿態傳感器(IMU)獲取機器人的姿態和加速度信息，利用Gmapping算法實現機器人的自主定位與建圖.路徑規劃部分又分為全局路徑規劃及局部路徑規劃.由于傳統A*算法的時間復雜度與起始點位置、節點數量密切相關，為了提高搜索效率，本文對A*算法進行改進，采用基于頭尾雙向搜索的A*算法作為全局路徑規劃方法，局部路徑規劃方法采用經典的DWA算法[5]來實現避障.

1 自主導航系統設計

1.1 AMCL定位系統

AMCL定位系統是移動機器人在二維環境中的概率定位系統，核心是一種利用粒子表示置信度的自適應蒙特卡羅算法[6]，其主要原理是根據環境地圖，利用粒子濾波器來跟蹤機器人的位姿，根據采樣粒子估計機器人位姿的概率分布，機器人位姿估計的可靠程度由粒子權重決定[7].算法主要由預測位姿和更新位姿兩部分組成，首先將隨機抽取的位姿粒子均勻分布在整個位姿空間，之后根據傳感器信息，為每個粒子分配重要性系數，預測t時刻的機器人位姿，并估計其運動軌跡[8]；然后再次通過傳感器的掃描數據，更新采樣粒子的重要性系數，重要性系數越大，說明其位姿越接近于真實位姿.同時，對粒子進行重采樣，使得定位更加準確，在采樣粒子小于閾值時不斷重復這一過程.

在啟動導航時，AMCL根據所配置的參數情況初始化其粒子濾波器，并在Rviz中使用2D Pose Estimate按鈕來初始化位姿.

1.2 基于2D激光雷達的SLAM系統

根據傳感器進行分類，SLAM的實現方法主要分為兩類：利用攝像頭獲取外部圖像數據來解析環境信息的視覺SLAM和依賴于激光雷達的激光SLAM.其中，視覺SLAM由于圖像信息復雜、包含信息量大，很難做到實時運算并占用較大的CPU空間和內存空間.相比較下，本文所使用的激光雷達通過向目標環境發射激光數據，并根據接收反射回來的數據進行對周圍環境的感知，進而進行地圖和位置的計算.此外，攝像頭在室外工作時受環境、光照影響較大，而激光雷達受光照影響較小，即使是在黑暗條件下，也能很好地工作.

本文采用基于RBPF濾波算法的Gmapping算法進行SLAM.由于傳統的RBPF算法在實際應用中計算量較大，在構建地圖時需要較多的粒子進行重復采樣[9]，導致粒子退化、多樣性降低.Gmapping算法通過改進粒子采樣過程中的提議分布來降低計算位姿所需要的粒子數量；同時提出選擇性重采樣，為采樣粒子數設置一個合適的閾值，只有在滿足條件時才進行重采樣.

1.3 路徑規劃

1.3.1 全局路徑規劃

傳統A*算法是一種啟發式的路徑搜索方法，它結合了BFS和Dijkstra算法的優點，其估價函數的一般形式為

f(n)=g(n)+h(n)

(1)

式中：f(n)為節點n從初始節點到目標節點所消耗代價值；g(n)為初始節點到任意節點n耗費的真實代價值；h(n)為機器人從節點n移動到目標節點所消耗代價值的啟發函數[10].整個算法的關鍵是對啟發函數h(n)的選取.

預估移動代價h(n)的計算方法有多種，例如曼哈頓距離、歐幾里得距離、對角線距離[11]等，其中，曼哈頓距離表示為

h(n)=|xd-xn|+|yd-yn|

(2)

式中：xd、xn分別為目標點和節點n的橫坐標；yd、yn分別為目標點和節點n的縱坐標.

A*算法的時間復雜度與起始點位置以及節點數量密切相關.實驗環境1如圖1所示，當起始點(淺色方塊)位于寬闊地帶，目標點(深色方塊)位于狹窄地帶時，擴展的節點較多(如圖1a所示).而當起始點與目標點互換位置時，擴展節點明顯減少(如圖1b所示).

圖1 實驗環境1下A*算法仿真圖Fig.1 Simulation diagram of A* algorithm under experimental environment 1

為了證明結果的非偶然性，在兩種不同環境中再次進行了仿真對比，如圖2、3所示.

圖2 實驗環境2下A*算法仿真圖Fig.2 Simulation diagram of A* algorithm under experimental environment 2

圖3 實驗環境3下A*算法仿真圖Fig.3 Simulation diagram of A* algorithm under experimental environment 3

多組實驗證明，A*算法的搜索效率與始末位置密切相關.不同起始位置下的算法性能分析表如表1所示.由表1可知，起始位置對搜索效率影響很大，當起始位置位于開闊位置時，搜索節點數是位于狹窄位置的一倍，搜索時長增加，因此，傳統A*算法存在明顯的缺陷.

表1 不同實驗環境下A*算法不同起始位置性能分析

Tab.1 Performance analysis of different starting positions in A*algorithm under different experimental environments

實驗環境起始位置搜索時間/ms擴展節點數環境1環境2環境3對調前2.345202對調后1.25095對調前1.850143對調后0.89074對調前2.255199對調后1.10086

為了提高路徑規劃的效率，改善上述缺陷，在傳統A*算法搜索策略上提出了改進方法——基于頭尾雙向搜索的A*算法[11].傳統A*算法搜索范圍是從初始節點開始的，而改進算法是對初始節點以及目標節點同時進行擴展搜索，直到擴展節點為同一個子節點時，結束搜索.此時遍歷所有父節點，可得到最優路徑.雙向A*算法不僅改變了搜索策略，同時相應調整了啟發函數h(n).改進后的A*算法啟發函數為

h(n)=|xsn-xen|+|ysn-yen|

(3)

式中：xsn為當前節點橫坐標；xen為另一搜索方向上當前節點橫坐標；ysn為當前節點縱坐標；yen為另一搜索方向上當前節點縱坐標.

對于每個節點需要進行上、下、左、右等8個方向的遍歷，直到某個擴展節點的啟發函數為0，結束搜索.

在不同的實驗環境下進行仿真實驗，對調起始位置和終點位置，得到改進A*算法效果圖如圖4所示.

圖4 不同實驗環境下改進A*算法仿真圖Fig.4 Simulation diagram of improved A* algorithm under different experimental environments

不同實驗環境下改進A*算法性能表現如表2所示，通過與表1對比可知，改進后的A*算法性能較穩定，無論初始位置位于狹窄地帶還是寬闊地帶，其搜索時間和擴展節點數近似一致.對比傳統A*算法，搜索效率有所提高.

表2 不同實驗環境下改進A*算法性能分析Tab.2 Performance analysis of improved A* algorithm under different experimental environments

1.3.2 局部路徑規劃

DWA算法可以實現實時路徑規劃以及實時避障，其基本原理是：計算移動機器人每個周期內應該行駛的線速度和角速度.先離散采樣機器人控制空間，再根據機器人當前的狀態預測可能出現的狀況，并根據一些度量標準(如障礙物接近目標、接近全局路徑和速度等)來評價每個軌跡結果，舍棄不合適的路徑(有障礙物碰撞的)，最后挑選得分最高的運動軌跡，并發布相關的速度給移動平臺.DWA算法流程圖如圖5所示.

圖5 DWA算法流程Fig.5 Flow chart of DWA algorithm

2 移動機器人真實環境實驗

2.1 硬件平臺

硬件平臺主要以主控電腦為核心，外部搭載激光雷達傳感器，用來采集機器人周圍所有與物體相關的距離信息；同時也搭載了姿態傳感器(IMU)，用來獲得機器人整體的姿態信息和加速度信息.使用直流無刷電機來控制其驅動裝置；使用舵機用來控制整體的轉向，通過單片機作為下位機的控制器，接收PC端傳輸的控制信號，進一步再對直流無刷電機和舵機進行控制，進而實現對車體速度和方向的控制.由于機器人并不配有屏幕，所以為其搭載了路由器，方便操作人員進行遠程控制的工作.硬件系統總體框架如圖6所示.

2.2 軟件平臺

軟件主要以ROS機器人操作系統為框架主體，在ROS機器人操作系統上，添加激光雷達和IMU的姿態傳感器的驅動程序，這樣就可以獲得周圍環境的信息和機器人整體的姿態信息；通過無刷直流電機和舵機控制機器人的移動來采集整體的環境信息.將這些采集到的數據進行處理，利用Gmapping算法完成對整個環境地圖的構建，獲得全局靜態地圖；再通過路徑規劃和自主導航來控制機器人的位置.在電腦的ROS端上，啟用ssh遠程訪問，用以遠程控制整個系統.系統軟件框架如圖7所示.

圖6 系統總體框架Fig.6 General frame of system

圖7 系統軟件框架Fig.7 Software frame of system

2.3 路徑規劃實驗

本文以實驗室走廊為基本環境，在其運動過程中，添加紙盒、錐桶等作為障礙物，模擬室內基本環境.實驗環境如圖8所示.

通過使用ROS中的Gmapping功能包完成建圖工作，得到二維環境地圖.使用Navigation功能包完成自主導航工作，并在Rviz可視化界面中觀察規劃路徑情況.圖9即為規劃出的全局路徑，局部代價地圖隨著機器人的移動不斷更新，相應地，局部路徑也發生改變，順利避開障礙物.

3 結 論

本文針對移動機器人導航缺乏自主性問題，設計了基于2D激光雷達的SLAM系統；提出基于頭尾雙向搜索的A*算法用于路徑規劃工作，并進行仿真研究.仿真結果表明，與傳統A*算法相比，該方法的搜索效率大大提高.在真實環境中進行實驗驗證，結果表明，采用基于頭尾雙向搜索的A*算法可以規劃出一條最優路徑，同時，DWA算法能夠完成對動態未知障礙物的檢查與躲避任務，最終到達目標位置，從而提高移動機器人的自主導航性能及工作效率.

圖8 實驗環境Fig.8 Experimental environment

圖9 全局路徑規劃提取Fig.9 Global path planning extraction