多幀時間窗輪換算法規劃倉儲多AGV小車路徑

陳廣鋒，余立潮

東華大學 機械工程學院，上海 201620

1 引言

隨著智能物流技術的不斷發展，在倉儲環境中利用自動引導車輛（Automated Guided Vehicle，AGV）或自動引導車輛在傳感器及其他智能設備[1]的共同作用下實現自動揀貨能夠提高企業的核心競爭力和降低人工成本，因此AGV 小車在車間的實現智能路徑調度是一個重要的研究課題。

通常情況下，倉儲環境中的AGV 小車的工作環境包含靜態貨架和動態小車，實現小車的運動路徑規劃需要將幾何約束（例如避障）、運動學約束（速度、加速度、邊界）和動態約束組合起來轉化成復雜的多變量優化問題，研究人員對于復雜的優化問題均采用了耦合和解耦的運動規劃方法[2]。耦合的方法考慮簡化的環境[3]，但是速度太慢且無法在線使用。因此，大多數采用解耦方法，即運用基于圖的方法和基于隨機抽樣的方法解決預先路徑規劃，如 A*尋路算法[4]、Dijkstras 算法[5]、蟻群算法[6]、概率路線圖（PRM）[7]、基于人工勢場[8]。趙江等學者[9]運用幾何方法優化傳統A*算法規劃出距離短且平滑的軌跡，劉新宇等學者[10]采用蟻群聚類自適應方法在動態環境下實現單條軌跡的小車軌跡規劃，然而他們均未對較多小車情況下的軌跡規劃。Mercy等學者[11]將軌跡規劃問題提煉成最優控制問題（OCP），然后提供將OCP 問題轉換為鏈式系統的可行性解決方案。后續路徑規劃階段通常使用模型預測控制（MPC）[12]引導小車系統沿著預先計算路徑行駛，然而解耦方法在后續路徑規劃階段不考慮車輛的運動學及碰撞約束導致計算出的路徑通常是不可行的。

對多AGV 小車的控制非常復雜，現有的各種MPC方法不能靈活處理任意（非凸）約束問題和碰撞避免約束[13-15]，而且解決非凸多車問題方法大多局限于解決離線最優控制問題[16-17]。對于現有的DMPC策略以分布式方式通過多次更新解決優化問題[18-19]，通常涉及多次迭代，每次迭代都需要解決局部優化問題和相鄰小車之間的通信，由于計算能力和通信能力導致更新太慢。

綜上所述，本文提出一種基于多幀時間窗輪換算法，運用A*尋路算法進行全局路徑規劃，結合B樣條的特性保證約束，降低優化問題的變量維度。通過多幀切換以及每一幀建立的目標優化函數擴展小車運動規劃中的避障限制，通過運用時變分離超平面的技術實時應對運動規劃中環境的不確定性和干擾。

2 問題模型

2.1 問題描述

在倉儲作業環境中，AGV 小車在從初始位置移動到目的地旨在解決一個全局最優控制問題，將系統運動學限制和所有動態障礙物體考慮在內，假設AGV 小車模型完全對稱的，如圖1所示，記小車的軌跡坐標為：

其中，N為小車的數量，χi表示第i輛小車，則q(χi(t))、x(χi(t))、y(χi(t))分別表示第χi輛小車軸中心x方向和y方向所在的位置。

圖1 小車模型運動學參數示意圖

小車的約束包括動力學約束、運動學約束以及執行機構約束，這是一個非常復雜的綜合問題，本文僅考慮基本的運行學約束。假設每輛小車χi從初始位置到達目的地所用的時間為，那么小車χi在t時刻的運行學參數如下所示：

其中，ux(χi(t))為小車χi在x方向的速度，uy(χi(t))為小車χi在y方向的速度，θ(χi(t)) 為小車χi的方向角，θ(χi(t))的導數θ′(χi(t))為小車χi在軌跡點pi的角速度，φ(χi(t))為小車χi車輪的轉向角，Lw為小車χi前后輪軸距。因此，小車χi在t時刻的狀態為：

引入小車的初始位置和終點位置約束：

