一種AGV路徑規劃的融合改進A＊算法

趙云龍，王新杰，成 奎，唐 林

（1.四川輕化工大學自動化與信息工程學院，四川 宜賓 644000；2.宜賓學院三江人工智能與機器人研究院，四川 宜賓 644000）

引言

制造業是中國發展的經濟支柱，也是我國轉型發展為制造強國的主戰場，為了實現制造業的轉型升級，中國制造2025計劃提出將信息技術與制造業融合發展，推動傳統制造業轉型發展為智能化、數字化和自動化[1]。工業機器人作為制造業轉型發展的關鍵，發展工業機器人技術成為制造業智能數字化的重要一環將會迎來新的發展機遇[2]。自動導引車作為智能工業機器人中重要的分支，在智能制造中發揮著重要的作用。在智能工廠中自動導引車(Automated Guided Vehicle，AGV)承擔流水作業中物料的搬運、回收、配送的角色，不僅極大地提高了智能工廠的生產效率，還能提高企業管理智能化生產的水平[3]。通過AGV 作為生產的物流運輸下，企業建造智能工廠自動化配送系統對AGV 的路徑規劃能力有著更高的要求[4-7]。

針對傳統A*算法搜索效率低、冗余節點多等問題，國內學者提出了諸多改進方法。陳若男等[8]利用角度與距離信息改進傳統A*算法的代價函數，提升了算法的搜索效率和安全性。沈可宇等[9]通過加入父節點對當前節點的影響力、轉彎懲罰函數和冗余節點平滑等政策，提升了算法的效率和路徑的平滑度。何光層等[10]利用三次樣條插值法減少路徑冗余點，提高了路徑的平滑度。曹祥紅等[11]通過優化儲存結構和擴大G值來解決火災情況下的疏散路徑規劃的問題，以適應火場。辛煜等[12]解決了A*算法搜索領域個數的限制，擴展為無數個任意搜索方向，這樣搜索出來的路徑降低了轉折點的個數，路徑更加平滑。張輝等[13]利用提取預覽地圖中的關鍵節點判斷是否增量搜索，并加入安全距離改進代價函數，改善了路徑的安全性。陳靖輝等[14]利用算法定向搜索消除路徑的對稱性，極大地提高了搜索效率。以上文獻對改進A*算法提供了理論依據和思路。

綜上所述，針對傳統A*算法在路徑規劃中出現的搜索效率低下、節點冗余和路徑不平滑等問題都提出了解決辦法，但都是在已知全局環境下進行路徑規劃，無法對環境中的移動或未知障礙物做出避障，且是在犧牲其他指標性能的前提下對特定指標進行優化。為了獲得更好的路徑規劃效果，本文提出了一種改進A*算法和動態窗口法結合的融合算法，通過對啟發式函數、優化冗余節點、擴展安全搜索領域和改進動態窗口法評價函數等策略，AGV 在利用融合算法路徑規劃時，以達到實時避障且安全到達目標點完成作業的目的。

1 傳統A*算法

A*算法是一種常見的經典啟發式搜索算法，其融合了Dijkstra 算法的和最佳優先算法搜索（Greedy Best First Search）的優勢[15]。算法核心是在已知地圖上規劃出一條從起點到目標點的具有最小移動代價的路徑[16]，起始點到目標點的移動代價估值函數可以表示為：

其中，n表示在搜索過程中的當前節點位置，g(n)是距離函數，表示當前節點與起點的實際距離，h(n)表示當前節點到目標節點的距離估計即代價函數。

在規劃過程中，AGV 小車通過不斷的迭代循環找到終點，圖1所示為傳統A*算法的流程圖。

圖1 傳統A＊算法流程圖

2 改進A*算法

傳統A*算法在任意已知地圖中自適應性較差，在路徑規劃過程中容易存在重復搜索節點從而使搜索效率較低、啟發式函數h(n) 代價值與實際代價差距較大從而導致搜索空間較大和路徑冗余、將機器人看作了一個質點和路徑拐點較多，容易導致機器人斜穿障礙物使機器人出現故障[17]。本文針對性改進以上不足問題，對傳統A*算法進行以下改進：

1）擴展A*算法的搜索鄰域，使之不再局限與搜索方向夾角為0.25π，提高搜索效率。

2）改進啟發式函數，縮小在路徑規劃過程中估計的代價值與實際代價值差距，使擴展的節點更有貪心性和啟發性。

3）使用垂距限制法剔除多余冗余節點，最后用3 次均勻B 曲線優化路徑，使路徑更加符合機器人運動學要求。

2.1 安全擴展搜索領域策略

傳統A*算法一般為3×3 搜索領域（圖2(a)），搜索領域過小會導致AGV 在作業時規劃路徑不是最優，搜索領域過大會增加規劃過程中計算量從而降低效率[18]。為此，在改進過程中適當擴大搜索領域不僅可以提高搜索效率，還能去除多余的子節點，因此本文采用5×5 搜索領域（圖2(b)）。安全擴展領域如圖2(c)所示，其中A表示子節點，B表示父節點，黑色方塊表示障礙物，在節點擴展的同時，需要根據父節點周圍的環境信息調整搜索方向，若在搜索過程中，存在障礙物，則將該節點的評價函數設置為無窮大，調整擴展其他節點，避免了AGV 碰到障礙物，如果周圍沒有障礙物，則對周圍子節點進行正常擴展。

