基于A-star算法與DWA算法的小型農用運輸車智能避障系統研究

安燕霞，黃 凱

（晉中信息學院，山西 太谷 030800）

矮化密植果園勞動環境差、人工勞動強度大，化肥、工具和果實等運輸全靠人工作業，現有的大型農業設備無法進入田間作業，且在運輸過程中，矮化密植果樹對人的視線遮擋較多，因此對小型農業運輸車需求不斷增加。但由于果園環境復雜，為避免小型農用運輸車在正常工作時出現碰撞現象，要求小型農業運輸車對道路上存在的各種障礙物進行感知與識別，實現路徑規劃與其主避障。本文設計側重于小型農用運輸車的路徑規劃與避障系統的研究，在小型農用運輸車上加上路徑規劃與自動避障模塊，使得小型農用運輸車在工作過程中時可以根據指令完成路徑規劃，得到到達目的點的最優策略，并在移動過程中檢測環境的障礙物，做出相應的避障動作。

1 小型農用運輸車智能避障總體設計思路

本文研究的小型農用運輸車主要滿足在矮化密植果園環境中，自動避障、保護果樹，特別是在拐彎處、果樹密集的地方。結合系統需求分析，小型農用運輸車智能避障系統總體方案設計見圖1。由圖1 可知，小型農用運輸車智能避障系統由環境感知模塊、路徑規劃與避障模塊、運動感知模塊、運動控制模塊、人機交互模塊組成。

圖1 小型農用運輸車智能避障系統設計思路Fig.1 Design of intelligent obstacle avoidance system for electric luggage compartment

環境感知模塊通過IMU、里程計、單線激光雷達等傳感器進行信息讀取，獲取周圍環境信息，判斷小型農用運輸車前進方向是否存在障礙物。IMU、里程計、單線激光雷達的作用在6 m 范圍內，進行360°二維平面掃描，生成平面點云圖，推算小型農用運輸車行駛的軌跡以及其在環境的位置。

路徑規劃與避障模塊讀取預設的全局地圖以及用戶輸入的目標點信息，完成從起始點到目標點的無碰撞路徑規劃。當行駛路徑上存在障礙物時，進行局部路徑規劃，生成繞開障礙物的控制指令。

運動感知模塊讀取加速度計、里程計、羅盤等傳感器的數據，計算得到小型農用運輸車的位姿信息，為路徑規劃與避障提供輸入信息。

運動控制模塊是控制小型農用運輸車運行的模塊。本文選用2iMXRT1021 微控制器來根據接收到的控制指令，控制電機的運行。

人機交互模塊提供用戶選擇環境模式。在用戶設定目標點后，小型農用運輸車通過智能避障系統內部運算推演出一條最適路線并自動行駛到目標位置；在自主模式下，小型農用運輸車將跟隨用戶移動。本文主要研究在模擬環境中對障礙物的線路規劃避障分析。

2 避障算法研究

A-star算法是基于Dijkstra改進的算法，在起點與終點之間距離較遠時，全局搜索的距離和方向較Dijkstra 算法更有效率，減少了路徑搜索的時間。A-star 算法主要是計算評估網格中每個節點的最佳值，即代價值，這里設定一個等式為F=G+H，其中G代表從起點到指定柵格點的代價值，一般是所走的實際值；H代表從終點到指定柵格點的估算代價值。估算H的公式有多種，比如曼哈頓距離、歐幾里得距離等。為了簡化計算，在該仿真實驗中，選取的是曼哈頓距離（d=|x1-x2|+|y1+y2）|。A-star算法的流程見圖2。

避障系統最重要的是路徑規劃與自主避障，路徑規劃軟件是在給定全局地圖，在給定起始點與目的點的情況下，采用柵格法構建運動模型路徑規劃的環境，對小型農用運輸車進行路徑規劃，避開環境中的障礙物，得到最優路徑。

A-star 算法是人工智能方面采用最廣泛的路徑規劃算法，盡管它在正常情況下只有較優解，而不是唯一的解，但因為其算法效率高，應用較為廣泛，因此在對于小型農用運輸車路徑規劃采用A-star算法。

路徑規劃的實現主要基于A-star 算法實現，本文通過激光雷達等單片機獲取相關信息編入ROS 平臺的Gazebo 軟件包，寫入代價函數并進行運算。

2.1 A-star算法

2.2 路徑規劃

避障問題屬于局部的路徑規劃，在無法預測的障礙物的動態的環境下，給定起止點和目標點，讓其識別障礙物并完成自主導航。動態窗口算法（dynamic window approach，DWA）由于計算復雜度低、采樣時間短、軌跡空間小、局部路徑可以完成離線計算，因而在局部路徑規劃中被采用的較為頻繁，因此本文自主避障也選擇DWA算法。

