基于多AGV協(xié)同搬運的分布式PID隊形控制與保持策略

潘天宇，樓航飛，劉耀強(qiáng)

(1.中國電子科技集團(tuán)第二十八研究所，江蘇 南京 210016)

(2.中興通訊股份有限公司，江蘇 南京 210016)

隨著單臺自動導(dǎo)引運輸車(automated guided vehicle, AGV)在物流運輸中的應(yīng)用日益廣泛，多AGV協(xié)同搬運在工業(yè)生產(chǎn)中的需求也逐漸顯現(xiàn)。多AGV系統(tǒng)的自由組合不再被輸送部件的形狀、種類、質(zhì)量限制，從而進(jìn)一步提高了AGV單體小車的利用率。目前中國電子科技集團(tuán)第二十八研究所正在不斷拓展軍民用機(jī)動式裝備、特種車輛及方艙等系統(tǒng)裝備在搬運和運輸領(lǐng)域的應(yīng)用范圍，逐步探索多AGV協(xié)同應(yīng)用場景與大型物料搬運與制造的有效結(jié)合。

如何形成特定的協(xié)同隊形以及運動過程中的隊形保持與控制是多AGV協(xié)同搬運首先要考慮的問題。從以往關(guān)于隊形的文獻(xiàn)中可以總結(jié)出以下的控制策略。基于行為方式的編隊控制[1]：首先將機(jī)器人的整個運動分解為不同的子行為，在子行為設(shè)計中引入階梯控制取法，給予每個成員一定的響應(yīng)權(quán)重和相應(yīng)的響應(yīng)行為，從而提高隊形控制的容錯率。這種方式的適用范圍較小，很多情況下難以抽象出具體的數(shù)學(xué)模型。基于“長機(jī)-僚機(jī)”方式(也可稱為“Leader-Follower”“領(lǐng)航者-跟隨者”“主從”等方式)的編隊控制[2-4]：長機(jī)負(fù)責(zé)隊形的成形與運行路線的軌跡跟蹤，而從機(jī)通過對長機(jī)的航向速度、航向角和高度跟蹤從而實現(xiàn)自我調(diào)整的效果，達(dá)到保持編隊隊形的目的。這種方式應(yīng)用范圍較廣，簡單易于實現(xiàn)，且結(jié)構(gòu)穩(wěn)定可靠，但是只考慮到了隊形保持問題，卻忽略了環(huán)境的約束以及突發(fā)事件的避障。虛擬結(jié)構(gòu)隊形控制：文獻(xiàn)[5]、[6]根據(jù)目標(biāo)協(xié)同隊形和任務(wù)過程中的已知航路數(shù)據(jù)點位置規(guī)劃得到期望的虛擬隊形和路線，從而設(shè)計非線性航路跟隨制導(dǎo)率參數(shù)值，基于李雅普諾夫理論設(shè)計隊形誤差控制器，用來補(bǔ)償航跡切向隊形誤差，最后綜合制導(dǎo)率和控制器得到協(xié)同控制的運動指令。人工勢場法[7-8]：首先采用基于動態(tài)網(wǎng)絡(luò)拓?fù)涞木庩牱椒ǎ瑢⒆灾魇剿聶C(jī)器人(autonomous underwater vehicle，AUV)看作網(wǎng)絡(luò)的節(jié)點，對于同時存在的目標(biāo)和障礙區(qū)域建立勢場函數(shù)，使AUV完成在線規(guī)劃與集群避障。

在實際的應(yīng)用中，通常將其中的兩種或者多種控制方法結(jié)合使用，從而有效提升運行過程中隊形保持的效果。羅欽瀚等[9]在復(fù)雜空間環(huán)境背景下將三維地理信息轉(zhuǎn)換為二維坐標(biāo)圖，通過機(jī)器學(xué)習(xí)算法改進(jìn)A*算法尋找到協(xié)同最優(yōu)路徑；文獻(xiàn)[10]提出了基于虛擬結(jié)構(gòu)法和“Leader-follower”法的協(xié)同控制策略，前者將整個協(xié)同AGV協(xié)同系統(tǒng)退化為一個剛體，設(shè)計協(xié)同運動控制算法，后者通過主-從原理將AGV的隊形控制問題轉(zhuǎn)化為從車跟蹤主車的位置和方向問題，再分解到每個AGV單元得到各自的運動控制量；而文獻(xiàn)[11]則在以往的主-從隊形控制算法中添加模糊控制器進(jìn)行智能調(diào)節(jié)，將多機(jī)器魚系統(tǒng)的隊形控制問題轉(zhuǎn)化為具有反饋調(diào)節(jié)的跟蹤問題，當(dāng)從動系統(tǒng)(Follower)存在位置偏差時，通過實時的位置變化從而實現(xiàn)對速度的調(diào)整。

