基于一致性理論的多旋翼飛行器編隊(duì)控制方法

黎海青，苗昊春，張健松，張一杰

(1.西安現(xiàn)代控制技術(shù)研究所，陜西 西安 710065；2.華中科技大學(xué) 人工智能與自動(dòng)化學(xué)院，湖北 武漢 430074)

近年來(lái)，四旋翼飛行器因其體積小、可垂直起降和機(jī)動(dòng)靈活等優(yōu)點(diǎn)受到了廣泛關(guān)注。與單架飛行器相比，多架飛行器組成編隊(duì)協(xié)同執(zhí)行任務(wù)能提高工作效率并提高系統(tǒng)容錯(cuò)率，可以完成更復(fù)雜的任務(wù)，在通信中繼、搜救及偵察等方面有著廣泛的應(yīng)用[1-3]。

在多架飛行器協(xié)同工作時(shí)，機(jī)群需要組成合適的隊(duì)形。一方面，飛行器之間需要保持適當(dāng)?shù)拈g距以避免發(fā)生碰撞和離群等情況，另一方面，許多特定的任務(wù)，如通訊中繼、搜索救援等，需要飛行器組成特定的隊(duì)形。目前，已有許多飛行器編隊(duì)方法，如領(lǐng)導(dǎo)者-跟隨者方法[4]、行為策略方法[5]和虛擬結(jié)構(gòu)方法[6]等。文獻(xiàn)[7]研究了針對(duì)智能體的編隊(duì)控制，提出一種基于虛擬結(jié)構(gòu)的分布式編隊(duì)控制算法，并對(duì)該算法的穩(wěn)定性進(jìn)行了理論分析。文獻(xiàn)[8]針對(duì)多無(wú)人機(jī)系統(tǒng)建立虛擬剛體模型,實(shí)現(xiàn)了實(shí)時(shí)的人機(jī)交互，并在虛擬剛體坐標(biāo)系下研究了無(wú)人機(jī)的隊(duì)形變換和避障問(wèn)題[9]。這些方法既有其自身的優(yōu)勢(shì)，但也存在一些不足。領(lǐng)導(dǎo)者-跟隨者方法易于實(shí)現(xiàn)，但沒(méi)有跟隨者的信息反饋。因此，整個(gè)編隊(duì)的自主性不足，而且領(lǐng)導(dǎo)者的故障將可能導(dǎo)致整個(gè)編隊(duì)的失控。行為策略方法是一種基于仿生學(xué)的方法，其優(yōu)點(diǎn)是可以分布式實(shí)現(xiàn)，并可以擴(kuò)展到很大的集群，缺點(diǎn)則在于編隊(duì)的穩(wěn)定性無(wú)法保證，并且很難分析編隊(duì)的特征。虛擬結(jié)構(gòu)方法物理意義明確，具有良好的魯棒性，但該方法需要大量的計(jì)算能力和較寬的通訊帶寬，難以在工程中應(yīng)用。

一致性理論已被大量用于地面無(wú)人車(chē)和多衛(wèi)星系統(tǒng)在固定拓?fù)?、時(shí)變拓?fù)湟约巴ㄐ叛訒r(shí)等情況下的編隊(duì)控制。文獻(xiàn)[10-12]提出了一種基于一致性編隊(duì)控制方法，其基本思想是每個(gè)個(gè)體根據(jù)其相鄰個(gè)體的狀態(tài)更新其自身狀態(tài)，最終使所有個(gè)體的狀態(tài)收斂到共同的值。但是，上述文獻(xiàn)都將智能體描述成一階模型，系統(tǒng)較為簡(jiǎn)單。

針對(duì)以上研究存在的對(duì)象模型過(guò)于簡(jiǎn)單和穩(wěn)定性不足等問(wèn)題，將四旋翼飛行器描述為二階系統(tǒng)，擬提出基于二階一致性算法的多旋翼飛行器編隊(duì)控制方法。基于一致性的編隊(duì)策略，通過(guò)圖論的方法構(gòu)建了飛行器編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)，并利用有向圖表示集群中個(gè)體和個(gè)體之間的作用關(guān)系，設(shè)計(jì)了一種分布式編隊(duì)控制器。其中，位置控制器用于實(shí)現(xiàn)期望編隊(duì)軌跡的跟蹤并保持指定的編隊(duì)隊(duì)形，姿態(tài)控制器用于穩(wěn)定每架飛行器的姿態(tài)。最后，通過(guò)仿真驗(yàn)證了所設(shè)計(jì)的編隊(duì)控制方法的有效性和優(yōu)越性。

1 基于圖論的編隊(duì)結(jié)構(gòu)描述

考慮N架同構(gòu)的飛行器組成編隊(duì)，在編隊(duì)飛行中，每架飛行器除了完成自身的閉環(huán)控制外，還需與其他飛行器保持指定的距離。為了便于描述編隊(duì)中個(gè)體之間的通訊關(guān)系，用有向圖描述飛行器編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)。