為了倉儲保證小車能夠平穩運行和不發生側翻，同時能使小車揀貨過程定位精度更高，引入小車的速度、加速度和角速度約束：

本文研究的倉儲環境為結構化環境，如圖2所示。

圖2 倉儲貨架結構圖

2.2 動態小車防撞約束

多輛小車在運行過程必須考慮小車的防碰撞問題，為了解決這個問題，為每輛小車χi建立線性碰撞模型，該模型能夠實時表示出小車每一時刻所在的位置pt(χi(t))，如下式所示：

為了保證每輛小車χi不發生碰撞，即小車的坐標位置在{x,y}∈Rn的二維環境下不會發生重疊，可用如下模型表示：

其中，Pt(χi+k(t))和Pt(χi(t))為相遇兩輛小車χi+k和χi在t時刻所在的位置，η為確保小車χi+k和χi保持安全距離的安全系數。實際中多輛小車的軌跡規劃問題比較復雜，為了更有效解決上述小車防碰撞模型以及防止小車χi在t時刻所在的位置Pt(χi(t))出現偏差導致的防碰撞模型失效，文獻[20]提出可以用最優超平面將兩個凸集數據集分成兩部分，修正小車χi從t到t+1 時刻的輪廓位置坐標Ωt(χi(t))和Ωt+1(χi(t+1))，如下式所示：

其中，Ωt(χi(t))和Ωt+1(χi(t+1))為小車χi的輪廓四個頂點坐標的集合，R(t)為旋轉矩陣。因此，用r(χi)表示小車χi輪廓外接圓半徑，小車χi+k和χi在t時刻構造最優超平面方程如下式所示：

小車與過道貨架的防碰撞也需要考慮在內，假設貨架為規則的矩形形狀，矩陣ω表示貨架的頂點坐標集合，小車χi在t時刻構造最優超平面方程如式（10）所示。

綜上所述，多小車需在滿足上述所有約束下實現最優路徑規劃問題，因此，建立t時刻小車所在的窗體的狀態與所在窗體的目標位置狀態的一范數的積分為優化模型目標函數，如式（11）所示。目標函數的物理意義是保證t時刻小車所在的窗體的位置狀態與所在窗體的目標位置狀態的誤差最小，為了解決問題的需要，將一范數積分獲得具有凸集特征的優化模型目標函數。

3 B樣條參數化變量

B樣條曲線是個逐段曲線且在連接處可微，其強凸包性質可以進行更精細的局部形狀控制。因此，可以采用k次的B樣條曲線構造復雜的小車路徑，有效地減少變量的維度以及替代上述所有約束。

為了將小車路徑q(χi(t))參數化為B 樣條多項式曲線，將時間t無量綱為：

已知n+1 個控制點集C={c0,c1,…,cn}和一個節點向量集合U={τ0,τ1,…,τm}，小車路徑q(χi(t))可參數化為k次B樣條曲線如下所示：

其中，0 ≤l≤n-k-1，1 ≤k≤n-1，規定[0,1]。

根據小車實際路徑軌跡規劃的需要和B 樣條曲線的性質，為了最后計算結果的合理性，選取節點向量集合，通過計算k次B樣條曲線的基函數導數，將小車的所有速度和加速度約束轉化成k次B樣條曲線形式。

因此，通過對B樣條曲線系數的控制實現小車約束的轉化，將式（2）改寫為：

其中，每輛小車χi的速度和加速度約束轉化如下：

將超平面方程的系數B樣條參數化為：

4 問題模型解決

4.1 多幀時間窗分割

所提算法創新性地將小車置于圖3 所示的各種虛線框中，將全局路徑規劃簡化為虛線框局部環境的軌跡規劃問題，結合B 樣條曲線的性質，保證每個虛線框內小車軌跡的平滑性和運動時間的連續性。在每個虛線框建立小車的運動模型，進而求解每個局部環境小車的軌跡規劃問題。小車完成全局軌跡規劃需要對多幀窗體進行切換，在同一個虛線框運行的小車數目小于等于1 時，將不用考慮動態避障；當小車數目大于等于2 時，通過設置布爾向量表，控制超平面約束方程的使用，進而保證小車動態運行時實現協同規劃。

