大規模AGV的改進時間窗路徑規劃

摘要：在機器人路徑規劃中，A*算法搜索路徑時存在大量冗余節點，隨著任務量增加，其搜索效率也會急劇下降，因此無法適應大規模任務下的路徑規劃。為此提出一種改進時間窗的有界次優A*算法用于求解大規模自動導引車（automatic guided vehicle，AGV）路徑規劃問題。算法使用時間啟發式，并在搜索過程中采用時空搜索，規劃無沖突的最優或次優路徑。算法主要進行了三處改進：采用時間啟發式，縮短了路徑時間；采用動態時間窗算法，避免多次路徑規劃；優化了聚焦搜索算子，降低負反饋。通過MATLAB實驗結果證明改進后的算法在進行多機器人路徑規劃時，能快速有效地規劃出無沖突的平滑次優路徑，搜索效率高，穩定性強。

關鍵詞：A*算法；時間窗；次優搜索；負反饋；多機器人

中圖分類號：TP18；TP242文獻標志碼：A

文章編號：1001-3695（2023）01-008-0052-05

doi：10.19734/j.issn.1001-3695.2022.06.0288

Improved time window path planning for large-scale AGV

Liu Chun，Peng Taiping

（School of Computing Science，Hubei University of Technology，Wuhan 430068，China）

Abstract：In robot path planning，A* algorithm has a large number of redundant nodes when searching paths.With the increase of tasks，its search efficiency will decrease sharply，so it cannot adapt to path planning under large-scale tasks.Therefore，this paper proposed an improved time-window bounded suboptimal A* algorithm to solve the path planning problem of large-scale AGV.The algorithm used time heuristic and spatio-temporal search in the search process to plan the optimal or suboptimal path without conflict.The algorithm mainly has three improvements.This paper used time heuristic to shorten the path time.This algorithm adopted dynamic time window algorithm to avoid multiple path planning.This paper optimized the focal search operator to reduce negative feedback.MATLAB experimental results show that the improved algorithm can plan a smooth suboptimal path quickly and effectively without conflicts when multi-robot path planning，with high search efficiency and strong stability.

Key words：A* algorithm；time window；bounded-suboptimal search；negative feedback；multi-robot

隨著制造業以及服務業的迅速發展，自動導引車［1］在倉儲物流、巡檢安防、碼頭裝卸、自動泊車等領域的應用越來越廣泛。AGV適用性好、柔性程度高等特點使其在這些領域中發揮著重要作用。

由于在AGV工作的大部分場景中其路徑區間為單行雙向路徑，在進行多路徑任務路徑規劃時，經常會出現各路徑之間相互沖突的情況。針對多AGV調度［2］無沖突路徑規劃問題，國內外學者進行了大量研究。黃翼虎等人［3］提出一種基于時間窗防沖突的最短路徑規劃算法，該方法簡單易行，但忽略了時間對最優性的影響，得到的路徑可能不是最優。張中偉等人［4］將等待時間、行駛距離、任務緊急程度作為量化動態優先級的考慮因素，提出了一種基于動態優先級策略的多AGV 無沖突路徑規劃方法，該方法能夠保證最優解，但是計算量較大，只適用于少量AGV路徑規劃。Dai等人［5］建立了以最小化能耗和完工時間的多目標路徑規劃優化模型，提出了一種改進遺傳算法來求解該調度問題，但由于計算量大，不適用大規模AGV下的路徑規劃。Mhring等人［6］提出一種最短路徑計算方法計算每個AGV的最短可行路徑，該方法能保證從起點到終點存在可行路徑，且能確保路徑最優，但當系統發生連續的沖突時，容易陷入死鎖。曹小華等人［7］提出了基于沖突預測的避碰策略，但沒有考慮減少沖突次數來提高系統的運行效率。Boyarskip等人［8］提出一種改進的CBS算法，通過合并重啟和優先沖突處理方法一定程度上提高了搜索效率，但用于實際情況中依然需要進行改進，且當agent數量超過60后將很難找到可行解。現有大量文獻研究了在20～30臺AGV下的集群路徑規劃［9］，而目前國內主要智能倉儲AGV數量為18、38、50臺，京東5G智能無人倉內AGV數量更是多達一百多臺。隨著自動化發展，解決100臺以上大規模的AGV無沖突路徑規劃有著重要作用和意義。

