基于ROS 改進的A*算法在移動小車上的應用

王世元，李季睿，陶寧果

（江漢大學 智能制造學院，湖北武漢，430056）

1 傳統的路徑規劃算法

1.1 路徑規劃算法的原理

路徑規劃主要包括局部規劃和全局規劃兩大類，全局路徑規劃模塊根據評價指標搜索得到一條最優路徑，局部路徑規劃負責完成路徑的跟蹤和局部自主避障等任務，保證機器人安全移動至目標點[1]。全局路徑規劃的評價指標通常是與距離相關的估價函數，也就是最終搜索到的路徑要距離最優，而在復雜的地圖環境中，我們無法直接判斷起始點與目標點之間是否存在有效路徑，因此，從起始點開始，不斷選取周圍鄰域節點，根據估價函數選取距離最小的節點進行擴展，直到搜索到目標點，這樣就得到一條距離代價小的路徑。

1.2 A*算法與Dijkstra 算法

A*算法是一種在靜態環境中尋找最優路徑的高效算法，是將啟發式方法和常規方法相結合提出的一種算法[2]，其決定代價指標的估價函數為：

g(s)是小車當前位置與起始點的真實代價，用來反映小車當前位置與起始點之間的距離，h(s)是小車當前位置與目標點之間的估計代價，用來反映小車當前位置與目標點之間的距離，因此，f(s)就能比較準確的反映出起始點與目標點之間的距離。

Dijkstra 算法則不考慮h(s)時，即h(s)為0，此時算法會從起始點向四周沒有方向性的擴散，需花費大量時間，進行較大范圍的搜索。

正是由于h(s)的存在，讓起始點與目標點之間有了聯系，A*算法的搜索才更具方向性，搜索的時間和范圍大大減小。

2 A*算法的改進與MATLAB 驗證

2.1 A*算法的改進

傳統A*算法進行路徑規劃時，每次只搜索中心節點臨近的8 個節點，也就是由中心節點向外拓展一層，運動方向只能是45°的整數倍。將傳統A*算法的每個中心節點擴展鄰域由8 個增加至24 個，也就是由中心節點向外擴展兩層，改進成24 鄰域的A*算法[3]。

2.2 改進算法在MATLAB 中的驗證

在MATLAB2016b 的實驗環境條件下，應用A*算法對移動機器人進行3 組路徑規劃仿真實驗。為了精確環境范圍，仿真環境設定為20×20 的柵格地圖，每個柵格的邊長設定為1 米，障礙物為模擬環境。分別采用傳統A*算法與24 鄰域的A*算法在環境空間已知的柵格地圖上進行3 組改變起始點的不同距離長度的路徑規劃實驗。圖1～3 為3 組不同狀態下移動機器人分別應用A*算法與24 鄰域A*算法路徑規劃仿真結果。

圖1 第1 組實驗路徑仿真對比

圖2 第2 組實驗路徑仿真對比

圖3 第3 組實驗路徑仿真對比

3 組實驗的具體數據分別在表1 與表2 中體現，其中表1 列舉了傳統A*算法與24 鄰域A*算法3 組實驗的路徑長度的數據對比；表2 列舉了傳統A*算法與24 鄰域A*算法在擴展點數與搜索時間上的具體情況。

表1 傳統A*算法與24鄰域A*算法路徑長度對比

表2 傳統A*算法與24鄰域 A*算法擴展節點與搜索時間對比

其中，表1 中起止點距離使用歐幾里算法計算起始點到目標點的直線距離（不考慮障礙物），路徑長度是規劃結果的實際長度。從表1 中可以看出24 鄰域的A*算法相較傳統A*算法能規劃出距離更短的路徑并具有可操作性。

在表2 中，由于24 鄰域A*算法的原理，擴大鄰域的同時必然會導致擴展節點數增多，增加節點數雖然可以間接解決優秀節點被過早刪除的問題，但是也不可避免的帶來運行時間加長的弊端。

通過對比傳統A*算法與24 鄰域A*算法在同一環境下的實驗結果可以看出，傳統A*算法在規劃路徑的過程中由于受到自身運動方向的限制導致規劃路徑轉折點過多、路徑不平滑，通過24 鄰域A*算法路徑仿真圖可以看出，改進后的算法在路徑選擇上與傳統A*算法存在不同，并且在轉折點個數和路徑平滑上有明顯好轉。

3 移動小車及仿真環境搭建

3.1 系統的軟件平臺

本研究所設計的移動機器人及仿真環境是基于機器人操作系統（ROS）來實現的，ROS 是基于Linux 開發的一套機器人通用軟件框架，其集成了大量的工具、庫、協議，提供類似OS 所提供的功能，可以提升功能模塊的復用性，簡化對機器人的控制，使機器人的相關研究更加方便。ROS的架構圖如圖4 所示，主要分為三個層次：基于Linux 系統的OS 層，用于實現不同程序功能包之間的數據交流且提供豐富的開發庫供開發者使用的中間層，在Master 管理下保證各節點正常運行的應用層[4]。

圖4 ROS 架構圖

Gazebo 與Rviz 是ROS 中集成的兩個強大工具，大大提高了機器人的開發效率。Gazebo 是一款3D 動態模擬器，提供高保真度的物理模擬，并提供了一整套傳感器模型，用于顯示機器人模型并創建仿真環境，能夠在復雜的室內和室外環境中準確有效地模擬機器人。Rviz 是三維可視化工具（ROS Visualization Tool），它的主要目的是以三維方式顯示ROS 消息，可以將數據進行可視化表達。