圖3 多幀窗體分割類型

4.2 靜態環境小車軌跡規劃求解步驟

算法1靜態環境小車軌跡規劃

步驟1初始化小車的起始位置pstart和小車的終點位置pend，運用網格劃分方法計算靜態障礙物坐標點集γ=(γ0,γ1,…,γm)，將pstart賦值給當前節點和放置小車軌跡節點的closelist向量表，初始化臨時向量表openlist。

步驟2判斷小車的起始位置pstart和小車的終點位置pend是否屬于靜態障礙物坐標點集γ，如果是，修正pstart和pend，重新初始化當前節點和放置小車軌跡節點的closelist向量表。

步驟3判斷當前節點周圍的8 個位置坐標點是否屬于靜態障礙物坐標點集γ，如果滿足，則退出本次算法；如果不滿足，則進入步驟4。

步驟4遍歷當前節點周圍的8個位置坐標點，遍歷向量表closelist，如果當前節點周圍的8 個位置第i個坐標點i∈Z 且i∈[0,8]屬于closelist，則標記當前節點周圍的8 個位置的第i個坐標點為舊的坐標點；遍歷向量表openlist，如果當前節點周圍的8個位置第i個坐標點i∈Z 且i∈[0,8]屬于openlist，則標記當前節點周圍的8 個位置的第i個坐標點為舊的坐標點，重新計算該節點較小的評價函數g、f、h；如果當前節點周圍的8個位置第i個坐標點i∈ Z 且i∈[0,8]不屬于closelist和openlist，則將該位置坐標點放入臨時向量表openlist中，創建新的當前節點并將其設置為舊的當前節點的子節點，計算該節點的評價函數g、f、h。

步驟5判斷i是否等于8，如果是進入步驟6；如果否則跳回步驟4。

步驟6挑選節點中評價函數f最小的節點作為當前節點，從openlist移除它并把它加入closelist。判斷當前位置是否到達終點位置，如果是，則退出程序；否則，跳回步驟2。

4.3 動態環境小車軌跡控制點更新優化策略

如圖3 所示，每一時刻每一個虛線框內的小車數目都在動態變化，為了實現多小車軌跡的動態規劃，所提算法將對虛線框內小車軌跡控制點進行動態更新，確保各個小車軌跡不發生沖突。因此，構造障礙函數f(τ)更新控制點，將所有不等式約束簡化成g(τ)=(g0(τ),g1(τ),…,gm(τ))≥ 0 ，將所有等式約束簡化為h(τ)=(h0(τ),h1(τ),…,hp(τ))=0 。根據牛頓迭代法定義Δτk、Δyk、Δzk為牛頓下降方向，獲得式（20）拉格朗日方程：

結合式（20）得到式（21）：

其中，Wk為海森矩陣，在固定μ的情況下，運用牛頓迭代法獲得一系列最優解組成的集合τ?(μ)，海森矩陣的引入可以避免出現全局無法收斂。再判斷求得的牛頓步長是否滿足如式（23）所示的條件。

運用回溯直線法更新牛頓步長，其步長如式（23）所示，當前迭代結束后，運用式（24）進入下一個迭代循環。一旦τ?(μ)滿足設定的誤差范圍，算法結束。

算法2小車的軌跡控制點更新算法

輸入：目標函數f(τ)，不等式約束g(τ)和等式約束h(τ)

輸出：小車的軌跡控制點集ψ=(ψ0,ψ1,…,ψn)

1.初始化ε,μ0,εtol,l← 0,k← 0,τ←τ0

2.Repeat

3.do

4.計算式（21）Newton方向Δτk、Δyk、Δzk

5.If 式（22）滿足then

6.計算Newton減量λ2(τ)=ΔτkT?2φμ(τ)Δτk

7.計算回溯直線法確定步長式（23）

8.計算式（24）

9.else

10.break

11.Untilλ2(τ)/2 ≤ε

12.μj+1=1/μ×μj,μ<1

13.Untilmμj+1≤εtol

4.4 算法流程

算法總的流程圖如圖4所示，其具體步驟如下：

