融合改進A*算法和動態窗口法的AGV路徑規劃

房殿軍，王少杰，蔣紅琰，，陸謙謙，Rolf Schmidt

（1.同濟大學 機械與能源工程學院，上海 200092；2.青島中德智能技術研究院，山東 青島 266000；3.蘇州羅伯特木牛流馬物流技術公司，江蘇 蘇州 215000）

0 引言

自動導引車（Automated Guiede Vehicle,AGV）作為物流自動化技術領域重要的代表之一，已經廣泛地應用于倉儲物流領域。在現代物流系統中，AGV已經成為自動化立體倉庫和分布式物流系統中最重要的無人駕駛操作工具之一。為了使AGV 能夠高效、準確、安全的運行，路徑規劃成為了關鍵的技術保障之一[1]。路徑規劃是指用算法為AGV在地圖中搜索一條無障礙的最優或次優路徑。根據周圍環境是否已知，AGV的路徑規劃可分為全局路徑規劃和局部路徑規劃。全局路徑規劃用于解決環境中具有已知、靜態障礙物的路徑規劃問題，常用的全局路徑規劃算法包括A*算法、Dijkstra 算法、快速擴展隨機樹算法等；局部路徑規劃用于解決環境中具有未知、動態障礙物的路徑規劃問題，常用的局部路徑規劃問題包括動態窗口法、人工勢場法、粒子群算法等。

A*算法是目前最常見的全局路徑規劃算法，但是其需要搜索較多的節點、求解軌跡有較多的轉彎。張輝，等[2]針對A*算法在路徑規劃時存在路徑轉折多、與障礙物過近、軌跡不平滑以及求解時間隨著柵格規模指數增長等問題，通過碰撞場模型改進了A*算法；張建光，等[3]將估計代價影響系數引入到評價函數，并且優先擴展目標節點方向的相鄰節點，有效提高A*算法的計算效率。

動態窗口法（Dynamic Window Approach，DWA）是一種較為常見的局部路徑規劃算法，它的避障能力很強，而且軌跡平滑，但是在復雜環境中易陷入局部最優，無法到達終點。Chang，等[4]通過添加新的評估函數來改進原始評估函數增強全局導航的能力，通過Q-learning 算法學習提出的DWA參數獲得訓練后的agent以適應未知環境，該算法具有較高的導航效率和成功率。

當路徑規劃的場景變得復雜時，需要將全局路徑規劃與局部路徑規劃相結合進行研究。郭園園，等[5]針對復雜場景中移動機器人路徑規劃耗時長的問題，將改進的A*算法與動態窗口法相結合，使移動機器人在復雜場景中的規劃效率更高；勞彩蓮，等[6]針對轉折點多的問題，改進了選擇關鍵點的策略，通過超聲波傳感器實現局部避障，實現實時最優的路徑規劃；槐創鋒，等[7]將搜索領域擴展，去除多余子節點，最后融合了動態窗口法，實驗結果表明，所提出的算法求解效率更高；Sun，等[8]通過優化A*算法的評估函數提高其搜索效率，使用策略去除冗余點，在相鄰節點之間使用動態窗口法進行動態路徑規劃，實驗證明該算法可以減少規劃時間和縮短軌跡長度，并且可以可視化隨機避障的分辨率；吳飛龍，等[9]將AGV的位置信息引入代價函數，提出了權重函數，通過去除多余的節點平滑軌跡，最后融合了A*算法與動態窗口法，從而提高了路徑規劃的實時性。

上述文獻使用不同的方法改進了A*算法和動態窗口法，但是都沒有完全解決搜索節點多、轉折點多、路徑不夠平滑和易陷入局部最優的問題。本文融合了改進的A*算法和動態窗口法，實驗表明本文的融合算法可以為AGV規劃一條距離較短且平滑的路徑。

1 改進A＊算法

1.1 環境模型描述

常用的環境建模方法有柵格法、拓撲法和幾何法，其中柵格法描述環境較為準確，本文采用柵格法為AGV建立環境模型。如圖1所示，地圖被分為大小相等的柵格，其中用白色柵格表示可通行空間，用黑色柵格表示環境中的障礙物。根據直角坐標系，橫軸用x坐標表示，縱軸用y坐標表示。

圖1 柵格地圖

1.2 傳統A＊算法

A*算法是最有效的求解全局路徑的啟發式搜索方法，是貪心算法和Dijkstra 算法的結合[10]。傳統A*算法的代價函數如下：

其中，f(n)為子節點n 的代價，即實際代價與估計代價之和；g(n)為子節點n 距離起點的實際代價；h(n)為子節點n距離終點的估計代價。

本文選擇歐氏距離計算子節點的估計代價h(n)，歐氏距離的公式為：

其中，(xn,yn)表示子節點坐標；(xs,ys)表示起點坐標；(xg,yg)表示終點坐標。

1.3 引入對數衰減因子