圖2 搜索領域對比圖

2.2 改進代價函數

考慮AGV 在實際作業過程中，作業環境中的障礙物較多，且AGV 在大部分情況下不能直線到達終點，為了增加路徑選擇的多樣性，采用曼哈頓距離表示，如式(2)所示：

其中，(Sx,Sy)為當前節點坐標，(Gx,Gy)為目標節點坐標。

針對傳統A*算法路徑規劃時間較長、路徑拐點較多，對h(n)進行優化，首先適量增大h(n)值，提高效率，加入拐彎懲罰函數，避免算法在出現局部最優解時，出現不必要的拐彎。由式(2)，設初始節點為(Nx,Ny)，則x和y方向的曼哈頓距離表示為：

由向量平行原理得到一個拐彎懲罰函數：

其中k為常數，取值區間為（0，1），用于提高f(n)的精度。其改進的代價函數如式(5)所示：

其中R為初始節點與目標節點的曼哈頓距離。

2.3 冗余節點平滑策略

對于AGV 在路徑規劃過程中存在大量冗余路段，采用垂距限值法優化冗余節點。傳統A*算法的規劃后的路徑如圖3(a)所示，路徑不平滑且拐點較多；優化后路徑如圖3(b)所示，路徑平滑度大幅提高，拐點數量也大幅減少。

圖3 傳統A＊算法優化前后對比

垂距限制法示意圖如圖4 所示，圖中Last-node表示先前節點，pos表示當前節點，Next-node表示下一節點，其核心思想是計算pos 到last-node 與nextnode連線的距離d，d超過一定的閾值就從總集合中刪除當前節點pos。但垂距限值法并非從整體角度去優化一條完整的路徑，而是從第一個節點即起點開始依次篩選，去除冗余點。其具體實現步驟如下：

圖4 垂距限制法

1)首先將路徑規劃中的所有節點加入總集合{x1,x2,…,xn}中，設置w為關鍵節點集合。

2)從x1出發連接所有節點，在連接節點過程中如果出現障礙物，則將該節點xi的商議節點加入w中，則x1～xi-1加入優化節點集合Z。

3)利用垂距限值法優化冗余節點集合Z。

4)采取均勻B曲線優化路徑。

3 動態窗口算法

在AGV 執行作業時，在已有地圖環境的情況下，在實時作業的時候需要觀察周圍環境，因此本文在使用改進的A*算法作為全局路徑規劃的基礎上，局部路徑規劃上使用動態窗口法（Dynamic Window Approach，DWA）。AGV 通過自身所帶傳感器利用DWA 對作業環境進行檢測，在進行實時路徑規劃時，可以使AGV 發現周圍障礙物，增加了AGV 的實時避障能力[19]。在AGV 獲取作業環境的柵格地圖的基礎上，通過優化后的A*算法進行全局路徑規劃，然后AGV 收到電腦下達的行動命令開始行動，由于AGV 受現場環境的影響和運動學約束，AGV 在作業過程中會產生誤差，因此需要針對DWA 算法的不足進行改進。DWA 算法的原理是基于在速度空間中采樣多組線速度和角速度的基礎上，預測下一時間的運動軌跡，通過評價函數對所有獲取的運動軌跡得到一條最優的實時軌跡。

3.1 機器人運動模型

本文AGV 采用四輪全向機器人并建立運動學模型，如圖5所示。

圖5 AGV運動學模型

設作業的AGV 在△t時間內是直線運動且全向行駛，坐標系為世界坐標系，圖5 中AGV 的自身實時水平速度和垂直速度分別為vx、vy，角速度為ω，機器人的中心為Or，機器人的航向角為θt，那么可以得到機器人的航跡推算公式[20]：

3.2 速度采樣

在速度空間中存在無窮多個數據，由于AGV 自身實際的硬件條件以及工作環境影響，需要對采樣速度進行約束。

1）機器人實際最大、最小速度約束為：

2）機器人由于自身電機功率有限，因此機器人運動過程中電機加速度約束為：

其中，αx,min、αx,max為AGV 在X軸上的最大減速度和最大加速度；αy,min、αy,max為AGV 在Y軸上最大減速度和最大加速度；αω,min、αω,max為AGV 在角速度上的最大減速度和最大加速度。

3）為了保障機器人的安全，在機器人與障礙物碰撞之前應立即停止，保持安全距離，則安全距離約束為：

其中dist(vx,vy,ω)為機器人當前速度對應的軌跡距離障礙物的最小值。

3.3 改進評價函數

在速度空間中有無窮多組個速度組，而在采樣的速度中有多組軌跡，在機器人選出最優軌跡時需要一個評價函數，傳統DWA評價函數如下：