綜上可知，協(xié)同隊形控制算法大多應(yīng)用在飛行器與AUV的隊形設(shè)計上，而應(yīng)用在多AGV協(xié)同搬運的場景較少，尤其是多AGV小車以結(jié)構(gòu)化的隊形共同協(xié)作完成大型物件的運輸任務(wù)。本文針對多AGV協(xié)同搬運中的隊形控制和保持問題，設(shè)計多AGV協(xié)同搬運系統(tǒng)分布式PID(proportion-intergration-differentiation)隊形控制器，在協(xié)同隊形中的各AGV的物理狀態(tài)最終趨于一致的條件下，基于圖論和矩陣論理論的知識，得到多AGV協(xié)同系統(tǒng)一致性協(xié)議式。

1 多AGV協(xié)同搬運隊形一致性協(xié)議式

在AGV運動過程中，各AGV的狀態(tài)量之間的關(guān)系可以表示為：

(1)

式中：si(t),vi(t),ai(t)分別為AGV的位移量、速度量和加速度量；ui(t)為作用于AGV的控制輸入量。在隊形保持一致的過程中，最終目的就是在運動控制算法下的各AGV的位移、速度和加速度最后趨向一致，即表現(xiàn)形式為limt→∞|si(t)-sj(t)|= 0，limt→∞|vi(t)-vj(t)|=0，limt→∞|ai(t)-aj(t)|=0，其中i，j為鄰居關(guān)系。

模型假定：存在一個由N臺AGV組成的協(xié)同搬運系統(tǒng)。其中復(fù)數(shù)s1,s2,…,sn表示各AGV的位移狀態(tài)；v1,v2,…,vn表示各AGV的速度；a1,a2,…,an表示各AGV的加速度。則可以推演出協(xié)同系統(tǒng)下的一致性協(xié)議[12]：

km((vj(t)-vi(t))+kn((aj(t)-ai(t))]

(2)

式中：ai(t),vi(t),si(t)分別為AGVi在t時刻的加速度、速度與位移信息；aj(t),vj(t),sj(t)分別為AGVj在t時刻的加速度、速度與位移信息；wij為一個隨著i,j取值不同而不同的系數(shù)；ui(t)為控制輸入；kl,km,kn為需要求解的控制參數(shù)。則一致性協(xié)議用矩陣的方式表達(dá)為：

(3)

假定n個多AGV協(xié)同系統(tǒng)中的個體模型G(s)為：

(4)

式中：s為拉普拉斯變換中的復(fù)參變量。

參考文獻(xiàn)[13]對式(2)進(jìn)行拉普拉斯變換得到:

(5)

式中：U(s)為關(guān)于s的拉氏函數(shù)；zi，zj為在復(fù)頻域下的AGV物理狀態(tài)。

通過上述研究可知，使用一致性協(xié)議轉(zhuǎn)換以及拉普拉斯變換，能夠?qū)⒍郃GV協(xié)同的一致性問題轉(zhuǎn)化為分布式控制器的設(shè)計求解問題，即求解控制參數(shù)的穩(wěn)定域。

2 多AGV協(xié)同搬運系統(tǒng)分布式PID隊形保持器設(shè)計

2.1 復(fù)常系數(shù)線性方程的穩(wěn)定性判據(jù)

常見的復(fù)常系數(shù)線性系統(tǒng)方程為：

(6)

式中：y為自變量；t為時間；aj、bj為常實數(shù)，j=1,2,…,n；i為虛數(shù)單位。則對應(yīng)的特征方程為：

P(λ)=λn+(a1+ib1)λn-1+…+(an+ibn)=0

(7)

式中：P(λ)為關(guān)于λ的特征多項式；λn為關(guān)于λ的n階變量。

對于特征多項式P(λ)，整理可得：

Q(λ)=a1λn-1+ib2λn-2+a3λn-3+ib4λn-4+…+q

(8)

式中：q為常數(shù)項。

文獻(xiàn)[14]通過對特征方程式(7)的系數(shù)進(jìn)行實數(shù)算數(shù)四則運算的遞推公式，給出了復(fù)常系數(shù)線性系統(tǒng)的穩(wěn)定性判據(jù)，系統(tǒng)內(nèi)部參數(shù)ai和bi的數(shù)值的不同變化將直接影響系統(tǒng)的穩(wěn)定性。

由文獻(xiàn)[15]中的定理可得復(fù)常系數(shù)多項式特征方程P(λ)的根分布在復(fù)平面左半平面(Reλ<0)的充分必要條件：

(9)

式中：Ci都是正實數(shù)，Ki都是實數(shù)，i=1,2,3,…,n。

對式(6)和式(8)的系數(shù)進(jìn)行變量定義，令：

