融合改進A*算法和動態(tài)窗口法的AGV路徑規(guī)劃

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

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

0 引言

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

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

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

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

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

1 改進A＊算法

1.1 環(huán)境模型描述

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

圖1 柵格地圖

1.2 傳統(tǒng)A＊算法

A*算法是最有效的求解全局路徑的啟發(fā)式搜索方法，是貪心算法和Dijkstra 算法的結(jié)合[10]。傳統(tǒng)A*算法的代價函數(shù)如下：

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

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

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

1.3 引入對數(shù)衰減因子

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

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

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

表1 傳統(tǒng)A*算法和改進A*算法的性能對比

圖2 傳統(tǒng)A＊算法規(guī)劃的路徑

圖3 傳統(tǒng)A＊算法遍歷的節(jié)點

圖5 改進A＊算法遍歷的節(jié)點

1.4 關(guān)鍵節(jié)點提取

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

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

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

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

圖6 關(guān)鍵節(jié)點路徑

表2為提取關(guān)鍵節(jié)點前后的對比，由實驗數(shù)據(jù)可知，提取關(guān)鍵節(jié)點后的路徑長度從24.485 縮短到了22.882，優(yōu)化了6.5%；轉(zhuǎn)彎次數(shù)從7 次變?yōu)榱? 次，優(yōu)化了71.4%；路徑累積轉(zhuǎn)彎角度從494.771 降為158.606，優(yōu)化了67.9%。

表2 提取關(guān)鍵節(jié)點前后對比

可見，利用關(guān)鍵節(jié)點提取策略對路徑優(yōu)化之后，路徑長度縮短、轉(zhuǎn)彎次數(shù)減少，但是路徑仍然存在不夠平滑、與障礙物過近的問題。

2 改進動態(tài)窗口算法

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

2.1 運動學(xué)模型

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

2.2 速度采樣

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

（1）AGV的速度約束：

（2）在預(yù)測的時間間隔內(nèi)電機的加速約束和減速約束：

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

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

2.3 改進評價函數(shù)

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

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

2.4 融合改進A＊算法和動態(tài)窗口法

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

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

圖7 融合算法的流程圖

本文對比了傳統(tǒng)動態(tài)窗口法和本文融合算法的效果，圖8和圖9為兩種算法規(guī)劃的路徑，傳統(tǒng)動態(tài)窗口法和本文融合算法的對比數(shù)據(jù)見表3。

表3 動態(tài)窗口法與本文融合算法性能對比

圖8 傳統(tǒng)動態(tài)窗口法規(guī)劃路徑

圖9 本文融合算法規(guī)劃路徑

通過對比可知，本文融合算法相比傳統(tǒng)動態(tài)窗口法規(guī)劃時間從31.502 減少到9.031，優(yōu)化了71.3%；路徑長度從23.156 縮短到22.417，優(yōu)化了3.2%；累計轉(zhuǎn)彎角度從2 908.410 減少到449.541，優(yōu)化了84.5%?？梢姳疚娜诤纤惴涌炝怂阉魉俣?，縮短了路徑長度，轉(zhuǎn)彎也更加平滑，提高了算法的效率。

3 仿真實驗與分析

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

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

采用傳統(tǒng)A*算法、傳統(tǒng)動態(tài)窗口法和本文融合算法仿真的結(jié)果如圖10-圖12所示，三種算法的對比數(shù)據(jù)見表4。

表4 三種算法性能對比

圖10 傳統(tǒng)A＊算法規(guī)劃仿真實驗結(jié)果

圖11 傳統(tǒng)動態(tài)窗口法規(guī)劃仿真實驗結(jié)果

圖12 本文融合算法規(guī)劃仿真實驗結(jié)果

采用傳統(tǒng)動態(tài)窗口法得到的路徑如圖11所示，此時由于環(huán)境復(fù)雜，算法陷入局部最優(yōu)，導(dǎo)致尋路失敗。

由表4可知，雖然傳統(tǒng)A*算法規(guī)劃時間更短，但是它不能考慮動態(tài)障礙物；本文融合算法路徑長度比傳統(tǒng)A*算法減少了3.942，優(yōu)化了14.2%；累計轉(zhuǎn)角比傳統(tǒng)A*算法減少了170.229，優(yōu)化了34.4%。

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

4 結(jié)語

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