基于變鄰域的改進AGV路徑規劃算法

陳遠浩，吳明暉，李 陽，洪孔林

（上海工程技術大學機械與汽車工程學院，上海 201620）

0 引言

在移動機器人領域中，自動引導車（Automated Guided Vehicle，AGV）廣泛應用于工業場景的物料搬運工作［1］。路徑規劃是指移動機器人通過即時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）獲得環境地圖信息后，通過路徑規劃算法獲得最優路徑，決定AGV 在工廠環境中的工作效率［2］

目前，國內外主流路徑規劃算法包括遺傳算法（Genetic Algorithm，GA）［3］、快速隨機搜索樹（Rapidly-exploring Random Tree，RRT）［4］、A*算法［5］等。

（1）GA 為全局智能優化搜索算法。Elhoseny 等［6］提出一種在動態環境下基于貝塞爾曲線的改進GA 路徑規劃算法，提高生成解的多樣性。在染色體多樣性度量值低于閾值時，才在每次迭代中接受染色體多樣性，從而幫助GA 跳出局部最優解。但由于工廠環境復雜，GA 算法會隨著種群增加，運算速度降低，最終還是會陷入局部最優解。

（2）RRT 屬于采樣搜索算法。Wang 等［7］提出一種基于強化學習的多RRT 方法，用于規劃窄道機器人路徑，將樹的選擇過程抽象為多臂賭博機問題，使用優化的強化學習算法進行選擇。該算法運算速度快、搜索能力強，但在工廠環境下搜索時盲目性大、在高維度環境下耗時長、易陷入局部最優。

（3）A*算法屬于啟發式搜索算法。現已經在無人機［8］、智能工廠［9］、智慧農業［10］等領域廣泛應用。Bing等［11］通過后階段局部路徑平滑算法提高A*算法的規劃成功率。Xin 等［12］基于無限鄰域思想優化傳統A*算法的搜索方向，但降低了運算速度。陳偉華等［13］提出一種基于雙重A*算法的移動機器人路徑規劃方法，使機器人在動態環境中也能實現安全導航，提高了路徑規劃效率。

雖然A*算法已廣泛應用于工廠AGV 作業場景，但仍存在以下不足：①當SLAM 建立的環境地圖較大時，傳統A*算法會計算許多冗余節點，計算時間顯著增長；②廠區規劃AGV 安全行駛區域，相較于其它環境地圖更規律，但A*算法的評價函數尚未對此進行優化；③A*算法的路徑冗余節點較多，所規劃的路徑平滑度較差。

針對上述問題，本文提出一種基于變鄰域的改進A*路徑規劃算法。首先，將傳統3×3 鄰域搜索方式改進為7×7 鄰域搜索；然后，基于終點向量的變鄰域搜索方法動態優化搜索鄰域范圍；接下來，融合預先建立的地圖信息優化代價函數；最后，通過插點法優化冗余節點。

1 改進A*路徑規劃算法

1.1 傳統A*算法

Hart［14］提出的A*路徑規劃算法在Dijkstra［15］算法的基礎上增加啟發式函數，在保證最優性的同時，增加目標節點信息以提升搜索效率。算法具體步驟如下：

步驟1：初始化柵格地圖及起點終點信息。

步驟2：初始化close 及open兩個空列表。

步驟3：將起點坐標設置為當前坐標并存入open。

步驟4：判斷當前節點是否為終點，若是則輸出路徑結束算法，反之執行步驟5。

步驟5：搜索當前可擴展的所有子節點。

步驟6：計算子節點代價值F(n)并將子節點信息存入open。

步驟7：判斷是否遍歷所有可擴展節點，若遍歷完執行步驟8，反之執行步驟5。

步驟8：計算F(n)最小子節點并從open 中刪除該節點，之后存入close。

步驟9：設置該節點為當前節點，并執行步驟4。

算法的代價值表達式為：

式中，n代表各個節點，F(n)代表當前節點n的總體代價值，G(n)代表AGV 的起點坐標到當前節點n所在坐標的代價值，H(n)代表AGV 當前節點n所在節點的坐標到終點坐標的代價值。

常見的H(n)計算方法包括曼哈頓距離、歐幾里得距離、對角線距離等［16］。本文選擇歐幾里得距離作為H(n)的代價函數，數學表達式如式（2）所示：

式中，(xn，yn)為當前節點所在坐標，(xd，yd)為終點所在坐標。

1.2 多鄰域拓展及變鄰域搜索方法

如圖1（a）所示，傳統A*算法采用3×3 鄰域搜索方式，當轉角過大時，會對AGV 的運動會造成影響，因此設置當前節點到子節點的轉角均為45°的整數倍。

當面對大且復雜的環境地圖時，該方式會計算冗余搜索節點，浪費計算資源。為此，通過多領域拓展搜索方法將當前節點到子節點的轉角從45°的整數倍優化為多角度。同時，采用7×7 鄰域搜索方法減少冗余節點的計算，如圖1（b）所示。此外，將原有8 個子節點增加到48 個子節點，并將AGV 的移動方向從8個優化到32個。

在AGV 實用場景中，由于受安全區域限制，AGV 所處環境較為簡單，不容易陷入“死胡同”，死鎖發生可能性較低。因此，大量與終點方向相反的子節點往往是冗余搜索節點。針對該問題，本文基于終點向量的變鄰域搜索方法，根據節點到終點的向量，將48 鄰域搜索優化至27 鄰域搜索，如圖1（c）所示。

