拓撲切換下基于SO(3)的四旋翼時變編隊跟蹤控制

胡樹欣, 畢文豪, 李銘浩, 張 安

(西北工業大學航空學院,西安 710000)

0 引言

隨著單架無人機技術的發展,越來越多的任務可以通過單架無人機完成。為滿足越來越高的效率、性能和成本需求,多架無人機協同控制的集群系統框架已成為新的研究方向。集群系統的編隊控制獲得了廣泛關注和研究。通過編隊控制,集群系統可以執行協同定位[1]、載荷運輸[2]、偵察監視[3]、目標包圍[4]等多項協同任務。

文獻[5-10]中提出了一些具有代表性的編隊控制方法,包括領航-跟隨法、虛擬結構法和基于行為的編隊控制方法。文獻[11]中已經指出,以上3種傳統的編隊控制方法都存在一定的缺陷。對于無人機集群系統,采用分布式、魯棒的方式實現所期望的編隊,具有極大的工程意義。在過去的幾十年里,采用局部鄰居信息的控制方式所帶來的分布式和可擴展的特性,使得群系統一致性理論的研究出現了一個熱潮。由于群系統的群體控制也需要這些特征,研究者們開始從一致性理論的角度來處理群體控制問題,從而產生了基于一致性的群體控制方法。文獻[12]在分布式框架下,基于一致性理論設計控制律,實現了集群系統時變編隊控制,有效避免了Lur’e型非線性項在編隊形成過程中的影響,該控制協議同時減少了群系統內通信數量,降低了對通信系統的要求。文獻[13]基于一致性理論和線性矩陣不等式方法設計了編隊控制協議,有效解決了通信拓撲切換條件下的UAV集群系統路徑追蹤與時變編隊控制問題;同時,通過構造分段Lyapunov函數,驗證了當通信系統拓撲切換的駐留時間大于固定閾值時,集群系統閉環控制是收斂的。文獻[14]研究了集群系統在固定拓撲和切換拓撲下的軌跡跟蹤和編隊控制問題,在分析集群系統無控制輸入下靜態穩定的基礎上,基于一致性算法設計控制協議,實現了固定和切換拓撲下的軌跡追蹤和編隊控制;同時,驗證了當系統采樣間隔在滿足一定約束條件時,閉環系統是穩定的;最后通過仿真實驗驗證了所提算法的有效性。

基于歐拉角的姿態表示方法具有直觀形象、便于理解等特點,研究人員大多在此基礎上建立運動學模型和動力學模型?；诖嗽O計的控制器往往要求四旋翼無人機在飛行過程中俯仰角和滾轉角變化較小,這限制了飛行器的敏捷性。近幾十年來,基于幾何控制的理論方法獲得了快速發展,在四旋翼無人機控制應用中也取得了良好的控制效果。不同于傳統姿態(俯仰/滾轉/偏航)三通道模型,文獻[15]設計了SO(3)空間下多四旋翼無人機系統控制模型,研究了SO(3)空間內無人機間相對誤差的表示方法,并在此基礎上設計控制器;實驗表明在該控制算法下,各無人機可以更平滑、快速地形成編隊隊形和姿態的一致性。

本文在一致性理論和切換系統理論研究成果基礎上,重點解決有向通信拓撲切換條件下無人機時變編隊跟蹤控制問題。相比已有的研究成果,本文對集群系統間通信拓撲的要求更為普通,不但針對有向通信網絡,而且適用于隨時間切換通信拓撲的系統;采用SO(3)空間中的旋轉矩陣表征四旋翼無人機的姿態,在此基礎上設計姿態控制器,提高了編隊控制的收斂速度;構造了分布式性能指標來描述編隊控制的調節性能,通過分析證明了編隊誤差小于構造的Lyapunov函數的初值,滿足性能約束要求。

1 多四旋翼無人機系統建模及問題描述

1.1 SO(3)四旋翼無人機模型

四旋翼無人機模型及對應坐標系如圖1所示。將地面坐標系Oe-xeyeze表示為A,機體坐標系Ob-xbybzb表示為B。用坐標系A的軸來表示坐標系B的基底,坐標為xab,yab,zab∈R3。定義矩陣Rab=[xabyabzab]為從坐標系B到坐標系A的旋轉矩陣,對于坐標系B中的任意點qb,其在坐標系A中的坐標為qa=Rabqb。

圖1 四旋翼無人機模型及對應坐標系Fig.1 Model and coordinates of the quadrotor UAV

所有可能的旋轉矩陣形成R3的旋轉群,即SO(3),算式為

SO(3)={R∈R3×3|RRT=I3,det(R)=1}。

