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

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

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

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

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

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

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

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

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

2 旋翼飛行器模型

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

在四旋翼無人飛行器的模型當(dāng)中，假設(shè)定義α={Eαx,Eαy,Eαz}表示地球慣性的坐標(biāo)系，β={Eβx,Eβy,Eβz}表示原點在四旋翼質(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)動力學(xué)模型的表達(dá)式分別為

(1)

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

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

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

3.1 控制問題描述

(2)

(3)

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

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

(4)

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

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

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

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

(5)

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

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

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

基于小增益理論[15]可以證明，整個閉環(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架四旋翼編隊的通訊拓?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架四旋翼可以從虛擬編隊領(lǐng)導(dǎo)者獲得信息。因此，α1=1.5,α2=0，α3=0，α4=0。四旋翼編隊的通訊拓?fù)浣Y(jié)構(gòu)及編隊隊形如圖2所示。

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

需要跟蹤的虛擬編隊領(lǐng)導(dǎo)者的期望軌跡為pr(t)=[3t,3t,4t]T，并且3個四旋翼需要保持一個恒定的三角形隊形，3個四旋翼之間的距離分別為δ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。

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

圖3 四旋翼編隊三維軌跡

圖4 四旋翼編隊X-Z視圖

圖5 四旋翼編隊X-Y視圖

圖6 四旋翼編隊位置跟蹤誤差

圖7 四旋翼編隊速度曲線

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

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

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

5 結(jié)語

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