基于安全A*算法的AGV路徑規(guī)劃

黃令葦，全燕鳴，王榮輝

（華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣州510641）

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

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

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

1 環(huán)境建模

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

圖1 柵格地圖Fig.1 Grid map

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

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

2 安全A*算法

2.1 傳統(tǒng)A*算法

傳統(tǒng)的A* 算法是在Dijkstra 算法的基礎(chǔ)上引入啟發(fā)式函數(shù)，用于引導(dǎo)路徑的搜索方向。算法的評價函數(shù)為

該算法步驟描述如下：

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

步驟2將起始點加入openlist。

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

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

步驟5循環(huán)執(zhí)行步驟3 和步驟4，直至算法結(jié)束。

2.2 安全A* 算法

所提出的安全A* 算法是在傳統(tǒng)A* 算法代價函數(shù)的基礎(chǔ)上，引入當(dāng)前節(jié)點n 距最近障礙物的代價，從而使生成的路徑遠(yuǎn)離障礙物，保證路徑的安全性，提高工作效率。

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

2.2.1 凸走廊生成

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

圖2 節(jié)點n 對應(yīng)的凸走廊Fig.2 Convex corridor corresponding to node n

本文生成凸走廊的方法為： 以n 為起始點，按順序依次沿y 軸正向y+，y 軸負(fù)向y-，x 軸正向x+，x軸負(fù)向x-進(jìn)行擴(kuò)展，當(dāng)?shù)竭_(dá)地圖邊界或遇到障礙物時停止擴(kuò)展。其算法流程如圖3 所示。

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

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

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

2.2.2 搜索最近障礙物

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

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

2.2.3 評價函數(shù)改進(jìn)

將傳統(tǒng)A*算法的評價函數(shù)改為

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

圖4 生成的安全區(qū)域Fig.4 Generated security area

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

3 仿真試驗

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

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

設(shè)置safe_dis=min（length，width），分別使用傳統(tǒng)A*算法和安全A*算法進(jìn)行仿真試驗，結(jié)果如圖7 所示。圖中，路徑的起點為S，終點為E?？梢娫诋?dāng)前情況下，由于safe_dis 較大，圖7（a）中路徑所在通道寬度較小，所有點的代價值較大，故會優(yōu)先選擇圖7（b）所示通道寬度較大處的路徑。

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

選取相同起點與終點， 分別進(jìn)行10 次路徑規(guī)劃后的統(tǒng)計結(jié)果見表1。

表1 算法改進(jìn)前后10 次的結(jié)果統(tǒng)計Tab.1 Result statistics of 10 runs before and after algorithm improvement

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

由于設(shè)置不同的safe_dis 對結(jié)果會產(chǎn)生影響，故令

式中：α 為取值權(quán)重。在此，對α 取不同值后，分別進(jìn)行試驗，結(jié)果統(tǒng)計見表2。

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

表2 α 不同取值時安全A*結(jié)果統(tǒng)計Tab.2 Safe-A* result statistics with different values of safe_dis

4 結(jié)語

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