(1)

根據歐拉旋轉定理,SO(3)中的任意元素R都等價于繞著固定軸旋轉角度θ∈[0,2π)。R∈SO(3)的正則表示是指數坐標,即

(2)

式中,角速度ω=[ωxωyωz]T∈R3,且‖ω‖=1。定義hat映射為

(3)

定義vee映射為hat映射的逆運算,以∨進行表示,其代表將一個三維反對稱矩陣映射為三維向量。

本文采用“X”型結構的四旋翼無人機,每個螺旋槳產生的扭矩垂直于機架底座所在的平面,選擇局部北-東-地(NED)坐標系作為地面坐標系。

基于此模型,定義R∈SO(3)為從機體坐標系到地面坐標系的旋轉變換。姿態運動學和動力學方程為

(4)

考慮一個具有n架無人機的編隊系統,如果一架無人機沒有鄰居,則稱為領航無人機(leader UAV),否則稱為跟隨無人機(follower UAV)。在不失一般性的前提下,假設UAV 1是領航無人機,其余(n-1)架UAV是跟隨無人機。設F={2,…,n}代表跟隨無人機的集合。針對位置和速度外環,對leader UAV和follower UAV進行外環建模,即

(5)

(6)

其中:p1(t),v1(t)為領航UAV 1的位置和速度向量;pi(t),vi(t),ui(t)為跟隨UAVi(i∈F)的位置、速度和控制輸入向量;ap和av為已知常數。則領航UAV和跟隨UAV在狀態空間下的表達式為

(7)

1.2 圖論基礎

將無人機編隊系統的通信方式抽離出來,從而構成“通信圖”,用圖G=(V,E)來描述。圖G中的節點(node)i代表集群系統中的第i架無人機,圖G中所有節點的集合記為V={1,2,…,n};用(i,j)來表示邊,其含義是節點i能夠獲取節點j的狀態信息,此時稱節點j是節點i的鄰居,所有邊構成了集合E?V×V,其中,i∈V,j∈V。用Ni={j|(i,j)∈E}表示節點i的鄰居集。若無人機i與j之間可雙向通信,則圖G為無向圖。其拉普拉斯矩陣定義為L=D-A,D=diag({di}),表示其入度矩陣,di為向節點i傳輸信息的節點數目,形式為

(8)

A=[aij],表示其鄰接矩陣,aij為邊(i,j)的權值,

(9)

1.3 編隊問題描述及相關引理

1.3.1 編隊問題描述

任意UAVi都可以被看作是有向圖Gσ(t)中的一個節點vi。對于UAVi和UAVj,定義一條從UAVi到UAVj的通信通道為邊eij。對于每架無人機,姿態動力學的響應時間常數比位置環小得多。根據時間尺度分離原理,時變編隊跟蹤控制可以在內外環架構[16]中進行,其中姿態穩定在內環,外環跟蹤軌跡。雙環級聯跟蹤控制框架如圖2所示。

圖2 時變編隊跟蹤控制框架Fig.2 Time-varying formation tracking control framework

定義1對于任意給定的有界初始狀態,如果滿足

(10)

1.3.2 相關引理

假設1 對于每架跟隨UAVi(i∈F),在交互拓撲圖Gσ(t)中至少存在一條從領航UAV 1到它的路徑。

如果假設1成立,則可以得到圖Gσ(t)存在以領航UAV 1為根節點的生成樹。

引理1[18]當假設1成立時,Lσ(t)有一個零特征值,1n為其對應的右特征向量,其余n-1個特征值的實部均為正。

引理2[19]對于任意正定矩陣Φ1∈Rn×n和對稱矩陣Φ2∈Rn×n,可以證明

(11)

引理3[20]考慮SO(3)下的系統

(12)

式中:g∈SO(3);Vb為機體坐標系下的角速度。令Kp和Kd是對稱、正增益矩陣?？刂坡蒛為

U=-f(g,Vb)-Kplg(g)-KdVb。

(13)

若滿足

(14)

對于任意初始條件trg(0)≠-1,對所有Kp和Vb(0),狀態g∈SO(3)指數穩定。式中,λmin(Kp)為Kp的最小特征值。通過Lyapunov穩定性原理可以證明該控制律的穩定性。

2 編隊位置/速度控制設計

提出了有向拓撲切換下編隊控制器設計方法,并通過構造Lyapunov函數證明了編隊協同的穩定性。

2.1 控制器設計

對有向拓撲切換下的無人機集群系統,構建以下時變編隊控制律

(15)

式中:i∈F;K=[k11k12],表示常數增益矩陣。對于leader-follower拓撲結構下的多無人機系統,可以得到拉普拉斯矩陣Lσ(t)為