綜上所述，針對百臺以上AGV的無沖突路徑規劃，本文提出了一種改進時間窗的有界次優A*算法。算法優化啟發式算子保證了時間最優性，相對于靜態時間窗規劃，算法采用動態時間窗規劃，避免重復規劃和局部最優，同時利用顯式估計對聚焦搜索算子進行優化，進一步提高了搜索效率。

1算法基礎

1.1A*算法

A*算法［10，11］作為一種啟發式搜索算法，在保證較高搜索效率的同時，找到一條最優路徑，A*算法的啟發式主要包括兩部分：

f（n）=g（n）+h（n）（1）

其中：g（n）表示節點到起點的實際成本；h（n）表示節點n離目標節點的預估成本。當對h（n）完全等于節點n到目標點的距離時，A*算法將找到最佳路徑且搜索速度很快。于幾何網格圖而言，h（n） 常取歐氏距離和哈夫曼距離，計算方法如下：

h（n）=（nx-goalx）2+（ny-goaly）2

abs（nx-goalx）+abs（ny-goaly）（2）

其中：（nx，ny）和（goalx，goaly）分別為當前節點的位置和目標節點的位置；abs用于求解絕對值。

1.2時間窗

時間窗［12，13］常分為保留時間窗和空閑時間窗：

Rn={rkn=［ckn，dkn］}

Fn={fkn=［akn，bkn］}（3）

其中：rkn和fkn分別表示節點n的第k個保留時間窗和空閑時間窗的時間區間。

規劃后的時間窗需要進行沖突檢測，時間窗沖突一般分為追擊沖突、相向沖突和交叉沖突三種類型（圖1）。對于追擊沖突和交叉沖突，一般可以通過等待策略解決沖突，而相向沖突為不可避免的，此時需要重新規劃路徑來解決沖突問題。

2算法改進

2.1基于時間因子啟發式

經典A*算法的啟發式在地圖比較復雜時，h（n）往往要小于節點n到終點的代價，且當h（n）越小，算法將遍歷越多的節點。算法需要大量的搜索來繞過障礙物，并且路徑平滑性不能得到保證，當存在多個最小值時，A*算法不能保證搜索的路徑最優。

為提高算法對靜態障礙物的避讓能力以及最優性，算法在啟發式上進行了修改，首先利用運動公式將距離代價和轉向懲罰代價換算成時間代價，啟發式的值即為最優時間。其次為讓h（n）更加接近節點n到目標節點的代價，提高搜索效率，本文利用Floyd算法［14］預處理計算出各節點間的最短距離，即距離矩陣Dn=（dnij）n×n。改進后的啟發式如下：

f（n）=gt（n）+ht（n）（4）

ht（n）=d（n，goal）v（5）

其中：gt（n）表示到達節點n的時間；ht（n）表示節點n到目標的最優估計時間。改進后的啟發式由于各項都是時間因子，避免了懲罰因子導致的非最優解的情況，同時，由于是基于時間進行計算，改進后的啟發式適用于異速移動機器人的路徑規劃。

2.2動態時間窗模型

一般時間窗規劃首先利用單路徑規劃算法找到一條最優路徑，然后通過時間窗規劃計算出節點的占用時間。由1.2節可知，當發生相向沖突時，當前路徑需要舍棄并重新規劃路徑，這種重復搜索浪費了搜索資源，而時空搜索是在搜索過程中規劃當前時間窗，當發生不可避免沖突時只需要舍棄當前節點并對其父節點繼續規劃路徑，從而避免重新規劃。在時空搜索過程中，節點可能對應多個子節點，而每個子節點對應時間窗的到達時間也不一樣，因此它們共同父節點的離開時間是不確定的。如圖2所示，節點1的到達時間為0，當離開時間為1時，到達下個節點的時間為2，對應子節點2，當離開時間為2時，到達下個節點的時間為3，對應子節點3，此時對于節點1，其離開時間是不確定的，因此在找到解之前無法確定每個節點的離開時間。

為更方便描述節點的離開時間，算法在時間窗的計算上將離開時間分解成最早離開時間和最晚離開時間。雖然無法確定準確的離開時間，但是可以通過到達時間來計算出離開節點的時間范圍。

