何均鋒，李 鵬，申艷紅，鄒安幫，熊 威，吳婷婷

(南京林業(yè)大學(xué) 汽車與交通工程學(xué)院， 南京 210037)

自動導(dǎo)引車(automated guided vehicle，AGV)是指在導(dǎo)航信息的導(dǎo)引下在地面上移動的移動機器人[1]。合理的AGV路徑規(guī)劃不但能提高倉庫的貨物周轉(zhuǎn)率和訂單完成效率，也能使AGV的行駛更平穩(wěn)[2]。運動規(guī)劃是AGV完成作業(yè)任務(wù)運動控制的基礎(chǔ)，直接決定了AGV的工作質(zhì)量[3]。

目前，路徑規(guī)劃算法主要分為基于圖搜索的路徑規(guī)劃、數(shù)值優(yōu)化、基于樣本的路徑規(guī)劃、插值算法這4類[4-5]。考慮到實際應(yīng)用的簡單性，圖搜索方法比傳統(tǒng)的路徑規(guī)劃策略更有效[6]。例如，Dijkstra和A*算法等經(jīng)典的圖搜索算法都被廣泛用于搜索最短路徑[7-8]。吳鵬等[9]在傳統(tǒng)A*算法的基礎(chǔ)上增加了正反向搜索的機制，然而其使用范圍只是局限于靜態(tài)障礙物環(huán)境下。Xu等[10]以時間最優(yōu)為代價函數(shù)，用帶有優(yōu)先約束(TSPP-PC)旅行商問題來表示AGV的路徑規(guī)劃問題,并基于精確算法提出了一種啟發(fā)式算法來進行求解。Lim等[11]和Zhang等[12]在Shih等[13]通過圖搜索器在時空域中規(guī)劃一個粗略的軌跡的基礎(chǔ)上進行了參數(shù)優(yōu)化計算，但未考慮與AGV運動學(xué)相關(guān)的非完整約束，并且障礙物的軌跡被限定，存在局限性。Corman等[14-15]提出將事件動態(tài)和連續(xù)時間動態(tài)模型離散，把AGV與碼頭QC和ASC的作業(yè)過程視為混合流水車間調(diào)度，以時間最短為代價函數(shù)，采用啟發(fā)式算法以一種近似最優(yōu)的方法來求解，但該過程只考慮靜止障礙物，并未將動態(tài)障礙物納入考慮范圍。Xin等[16]在離散事件動態(tài)和連續(xù)時間動態(tài)的基礎(chǔ)上提出了滾動時域狀態(tài)監(jiān)測控制器,以能耗最優(yōu)為代價函數(shù)，采用事件驅(qū)動能效算法求解模型,但未對AGV的碰撞約束進行考慮。趙鑫等[17]基于A*算法改進得到ARA*算法，首先將約束條件松弛，搜索出一條次優(yōu)路徑，然后在加強約束條件的情況下改進次優(yōu)解。

本文將移動障礙物軌跡隨機環(huán)境下的AGV軌跡規(guī)劃問題歸結(jié)為一個最優(yōu)控制問題。利用直接配點法將最優(yōu)控制問題離散成一個非線性規(guī)劃問題，然后用常見的內(nèi)點法進行尋優(yōu)。同時，為了快速求解非線性規(guī)劃問題，對傳統(tǒng)的A*算法做了改進，生成一條初始軌跡作為優(yōu)化過程的初始解。仿真表明，改進A*算法能有效搜索出一條無碰的初始軌跡，利用直接配點法優(yōu)化后的軌跡在滿足約束的前提下與初始軌跡基本一致。

1 避障描述

一般AGV軌跡規(guī)劃問題可以轉(zhuǎn)化為一個最優(yōu)控制問題，即在滿足運動學(xué)約束、避碰約束和邊界條件的情況下，使得代價函數(shù)最小化。

1.1 AGV運動學(xué)

兩輪差速AGV運動平面圖如圖1所示，X-Y坐標(biāo)系為全局坐標(biāo)系，XR-YR坐標(biāo)系為AGV車身坐標(biāo)系，M處于驅(qū)動軸的中間位置，C為小車的質(zhì)心所在，設(shè)方向以逆時針為正，AGV運動學(xué)描述為：

(1)

圖1 AGV運動平面圖

具體的參數(shù)含義見表1：

表1 參數(shù)與定義說明

在[0,tf]時間內(nèi)AGV移動過程中，存在幾個邊界限制其移動，運動約束如下：

(2)

其中，vmax、amax和ωmax分別代表相應(yīng)變量的最大值。

1.2 避碰約束