本文采用DWA 算法進行動態軌跡分析，通過在速度空間內采用多組不同的線速度和角速度來模擬運動模型所產生的運動軌跡建立評價函數，從不同運動軌跡中選取評分最高的軌跡，MATLAB 平臺下編寫DWA 算法程序，通過改變障礙物的位置、數量、大小對避障過程進行仿真。

DWA 算法是當前在局部路徑規劃中經常使用的一種算法。它的主要思想是從運動模型當前所處的狀態出發，在速度空間(v,w)內通過采用多組不同的線速度和角速度來模擬運動模型所產生的運動軌跡，再建立起評價函數對不同的運動軌跡進行評分，從中選取處評分最高的軌跡，該軌跡所對應的線速度和角速度在作為下一時刻的速度狀態來繼續引導運動模型運動。

在模擬小型農用運輸車的運動軌跡時，需要知道小型農用運輸車的運動模型，常用的運動模型主要是2種，分別是在相鄰時間內沿直線行走的運動模型以及在相鄰時間內沿圓弧行走的運動模型。考慮到小型農用運輸車的使用環境，本文采用沿圓弧行走的運動模型見圖3。

圖3 小型農用運輸車的運動模型Fig.3 Motion model of small farm transporter

運動模型的線速度與角速度之間關系見式（1）：

式中：v為運動模型的線速度；w為運動模型的角速度；r為圓弧半徑。

根據運動模型的示意圖可以得到相鄰時間間隔的位移增量和下一時刻的位置，位移增量見式（2）：

式中：vi為運動模型當前運動線速度；wi為運動模型當前運動角速度；(xi,yi)為運動模型當前時刻的位置；θi為運動模型當前的速度與水平線所呈的夾角。

下一時刻運動模型的位置見式（3）：

式中：vi為運動模型當前運動線速度；wi為運動模型當前運動角速度；(xi,yi)為運動模型當前時刻的位置；(xi+1,yi+1)是運動模型下一時刻所處的位置；θi為運動模型當前的速度與水平線所呈的夾角。

相鄰時間內沿圓弧行走的運動模型將角速度加入考慮，使得運動模型的運動方向不斷發生改變并進行調整，與相鄰時間內沿直線行走的運動模型相比較而言，與實際運動軌跡之間的誤差較小。

DWA算法的流程見圖4。

圖4 DWA算法流程圖Fig.4 DWA algorithm flow chart

3 算法驗證

3.1 A-star算法驗證

環境感知模塊根據接收的數據給出柵格地圖，即畫出障礙物的位置、起始點、目的地，有網格的為障礙物，左下為起始點，右上為目的地點，并賦予運動模型前后左右4個移動方向，按照A-star 算法，設計路徑規劃程序。運行程序，在柵格地圖上得到一條從起始點到達目的地的最佳路徑，見圖5。

圖5 柵格地圖及由A-star算法得到最優路徑Fig.5 Grid map and optimal path obtained by A-star algorithm

3.2 DWA算法驗證

在MATLAB 平臺下編寫DWA 算法程序，對避障過程進行仿真。通過改變障礙物的位置、數量、大小，總能得到最優的避障路徑。DWA算法仿真見圖6。

圖6 DWA算法仿真結果Fig.6 Result of DWA algorithm simulation

由圖6的DWA算法的仿真結果能夠看出，隨機改變障礙物的位置、數量、大小等，DWA 算法都可以幫助運動模型規劃出最優路徑，并使其達到目的地。

4 虛擬環境下避障系統驗證

ROS 支持多種通訊類型和編程語言，易于擴展，已逐漸成為智能運動模型開發的標準平臺。本文采用ROS 平臺對整個系統設計進行虛擬現實模擬驗證。

環境感知軟件采用ROS 發布的SLAM 下的gmapping 功能包設計，gmapping 功能包提供了使用激光雷達數據構建二維柵格地圖的功能。環境感知軟件訂閱tf 與scan 話題，并發布tf、map_metadata 以及map 話題。其中，tf 話題包含地圖與里程計之間的坐標變換信息，map_metadata 描述了發布的地圖屬性，map則是周期性發布的柵格地圖。

在Gazebo軟件中搭建運動模型以及虛擬環境模型，并載入避障算法，對避障系統進行功能驗證。最后在虛擬環境中，運動在安裝各個傳感器與模塊的情況下，也能夠獨立完成路徑規劃與避障功能，見圖7。

圖7 ROS建圖效果Fig.7 ROS drawing effect

5 結語

針對矮化密植果園運輸問題，本文研究了小型農用運輸車的路徑規劃與避障問題。研究采用A-star 算法進行路徑規劃，DWA 算法實現局部避障，為了驗證2 種核心模塊的功能，本文使用MATLAB 軟件，對A-star 算法與DWA 算法進行算法仿真，使用仿真結果驗證了該2種軟件模塊的可行性。