將編隊(duì)中所有飛行器用一組節(jié)點(diǎn)集合V={v1,v2,…,vN}表示，節(jié)點(diǎn)vi表示飛行器i。飛行器j對(duì)飛行器i的影響，用有向圖中的一條邊(vi,vj)表示，編隊(duì)中所有的影響關(guān)系用一組邊集合E?{(vi,vj∶vi,vj∈V)}描述。令π={1,2,…,N}表示飛行器編號(hào)，各架飛行器之間的影響強(qiáng)度由加權(quán)矩陣W=[wij]∈RNN表示，若(vi,vj)∈E，則wij>0，表示飛行器j對(duì)飛行器i存在通訊關(guān)系。而當(dāng)飛行器j對(duì)飛行器i無(wú)通訊關(guān)系時(shí)，則wij=0。此外,對(duì)于任意i∈∏，wii=0。使用有向圖G=(V,E,W)描述飛行器編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)。在有向圖G中，如果存在一個(gè)邊的子集，且可連接有向圖中所有節(jié)點(diǎn)，那么稱(chēng)該圖包含一個(gè)有向生成樹(shù)。由文獻(xiàn)[13]可知，在時(shí)不變固定通信拓?fù)錀l件下，當(dāng)且僅當(dāng)通信拓?fù)浣Y(jié)構(gòu)存在一簇有向生成樹(shù)時(shí)，飛行器編隊(duì)可達(dá)到漸近一致。

2 旋翼飛行器模型

考慮所研究的飛行器是同構(gòu)的，以其中一架飛行器i為例，建立其平移和旋轉(zhuǎn)動(dòng)力學(xué)模型。

在四旋翼無(wú)人飛行器的模型當(dāng)中，假設(shè)定義α={Eαx,Eαy,Eαz}表示地球慣性的坐標(biāo)系，β={Eβx,Eβy,Eβz}表示原點(diǎn)在四旋翼質(zhì)心上的載體坐標(biāo)系，四旋翼飛行器模型如圖1所示。

圖1 四旋翼飛行器模型

令pi=[pxi,pyi,pzi]T∈R3×1和Θ=[φi,θi,ψi]T∈R3×1分別表示飛行器i的位置和姿態(tài)角。其中，φi、θi和ψi分別為滾轉(zhuǎn)角、俯仰角和偏航角。Rbi表示坐標(biāo)轉(zhuǎn)換矩陣，其表達(dá)式為

那么，飛行器i的平移和旋轉(zhuǎn)動(dòng)力學(xué)模型的表達(dá)式分別為

(1)

4個(gè)電機(jī)的控制輸入指令的表達(dá)式分別為

由于飛行器i具有6個(gè)自由度即高度、橫向和縱向位置以及3個(gè)姿態(tài)角和4個(gè)控制輸入即uzi,uΘ1,i,uΘ2,i,uΘ3,i，其是一種多輸入多輸出的欠驅(qū)動(dòng)系統(tǒng)。此外，可以看出，式(1)中描述的飛行器i的動(dòng)力學(xué)模型是高度非線性和耦合的。

3 編隊(duì)控制器設(shè)計(jì)

3.1 控制問(wèn)題描述

(2)

(3)

3.2 位置控制器設(shè)計(jì)

位置控制器用于實(shí)現(xiàn)期望編隊(duì)軌跡的跟蹤，并保持指定的編隊(duì)隊(duì)形。位置線性二次型調(diào)節(jié)器(Linear Quadratic Regulator，LQR)控制upi可以設(shè)計(jì)為

(4)

式中：N為飛行器個(gè)數(shù)；μP為耦合參數(shù)；對(duì)角矩陣Kp∈R3×3和Kv∈R3×3為位置LQR控制參數(shù)；αi是常數(shù)，取值范圍為0～2，表示虛擬編隊(duì)領(lǐng)導(dǎo)者和飛行器i之間的通訊權(quán)重，若αi>0，那么虛擬編隊(duì)領(lǐng)導(dǎo)者可以向飛行器i發(fā)送信息，否則αi=0。定義KL=[Kp,Kv]為位置LQR控制參數(shù)矩陣，令

其中，PL是Riccati方程的正定解，則有

3.3 姿態(tài)控制器設(shè)計(jì)

姿態(tài)LQR控制器輸出uΘi可以設(shè)計(jì)為

(5)

式中，KΘ,Kω∈R3×3是姿態(tài)LQR控制參數(shù)矩陣。KF=[KΘ,Kω]表示姿態(tài)LQR控制器參數(shù)。定義矩陣

式中，PF是Riccati方程的正定解，則有