用包絡(luò)圓柱來覆蓋障礙物，其在AGV運動平面上的投影是圓形。假設(shè)在[0,tf]內(nèi)每個障礙物的位置都可以被正確地預(yù)測，則第i個障礙物的圓心(xobs_i(t),yobs_i(t))和半徑Robs_i(t)在t∈[0,tf]內(nèi)任何時刻的位置已知。

在實際工作中，AGV的車身外形是矩形，但涉及矩形的避碰約束非常復(fù)雜，因此采用外接圓來覆蓋AGV的矩形車身，其避碰約束如式(3)所示：

(3)

1.3 邊界約束

假設(shè)AGV在t=0和t=tf的2個時刻分別處于起始位置和終端位置，則起始時刻約束設(shè)置如式(4)所示：

(4)

式中：x(0)，y(0)，θ(0)，v(0)，a(0)和ω(0)是初始時刻參數(shù)。

由于實際情況下的AGV并不一定能夠按期望到達理想位置，因此需要對終點約束進行松弛，部分參數(shù)[v(tf),a(tf),ω(tf)]設(shè)置如式(5)所示：

[v(tf),a(tf),ω(tf)]=[vf,af,ωf]

(5)

式中：v(tf)，a(tf)，ω(tf)是終止時刻參數(shù)。

1.4 代價函數(shù)

在實際情況下，由于AGV的定位誤差等原因，AGV并不一定能嚴(yán)格地到達期望的目標(biāo)位置，為了使AGV在終止時刻的位置[x(tf),y(tf),θ(tf)]盡可能地靠近期望的終端位置[xf,yf,θf]，則以x(tf)、y(tf)、θ(tf)分別與xf、yf、θf的差的平方和作為代價函數(shù)，即代價函數(shù)為：

(6)

1.5 最優(yōu)控制問題建立

綜合上述約束條件以及代價函數(shù)，AGV的避障軌跡優(yōu)化問題可以歸結(jié)為以J為代價函數(shù)，以車體質(zhì)心加速度α和車體質(zhì)心角速度ω為輸入，滿足運動約束、避碰約束、邊界約束的最優(yōu)控制問題，如式(7)所示：

(7)

2 軌跡規(guī)劃

2.1 基于改進A*算法的軌跡規(guī)劃

為獲取一個初始軌跡作為非線性規(guī)劃問題的可行初始解，運用一種改進A*算法來生成運動軌跡。與傳統(tǒng)的A*算法相比，改進A*算法在AGV二維運動平面的基礎(chǔ)上加入了時間維作為第三維度,形成軌跡空間。

改進A*算法建立了軌跡空間，然后將連續(xù)x-y-t狀態(tài)空間用有限網(wǎng)格表示，定義抽象空間中的每個網(wǎng)格為節(jié)點，指定起始節(jié)點A和目標(biāo)節(jié)點B，并標(biāo)記移動障礙物占據(jù)的節(jié)點。

與傳統(tǒng)A*算法一樣，改進A*算法也有2個函數(shù)，即g(s)和f(s)。g(s)衡量從起始節(jié)點A到s的路徑成本，而f(s)=g(s)+h(s)用來估計經(jīng)由s從起始節(jié)點A到目標(biāo)節(jié)點B的總成本，選取歐氏距離作為h(s)的代價函數(shù)，函數(shù)表示為：

(8)

式中：(xs,ys)代表當(dāng)前狀態(tài)節(jié)點的坐標(biāo)，(xB,yB)代表目標(biāo)點節(jié)點坐標(biāo)。

改進A*算法和傳統(tǒng)A*算法一樣，都有一個OPEN表，將起始節(jié)點A加入OPEN表中，并且將要展開的節(jié)點以f(s)的遞增序列記錄。一旦有效節(jié)點s被擴展，則將其從OPEN表中移除，并且將其所有未被擴展的有效子節(jié)點添加到OPEN表中，同時把這些有效子節(jié)點的父節(jié)點設(shè)為s，路徑成本則為：

f(s′)=g(s)+c(s,s′)+h(s′)

(9)

式中：c(s,s′)為計算相鄰兩級節(jié)點s和s′之間的代價。如果s′已經(jīng)在OPEN表中，則表明s′已經(jīng)預(yù)先得到f(s)的值，這種情況下令

f*(s′)=g(s)+c(s,s′)+h(s′)

(10)

如果f*(s′)

圖2 改進A*算法流程框圖

2.2 基于直接配點法的軌跡優(yōu)化

由改進A*算法搜索出的軌跡存在尖點，無法滿足AGV的運動學(xué)約束，因此需要進行軌跡優(yōu)化。針對軌跡優(yōu)化問題，采用數(shù)值方法求解前文所提出的最優(yōu)控制問題，采用直接配點法將最優(yōu)控制問題離散成一個多約束的非線性規(guī)劃(nonlinear programming，NLP)問題。

