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

王世元，李季睿，陶寧果

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

1 傳統(tǒng)的路徑規(guī)劃算法

1.1 路徑規(guī)劃算法的原理

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

1.2 A*算法與Dijkstra 算法

A*算法是一種在靜態(tài)環(huán)境中尋找最優(yōu)路徑的高效算法，是將啟發(fā)式方法和常規(guī)方法相結合提出的一種算法[2]，其決定代價指標的估價函數(shù)為：

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

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

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

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

2.1 A*算法的改進

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

2.2 改進算法在MATLAB 中的驗證

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

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

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

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

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

表1 傳統(tǒng)A*算法與24鄰域A*算法路徑長度對比

表2 傳統(tǒng)A*算法與24鄰域 A*算法擴展節(jié)點與搜索時間對比

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

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

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

3 移動小車及仿真環(huán)境搭建

3.1 系統(tǒng)的軟件平臺

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

圖4 ROS 架構圖

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

3.2 移動小車模型搭建

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

圖5 Gazebo 中顯示的小車模型

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

圖6 移動小車的結構圖

3.3 地圖環(huán)境搭建

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

圖7 地圖環(huán)境一

圖8 地圖環(huán)境二

3.4 柵格地圖構建

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

圖9 柵格地圖一

圖10 柵格地圖二

4 改進A*算法在仿真環(huán)境中的應用

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

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

圖11 24 鄰域Astar 算法流程圖

4.2 傳統(tǒng)A*算法與改進A*算法的對比

路徑規(guī)劃算法的優(yōu)劣主要體現(xiàn)在規(guī)劃出的路徑長短、路徑規(guī)劃所耗費的時間、搜索的節(jié)點數(shù)量這3 個方面，本文將在兩個不同的地圖環(huán)境中，用傳統(tǒng)A*算法與改進A*算法，選取相同的起始點和目標點進行路徑規(guī)劃，通過這3 個方面的數(shù)據(jù)進行對比。

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

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

圖12 柵格地圖一中傳統(tǒng)Astar算法規(guī)劃路徑

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

圖14 柵格地圖二中傳統(tǒng)Astar算法規(guī)劃路徑

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

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

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

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

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

5 結束語

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