A*算法使用傳統的代價函數進行路徑搜索時，容易出現往返搜索的情況，從而使算法效率降低。當節點n的估計代價h(n)的值小于實際路徑距離時，會導致搜索節點數量過多，因此，本文采用動態權重的方法，在起點附近增大估計代價h(n)的權重來減少不必要的搜索節點數量，以便更好地指導搜索算法朝著正確的方向前進，提高搜索效率。

本文考慮到當前節點的位置對于估計代價占實際代價的比重具有影響，因此，當前節點到目標點的估計路徑代價被視為最小路徑代價，其值小于實際路徑代價。本文為啟發函數引入了對數衰減因子，可以動態地調整h(n)的權重。當h(n)較大時其權重較大，這樣可以使節點快速朝著目標點移動；當h(n)較小時，權重變小，確保能夠到達目標點。改進A*算法的代價函數如下：

圖2-圖5為采用傳統A*算法和改進A*算法規劃的路徑和遍歷的節點。由表1中的實驗數據可見，雖然改進A*算法和傳統A*算法的路徑長度相同，但是相比傳統A*算法的結果，改進A*算法的規劃時間減少了21.5%，累計轉彎角度減少了8.3%，遍歷節點總數減少了43.8%，因此，本文的改進A*算法可以提高傳統A*算法的效率。

表1 傳統A*算法和改進A*算法的性能對比

圖2 傳統A＊算法規劃的路徑

圖3 傳統A＊算法遍歷的節點

圖5 改進A＊算法遍歷的節點

1.4 關鍵節點提取

由圖4可知，本文的改進A*算法仍然有較多的轉折點，AGV轉彎時的速度變慢，因此會降低AGV的工作效率。針對這種問題，本文提出一種關鍵節點提取策略，以減少路徑中的轉折點，提取關鍵節點策略的步驟如下：

（1）將改進A*算法規劃得到的全部節點存入節點集U{Pi,1 ≤i ≤n}，P1表示路徑的起點，Pn表示路徑的終點。初始化關鍵節點集V，將起點和終點存入關鍵節點集。

（2）將P1依次與P3，P4，…，Pm連接，檢查直線是否穿過障礙物，若P1Pm為第一條穿過障礙物的直線，則節點Pm-1為路徑的關鍵節點，將節點Pm-1添加到關鍵節點集V 中，將P1與Pm-1之間的節點判定為冗余節點。然后以同樣的方式從Pm開始依次連接后面的節點，直到連接到終點Pn。

（3）按順序連接關鍵節點集V 中的所有節點，所得到的軌跡即為提取關鍵節點后的路徑，本文改進A*算法提取關鍵節點后的路徑如圖6所示。

圖6 關鍵節點路徑

表2為提取關鍵節點前后的對比，由實驗數據可知，提取關鍵節點后的路徑長度從24.485 縮短到了22.882，優化了6.5%；轉彎次數從7 次變為了2 次，優化了71.4%；路徑累積轉彎角度從494.771 降為158.606，優化了67.9%。

表2 提取關鍵節點前后對比

可見，利用關鍵節點提取策略對路徑優化之后，路徑長度縮短、轉彎次數減少，但是路徑仍然存在不夠平滑、與障礙物過近的問題。

2 改進動態窗口算法

動態窗口法是路徑規劃領域常用的局部路徑規劃算法，此算法可以使AGV順利避開場景中的未知障礙物，也可以增加路徑的平滑性。該算法首先在速度空間中隨機采樣多組線速度和角速度，然后使用這些速度組預測AGV在下一段時間內的運動軌跡。接下來，對于每個預測的軌跡，判斷是否與障礙物發生沖突，如果有，則將其從速度組中剔除。最后，通過評價函數選取最優的線速度和角速度，作為AGV的控制指令，使其能夠安全地運行。

2.1 運動學模型

動態窗口法是通過對機器人在一個窗口區域內的速度空間（υt,ωt)進行采樣來模擬機器人在時間t內的行駛軌跡。機器人的運動狀態由線速度υt和角速度ωt表示。利用評價函數篩選出在所有可行軌跡中表現最優的軌跡。AGV在一個時間間隔Δt內的運動學模型如下：

2.2 速度采樣

在AGV的速度空間中，有多個速度組(υ,ω)可供選擇。然而，由于AGV受到自身硬件條件和外部環境的限制，其速度被限制在一個特定的范圍內。這些限制條件如下：

（1）AGV的速度約束：

（2）在預測的時間間隔內電機的加速約束和減速約束：

（3）為了確保AGV能夠安全地避開動態障礙物，需要在碰撞前以最大減速度將速度降為0。此時，AGV的制動速度受到一定的限制，約束條件如下：

其中，dist(υ,ω)為AGV 當速度為(υ,ω)時與障礙物的最短距離。

2.3 改進評價函數

為了選取一條最優軌跡作為AGV的實際軌跡，速度空間中有多組采樣速度是可行的。為此，需要設計評價函數進行評估。本文評價函數的設計準則是，在與障礙物保持一定距離的前提下，盡可能快地到達終點。改進的評價函數為：