通過(guò)基于圖論構(gòu)建的通信有向圖和上述編隊(duì)控制器設(shè)計(jì)方法可知，編隊(duì)軌跡的參考信號(hào)由虛擬領(lǐng)導(dǎo)者提供，只有部分的飛行器個(gè)體可以獲得參考信號(hào)。因此，整個(gè)位置控制器是分布式的和時(shí)不變的，意味著飛行器i的控制器設(shè)計(jì)僅取決于自身及其鄰居的位置和速度信息。

基于小增益理論[15]可以證明，整個(gè)閉環(huán)控制系統(tǒng)是漸進(jìn)穩(wěn)定的。

4 數(shù)字仿真

在數(shù)字仿真中，3架四旋翼的模型參數(shù)選擇為mi=2 kg,lbi=0.5,kσi=2.0,kτi=0.96,gi=9.81 m/s2及Ji=diag{0.1,0.1,0.06}kg·m2(i=1,2,3)，參數(shù)矩陣Bpi和BΘi的表達(dá)式分別為Bpi=diag(1,1,)和BΘi=diag{10.0,10.0,16.0}。

有向圖G=(V,E,W)用于描述3架四旋翼編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)。其中：V={v1,v2,v3},E={(v1,v2),(v2,v3)}，W=[wij]。w12=0.8,w23=0.7，若(vi,vj)?E，那么wij=0。只有第1架四旋翼可以從虛擬編隊(duì)領(lǐng)導(dǎo)者獲得信息。因此，α1=1.5,α2=0，α3=0，α4=0。四旋翼編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)及編隊(duì)隊(duì)形如圖2所示。

圖2 四旋翼編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)及編隊(duì)隊(duì)形

需要跟蹤的虛擬編隊(duì)領(lǐng)導(dǎo)者的期望軌跡為pr(t)=[3t,3t,4t]T，并且3個(gè)四旋翼需要保持一個(gè)恒定的三角形隊(duì)形，3個(gè)四旋翼之間的距離分別為δ1=[1,1,0]T,δ2=[-1,1,0]T，δ3=[-1,-1,0]T。此外，每架四旋翼的偏航角需要穩(wěn)定在0°。

位置和姿態(tài)的LQR控制參數(shù)選擇分別表示為QL=diag{8,8,8,1,1,1},ΓL=I3,ΓR=I3,QR=1 000×diag{2.3,2.3,2.3,1,1,1}，μp=3.5。

四旋翼編隊(duì)三維軌跡、X-Z視圖、X-Y視圖、位置跟蹤誤差、編隊(duì)速度曲線、編隊(duì)姿態(tài)曲線、位置控制器輸出、姿態(tài)控制器輸出如圖3至圖10所示。其中：圖3至圖5為四旋翼編隊(duì)位置軌跡圖，所采用的編隊(duì)控方法可以實(shí)現(xiàn)期望的編隊(duì)軌跡參考信號(hào)的跟蹤，全程保持指定的三角編隊(duì)隊(duì)形；圖6至圖7為位置跟蹤誤差及速度曲線，四旋翼編隊(duì)位置跟蹤速度較快，在2 s左右達(dá)到收斂，穩(wěn)態(tài)誤差小于0.2 m；圖8為編隊(duì)姿態(tài)曲線，在俯仰角和滾轉(zhuǎn)角在2 s左右收斂，偏航角穩(wěn)定保持在0°，跟蹤效果較好；圖9至圖10為位置控制器和姿態(tài)控制器輸出曲線，可見(jiàn)控制器輸出滿足要求。

圖3 四旋翼編隊(duì)三維軌跡

圖4 四旋翼編隊(duì)X-Z視圖

圖5 四旋翼編隊(duì)X-Y視圖

圖6 四旋翼編隊(duì)位置跟蹤誤差

圖7 四旋翼編隊(duì)速度曲線

圖8 四旋翼編隊(duì)姿態(tài)曲線

圖9 四旋翼位置控制器輸出

圖10 四旋翼姿態(tài)控制器輸出

5 結(jié)語(yǔ)

針對(duì)多架飛行器的編隊(duì)控制問(wèn)題，提出一種基于一致性理論的編隊(duì)控制算方法。利用圖論方法對(duì)整個(gè)系統(tǒng)進(jìn)行了建模，構(gòu)建了飛行器編隊(duì)的通訊拓?fù)浣Y(jié)構(gòu)，并以3架飛行器為例，設(shè)計(jì)了由位置LQR控制器和姿態(tài)LQR控制器組成的分布式控制器，實(shí)現(xiàn)了編隊(duì)飛行軌跡跟蹤與隊(duì)形保持任務(wù)。仿真結(jié)果表明，該控制方法能夠?qū)崿F(xiàn)多架四旋翼飛行器的編隊(duì)控制，清晰描述指定編隊(duì)狀態(tài)，飛行軌跡跟蹤和隊(duì)形保持精度較高，編隊(duì)控制算法有效。