直接配點法(direct collocation method，DCM)最早由Hargraves等[18]提出，可以同時離散控制量和狀態(tài)變量。首先，整個時間過程將被分割成N個小段，分割后的每一小段端點稱為“節(jié)點”。對于節(jié)點之間狀態(tài)變量隨時間的變化關(guān)系，通常采用高斯-洛巴托多項式族來表示，由于在區(qū)間 [-1,1]上，Jacobian多項式是正交多項式族，因此配點依據(jù)具體的Jacobian多項式來選取。非線性規(guī)劃問題的優(yōu)化設(shè)計變量主要包括節(jié)點處的狀態(tài)變量、節(jié)點和配點處的控制變量。最后，在滿足各類約束的條件下對代價函數(shù)進行尋優(yōu)。

針對軌跡優(yōu)化問題，選用三階Simpson公式來擬合節(jié)點間狀態(tài)變量，具體描述如下：

1) 離散時間歷程，即：

t0=t1

(11)

用(X1,X2,X3,…,XN)表示時間點對應(yīng)的狀態(tài)變量，(U1,U2,U3,…,UN)表示時間點對應(yīng)的控制變量。

2) 參數(shù)化狀態(tài)變量與控制量。

用三次插值多項式來近似表示子區(qū)間上的狀態(tài)變量，即：

(12)

子區(qū)間[ti,ti+1]的邊界條件如式(13)(14)所示：

X(0)=Xi,X(1)=Xi+1

(13)

(14)

則：

(15)

式(15)中的多項式系數(shù)、區(qū)間端點狀態(tài)變量和其導(dǎo)數(shù)的代數(shù)方程組可以依據(jù)式(14)中的邊界條件建立，求解式(15)可得：

(16)

取子區(qū)間中點處(h=0.5)作為配點，將式(16)代入式(12)，可得：

(17)

采用分段線性函數(shù)來近似控制變量，設(shè)uj為某個區(qū)間控制變量的第j個值，則：

(18)

3) 計算配點的defect向量(該向量構(gòu)成了系統(tǒng)的非線性等式約束)。

為確保三次多項式能更好地擬合狀態(tài)變量的變化，應(yīng)使式(19)中defect向量的期望值無限逼近于零。

(19)

式中：M為子區(qū)間配點處的狀態(tài)方程計算的狀態(tài)變量導(dǎo)數(shù)，XM為多項式求得的狀態(tài)變量導(dǎo)數(shù)。

4) 求解非線性規(guī)劃問題。

求解代價函數(shù)J最優(yōu)解的非線性規(guī)劃問題需要滿足以下2個方面要求：① 滿足配點處約束，即defect=0；② 滿足原最優(yōu)控制問題的約束條件。設(shè)計變量y如式(20)所示。

(20)

軌跡優(yōu)化問題可轉(zhuǎn)化為式(21)中帶約束的非線性規(guī)劃問題，即：

(21)

式中：Q為代價函數(shù)；pk(y)為非線性規(guī)劃問題的不等式約束；qk(y)為等式約束。

綜上所述，最優(yōu)控制問題可以以式(21)的形式來表述，其中Q表示代價函數(shù)J；pk(y)表示運動約束、避碰約束；qk(y)表示兩點邊界約束；y表示x(t)，y(t)，θ(t)，v(t)，a(t)，ω(t)等變量。

離散后得到(2N+1)×4個狀態(tài)變量的離散值和(2N+1)×2個控制變量的離散值。離散化后的狀態(tài)變量和控制變量為：

(22)

將控制變量的約束條件施加于節(jié)點及配點的控制變量離散值處，則可得：

(23)

碰撞臨界約束轉(zhuǎn)化為節(jié)點處的狀態(tài)變量離散值的約束：

(xAGVk-xobs_ik)2+(yAGVk-yobs_ik)2≥(RAGV+Robs_i)2

k=0,1,2,…,N-1,N

(24)

將代價函數(shù)轉(zhuǎn)化為末端配置點處與期望位置的距離，即：

minJ=(xN-xf)2+(yN-yf)2+(θN-θf)2

(25)

最后，采用在求解非線性規(guī)劃問題過程中常見的內(nèi)點法來優(yōu)化求解。

基于直接配點法的優(yōu)化求解原理如圖3所示。

圖3 基于直接配點法的優(yōu)化求解原理示意圖

3 仿真結(jié)果

為了驗證所提出的軌跡規(guī)劃的有效性，在Matlab平臺上建立仿真算例，基本參數(shù)設(shè)置如表2所示。

表2 AGV參數(shù)