步驟1分割窗體，設置倉儲環境參數以及小車變量參數，設置所有窗體切換臨界值向量表和超平面臨界值向量表，設置采樣周期ΔT，設置活動的小車數目。

步驟2遍歷所有活動的小車數目，利用4.2 節的結果形成所有活動小車的全局路徑離散點，解耦倉儲環境參數以及小車變量，獲取初始窗體切換臨界值向量表，超平面臨界值向量表。

步驟3判斷當前活動小車數目是否為0，如果為0，則跳轉到步驟8，否則跳轉到步驟4。

圖4 算法流程

步驟4初始化當前幀各小車的起始時間，設置所有活動小車的B樣條參數，創建待優化的所有樣條控制點符號變量，創建小車運行學約束矩陣和優化模型。

步驟5將算法1 獲得的離散點集的起始點作為算法2變量的初值，調用算法2求解含有約束的小車軌跡，提取小車的軌跡點集ψ=(ψ0,ψ1,…,ψn)。

步驟6判斷當前所有活動小車是否達到目標位置，如果是，跳轉到步驟8，否則跳轉到步驟7。

步驟7獲取當前幀所有小車最小的時間節點，利用式子qk+1(χi(t))=qk(χi(t))(k+1)ΔT更新下一幀偽軌跡集合qk+1(χi(t))，同時，下一幀偽速度集合uk+1(χi(t))由uk+1(χi(t))=uk(χi(t))(k+1)ΔT得到，跳轉到步驟3。

步驟8算法結束，取得最終小車路徑軌跡點的集合ψ=(ψ0,ψ1,…,ψn)。

5 算例實驗與分析

5.1 參數設置

為驗證所提算法的有效性，通過建立可視化環境對小車的最優軌跡規劃進行算例仿真。小車的速度、加速度等約束的數值根據小車實際動力學的向心力和靜摩擦力公式得到，但在實際上考慮到各小車的質量不同、地面的靜摩擦系數不同、轉彎半徑不同等因素，將模型進行簡化，用一定半徑的圓代表小車，小車的最大速度和最大加速度約束數值如表1所示。

表1 小車參數

實驗中，采用正方形矩形框代表倉儲環境，其參數設置如表2所示。

表2 倉儲環境參數

因此，將實驗的倉儲的過道進行窗體劃分，劃分結果如表3所示，采用該窗體劃分有利于算法2路徑尋優。實驗中采用的B 樣條曲線參數化小車路徑以及小車的各種約束，選取B樣條的階次k=3，根據m=n+k+1 以及B樣條的性質，B樣條的節點向量，控制節點n=13。

表3 多幀窗體分割參數 m

5.2 仿真實驗分析

5.2.1 靜態環境小車軌跡尋優規劃

圖5（a）是單輛小車運用4種算法仿真結果對比圖，其中藍色虛線為所提算法仿真軌跡，藍色實線為A*尋路算法規劃的小車軌跡，紅色和綠色曲線分別為運用文獻改進的人工勢場蟻群算法[8]和蟻群算法[21]優化后規劃出的小車軌跡，圖中可以看出所提算法具有較好的平滑性，蟻群算法規劃的軌跡較其他算法具有較大的小車曲率，使得小車在轉彎處容易發生側翻。將文獻[8，21]算法的參數設置為：迭代代數N=100，種群大小設置為45，啟發式因子α=1，β=1，信息素揮發系數p=0.15，信息素強度系數q=1。文獻[8，21]算法實驗結果的收斂曲線如圖5（b）所示，改進的人工勢場蟻群算法相對于蟻群算法具有更強的局部搜索能力，但是對于這兩種啟發式算法是通過多次遞歸迭代無限逼近全局最優解，所提算法是結合梯度的思想采用二次擬合曲面尋找全局最優解，相對于傳統啟發式算法線式搜索，本文算法更具有更廣的面上搜索能力，使得CPU運行時間更短，同時所提算法的迭代代數是多個迭代的平均值。

圖5 各種算法實驗結果