假設移動機器人采用四輪差速驅動的運動方式，轉向角速為ω，直線行駛速度為v，忽略加減速過程。算法在擴展節點時對空間連續且不構成環路的節點進行時間窗規劃，于是改進后的節點時間窗模型為

N（n）={tkn=［skn，dkn，ekn，vkn］}（6）

其中：N（n）表示路徑節點n的可行時間窗；tkn表示節點n在空閑時間窗k上規劃的時間窗；skn、dkn、ekn、vkn分別表示最早進入時間、到達時間、最早離開時間和最晚離開時間。

如圖3所示，Lv為小車的長度尺寸，Ln為節點直徑，D（p、q）為節點p、q的直線距離，v為小車的移動速度，小車的轉向速度為ω，動態時間窗的最早進入時間需要根據前驅節點時間窗確定，需要滿足

eip+lv+tr（q）≤sjq≤bip+lv+tr（q）

ajq≤sjq≤bjq-Ln+Lvv（7）

其中：tr（n）表示到達節點n的轉向時間；σ（n）表示到達節點n的方向角度。

tr（n）=abs（σ（n）-σ（n-1））ω（8）

σ（n）=0n=1

arctan（yn-yn-1xn-xn-1）ngt;1（9）

對不等式（7）進行求解，得到進入節點的時間區間左右端點為

left=max（eip+lv，ajq）

right=min（bip+lv，bjn-Ln+Lvv）（10）

于是最早進入時間為

sjq=leftleft≤right

leftgt;right（11）

當sjq有解時，節點的動態時間窗為

sjq=max（eip+lv，ajq）

djq=sjq+Ln+Lv2v

ejq=sjq+Ln+Lvv

vjq=bjq（12）

當sjq為時，則當前時間窗不滿足規劃條件，繼續搜索其他時間窗。

2.3顯式估計搜索

在聚焦搜索［15］中，Focal中的次優解節點滿足

S={s∈Open|f（s）≤w×f（bestf）}（13）

其中：bestf為f最小的節點；w為次優系數，且w≥1，w越接近1，解越接近最優解，但搜索效率也越低。Focal中節點以ht（n）排序，在搜索過程中實際搜索的是Focal中ht（n）最小的節點，因此f（bestf）增長得比較慢，而被擴展的節點代價會不斷上升，當代價超過了次優閾值，此時系統就會轉向其他次優節點進行搜索，直到最優代價上升到一定程度才可能重新搜索該節點，如圖4所示。

顯式估計搜索（EES）［16］作為一種有界次優算法，可用于解決聚焦搜索導致的負反饋，它引入第三個函數用于估計節點的搜索成本，如果聚焦搜索中最優節點不在次優范圍中，則優先處理搜索成本較低的節點用來提升當前的次優界限。EES主要維護Cleanup、Open和Focal三個隊列，其中Cleanup以節點代價f排序，Open以估計搜索成本排序，Focal以離目標點距離d排序。本文算法引入一個變量hF（n）來估計擴展當前節點發生沖突的可能性。當hF（n）越大，經過該節點后發生沖突的概率越大，反之則越小，該函數如下：

hF（n）=0n=1

dpn-dqn-1-D（n-1，n）v-tr（n）ngt;1（14）

其中：hF（n）描述從節點n-1到n的沖突等待時間。改進后的聚焦搜索需要維護Cleanup、Open和Focal三個優先隊列。Cleanup中存放的是所有可行點，以代價f進行排序；Open中存放的是次優點，以hF（n）進行排序；Focal存放的是次優點集合，以h排序。節點擴展規則如下：

if f（besth）≤w×f（bestf） select besth from Focal

else if f（besthF）≤w×f（bestf） select besthF from Open

else select bestf from Cleanup

與聚焦搜索算子相比，改進后的搜索算子中最優代價會隨著次優節點的搜索更快地提升，從而提高了搜索效率，更快地找到最優或次優解。

2.4改進時間窗的有界次優A*算法

算法對A*算法啟發式和時間窗進行了改進，利用啟發式時間因子將兩種方法融合在一起進行時空搜索，能在搜索到最短時間的同時避免二次規劃；同時采用了改進后的聚焦搜索算子，提高了算法搜索效率的同時減輕了負反饋對搜索效率帶來的影響。

