A*算法在AGV路徑規劃上的改進與驗證

耿宏飛 神健杰

(南京理工大學自動化學院 江蘇 南京 210094)

0 引 言

AGV小車憑借自動化程度高、靈活性強、安全性好等特點受到了公司的青睞，特別在環境精度要求高或者特殊環境的行業需求在迅速增長，但與此同時AGV運輸路線的規劃也成了關鍵問題[1]。尤其在多AGV、多任務的工作環境下，優良的路徑規劃方法能夠使AGV更快速地處理分配的任務，對于增加客戶的滿意度、提高倉儲貨物調度效率減少倉儲運行成本、提升企業科技含量和增加企業利潤有著至關重要的作用[2]。

路徑規劃是指在已知起點和終點，所在環境地圖存在障礙物的情況下，為控制對象規劃一條符合要求的高效且最優路徑。常見的路徑規劃算法有Dijkstra算法、A*算法，此外還包括智能優化算法中的蟻群算法、遺傳算法等。A*算法憑借在靜態網絡中求解路徑最短和最有效的優點被廣泛使用，但該算法的啟發函數采用曼哈頓距離時會出現不必要的局部繞行和較多的轉彎次數等問題。文獻[3]在啟發函數中加入附加因子，有效地提升了啟發函數取舍相同值的能力，從而避免繞行和搜索時間的浪費，但該算法應用場景比較單一，對其他場景并不適用。趙江等[4]利用三點共線原理，判斷中間節點是否為無用的點，并加以剔除，再利用向量概念，使用兩個向量的乘積來判斷障礙物與AGV的位置，從而高效快速地繞開障礙物。但這導致規劃路徑過程中計算過于復雜，無形中增加了運行的時間。文獻[5]中針對傳統A*算法規劃所得路徑在安全性與平滑性方面的不足的情況，提出在估價函數中加入安全威脅代價，代價值大小反比于節點與障礙物之間的最小距離，使得規劃路徑盡可能遠離障礙物。這種方法雖然有效地避開了障礙物，但無形中增加了路徑總體的長度，使得規劃效率降低。于赫年等[6]首先針對采用A*算法規劃時AGV不必要轉彎過多問題，引入了轉向修正概念，在AGV轉彎時加入轉彎代價k，有效地減少了運行時的轉向。其次針對A*算法采用曼哈頓距離接近目標終點時可能出現局部繞行的情況，提出了歐氏距離和曼哈頓距離結合的方法，利用歐氏距離采用對角線定義的特性，有效避免了局部繞行，但此方法只針對距離目標點較近的情況，并不適用于全局規劃。

本文在文獻[6]加入轉彎代價的A*算法的基礎上進行改進，針對啟發函數采用曼哈頓距離遇到障礙物時，可能出現局部繞行的問題，分別從修改啟發函數和幾何方法出發，提出采用局部歐氏距離和障礙物端點距離判斷方法，仿真結果表明改進算法有效避免了局部繞行，縮短了路徑的長度以及減少了AGV路徑規劃的時間。

1 AGV運行環境建模

經常使用的AGV運行環境表示方法主要有集合地圖法、拓撲地圖法和柵格地圖法等。柵格地圖法是當前使用最多的方法之一，本文采用柵格法來建立AGV的運行環境。該方法將AGV的運行區域劃分成為多個有序的方格，這些方格叫做柵格。當使用規劃算法進行規劃時，單位柵格長度表示AGV每走一步的距離，在一個完整路徑規劃過程中，會將一些柵格連接起來構成一條顯式連通圖，該連通圖表示為規劃的路徑[7]。柵格地圖中不包含障礙物的柵格賦值為0，包含障礙物的柵格賦值為1。柵格地圖中的路徑易于觀察，地圖現實的信息與創建初始環境一一對應，并且AGV的定位準確易于修改與創建。

在實際應用場景中，環境較為復雜，為了減少AGV之間的路徑沖突，也有利于對A*算法進行改進，本文采用上下左右4個方向的拓展搜索方法。如圖1所示，中間帶有圓圈方格為當前節點，箭頭指向為四個拓展方向。此方法不但穩定可靠而且在多AGV中也易于管理。

圖1 路徑規劃節點拓展方向

2 A*算法

2.1 基本原理

A*算法是一種啟發式算法，是在廣度優先算法的基礎上引入一個啟發函數，與傳統深度優先廣度優先搜索算法有所區別，在A*算法中，并不是將所有的可遍歷的點都進行搜索，雖提高了搜索效率，但不一定是最優的路徑。事實上，A*算法在傳統搜索算法基礎上添加了限定條件，相當于設置了閾值，從而判斷舍棄不符合要求的點[8]。下面對A*算法進行介紹。

A*算法如式(1)所示。

f(n)=g(n)+h(n)

(1)

式中：f(n)為起點經當前節點到目標節點的估計距離；g(n)為起點到當前節點的實際距離；h(n)為當前節點到目標節點的估計距離。