3.2 移動小車模型搭建

URDF（united robotics description format）是ROS提供的一個統一的機器人描述格式，建立在XML 語法框架下，為機器人建模仿真[5]。仿真所用到的移動小車就是通過URDF 搭建的兩輪差速移動小車，在Gazebo 中顯示的小車模型如圖5 所示。

圖5 Gazebo 中顯示的小車模型

該移動小車的整體結構如圖6 所示，圖中的矩形框是小車的連桿（Link）部分，橢圓形框是小車中連桿與連桿之間的關節（Joint），小車主要包含：小車主體（base_link），起驅動作用的左右驅動輪（left_wheel、right_wheel），起支撐作用的前后萬向輪（front_wheel、back_wheel），激光雷達（laser_link），單目相機（camera_link），深度相機（kinect_link）。

圖6 移動小車的結構圖

3.3 地圖環境搭建

在Gazebo 中搭建邊長為20 米的正方形地圖，并在地圖中的不同位置添加一些墻體作為障礙物，得到兩個不同的地圖環境進行仿真實驗，Gazebo 中的地圖環境一如圖7 所示，地圖環境二如圖8 所示。

圖7 地圖環境一

圖8 地圖環境二

3.4 柵格地圖構建

控制小車在搭建的物理環境中移動，通過Carto grapher SLAM 算法[6]，利用激光雷達進行珊格地圖的構建。得到的柵格地圖分辨率是0.05，即每個柵格的邊長是5厘米。在ROS 的柵格地圖中，每個柵格主要攜帶兩個重要信息：該柵格在地圖中的位置坐標，該柵格的狀態（空閑、被占據、未知）。兩個不同的物理地圖環境構建的柵格地圖一和柵格地圖二在Rviz 中顯示，分別如圖9、10 所示。圖中白色部分表示柵格狀態是空閑的，小車可以安全通過；黑色部分表示柵格狀態是被占據的，即有障礙物；灰色部分表示柵格狀態未知，也就是在建圖過程中激光雷達無法掃描到的部分。

圖9 柵格地圖一

圖10 柵格地圖二

4 改進A*算法在仿真環境中的應用

4.1 改進A*算法作為移動小車的全局路徑規劃器

ROS 中集成的move_base 功能包是用來進行路徑規劃并控制機器人運動的，將改進后的24 鄰域A*算法作為插件的形式插入move_base 中，啟動move_base 即可調用改進后的A*算法作為全局路徑規劃器進行路徑規劃。在ROS 中的24 鄰域A*算法流程圖如圖11 所示。

圖11 24 鄰域Astar 算法流程圖

4.2 傳統A*算法與改進A*算法的對比

路徑規劃算法的優劣主要體現在規劃出的路徑長短、路徑規劃所耗費的時間、搜索的節點數量這3 個方面，本文將在兩個不同的地圖環境中，用傳統A*算法與改進A*算法，選取相同的起始點和目標點進行路徑規劃，通過這3 個方面的數據進行對比。

在柵格地圖一中，傳統A*算法作為移動小車的全局規劃器，規劃的路徑如圖12 所示，24 鄰域A*算法作為全局規劃器，規劃的路徑如圖13 所示。在柵格地圖二中，傳統A*算法作為移動小車的全局規劃器，規劃的路徑如圖14 所示，24 鄰域A*算法作為全局規劃器，規劃的路徑如圖15所示，圖中的綠色路徑即為規劃的全局路徑。兩種算法的數據對比如表3、4 所示。

表3 柵格地圖一中兩種算法對比

圖12 柵格地圖一中傳統Astar算法規劃路徑

圖13 柵格地圖一中24鄰域Astar算法規劃路徑

圖14 柵格地圖二中傳統Astar算法規劃路徑

圖15 柵格地圖二中24 鄰域Astar算法規劃路徑

根據圖中的路徑數據可得：改進后的A*算法相較于傳統的A*算法，減少了轉彎的次數，路徑更加平滑，同時能更快的搜索到障礙物而提前改變方向，有效的減短了路徑長度。

由圖12、13 結合表3 中的數據可得：在不是很復雜的環境中，24 鄰域A*算法規劃的路徑長度明顯減短，但由于單個節點所搜索的鄰域從8 個擴大到了24 個，搜索的節點數也隨之增多，使規劃時間變長，減短了2 米左右的距離，大約增加了30 微秒的時間，這個代價完全是可以接受的。

由圖14、15 結合表4 中數據可得：在稍加復雜的環境中，改進后的算法就有非常明顯的提升，由于24 鄰域A*算法每個節點搜索的鄰域擴大，便能提前搜索到障礙物，從而提前改變搜索方向，因此在地圖二中的路徑長度減少了6米左右，規劃時間并沒有增加，反而有2 微秒左右的減少。

表4 柵格地圖2中兩種算法對比

5 結束語

本文主要對A*算法的搜素鄰域的各方向上擴大一個柵格，將原來的8 鄰域變為24 鄰域，通過MATLAB 進行思路驗證，并將算法用C++代碼重寫，通過ROS 應用到移動小車上，規劃出移動小車在仿真的物理地圖環境中的路徑，讓小車運動，得到算法在不同環境運行的實際效果。本文使用了一個稍簡單的地圖環境和一個稍復雜的地圖環境對算法進行測試，改進后的算法相較于傳統的A*算法都有優勢，隨著環境的復雜度提高，改進后的算法優勢也將越來越明顯。