在進行大規模的規劃時，算法選擇權重最大的任務進行規劃，在規劃過程中擴展節點同時進行碰撞檢測，對發生相向碰撞的節點進行舍棄，有效節點則計算出其改進時間窗并加入集合中，當找到目標點后回溯得到一條無碰撞的路徑，重復以上操作直到規劃完所有任務。搜索算子如算法1所示。

算法1搜索算子

輸入：priority queue。

輸出：the optimal path。

while Open≠ do

N←SelectNode（）

if N=goal then

path←Backtracking（N）

UpdateTimemap（path）

sol_flag=true

break

for N′ in Adjacentnodes（N） do

if N′ does not form a loop then

for N″ in TimeWindow（N′） do

if N″ has no time conflict then

Open.push（N″）

Close.push（N）

UpdateQueue（）

return path

算法主要步驟主要如下：

a）初始化地圖G、時間地圖T、任務集合S和次優系數w；

b）若任務未完成，選擇權重最大的任務進行規劃；否則返回所有路徑；

c）初始化起點位置，計算節點時間窗并將可行節點加入Open中；

d）更新隊列Cleanup、Open和Focal；

e）根據搜索規則，選擇節點N進行擴展，若隊列為空則保存路徑為空并跳轉到步驟b）；

f）判斷N是否為目標點，若是保存路徑返回步驟b）；否則執行步驟g）；

g）擴展N的相鄰節點N′，計算N′的動態時間窗并進行沖突檢測，將可行節點加入Open，完成后跳轉到步驟d）。

3仿真與實驗

為驗證改進時間窗的有界次優A*算法的可行性，實驗設計了多種仿真地圖，不同任務和算法進行實驗對比分析驗證。所有實驗在配置為Intel CoreTM i5-5200U CPU @ 2.20 GHz，8 GB的個人電腦上完成，并使用MATLAB進行仿真。

3.1改進啟發式性能分析

當A*算法在路徑規劃過程中存在多組最優值時，規劃出的路徑往往不是最優的，并且隨地圖復雜程度增加，搜索的效率也變低。為驗證改進啟發式的優化效果，實驗在柵格地圖中障礙物與總柵格比為障礙比的條件下，分別設置柵格比為0.1、0.2和0.3的柵格地圖，采用四鄰域搜索方式進行仿真。

經過多次實驗，結果如圖5所示，從左往右柵格比依次為0.1、0.2和0.3，其中綠色路徑為本文算法規劃結果，藍色為傳統A*算法的規劃結果。為更好地對比改進后算法在平滑性上的優化效果，實驗還采用了轉向懲罰因子的A*算法進行路徑規劃，圖中為紅色路線（見電子版）。

仿真實驗結果表明，改進后的啟發式在對路徑的搜索和選擇上有一定的優化效果，在拐點、任務時間和搜索效率上優化明顯。從表1可知，在30×30的柵格地圖中，改進后的啟發式相對于原啟發式拐點減少了52%～87%，任務時間縮短了1.9%～5.6%，搜索效率提升了32%～71%，整體上有大幅度提升。

3.2大規模下算法性能對比和分析

為測試和驗證時空融合的有界增強A*算法在大規模AGV下的性能，結合實例設計實驗的路徑圖如圖6所示。其中灰色的方塊為障礙物，圓圈為節點，橫線為路徑，實驗圖中的路徑皆為雙向路徑。假設節點半徑r=1 m，相鄰節點距離D（i，j）=4 m，規定小車的移動速度v=0.5 m/s，轉向速度ω=（π/4）rad/s，AGV的尺寸為Lv=1 m。

首先為驗證算法在防沖突上的效果，模擬了相向沖突和交叉沖突的情況進行仿真，如圖7為4輛AGV同時執行任務，其中任務1和2發生相向沖突，任務3和4發生交叉沖突，由于設定AGV速度相等，這里不考慮追擊情況。規劃出的路徑和等待時間如表2所示，路徑節點的第一個值表示節點id，第二個值表示在節點等待的時間，其中任務1和2在發生碰撞前通過繞過沖突點來避免了沖突發生，而任務4為避免與任務3發生碰撞，選擇等待策略，在節點598等待8 s后再通過。實驗結果證明，改進后的算法能較好地避免沖突。

