動(dòng)態(tài)環(huán)境下AGV避障軌跡規(guī)劃問(wèn)題研究

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

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

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

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

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

1 避障描述

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

1.1 AGV運(yùn)動(dòng)學(xué)

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

(1)

圖1 AGV運(yùn)動(dòng)平面圖

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

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

在[0,tf]時(shí)間內(nèi)AGV移動(dòng)過(guò)程中，存在幾個(gè)邊界限制其移動(dòng)，運(yùn)動(dòng)約束如下：

(2)

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

1.2 避碰約束

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

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

(3)

1.3 邊界約束

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

(4)

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

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

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

(5)

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

1.4 代價(jià)函數(shù)

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

(6)

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

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

(7)

2 軌跡規(guī)劃

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

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

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

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

(8)

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

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

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

(9)

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

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

(10)

如果f*(s′)

圖2 改進(jìn)A*算法流程框圖

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

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

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

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

1) 離散時(shí)間歷程，即：

t0=t1

(11)

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

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

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

(12)

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

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

(13)

(14)

則：

(15)

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

(16)

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

(17)

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

(18)

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

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

(19)

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

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

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

(20)

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

(21)

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

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

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

(22)

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

(23)

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

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

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

(24)

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

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

(25)

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

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

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

3 仿真結(jié)果

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

表2 AGV參數(shù)

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

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

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

圖5 多障礙運(yùn)動(dòng)軌跡在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，表明改進(jìn)A*算法生成的初始軌跡是可行的，驗(yàn)證了改進(jìn)A*算法在動(dòng)態(tài)障礙物環(huán)境下可以有效地搜索出一條無(wú)碰軌跡。

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

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

圖10、11分別為單個(gè)障礙物環(huán)境下優(yōu)化解的X、Y坐標(biāo)的變化曲線，圖12、13分別為多個(gè)障礙物環(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可知，代價(jià)函數(shù)在每一時(shí)刻的梯度都是下降的且在結(jié)束時(shí)刻趨于平緩，因此優(yōu)化后的軌跡能在避開(kāi)障礙物的同時(shí)向目標(biāo)點(diǎn)逼近。

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

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

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

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

4 結(jié)論

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