其中，α、β、γ分別為距離、夾角角度以及子函數的加權系數，為3 項子函數的歸一化參數，head(vx,vy,ω)為方位角偏差評價函數，評價目標點與機器人末端朝向的方位角偏差，vel(vx,vy,ω)為機器人當前速度評價函數。

由式(10)可知，α、β、γ作為加權系數是固定值，為了提高機器人快速向目標前行，并安全繞開障礙物，對加權系數做出調整，使加權系數具有動態性。當機器人離障礙物較遠時，提高機器人速度占比，使機器人快速到達目標點，當前進過程中，障礙物距離機器人較近時，提高機器人方向性占比，讓機器人安全通過障礙物到達目標節點[21]。改進的評價函數如式(11)所示：

其中，k表示障礙物直徑，在實際運動中，AGV 運行環境可能沒有障礙物，這將導致AGV 運行時速度占比較大導致失控，因此在計算過程中，設定dist(v,ω)最大值為1.5k，因此dist(v,ω) ∈(0,1.5k)。

4 融合算法

在路徑規劃過程中，全局和局部路徑規劃都有各自的缺點，A*算法擁有在已知地圖的基礎下尋找最優路徑的優點，卻無法有效地避開環境中的動態障礙物；DWA動態窗口法擁有避開未知環境下的障礙物的能力但容易陷入局部最優情況從而無法到達目標點[22]。本文在分別改進兩種算法的前提下，將A*算法使用垂距限制法優化后的節點作為DWA引導節點融合兩種算法，在兩種改進算法融合的基礎上，AGV不僅能在全局規劃出最佳路徑還能實時避開環境中的動態障礙物，融合算法流程圖如圖6所示。

圖6 融合算法流程圖

5 仿真分析

通過Matlab2021 平臺對算法進行仿真實驗，構造柵格地圖表示全局環境信息，黑色柵格表示環境中的障礙物，空白柵格表示空闊部分。分兩組實驗對改進算法進行驗證。

5.1 路徑優化程度和效率

對A*算法改進前后的路徑優化程度和效率進行驗證，如圖7所示。3種算法實驗結果見表1。

表1 不同路徑規劃算法實驗對比

圖7 不同算法路徑規劃對比圖

從表1 可知，相較于傳統A*算法，優化改進的A*算法路徑長度縮短了12.49%，路徑規劃時間縮短了36.23%，拐點減少了62.5%。文獻[10]算法僅對A*算法進行3次曲線優化，可以看出相比傳統A*算法其路徑規劃只有路徑長度提升相對較大，本文改進算法相比文獻[10]算法路徑規劃長度縮短了3.39%，時間縮短了32.87%，拐點數量減少了40%，路徑比文獻[10]更加平滑，更加符合AGV 安全運行和工作。綜上所述，本文優化后的A*算法符合AGV工作全局路徑規劃性能。

5.2 動態避障能力

在路徑規劃過程中加入未知障礙物和移動障礙物如圖8所示，其中實驗1(1)為加入一個障礙物和一個動態障礙物對比實驗，實驗0(1)為加入一個動態障礙物對比實驗，圖8(e)為本文算法加入兩個障礙物的規劃圖。圖9所示為加入兩個障礙物后AGV的姿勢變化，圖中可見AGV 在遇到障礙物變化較大，避開障礙物后趨于平穩。圖10所示為加入兩個障礙物后AGV 線速度和角速度變化情況，可以發現速度總體平穩，沒有出現較大的波動。

圖8 融合算法對比圖

圖9 AGV姿態角度圖

圖10 AGV速度對比圖

用改進DWA 算法優化路徑中加入動態障礙物和未知障礙物時，AGV 依舊能安全地從初始點到達目標點并且避開了優化路徑中的障礙物，完成動態避障，不同場景下融合算法實驗對比見表2。從表2可知，在加入動態障礙物后，本文融合算法規劃的路徑相比于文獻[10]算法減少了8.93%，時間減少了5.96%；在加入一個未知障礙物后，本文融合算法規劃的路徑相比于文獻[10]算法減少8.94%，時間縮短了5.83%。可以看出改進后的算法路徑更短，用時更少，進一步驗證了算法在AGV 作業過程中的可行性和效率。

表2 融合算法實驗對比

6 結束語

為了讓AGV 在路徑規劃時實現動態避障，本文針對傳統A*算法做了3 個方面的優化，分別是改進代價函數、領域擴展和平滑處理路徑。結果表明本文改進A*算法相較于文獻[10]算法分別從拐點、路徑規劃時間和路徑長度減少了62.5%、36.23%和12.49%，本文改進A*算法路徑搜索更高效和更加平滑。為了實現了動態避障，融合改進后的評價函數的動態窗口法，通過多次仿真測試結果顯示，融合算法相較于文獻[10]分別從路徑規劃時間和路徑長度方面減少了5.96%和8.93%。本文改進的融合算法可以讓AGV 在實現動態避障的前提下規劃出一條全局最優平滑的路徑，能滿足AGV 的實際需求，具有一定的應用價值。