Fig.1 A*algorithm neighborhood search methods圖1 A*算法鄰域搜索方式

設(xn，yn)為當前節點所在的坐標，(xd，yd)為終點所在坐標，則終點向量可定義為：

設θ為終點向量與x軸正向方向的夾角，則根據終點向量優化的搜索鄰域如表1、圖2所示。

Table 1 Endpoint vector optimization neighborhood表1 終點向量優化鄰域

Fig.2 Neighborhood coordinate number圖2 鄰域坐標編號

1.3 代價函數優化

傳統A*算法的啟發函數在代價函數中會影響A*算法的效率。當H(n)＜G(n)時，搜索節點多、效率低，路徑短；當H(n)＞G(n)時，搜索節點少、效率高，但無法保證規劃的是最優路徑。綜上所述，當環境地圖較為規則且搜索節點接近終點時，需要較大的啟發函數；當地圖環境較復雜且算法搜索節點較多時，則需要較小的啟發函數［17］。為此，融合當前節點及環境地圖信息，將代價函數改進為：

式中，d為當前節點到終點的距離，D為起點到終點的距離，N為可行區域占柵格地圖的比重。

1.4 插點法優化

插點法又稱Floyd 算法，利用動態規劃思想尋求兩點之間最短路徑［18］。如圖3 所示，為進一步優化A*算法規劃的冗余節點，在AB之間插入D點。其中，A、B、C為初次規劃的路徑，dist(A，C)為A到C的最短距離。

由于A、C間存在障礙物無法直接形成路徑，因此dist(A，C) 為+∞。通過計算可得dist(A，C) ＞dist(A，D) +dist(D，C)，并且滿足dist(A，B) +dist(B，C) ＞dist(A，D) +dist(D，C)，因此ADC為最優路徑。

Fig.3 Redundant path optimized by Floyd algorithm圖3 插點法優化的冗余路徑

1.5 改進A*算法流程

基于變鄰域的改進A*算法流程如下：

步驟1：初始化柵格地圖及起點終點信息。

步驟2：初始化close 及open兩個空列表。

步驟3：將起點坐標設置為當前坐標并存入open。

步驟4：判斷當前節點是否為終點，若是則插點法優化冗余路徑、輸出路徑結束算法，反之執行步驟5。

步驟5：搜索當前基于終點向量優化的7×7 鄰域子節點。

步驟6：計算子節點代價值F(n)并將信息存入open。

步驟7：判斷是否遍歷所有可擴展節點，若遍歷完則執行步驟8，反之執行步驟5。

步驟8：計算F(n)最小的子節點，并將其從open 中刪除，然后存入close。

步驟9：設置該子節點為當前節點，并執行步驟4。

2 實驗結果及分析

2.1 實驗設計

本文使用MATLAB 建立3 個30×30 的AGV 工廠仿真柵格地圖A、B、C，起點坐標均為（1.5，1.5），終點坐標均為（30.5，30.5），如圖4 所示。在3 組不同地圖中分別使用GA算法、RRT 算法、傳統A*算法和改進A*算法進行AGV 路徑規劃。并且，分別比較4 種算法的路徑長度、搜索節點個數、運行時間、累積轉角度數等性能指標。

2.2 結果分析

圖5 為地圖A 的實驗效果，3 組實驗的具體數據見表2。

Table 2 Experimental data表2 實驗數據

續表

通過比較每組實驗數據可得出以下結論：

（1）RRT 算法由于基于隨機采樣搜索，導致其搜索時間不穩定，且輸出的路徑隨機相較于傳統A*算法及改進A*算法更長。

（2）在柵格地圖環境中，RRT 算法及遺傳算法的復雜度較高，計算時間相較于A*算法及改進A*算法更長。

（3）改進A*算法相較于傳統A*算法，所規劃路徑長度更短，顯著提高了AGV 的作業效率。

（4）改進A*算法遍歷節點數更少，算法運算時間更短，算法性能存在一定提升。

（5）改進A*算法累計轉角度數更小，AGV 能夠規劃更合理的路徑，以減少不必要的轉向。

Fig.4 Simulation grid map of three different AGV plants圖4 3種不同AGV工廠仿真柵格地圖

Fig.5 Experimental results of different algorithms on map A圖5 不同算法在地圖A的實驗效果

2.3 實驗驗證

將改進A*算法應用于激光雷達AGV 中，AGV 實物主體如圖6（a）所示，圖6（b）為AGV 導航過程。

Fig.6 Application of A*algorithm on lidar AGV圖6 A*算法應用于激光雷達AGV

此外，利用改進A*算法在SLAM 算法建立的工廠環境地圖中，規劃初始點到終點的最優路徑。為了減少實際誤差和實驗偶然性，分別通過不同算法進行3 組由多個導航點組合而成的任務。由圖7 可知，改進A*算法相較于其它算法，有效減少了規劃路徑的長度及累計轉角。

3 結語

本文針對AGV 作業的環境特點，設計一種基于變鄰域的改進A*路徑規劃算法。該算法首先擴展傳統A*算法的搜索鄰域；然后引入終點向量優化冗余搜索鄰域，基于環境信息及當前節點信息優化代價函數；最后通過插點法優化冗余節點，提高路徑平滑度。

Fig.7 Experimental results of different algorithms圖7 不同算法實驗結果

仿真實驗及實物驗證表明，該算法在AGV 工廠環境下，路徑長度、遍歷節點數、運算用時、累計轉角等方面均優于傳統A*算法，工作效率及AGV 運動平滑度得到了顯著提升。