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

陳廣鋒，余立潮

東華大學(xué) 機械工程學(xué)院，上海 201620

1 引言

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

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

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

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

2 問題模型

2.1 問題描述

在倉儲作業(yè)環(huán)境中，AGV 小車在從初始位置移動到目的地旨在解決一個全局最優(yōu)控制問題，將系統(tǒng)運動學(xué)限制和所有動態(tài)障礙物體考慮在內(nèi)，假設(shè)AGV 小車模型完全對稱的，如圖1所示，記小車的軌跡坐標(biāo)為：

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

圖1 小車模型運動學(xué)參數(shù)示意圖

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

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

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

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

本文研究的倉儲環(huán)境為結(jié)構(gòu)化環(huán)境，如圖2所示。

圖2 倉儲貨架結(jié)構(gòu)圖

2.2 動態(tài)小車防撞約束

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

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

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

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

小車與過道貨架的防碰撞也需要考慮在內(nèi)，假設(shè)貨架為規(guī)則的矩形形狀，矩陣ω表示貨架的頂點坐標(biāo)集合，小車χi在t時刻構(gòu)造最優(yōu)超平面方程如式（10）所示。

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

3 B樣條參數(shù)化變量

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

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

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

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

根據(jù)小車實際路徑軌跡規(guī)劃的需要和B 樣條曲線的性質(zhì)，為了最后計算結(jié)果的合理性，選取節(jié)點向量集合，通過計算k次B樣條曲線的基函數(shù)導(dǎo)數(shù)，將小車的所有速度和加速度約束轉(zhuǎn)化成k次B樣條曲線形式。

因此，通過對B樣條曲線系數(shù)的控制實現(xiàn)小車約束的轉(zhuǎn)化，將式（2）改寫為：

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

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

4 問題模型解決

4.1 多幀時間窗分割

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

圖3 多幀窗體分割類型

4.2 靜態(tài)環(huán)境小車軌跡規(guī)劃求解步驟

算法1靜態(tài)環(huán)境小車軌跡規(guī)劃

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

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

步驟3判斷當(dāng)前節(jié)點周圍的8 個位置坐標(biāo)點是否屬于靜態(tài)障礙物坐標(biāo)點集γ，如果滿足，則退出本次算法；如果不滿足，則進(jìn)入步驟4。

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

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

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

4.3 動態(tài)環(huán)境小車軌跡控制點更新優(yōu)化策略

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

結(jié)合式（20）得到式（21）：

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

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

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

輸入：目標(biāo)函數(shù)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分割窗體，設(shè)置倉儲環(huán)境參數(shù)以及小車變量參數(shù)，設(shè)置所有窗體切換臨界值向量表和超平面臨界值向量表，設(shè)置采樣周期ΔT，設(shè)置活動的小車數(shù)目。

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

步驟3判斷當(dāng)前活動小車數(shù)目是否為0，如果為0，則跳轉(zhuǎn)到步驟8，否則跳轉(zhuǎn)到步驟4。

圖4 算法流程

步驟4初始化當(dāng)前幀各小車的起始時間，設(shè)置所有活動小車的B樣條參數(shù)，創(chuàng)建待優(yōu)化的所有樣條控制點符號變量，創(chuàng)建小車運行學(xué)約束矩陣和優(yōu)化模型。

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

步驟6判斷當(dāng)前所有活動小車是否達(dá)到目標(biāo)位置，如果是，跳轉(zhuǎn)到步驟8，否則跳轉(zhuǎn)到步驟7。

步驟7獲取當(dāng)前幀所有小車最小的時間節(jié)點，利用式子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得到，跳轉(zhuǎn)到步驟3。

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

5 算例實驗與分析

5.1 參數(shù)設(shè)置

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

表1 小車參數(shù)

實驗中，采用正方形矩形框代表倉儲環(huán)境，其參數(shù)設(shè)置如表2所示。

表2 倉儲環(huán)境參數(shù)

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

表3 多幀窗體分割參數(shù) m

5.2 仿真實驗分析

5.2.1 靜態(tài)環(huán)境小車軌跡尋優(yōu)規(guī)劃

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

圖5 各種算法實驗結(jié)果

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

表4 4種算法實驗結(jié)果

5.2.2 動態(tài)環(huán)境多小車軌跡規(guī)劃仿真

動態(tài)環(huán)境多小車軌跡的規(guī)劃是一個非常復(fù)雜的問題，各輛小車的運行情況會出現(xiàn)各種不確定性，實驗中將采用5輛小車進(jìn)行仿真模擬，假設(shè)倉儲環(huán)境是結(jié)構(gòu)化矩形，小車的初始位置和目標(biāo)位置設(shè)置如表5所示。

表5 5輛小車初始位置和目標(biāo)位置

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

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

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

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

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

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

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

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

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

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

5.3 算法性能分析

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

6 結(jié)論

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

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

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

表8 5輛小車動態(tài)軌跡規(guī)劃算法參數(shù)結(jié)果

圖9 多輛小車動態(tài)軌跡規(guī)劃算法參數(shù)結(jié)果

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