圖5（a）和表4 的4 種算法實驗結果數據所示，在仿真環境中，4種算法均可以達到軌跡尋優效果，文獻[21]所提算法在4種算法結果中表現較差，所提算法在最優路徑長度比文獻[8，21]算法分別優化了3.38%和0.99%，平均迭代數分別優化了20.24%和11.62%，本文算法在CPU 運行時間上具有明顯優勢，分別減少了51.06%和51.45%，提高了小車軌跡規劃的動態響應能力。

表4 4種算法實驗結果

5.2.2 動態環境多小車軌跡規劃仿真

動態環境多小車軌跡的規劃是一個非常復雜的問題，各輛小車的運行情況會出現各種不確定性，實驗中將采用5輛小車進行仿真模擬，假設倉儲環境是結構化矩形，小車的初始位置和目標位置設置如表5所示。

表5 5輛小車初始位置和目標位置

5 輛小車動態軌跡規劃仿真結果如圖6 所示，其中小的圓圈代表小車，實線是A*算法規劃出的小車軌跡，虛線是所提算法規劃的軌跡。從圖中可以看出，A*算法規劃出的軌跡曲率較不平滑，不利于小車平穩運行，所提算法在實現有效防止靜態和動態障礙物的情況下實現小車軌跡合理規劃，同時保證軌跡最優。如圖6所示，綠色小車和藍色小車（a）時刻將要發生碰撞，從（b）時刻可以看出藍色小車自動減緩速度，實現綠色小車平穩通過岔路口。圖6紫色小車和紅色小車在（a）時刻距離較近，在（b）時刻紅色小車先通過岔路口，紫色小車保持速度，防止撞上紅色小車。

圖6 5輛小車各時刻的位置

為了更好理解所提算法的規劃軌跡的合理性，用小車的運動變化關系來理解。5輛小車的速度和加速度隨時間的變化曲線如圖7 所示，模擬仿真中，設置每幀窗體小車運行的最大時間：30 s。根據圖示，圖中的速度-時間曲線和加速度-時間曲線均保持均勻變化，速度和加速度的峰值均滿足小車運動學的約束，小車可以比較平穩地運動并且加速度的變化幅度不會導致小車發生剛性沖擊，滿足小車的力學性能。

圖7 5輛小車運行速度和加速度隨時間變化曲線

表6和表7是上述5輛小車模擬的速度和加速度仿真結果的實驗數據，為了與圖7曲線數據變化情況保持一致，表中將數據負號表征小車速度和加速度大小，實際中負號代表小車的運行方向。表中的數據符合表2預設的數據范圍，由于5 輛小車同時動態運行，增加了小車運行情況的不確定性，導致在小車出現避障過程中出現小車的速度為0，充分說明所提算法能實現動態小車避障功能，實現小車軌跡動態規劃。綜合圖6 和圖7分析得知，小車在通過岔路口時，為了防止兩小車出現碰撞，其中一小車的速度逐漸減至為0，符合表6出現小車的速度為0 情況。結合圖7 和表6 及表7，2 號小車的速度和加速度方差最小，4號車的速度和加速度方差最大，從圖7 的曲線變化情況看出2 號小車的變化幅度較小，4號小車的變化幅度較大，但是總體上符合小車的運動學約束。

表6 5輛小車運行速度結果m·s?1

表7 5輛小車運行加速度結果m·s?2

為了更好地理解所提算法對解決多AGV小車路徑軌跡規劃的有效性，采用更多數量的AGV 小車進行仿真模擬，小車的起始位置和終點位置隨機生成。隨著小車數量的增加，路段沖突和節點沖突也會呈線性增加，實現多AGV小車的路徑規劃復雜度增加。如圖8（a）到圖8（e）的紅色框位置所示，框中共有5輛AGV小車，在圖8（a）時刻，位于紅色框內左側的藍色小車向上方向運行將要到達岔路口，位于紅色框內中間位置的淺綠色小車緊跟紫色小車向左方向運行，位于紅色框內中間位置的深綠色小車緊跟淺綠色小車向右下方向運行，位于紅色框內中間位置的淺黑色小車緊向左上方向運行。由圖8（b）時刻分析得知，淺綠色小車緊跟紫色小車向左方向直線運行，并未發生沖突，此時，位于紅色框內左側的藍色小車向上方向緩慢到達岔路口，未與向左方向直線運行的淺綠色小車和紫色小車發生沖突，位于紅色框內中間位置向左上方向運行的淺黑色小車與向右下方向運行的深綠色小車相向而行，向右下方向運行的深綠色小車先通過岔路口，使得兩車很好地實現避障。由圖8（c）、圖8（d）、圖8（e）時刻分析得知，位于紅色框內左側向上方向運行的藍色小車在淺綠色小車駛離岔路口時，向右駛入直線過道，隨著其他小車平穩地到達終止位置，最終15輛AGV小車在具有25個靜態障礙物的矩形網格中實現協同動態規劃。

