基于安全A*算法的AGV路徑規劃

黃令葦，全燕鳴，王榮輝

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

近些年來，自動導引車（AGV）在物料運輸、電力巡檢等方面得到廣泛應用。為完成工作任務，AGV 需進行自主運動規劃，實現從起點到終點的運動，主動避開障礙物。其中由于A*算法作為一種靜態路網中求解最短路徑最有效的方法被廣泛應用。

不少學者對其進行了研究。文獻[1]在評價函數中引入懲罰因子和獎勵因子， 解決了傳統A* 算法拐點多的問題；文獻[2]使用多層變步長搜索策略和姿態角信息，文獻[3]基于二叉堆優化，兩者均加快了路徑搜索效率， 但所得軌跡拐點數均有增加；文獻[4]采用雙向預處理結構減少冗余節點數，但增加了路徑長度；文獻[5]在評價函數中引入機器人轉向所需時間，減少了路徑拐點，縮短了路徑規劃所需時間，但目前僅能在靜態環境下運行。

以上研究均未能解決一個關鍵問題——傳統A*算法所得路徑易與障礙物十分接近， 對AGV 運行帶來安全隱患。而且，此時AGV 為保證避開障礙物，需要降低速度。故在此改進評價函數，引入障礙物距離，提高AGV 運行過程的安全性及工作效率。

1 環境建模

對于AGV 的運行環境， 可以用二維柵格單元進行劃分。對于一個大小為n×m 的柵格地圖，可以根據某一個柵格位置是否有障礙物將地圖分為2種區域， 即可行駛區域和不可行駛區域。當n=10，m=10 時，柵格地圖如圖1 所示。圖中，白色柵格為可行駛區域，灰色柵格為不可行駛區域。

圖1 柵格地圖Fig.1 Grid map

定義集合P=｛1，2，…，n*m｝表示所有的柵格編號，對于編號為i∈P 的柵格對應的坐標Pi=（xi，yi）有

式中：mod（）為取余函數；int（）為向下取整函數。

2 安全A*算法

2.1 傳統A*算法

傳統的A* 算法是在Dijkstra 算法的基礎上引入啟發式函數，用于引導路徑的搜索方向。算法的評價函數為

該算法步驟描述如下：

步驟1初始化2 個列表openlist 和closelist。openlist 用于存放已生成而未訪問的節點，closelist用于存放已訪問過的節點。

步驟2將起始點加入openlist。

步驟3若openlist 為空，則結束算法。否則計算代價最小的節點，并將該節點添置最優路徑。

步驟4對當前節點n，以4 鄰域或8 鄰域的方式計算鄰節點，遍歷openlist 和closelist。若均不包含鄰節點，則將其加入openlist。

步驟5循環執行步驟3 和步驟4，直至算法結束。

2.2 安全A* 算法

所提出的安全A* 算法是在傳統A* 算法代價函數的基礎上，引入當前節點n 距最近障礙物的代價，從而使生成的路徑遠離障礙物，保證路徑的安全性，提高工作效率。

在拔樁過程中，假定管樁被土體掩埋，且地下水位降至樁底以下，此時為最不利情況。如圖1所示，樁體受力作用分別為：樁體上拔力F，樁體自重G，樁底下吸力P以及樁周摩擦力 f。

2.2.1 凸走廊生成

要找到距n 的最近障礙物，需要計算柵格地圖M 中每一個柵格到n 的距離， 這樣做的效率很低，因此參考文獻[6]，在此引入了凸走廊的概念。節點n對應的凸走廊如圖2 所示。圖中，黑灰色節點為n；淺灰色節點為所生成的凸走廊；A，B，C，D 為該凸走廊的4 個頂點。

圖2 節點n 對應的凸走廊Fig.2 Convex corridor corresponding to node n

本文生成凸走廊的方法為： 以n 為起始點，按順序依次沿y 軸正向y+，y 軸負向y-，x 軸正向x+，x軸負向x-進行擴展，當到達地圖邊界或遇到障礙物時停止擴展。其算法流程如圖3 所示。

圖3 凸走廊生成流程Fig.3 Flow chart of convex corridor generation

圖中，ylo，yup，xlo，xup的計算公式為

式中：xmax為地圖在x 方向柵格的個數；ymax為地圖在y 方向柵格的個數；step 為搜索步長。

2.2.2 搜索最近障礙物

生成凸走廊后，選取安全距離safe_dis，該值可根據實際需求更改，取值范圍為［0，min（length，width）］，其中length 和width 分別為凸走廊的長和寬。然后，以n 為中心生成方形安全區域S，如圖4 所示。

圖中， 白色邊框包圍的區域為safe_dis=3 時的安全區域S。計算該區域中每一個障礙物到n 的距離，找到最小值min_dis。其算法流程如圖5 所示。

2.2.3 評價函數改進

將傳統A*算法的評價函數改為

式中：d（n）為n 到障礙物的代價；neutral_cost 為中等代價值，在此取常數50。

圖4 生成的安全區域Fig.4 Generated security area

圖5 最近障礙物搜索流程Fig.5 Flow chart of nearest obstacle search

3 仿真試驗

為驗證該算法的有效性，在此進行仿真試驗。試驗用計算機CPU 為Intel 酷睿i5-4200H 型，內存8 GB，在Gazebo 和機器人操作系統ROS （robot operating system）平臺下進行仿真試驗，仿真用AGV 為麥克納姆輪系。仿真的三維環境及其膨脹地圖如圖6 所示， 考慮到計算柵格距離方法的不同會帶來影響，統一設置使用曼哈頓距離計算。

圖6 仿真的三維環境及其膨脹地圖Fig.6 Simulation of 3D environment and its expansion map

設置safe_dis=min（length，width），分別使用傳統A*算法和安全A*算法進行仿真試驗，結果如圖7 所示。圖中，路徑的起點為S，終點為E。可見在當前情況下，由于safe_dis 較大，圖7（a）中路徑所在通道寬度較小，所有點的代價值較大，故會優先選擇圖7（b）所示通道寬度較大處的路徑。

圖7 不同算法所得到的路徑Fig.7 Paths obtained by different algorithms

選取相同起點與終點， 分別進行10 次路徑規劃后的統計結果見表1。

表1 算法改進前后10 次的結果統計Tab.1 Result statistics of 10 runs before and after algorithm improvement

由表可知， 雖然安全A* 算法所得平均路徑長度比傳統A* 算法長5.64%， 但其每個路徑點到最近障礙物的平均距離增加了50.00%，AGV 沿路徑運行平均速度提高了42.86%，平均所需時間減少了22.57%。

由于設置不同的safe_dis 對結果會產生影響，故令

式中：α 為取值權重。在此，對α 取不同值后，分別進行試驗，結果統計見表2。

由表可知，隨著safe_dis 取值增大，所規劃路徑的長度呈上升趨勢，到最近障礙物的平均距離呈上升趨勢，平均運行時間呈下降趨勢，平均運行速度呈上升趨勢。這說明在該范圍內，safe_dis 的取值應該越大越好。

表2 α 不同取值時安全A*結果統計Tab.2 Safe-A* result statistics with different values of safe_dis

4 結語

利用提出的安全A* 算法，通過引入凸走廊，建立了安全區域， 得到了一條遠離障礙物的安全路徑。經過試驗驗證，相比于傳統A*算法，安全A*算法提高了AGV 在運行過程中的安全性， 有效降低安全事故發生的概率， 且由于到障礙物的距離增大，可以給AGV 發送更大的速度指令，因此也縮短了AGV 執行任務的時間。