其中，σ為平滑系數；α、β、γ、λ為每一項的系數；head(υ,ω)是方位角的評價子函數，用于衡量軌跡終點方向與目標位置之間的方位角偏差；dist(υ,ω)是障礙物距離的評價子函數，用于計算模擬軌跡終點與任意障礙物之間的最短距離；vel(υ,ω)是評價當前速度大小的子函數；goal(υ,ω)是代價評價子函數，計算預測點到終點的代價值。

2.4 融合改進A＊算法和動態窗口法

改進的A*算法雖然可以為AGV迅速規劃一條全局最優路徑，但是它無法適應環境中動態障礙物的變化；改進的動態窗口法雖然在避開障礙物方面表現出色，但它也存在容易陷入局部最優解，而無法到達終點等問題。

為了解決A*算法和動態窗口法各自存在的問題，本文將兩種算法進行了融合，融合算法首先從改進A*算法規劃的路徑中提取關鍵節點，然后使用改進的動態窗口法在相鄰的節點間進行局部路徑規劃，直到終點。融合算法保證了路徑的全局最優性，并且克服了動態障礙物對路徑規劃的影響。相比于單獨使用兩種算法，該融合算法能夠更有效地為AGV進行路徑規劃。融合算法的流程圖如圖7所示。

圖7 融合算法的流程圖

本文對比了傳統動態窗口法和本文融合算法的效果，圖8和圖9為兩種算法規劃的路徑，傳統動態窗口法和本文融合算法的對比數據見表3。

表3 動態窗口法與本文融合算法性能對比

圖8 傳統動態窗口法規劃路徑

圖9 本文融合算法規劃路徑

通過對比可知，本文融合算法相比傳統動態窗口法規劃時間從31.502 減少到9.031，優化了71.3%；路徑長度從23.156 縮短到22.417，優化了3.2%；累計轉彎角度從2 908.410 減少到449.541，優化了84.5%。可見本文融合算法加快了搜索速度，縮短了路徑長度，轉彎也更加平滑，提高了算法的效率。

3 仿真實驗與分析

為了驗證本文融合算法的有效性，對傳統A*算法、傳統動態窗口法和本文融合算法進行了仿真對比實驗，并對實驗結果進行了分析。

在文獻[11]中的大小為20×20的柵格地圖上，分別采用三種算法對AGV的路徑規劃進行仿真，為了保證有較好的對比效果，AGV在仿真實驗中的參數都和文獻[11]中所設置的參數相同，最大線速度為1m/s，最大角速度為20rad/s，最大線加速度為0.2m/s2，最大角加速度為50rad/s2，線速度分辨率為0.01m/s，角速度分辨率為1rad/s，時間分辨率為0.1s，預測周期為2s。路徑規劃的起點為（1，1），終點為（11，20）。實驗環境所用的操作系統為64 位的WIN10 操作系統，運行內存為16G，實驗平臺為MATLABR2021b。

采用傳統A*算法、傳統動態窗口法和本文融合算法仿真的結果如圖10-圖12所示，三種算法的對比數據見表4。

表4 三種算法性能對比

圖10 傳統A＊算法規劃仿真實驗結果

圖11 傳統動態窗口法規劃仿真實驗結果

圖12 本文融合算法規劃仿真實驗結果

采用傳統動態窗口法得到的路徑如圖11所示，此時由于環境復雜，算法陷入局部最優，導致尋路失敗。

由表4可知，雖然傳統A*算法規劃時間更短，但是它不能考慮動態障礙物；本文融合算法路徑長度比傳統A*算法減少了3.942，優化了14.2%；累計轉角比傳統A*算法減少了170.229，優化了34.4%。

對比三種算法規劃的路徑可知，傳統A*算法規劃的全局路徑中有較多的轉彎和冗余路徑，路徑也不夠平滑；傳統動態窗口法規劃的路徑易陷入局部最優，傳統動態窗口法更加適用于局部路徑規劃，而在全局路徑規劃中難以到達終點；本文融合算法規劃得到的路徑轉彎明顯減少，路徑曲率變化比較連續，平滑度比較高。

4 結語

為了使AGV能夠在變化的倉儲物流環境中快速響應新的任務，針對傳統路徑規劃算法規劃的路徑轉折點多、冗余節點多、路徑不夠平滑和易陷入局部最優的問題，本文融合了改進的A*算法和動態窗口法。在全局路徑規劃方面，首先，通過將對數衰減因子作為權重改進A*算法，然后，使用關鍵點提取策略去除冗余節點；在局部路徑規劃方面，改進了動態窗口法，使AGV既能快速無碰撞到達終點，又能與障礙物保持一定的距離；最后，將改進A*算法與動態窗口法融合，彌補了A*算法和動態窗口法各自的不足。通過仿真實驗與其它路徑規劃算法進行對比，驗證了本文融合算法性能更好，效率更高。下一步我們計劃將本文融合算法應用在實際的AGV上，以進一步驗證該算法在實際場景中路徑規劃的有效性。