算法主要針對搜索效率和任務時間進行優化，在實驗環境不變的條件下隨機生成1～200個任務并初始化任務權重。實驗采用時間窗A*算法，改進Dijkstra算法［16］，基于沖突的增強搜索算法（ECBS）和本文算法對任務進行規劃。同時為驗證次優系數對搜索效率的影響，分別設置1.02、1.04和1.1逐漸增大次優系數進行實驗，如圖7所示為幾種算法在不同任務數量下的平均無效搜索節點數。在任務和解質量相同的情況下，搜索的無效節點數越多，表明算法的搜索效率越差。

由圖8可知，本文算法相對其他三種算法在搜索效率有了大幅度提升，并隨AGV數量增加，差距越來越大。在三種次優系數下，本文算法和ECBS算法隨w值增大，搜索效率也逐漸增加。當AGV數量超過100后，本文算法搜索效率依然相對穩定。綜上在三種次優系數下，本文算法在搜索效率都是最優的，證明了該算法在大規模路徑規劃下的搜索效率較高，適用于百臺以上的AGV路徑搜索。除此之外，算法啟發式采用時間啟發式，如圖9為不同次優系數下四種算法平均任務時間實驗結果。從圖中可知，當AGV數量超過15臺后，出現沖突的概率開始大幅度增加，導致原算法規劃的路徑中存在大量的等待節點，使得任務時間急劇增加，而改進后的算法采用時空搜索，可以確保任務時間的相對穩定。

如表3為200臺AGV下的實驗結果，綜合表3所示，時間窗A*算法的平均擁堵時間為80.220 1 s，本文算法在擁塞緩解上提升極大，平均擁塞時間為9.270 0～10.554 9 s；平均搜索深度上相對時間窗算法，本文算法搜索效率提升了66.7%～71.4%，在次優系數相等情況下，本文算法的搜索效率相對ECBS提升了22%～50.07%；在平均任務完成時間上，改進后的算法用時相對A*時間窗算法縮短了39.1%～40.2%，相對于ECBS平均任務時間縮短了9.6%～13.8%。

綜上分析，本文算法在100臺以上大規模的AGV下，相對時間窗A*算法、改進Dijkstra和ECBS算法在最優解和快速搜索上有較大提升，并且隨著任務數量的增加，規劃所需時間相對穩定。

4結束語

針對傳統的基于時間窗防沖突算法冗余節點多、不適用于大規模AGV路徑規劃的缺點，提出了一種改進時間窗的有界次優A*算法，充分結合時間窗和A*算法的特點。與時間窗防沖突A*算法、改進Dijkstra和ECBS算法進行仿真對比，實驗驗證了本文算法在大規模路徑規劃下快速搜索和任務時間的優化。 由于本文算法只研究了在靜態環境下的多AGV路徑規劃，未考慮動態環境，所以下一步將會對動態避障進行研究，實現動態環境下的多AGV路徑規劃。

參考文獻：

［1］牟勝輝，王有亮，張成，等.應用于智能倉儲的AGV貨架搬運機器人［J］.機電工程技術，2021，50（7）：56-59，153.（Mou Shenghui，Wang Youliang，Zhang Cheng，et al.AGV shelf handling robot applied for intelligent storage［J］.Mechanical amp; Electrical Engineering Technology，2021，50（7）：56-59，153.）

［2］王子意.多AGV系統的路徑規劃與調度算法的研究［D］.北京：北京郵電大學，2019.（Wang Ziyi.Research on path planning and scheduling algorithm for multiple AGV system［D］.Beijing：Beijing University of Posts and Telecommunications，2019.）

［3］黃翼虎，郝國笑.基于時間窗防沖突的最短路徑規劃研究［J］.電子測量技術，2020，43（18）：47-51.（Huang Yihu，Hao Guoxiao.Research on shortest path planning based on time window conflict prevention［J］.Electronic Measurement Technology，2020，43（18）：47-51.）

［4］張中偉，張博暉，代爭爭，等.基于動態優先級策略的多AGV無沖突路徑規劃［J］.計算機應用研究，2021，38（7）：2108-2111.（Zhang Zhongwei，Zhang Bohui，Dai Zhengzheng，et al.Multi-AGV conflict-free path planning based on dynamic priority strategy［J］.Application Research of Computers，2021，38（7）：2108-2111.）