(16)

(17)

為了描述編隊系統式(7)跟蹤誤差的調節性能,構建以下性能指標函數

Q((xi(t)-hi(t))-(xj(t)-hj(t)))dt

(18)

式中:Q為對稱正定矩陣;Ni為拓撲圖Gσ(t)第i個節點的鄰居集合。等式(18)右邊第1項表示主體自身的性能函數,第2項表示主體之間的性能函數。

下面將設計控制器使無人機集群系統保持時變編隊hi(t),且J

(19)

(20)

(21)

將式(21)代入式(20)得到

(22)

由式(16)可得

(23)

由于B1=[10]T,B2=[01]T,所以

(24)

根據式(22)～(24),有

(25)

令

(26)

則

(27)

由引理1可以證明Lσ(t)1n=0,從而

(28)

由式(28)可得

(29)

ψ(t)=(T?I)ζ(t)

(30)

將式(30)代入式(27),同時將式(27)兩邊乘以T-1?I2,得到

(31)

(32)

(33)

下面給出了有向拓撲切換下的時變編隊跟蹤算法流程。

算法1:有向拓撲切換下時變編隊跟蹤算法。

Step 1 對于由hF(t)指定的跟隨無人機編隊隊形,考慮以下時變編隊跟蹤控制可行性條件

(34)

如果條件式(34)滿足,則繼續;否則停止,所期望的時變編隊對于多無人機系統式(7)是不可行的。

Step 2 對于給定的ε>0,通過

(35)

2.2 穩定性分析

證明 令

(36)

(37)

(38)

(39)

當滿足時變編隊跟蹤可行性條件式(34)時,

(40)

下面分析拓撲切換下系統的穩定性,

(41)

構造分段Lyapunov函數

(42)

在時間段t∈[tk,tk+1],k∈N內對應圖Gσ(t)是固定的。對V(t)進行求導,可以得到對于任意t∈[tk,tk+1],

(43)

(44)

則

(45)

由式(19)、式(35)和式(45)可得

(46)

(47)

(48)

由于圖在t∈[tk,tk+1)內是固定的,可以得到

V(t)

(49)

(50)

由式(49)和式(50)可得

(51)

V(t)<μe-ε(t-tk-1)V(tk-1)

(52)

使用遞歸的方法,由式(52)可得

V(t)<μke-ε(t-t0)V(t0)。

(53)

可以證明

t-t0=t-tk+(tk-tk-1)+…+(t1-t0)≥kτ0。

(54)

(55)

2.3 性能分析

下面考慮性能約束問題,由構建的性能指標函數式(18)可得

(56)

由式(35),式(56)轉換為

(57)

(58)

由式(57)和式(58)可得

(59)

根據V穩定收斂于0和V的正定性,可得

J

(60)

即得到性能上界。

3 姿態控制系統設計

姿態控制輸入是作用于無人機的力矩τ。為了推導出控制輸入,對于式(4)中的系統,構造如下Lyapunov函數

(61)

則穩定控制律為

τ=ωb×Jωb-Kp(lg(R))∨-Kdωb。

(62)

1) 角速度控制為

(63)

(64)

2) 角加速度控制為

(65)

(66)

增益PID參數滿足引理3的條件。雖然在SO(3)上存在一個trR=-1的奇點,但控制律不會退化。實驗表明,即使無人機在飛行過程中到達奇點,它仍然可以恢復到懸停位置。

盡管仍然稱SO(3)控制器為俯仰、滾轉和偏航控制,但其從根本上不同于傳統的控制器,主要表現在滿足SO(3)約束的情況下,通過耦合得到其參考角速度。因此,在SO(3)下無人機飛行軌跡更為平滑。角速度環三通道可以采用傳統PID算法分別進行控制,這是因為SO(3)是矢量空間,所以在這個空間上的運動是相互獨立的。

4 仿真分析

4.1 4機平面隊形仿真實驗

考慮一個由4架無人機組成的編隊系統,在三維空間中進行時變編隊飛行。其中,UAV 1是領航無人機,其余為跟隨無人機。

各無人機初始狀態如表1所示。

表1 各無人機初始狀態Table 1 The initial state of each UAV m

時變編隊隊形設置為

(67)

編隊系統切換有向拓撲結構圖和通信拓撲切換信號分別如圖3和圖4所示。使用算法1,增益矩陣K=I2?[-1.83-2.28],通過計算設置駐留時間τ0=10 s。

圖3 切換有向拓撲結構圖(4機)Fig.3 Directional topology switching (4 UAVs)