A*算法的實現過程中要建立一個open表和一個close表，分別用來存放當前節點周圍四個方向的節點以及子節點f(n)和已經搜索過的點。在整個路徑規劃過程中要對這兩個表進行維護，路徑規劃過程中，將當前節點作為父節點，對此節點上下左右四個節點計算f(n)值，open表功能在于使用優先級隊列去選擇f(n)值最小的節點，并將其節點放在close表當中，在整個過程中重復此過程，從而規劃出距離最小的路徑。

在A*算法過程中，為保證找到最短路徑(最優解)需要選擇合理的函數h(n)。如果選定的h(n)值小于節點n到目標節點F的實際距離值，需要搜索的節點數目較多，搜索范圍較大，運算量增加，但能夠保證得到的解是全局最優解。若選擇h(n)的值大于節點n到目的節點的實際距離值，在求解的過程中，會根據f(n)值舍棄部分節點，加快求解速度，缺點在于最優解可能包含在舍棄的節點，只能保證最后的解是較優解。為求解最短路徑，估價函數的取值應盡量接近于實際值[9]。

2.2 加入轉彎代價

傳統的A*算法主要將長度作為標準，即總長度最短為最優路徑。但一味地追求長度最短，將會導致AGV在行駛過程中頻繁地轉向，而在實際應用中由于轉彎會進行加速和減速的過程，時間成本要比直線行駛高出許多，使得整體的效率下降，而且使整體路徑不夠平滑。針對此問題，其中一個改進策略是在原有的A*算法啟發函數上加一項轉彎代價k，k為轉向所花費的代價。改進后的A*算法如式(2)所示。

f(n)=g(n)+h(n)+k

(2)

在傳統A*算法啟發函數增加轉彎代價，路徑的評價不僅僅是路徑最短，還要考慮轉彎所帶來的路徑代價，長短仍然是路徑規劃的關鍵因素但不是唯一因素[6]。由于A*算法是按照代價最小的原則來規劃路線，所以減少一些不必要的轉彎，使得路徑更加平滑。因此本文中啟發函數為曼哈頓距離的A*算法是在加入轉彎代價的基礎上進行研究與改進。

3 改進A*算法

3.1 局部歐氏距離

A*算法的啟發函數通常采用歐氏距離和曼哈頓距離。具體表達式如下：

H(n)=abs(n.x-d.x)+abs(n.y-d.y)

(3)

H(n)=sqrt((n.x-d.x)2+(n.y-d.y)2)

(4)

式中：abs表示求取數值的絕對值；sqrt表示求取數值的正平方根；(n.x,n.y)為當前點的坐標；(d.x,d.y)為目標點的坐標。

A*算法采用兩種距離時各有優缺點：曼哈頓距離是根據橫坐標和縱坐標絕對值之和來定義的，根據此特性一般可以獲得轉彎較少的路徑，但遇到障礙物時會出現局部繞行的缺點；歐氏距離是根據對角線距離來定義的，根據此特性一般遇到障礙物時不會出現局部繞行，但在路徑規劃當中會出現轉彎較多的情況。本文綜合考慮兩種距離的優缺點，提出全局使用曼哈頓距離而遇到障礙物使用歐氏距離的方法。

設遇到障礙物的點為A點，障礙物的前一個路徑行駛點為B點，前兩個路徑行駛點為C點，如圖2所示。

圖2 行駛軌跡與障礙物的位置

設A點坐標為(a.x,a.y)，B點坐標為(b.x,b.y)，C點坐標為(c.x,c.y)。因此向量CB和向量BA分別表示為(b.x-c.x,b.y-c.y)和(a.x-b.x,a.y-b.y)。因為由實際AGV行駛情況可知，向量CB和向量BA只有垂直與水平兩種位置情況。利用兩個向量夾角θ的余弦值來表示兩個向量的位置關系。垂直和水平兩種位置情況對應的余弦值分別為0和1。

當余弦值為1時，表明向量CB和向量BA是水平的，此時AGV的運動軌跡與障礙物是垂直關系，那表明此時AGV剛好遇到障礙物，此時應將A*算法的啟發函數改為歐氏距離進行路徑規劃，以免使用曼哈頓距離導致局部繞行。因為本文方法只在運動軌跡與障礙物為垂直關系時使用歐氏距離，所以使用歐氏距離時不需要加入轉彎代價。

當余弦值為0時，表明向量CB和向量BA是垂直的，此時AGV的運動軌跡與障礙物是水平關系，那表明此時AGV沿著障礙物水平行駛，A*算法仍然繼續使用曼哈頓距離進行路徑規劃。

同理此方法也適用于障礙物垂直放置的情況。加入局部歐氏距離的改進A*算法執行流程如圖3所示。

圖3 加入局部歐氏距離的改進A*算法執行流程

3.2 障礙物端點距離判斷