［5］Dai Min，Tang Dunbing，Giret A，et al.Multi-objective optimization for energy-efficient flexible Job-Shop scheduling problem with transportation constraints［J］.Robotics amp; Computer Integrated Manufactu-ring，2019，59：143-157.

［6］Mhring R H，Khler E，Gawrilow E，et al.Conflict-free real-time AGV routing［M］//Operations Research Proceedings.Berlin：Springer，2004：18-24.

［7］曹小華，朱孟.基于沖突預測的多自動導引小車避碰決策優化［J］.計算機集成制造系統，2020，26（8）：2092-2098.（Cao Xiaohua，Zhu Meng.Multi-AGV conflict avoidance decision optimization method based on conflict prediction［J］.Computer Integrated Manufacturing Systems，2020，26（8）：2092-2098.）

［8］Boyarski E，Felner A，Stern R，et al.ICBS：improved conflict-based search algorithm for multi-agent pathfinding［C］//Proc of the 24th International Conference on Artificial Intelligence.Palo Alto，CA：AAAI Press，2015：740-746.

［9］郭昆侖，朱瑾.基于多智能體系統的自動化碼頭多AGV無沖突路徑規劃［J］.制造業自動化，2021，43（8）：83-89.（Guo Kunlun，Zhu Jin.Multi-AGV path planning in automated terminals based on multi-agent system［J］.Manufacturing Automation，2021，43（8）：83-89.）

［10］楊明亮，李寧.改進A*算法的移動機器人路徑規劃［J］.機械科學與技術，2022，41（5）：795-800.（Zhang Mingliang，Li Ning.Study on mobile robot path planning based on improved A* algorithm［J］.Mechanical Science and Technology for Aerospace Engineering，2022，41（5）：795-800.）

［11］孔慧芳，盛陽.基于改進A*算法的AGV路徑規劃研究［J］.現代制造工程，2021（10）：60-64.（Kong Huifang，Sheng Yang.Research on AGV path planning based on improved A* algorithm［J］.Modern Manufacturing Engineering，2021（10）：60-64.）

［12］李珺，郝麗艷，何奕濤，等.求解帶時間窗車輛路徑優化問題的改進細菌覓食算法［J］.計算機工程，2021，47（11）：44-53.（Li Jun，Hao Liyan，He Yitao，et al.Improved bacterial foraging algorithm for solving vehicle routing optimization problem with time windows［J］.Computer Engineering，2021，47（11）：44-53.）

［13］方景芳，袁沖.基于車間道路約束的物料配送模型及算法研究［J］.電子設計工程，2020，28（23）：18-24.（Fang Jingfang，Yuan Chong.Research on material distribution model and algorithm based on workshop road constraint［J］.Electronic Design Engineering，2020，28（23）：18-24.）

［14］許倫輝，黃寶山，鐘海興.AGV系統路徑規劃時間窗模型及算法［J］.廣西師范大學學報 ：自然科學版，2019，37（3）：1-8.（Xu Lunhui，Huang Baoshan，Zhong Haixing.Time window model and algorithm with AGV system path planning［J］.Journal of Guangxi Normal University：Natural Science Edition，2019，37（3）：1-8.）

［15］Li Jiaoyang，Ruml W，Koenig S.EECBS：a bounded-suboptimal search for multi-agent path finding［C］//Proc of AAAI Conference on Artificial Intelligence.2021.

［16］Thayer J T，Ruml W.Bounded suboptimal search：a direct approach using inadmissible estimates［C］//Proc of the 22nd International Joint Conference on Artificial Intelligence.Palo Alto，CA：AAAI Press，2011：674-679.

［17］姜辰凱，李智，盤書寶，等.基于改進Dijkstra算法的AGVs無碰撞路徑規劃［J］.計算機科學，2020，47（8）：272-277.（Jiang Chenkai，Li Zhi，Pan Shubao，et al.Collision-free path planning of AGVs based on improved Dijkstra algorithm［J］.Computer Science，2020，47（8）：272-277.）

收稿日期：2022-06-21；修回日期：2022-08-01基金項目：國家自然科學基金資助項目（61902116）

作者簡介：劉春（1972-），男，湖北武漢人，副教授，碩導，博士，主要研究方向為車聯網、汽車電子；彭太平（1997-），男（通信作者），湖北隨州人，碩士研究生，主要研究方向為嵌入式人工智能（446431200@qq.com）．