在倉儲環(huán)境中，存在的移動障礙物一般為除自身以外的其他AGV，設(shè)置所有AGV在40 m×10 m的區(qū)域上運行，運行時間tf設(shè)為40.0 s，AGV的起始位置與目標(biāo)位置分別設(shè)為(0,5)和(40,5)。在[0,tf]期間，每個移動障礙AGV的軌跡被設(shè)置為具有4個隨機節(jié)點的B樣條曲線，AGV的車身外接圓半徑設(shè)置為0.5 m。初始參數(shù)[x(0),y(0),θ(0),v(0),a(0),ω(0)]設(shè)置為[0,5,0,0,0,0]。

圖4為單障礙運動軌跡在XY平面內(nèi)的投影，圖5為多障礙運動軌跡在XY平面內(nèi)的投影，分別在單個和多個移動障礙物環(huán)境下，利用改進A*算法搜索生成的可行初始解，在障礙物軌跡投影平面內(nèi)的投影分別如圖6、7所示。

圖4 單障礙運動軌跡在XY平面內(nèi)的投影曲線

圖5 多障礙運動軌跡在XY平面內(nèi)的投影曲線

圖6 單障礙物軌跡投影平面內(nèi)的可行初始解投影曲線

圖7 多障礙物軌跡投影平面內(nèi)的可行初始解投影曲線

圖8為AGV在單障礙物環(huán)境下與障礙物圓心距離的變化曲線，圖9為AGV在多障礙物環(huán)境下與障礙物圓心距離的變化曲線。圖8中AGV與障礙物圓心的最小距離為3.19 m，圖9中AGV與障礙物圓心的最小距離為2.782 m，大于預(yù)設(shè)的安全距離1.2 m，表明改進A*算法生成的初始軌跡是可行的，驗證了改進A*算法在動態(tài)障礙物環(huán)境下可以有效地搜索出一條無碰軌跡。

圖8 AGV在單障礙物環(huán)境下與障礙物圓心距離的變化曲線

圖9 AGV在多障礙物環(huán)境下與障礙物圓心距離的變化曲線

圖10、11分別為單個障礙物環(huán)境下優(yōu)化解的X、Y坐標(biāo)的變化曲線，圖12、13分別為多個障礙物環(huán)境下優(yōu)化解的X、Y坐標(biāo)的變化曲線。如圖14、15所示，在單障礙物環(huán)境或者多障礙物環(huán)境下，優(yōu)化前后的軌跡大致上相同，但優(yōu)化后的軌跡更加平滑。

圖10 單障礙物環(huán)境下優(yōu)化解的X坐標(biāo)變化曲線

圖11 單障礙物環(huán)境下優(yōu)化解的Y坐標(biāo)變化曲線

由圖14、15可知，代價函數(shù)在每一時刻的梯度都是下降的且在結(jié)束時刻趨于平緩，因此優(yōu)化后的軌跡能在避開障礙物的同時向目標(biāo)點逼近。

圖12 多障礙物環(huán)境下優(yōu)化解的X坐標(biāo)變化曲線

圖13 多障礙物環(huán)境下優(yōu)化解的Y坐標(biāo)變化曲線

圖14 代價函數(shù)在多障礙物環(huán)境下的變化曲線

圖15 代價函數(shù)在單障礙物環(huán)境下的變化曲線

4 結(jié)論

將AGV在復(fù)雜動態(tài)障礙物環(huán)境下軌跡規(guī)劃問題轉(zhuǎn)化為最優(yōu)控制問題，將改進A*算法與直接配點法相結(jié)合進行軌跡規(guī)劃?；诟倪MA*算法進行搜索生成初始軌跡，結(jié)果表明，在單障礙物和多障礙物環(huán)境下，AGV外接圓和障礙物外接圓的最小圓心距分別為3.19 m和2.78 m，滿足本文預(yù)設(shè)的安全距離要求，因此利用改進A*算法能有效搜索出一條無碰的初始軌跡。然而，初始軌跡存在尖點，無法滿足AGV的運動學(xué)約束，因此，本文采用了直接配點法進行優(yōu)化。先對最優(yōu)控制問題中的變量進行離散，將最優(yōu)控制問題轉(zhuǎn)化為帶約束的非線性規(guī)劃問題，再利用非線性規(guī)劃問題求解中常見的內(nèi)點法尋優(yōu)。優(yōu)化結(jié)果表明，優(yōu)化后的軌跡在滿足運動學(xué)約束的情況下與初始軌跡基本一致。另外，代價函數(shù)在每一時刻的梯度都是下降的且在結(jié)束時刻趨于平緩，因此優(yōu)化后的軌跡能在避開障礙物的同時向目標(biāo)點逼近。本文軌跡規(guī)劃方法具有很強的應(yīng)用性，可適用于障礙物隨機移動的情況，對將來AGV在動態(tài)障礙物環(huán)境下的避障軌跡規(guī)劃研究有借鑒作用。