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

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

0 引言

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

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

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

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

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

1 改進(jìn)A＊算法

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

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

圖1 柵格地圖

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

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

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

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

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

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

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

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

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

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

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

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

圖5 改進(jìn)A＊算法遍歷的節(jié)點(diǎn)

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

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

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

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

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

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

表2為提取關(guān)鍵節(jié)點(diǎn)前后的對(duì)比，由實(shí)驗(yàn)數(shù)據(jù)可知，提取關(guān)鍵節(jié)點(diǎn)后的路徑長(zhǎng)度從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é)點(diǎn)前后對(duì)比

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

2 改進(jìn)動(dòng)態(tài)窗口算法

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

2.1 運(yùn)動(dòng)學(xué)模型

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

2.2 速度采樣

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

（1）AGV的速度約束：

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

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

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

2.3 改進(jìn)評(píng)價(jià)函數(shù)

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

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

2.4 融合改進(jìn)A＊算法和動(dòng)態(tài)窗口法

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

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

圖7 融合算法的流程圖

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

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

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

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

通過(guò)對(duì)比可知，本文融合算法相比傳統(tǒng)動(dòng)態(tài)窗口法規(guī)劃時(shí)間從31.502 減少到9.031，優(yōu)化了71.3%；路徑長(zhǎng)度從23.156 縮短到22.417，優(yōu)化了3.2%；累計(jì)轉(zhuǎn)彎角度從2 908.410 減少到449.541，優(yōu)化了84.5%?？梢?jiàn)本文融合算法加快了搜索速度，縮短了路徑長(zhǎng)度，轉(zhuǎn)彎也更加平滑，提高了算法的效率。

3 仿真實(shí)驗(yàn)與分析

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

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

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

表4 三種算法性能對(duì)比

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

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

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

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

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

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

4 結(jié)語(yǔ)

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