(10)

根據(jù)文獻(xiàn)[16]可以得到遞推公式為：

(11)

其中：

(12)

式中：ai,j，bi,j為式(8)變量λ多項式的實部和虛部；Ti為關(guān)于ai-2,1和ai-1,1的變量；Vi為關(guān)于bi,j的變量。

系統(tǒng)(式(6))漸近穩(wěn)定的充分必要條件是式(11)和式(12)成立，且Ti>0(i=1,2,…,n)，即Ti均大于0。該判據(jù)同樣是充分必要條件，反之若想得到系統(tǒng)不穩(wěn)定的結(jié)論時，只需要判定存在Ti≤0即可。

2.2 多AGV協(xié)同搬運系統(tǒng)分布式PID隊形保持器設(shè)計

AGV協(xié)同搬運系統(tǒng)的動力學(xué)模型為：

(13)

其中:i,j=1,2,3…,n；L=kl(sj(t)-si(t));M=km(vj(t)-vi(t));N=kn(aj(t)-ai(t))。

令

U(t)=[S(t)T,V(t)T,A(t)T]T

其中：

S(t)=[s1,s2,…,sn]T

V(t)=[v1,v2,…,vn]T

A(t)=[a1,a2,…,an]T

整理式(13)可以得到：

(14)

式中：In為單位矩陣；W為三階系統(tǒng)網(wǎng)絡(luò)拓?fù)鋵?yīng)的鄰接矩陣。簡化式(14)可得：

(15)

則式(15)的特征方程為：

det(sI3n-M)=0

(16)

式中：det為矩陣的行列式；I3n為三階單位矩陣。

將矩陣M代入式(16)可得：

(17)

對式(17)進(jìn)一步化簡求解可得：

det[s3In+A1M]=0

(18)

式中：A1=kns2+kms+kl。對矩陣M進(jìn)行對角化可以得到：

(19)

式中：λ1,λ2,…,λn∈C，為矩陣M的特征根；P的第j個列向量是矩陣M特征值λj對應(yīng)的特征值，P-1為P的逆矩陣。如此式(18)可以寫成：

det[s3In+A1P-1∧P]=0

(20)

(21)

得到式(21)的特征方程為：

(22)

由于特征方程(20)中的控制參數(shù)kl，km，kn的參數(shù)范圍在復(fù)數(shù)域內(nèi)，不能直接用實數(shù)求解的方法得出結(jié)果，因此需要根據(jù)1章中復(fù)常系數(shù)線性方程的穩(wěn)定性判據(jù)，得到系統(tǒng)穩(wěn)定的參數(shù)范圍。

3 多AGV協(xié)同搬運系統(tǒng)的隊形控制仿真實驗

圖1所示是AGV協(xié)同搬運系統(tǒng)的期望隊形，其中：○表示AGV，三角形為AGV期望隊形結(jié)構(gòu)。

圖1 AGV協(xié)同搬運系統(tǒng)期望隊形

圖1對應(yīng)的各AGV位置ζ為：

ζ=[0 2 1+i]T

(23)

選取的復(fù)數(shù)矩陣M如下：

(24)

因為rank(M)=2,所以所有的非零特征值為：

λ1=0.399 5-2.249 6i,λ2=3.600 5+0.249 6i

(25)

由式(15)得到對應(yīng)AGV系統(tǒng)個體模型(式(4))的特征方程為：

s3+(kns2+kms+kl)λi=0

(26)

令kn=kn1+ikn2,km=km1+ikm2,kl=kl1+ikl2,λi=λi1+iλi2。代入式(26)整理可得：

s3+[(kn1λi1-kn2λi2)+i(kn1λi2+λi1)]·s2+[(km1λi1-km2λi2)+i(km1λi2+km2λi1)]s+(kl1λi1-kl2λi2)+i(kl1λi2+λi1)=0

(27)

進(jìn)一步整理，令：

(28)

則式(27)可以簡化為：

s3+(a1+ib1)s2+(a2+ib2)s+a3+ib3=0

(29)

對式(6)和式(8)的系數(shù)進(jìn)行變量定義

(30)

根據(jù)文獻(xiàn)[16]得到的遞推公式如式(11)所示。

結(jié)合式(11)和式(12)可得：

(31)

根據(jù)2.1節(jié)復(fù)常系數(shù)線性系統(tǒng)穩(wěn)定性判據(jù)得到的系統(tǒng)穩(wěn)定充分必要條件是T1>0,T2>0,T3>0。將式(28)代入式(31)中，并滿足不等式均大于0的條件，由此可以得到控制參數(shù)kl，km，kn的不等式方程組：

(32)