圖8 15輛小車各時刻的位置

文獻[21]并未討論多輛AGV 小車協同軌跡規劃的避障仿真效果，僅對比了多種算法實現單條路徑規劃的優化效果；文獻[22]能實現AGV 小車的避障效果，但是僅限制在兩個目標的約束的路徑規劃；文獻[23]研究不同路網尺寸中各種算法的多AGV的沖突次數和時間的比較，但并未給出最后的仿真效果，無法直觀地體現多AGV的協同動態規劃結果。因此，由圖6～8的多種仿真結果和小車時間曲線的分析，表明所提算法能有效地實現多AGV小車實際路況的軌跡規劃和協同動態響應。

5.3 算法性能分析

表8和圖9是多輛動態小車軌跡規劃仿真中算法參數所得的數據結果。從圖中分析得知，5輛小車從起始點到達目標位置總共經歷了10 幀的時間窗，每一幀時間窗內小車的軌跡點的更新時間在較小的范圍內，表明小車在較短的時間內實現在有動態障礙物情況下小車軌跡的重新規劃。因為算法在更新控制點的過程中的時間復雜度接近對數級別，軌跡更新具有較強的動態響應性。圖9（a）為所提算法的目標函數值f(τ)，結合式（11）表明在每一幀的時間窗迭代中小車位置和目標位置保持較小的誤差，并且每一幀的時間窗迭代中最大值未發生較大的突變，具有良好的穩健性。μ是障礙函數的系數的對數化的結果，圖中表明μ滿足預設范圍，μ的方差s2在較小的范圍波動。圖9（c）為牛頓下降方向Δτk兩次對數化后的結果，每一幀的均值均穩定在10幀中最大平均值的37%以內，沒有出現較大波動，可以看出本文所提算法可以比較穩定地搜索最優解，不會出現過大或過小的搜索梯度。

6 結論

（1）針對倉儲環境中多AGV 小車的路徑規劃問題，綜合倉儲環境中多AGV 小車的幾何約束和運動學約束，建立多幀時間窗中的小車狀態模型，通過所提的基于多幀時間窗輪換算法可以有效地實現小車在每個局部時間窗網格中軌跡規劃，相對于傳統的蟻群算法、人工勢場算法等具有明顯的單條軌跡規劃優化效果。

（2）通過實驗仿真可知A*尋路算法得到的多AGV小車的路徑離散點的集合得到的轉彎曲率相對較大，不能使得AGV 小車平穩地通過岔路口，通過本文所提算法的B 樣條參數化處理，可以得到一條平滑的AGV 小車軌跡路線。

（3）實驗證明，引入超平面約束，將小車軌跡和運動學約束B樣條參數化，運用牛頓迭代融合回溯直線法更新步長的方法不斷更新迭代每一幀時間窗小車的運動學控制點，可以使得AGV 小車能有效地實現動態軌跡協同規劃，具備一定的智能避障能力。

表8 5輛小車動態軌跡規劃算法參數結果

圖9 多輛小車動態軌跡規劃算法參數結果

（4）所提算法每一幀時間窗切換都能保證AGV小車軌跡連續和時間連續，證明所提算法規劃出的AGV 小車軌跡是有效的。最后通過算法性能分析，采用牛頓迭代融合回溯直線法更新步長可以明顯地節省CPU運行時間，可以使得AGV小車迅速地對環境變化作出反應，實時地實現各AGV小車信息更新。