圖4 通信拓撲切換信號Fig.4 Communication topology switching signal

圖5為仿真中4架四旋翼無人機的三維位置軌跡,其中,無人機的初始位置用圓表示,領航無人機的目標位置用黑色三角形表示?？梢钥闯?隨著時間的推移,4架無人機形成了指定的時變編隊并可以保持穩定。

圖5 編隊各無人機位置軌跡Fig.5 The trajectory of each UAV in the formation

圖6～8分別為x,y,z方向上各無人機位置及跟蹤誤差曲線?？梢钥闯?10 s后,x,y方向上各無人機跟蹤誤差曲線逐漸趨于零,與編隊相對狀態趨于一致,1.3 s后,z方向各無人機跟蹤誤差均小于0.05 m,這些從側面反映了群系統實現了編隊形成,并可以保持穩定。

圖6 編隊各無人機x方向位置及跟蹤誤差曲線Fig.6 The x-direction position and tracking error curve of each UAV in the formation

圖7 編隊各無人機y方向位置及跟蹤誤差曲線Fig.7 The y-direction position and tracking error curve of each UAV in the formation

圖8 編隊各無人機z方向位置及跟蹤誤差曲線Fig.8 The z-direction position and tracking error curve of each UAV in the formation

文獻[13]、文獻[14]和本文控制方法下各無人機均能實現有向拓撲切換下時變編隊跟蹤控制。表2是3種控制方法下,無人機在x方向上對時變編隊的跟蹤誤差收斂時間。這里認為差值小于0.05 m時,編隊形成誤差趨于零。顯然,本文控制方法能夠使無人機集群系統更快地到達指定位置,即收斂速度更快。y方向具有相似的變化規律,這里略去。

表2 x方向各無人機跟蹤誤差收斂時間Table 2 Convergence time of the tracking error of each UAV in the x direction s

為驗證性能約束下時變編隊跟蹤控制,圖9給出了編隊飛行過程中實際消耗性能的函數曲線。

圖9 實際消耗性能函數曲線Fig.9 Actual consumption performance function curve

編隊系統實際消耗的性能指標由式(18)求解得到,由式(60)可得,性能約束的上界值即構造的Lyapunov函數的初值,J*=386.3。從圖9中可以看出,實際消耗的性能函數曲線逐漸收斂到有界值,同時低于性能上界值J*,滿足性能約束。

4.2 9機三維隊形仿真實驗

最后通過一個算例驗證該方法對集群系統三維時變編隊跟蹤控制的有效性?？紤]一個無人機集群系統,在三維空間中有9架四旋翼無人機,分別為8架跟隨無人機和1架領航無人機。

三維時變編隊隊形設置為

(68)

利用算法1,矩陣K同上,駐留時間為τ0=10 s。編隊系統切換有向拓撲結構如圖10所示,拓撲切換信號仍采用圖5所示形式。

圖10 切換有向拓撲結構圖(9機)Fig.10 Directional topology switching(9 UAVs)

選擇各無人機的初始位置(單位為m)為p1(0)=[0,0,0],p2(0)=[7,0,0],p3(0)=[8,1,0],p4(0)=[3,7,0],p5(0)=[-5,5,0],p6(0)=[-9,2,0],p7(0)=[-6,-7,0],p8(0)=[-1.2,-9,0],p9(0)=[9,-7,0],初始速度均為0 m/s。圖11給出了9架無人機在1 s,20 s,40 s和60 s時的狀態演化過程和編隊構型。

圖11 編隊系統狀態演化過程Fig.11 State evolution process of formation system

由圖11可以看出,在初始階段,8架跟隨無人機的構型為不規則圖形,隨著時間的推移,系統形成了指定的時變編隊并可以保持穩定。

圖12 編隊跟蹤誤差曲線Fig.12 Formation tracking error curve

5 結束語

本文研究了有向拓撲切換下基于SO(3)的多四旋翼無人機時變編隊跟蹤控制問題。僅利用相鄰無人機的相對信息,構造了受有向拓撲切換影響的控制協議,同時給出了集群系統實現期望時變編隊控制的充分條件,推導了編隊跟蹤可行性約束和駐留時間閾值。最后通過仿真實驗,驗證了算法的有效性。

本文控制方法對通信拓撲切換條件下無人機集群系統編隊控制具有一定的參考價值,至少在軌跡或編隊偏差不大時或者編隊保持過程中具有一定實用性和優越性。但該控制方法并未考慮輸入飽和的情況,當無人機初始軌跡或隊形偏差較大時,直接用該控制方法得到輸入量可能超過實際無人機的允許范圍,必須進行限幅。