本節解決的AGV路徑規劃同樣是3.1 節所針對的問題：AGV在進行路徑規劃過程中，當前點的四個搜索方向存在障礙物。與3.1節有所不同的是，無論向量CB和向量BA垂直或者水平，A*算法的啟發函數為曼哈頓距離保持不變；但在兩個向量水平時要進行當前點與障礙物兩段點的幾何判斷，如圖4所示。

圖4 當前點與障礙物兩段點的幾何判斷

M、N點分別為障礙物第一排幾何最左端、最右端的點，坐標分別為(m.x,m.y)和(n.x,n.y)。此時通過比較B點到M點的距離和B點到N點的距離的大小，即可得出B點更靠近障礙物左端還是右端。

B點到兩點的距離可以表示為|BM|和|BN|。具體表達式如下：

(5)

(6)

當|BM|大于|BN|時，表示B點更靠近障礙物右端，此時規定A*算法的規劃方向為向右，從而避免障礙物局部繞行。當|BM|小于|BN|時，表示B點更靠近障礙物左端，此時規定A*算法的規劃方向為向左，從而避免障礙物局部繞行。

同理此方法也適用于障礙物垂直放置的情況，不同的是改變AGV運動方向向下或者向上。加入障礙物端點距離判斷的改進A*算法流程如圖5所示。

圖5 加入障礙物端點判斷的改進A*算法流程

4 仿真驗證

本文的仿真平臺為MATLAB R2017a，并在Windows 10×64位操作系統下進行，AGV的運行環境采用25×25的柵格地圖。這里將(2,3)作為起點、(16,21)作為終點。如圖6和圖7所示，白色為起點和終點，淺灰色為AGV的行駛軌跡，深灰色為障礙物。

圖6 加入轉彎半徑的A*算法

圖7 本文改進的A*算法

圖6為添加轉彎代價的A*算法，可以看出在整個路徑規劃過程中，AGV的轉彎次數為8次，搜索的點數為42個，雖然與傳統A*算法相比，轉彎次數有所減少，路徑上搜索的點數也減少許多，路徑相對平滑，性能在一定程度上有所提高。但遇到障礙物時，由于曼哈頓距離本身的弊端，導致不能以最優路徑經過障礙物。特別是在障礙物比較密集的地方，例如圖中靠近目標點的區域，比其他區域更容易發生不必要的繞行。圖7為本文改進的兩種A*算法仿真結果，由于遇到障礙物時，兩種算法都是按照最短距離避開障礙物的，所以兩種算法的仿真結果一致。可以看出，AGV在整個路徑規劃過程中轉彎次數為6次，搜索的點數為38個，與僅添加轉彎代價的A*算法相比，轉彎次數減少2次，搜索點數減少了4個，特別在遇到障礙物時，本文改進的A*算法經過改變啟發函數和判斷幾何距離的方法能夠在行駛過程中對障礙物進行實時判斷，有效避免對障礙物不必要的繞行，從而提高了A*算法在路徑規劃上的搜索效率和性能，減少了AGV的運行時間。表1為添加轉彎代價的A*算法與本文兩種改進的A*算法的路徑規劃仿真數據。

表1 加入轉彎代價A*算法與本文兩種改進A*算法的仿真數據

可以看出，本文兩種改進的A*算法在路徑長度和轉彎次數這兩方面上與加入轉彎代價的A*算法相比減少9.52%和25.00%。而在運行時間方面，障礙物距離判斷的A*算法與加入轉彎代價的A*算法相比減少了7.38%，而局部歐氏距離的A*算法則減少了10.26%。這是由于每次遇到障礙物時，障礙物端點距離判斷的A*算法在遇到障礙物時，由于幾何距離的計算和比較，自然花費的時間會比局部歐氏距離的A*算法花費時間多一點，導致整體路徑規劃時間要稍微多一些，性能也略遜一籌。因此障礙物端點距離判斷的A*算法主要適用于障礙物比較少的情況，規劃路徑的性能與局部歐氏距離的A*算法相當。

因此對以上仿真結果的分析和比較可以得出本文兩種改進A*算法能夠克服啟發函數采用曼哈頓距離遇到障礙物時局部繞行的缺點，有效減少全局路徑規劃的長度和轉彎次數，從而使路徑規劃的效率更高。

5 結 語

本文在采用柵格地圖法構建AGV的運行環境的基礎上，針對啟發函數采用曼哈頓距離的A*算法對單AGV路徑進行規劃遇到障礙物時會出現局部繞行問題，提出兩種改進的A*算法：(1)在遇到障礙物時將A*算法的啟發函數換為歐氏距離，利用歐氏距離的特性解決繞行問題；(2)通過比較當前點到障礙物兩端的距離，確定AGV的行駛方向，從而局部繞行。通過仿真結果及對比可以看出，由于有效避免了遇到障礙物時的局部繞行，兩種改進的A*算法有效地減少了路徑長度和轉彎次數，從而提高了A*算法在路徑規劃上性能。