式中：a1,1=(kn1λi1-kn2λi2)3·(km1λi1-km2λi2)-(kn1λi1-kn2λi2)2·(kl1λi1-kl2λi2)+(kn1λi1-kn2λi2)2(kn1λi2+kn2λi1)(km1λi2+km2λi1)-(kn1λi1-kn2λi2)·(km1λi2+km2λi1)2；b1,2=(kn1λi1-kn2λi2)3(kl1λi2+kl2λi1)-(kn1λi1-kn2λi2)2·(kn1λi2+kn2λi1)·(kl1λi1-kl2λi2)+(kn1λi1-kn2λi2)·(km1λi2+km2λi1)·(kl1λi1-kl2λi2)。

將得到的特征根λ1,λ2代入到式(32)中的第一個公式可以得到：

(33)

由不等式(32)能夠得到控制參數(shù)kn的實部kn1與虛部kn2的取值范圍。但由于計算量較大，在kn目標(biāo)范圍內(nèi)需要選擇kn的一個定值，代入不等式計算得到kl和km的數(shù)值參數(shù)，顯然這個結(jié)果范圍是包含在保持協(xié)同隊形穩(wěn)定的控制參數(shù)kl和km正確值范圍之內(nèi)。需要注意的是，當(dāng)kn的值發(fā)生變化時，另外兩個值kl和km也會隨之發(fā)生變化。這里選取kn=1,進(jìn)而得到kl，km的取值范圍，從而得到4個不等式和4個未知解，按照同樣的解法選擇范圍內(nèi)的一個值，不斷迭代，得到最終的結(jié)果。

首先，選取其中的一組解，即控制參數(shù)kl=1、km=-3、kn=1，則系統(tǒng)的特征方程為：

s3+(s2-3s+1)(0.399 5-2.249 6i)=0

(34)

s3+(s2-3s+1)(3.600 5+0.249 6i)=0

(35)

根據(jù)復(fù)常系數(shù)線性方程穩(wěn)定性判據(jù)的成立條件：

T1=0.654 9>0，T2=2.151 6>0，T3=5.674 5>0

T1=11.854 2>0，T2=565.16>0，T3=3 123>0

即滿足所有的Ti>0的充要條件，由此可知選取的參數(shù)kl=1、km=-3、kn=1能夠使AGV協(xié)同隊形保持穩(wěn)定。得到的協(xié)同AGV編隊軌跡仿真結(jié)果如圖2所示，其中各AGV從初始位置點[-0.5+i，3.5-0.75i，-0.5+3.5i]開始運動，最終形成了所期望三角形隊形，并以一定的速度保持該三角隊形持續(xù)運行下去。

圖2 協(xié)同AGV編隊軌跡仿真圖

圖3為各AGV沿著兩個方向分量的時間坐標(biāo)值曲線圖，可以看出各AGV雖然初始位置各不相同，但是在分布式PID隊形保持器的控制作用下，各方向的坐標(biāo)值很快趨于穩(wěn)定，并沿著相同的變化曲率持續(xù)運動。

圖3 協(xié)同AGV隊形的坐標(biāo)值變化曲線圖

隨后，選取控制參數(shù)kl=-1、km=-3、kn=2為滿足不等式(31)的另一組解，則系統(tǒng)的特征方程為：

s3-(s2+3s+2)(0.399 5-2.249 6i)=0

(36)

s3-(s2+3s+2)(3.600 5-0.249 6i)=0

(37)

代入到復(fù)常系數(shù)線性方程穩(wěn)定性判據(jù)的成立條件：

T1=0.654 9>0，T2=2.151 6>0，T3=-2.883<0

T1=11.62>0，T2=-96.581<0，T3=-0.997 5<0

即存在Ti<0的情況，不滿足協(xié)同AGV系統(tǒng)編隊穩(wěn)定的充要條件。得到的協(xié)同AGV編隊軌跡仿真結(jié)果如圖4所示，可以看出在初始位置相同的情況下，該組控制參數(shù)下協(xié)同系統(tǒng)無法形成期望隊形，從另一個角度證明了本文隊形控制協(xié)議的有效性。

圖4 協(xié)同AGV編隊軌跡仿真圖

4 結(jié)束語

本文針對多AGV協(xié)同搬運中的隊形控制問題，提出了一致性協(xié)議式，通過拉普拉斯變換，將協(xié)同搬運隊形設(shè)計轉(zhuǎn)變?yōu)閰f(xié)同分布式隊形保持器設(shè)計，得到了該類系統(tǒng)穩(wěn)定狀態(tài)下的控制參數(shù)的遞推公式和判據(jù)范圍，最后通過選取正反兩組參數(shù)值進(jìn)行了驗證。后續(xù)還需要進(jìn)一步的試驗支撐或根據(jù)試驗結(jié)論對協(xié)議式和遞推公式進(jìn)行完善。