固定翼無人機編隊協(xié)同控制算法及避撞研究

代飛揚，周紹磊，肖支才，祁亞輝，劉玄冰

(海軍航空大學，山東 煙臺 264000)

1 引言

單架無人機(unmanned aerial vehicle,UAV)由于其續(xù)航時間短、活動半徑小及抗干擾能力差等固有缺點，在執(zhí)行大型復雜任務時效率較差、成功率低[1]。相對而言，多無人機編隊能解決執(zhí)行大型復雜任務存在的時間、空間和任務層面上的沖突，從而大大提高任務效率[2]。協(xié)同控制技術是多無人機作戰(zhàn)系統(tǒng)實現(xiàn)戰(zhàn)斗力融合發(fā)展的基礎，得到了廣泛的重視和發(fā)展，近年來涌現(xiàn)出豐富的研究成果。編隊控制作為協(xié)同控制的基礎性研究之一，也取得了長足的發(fā)展[3]。

現(xiàn)在編隊控制的主要方法有l(wèi)eader-follower法[4]、虛擬結構法[5]、人工勢場法[6]、基于行為法、預測控制法[7]及基于一致性的編隊控制法[8]等方法。leader-follower法具有控制簡單、較為直觀以及便于誤差分析的優(yōu)勢。近年來，leader-follower法因其優(yōu)點越發(fā)成為編隊控制的基礎性方法，研究人員針對leader-follower法和其他控制方法的糅合應用進行了大量研究。如文獻[4]利用滑膜控制理論設計了一種分布式控制律，實現(xiàn)了非完整機器人編隊的領導跟隨控制，保證了機器人方向的有界性。但是leader-follower法具有基礎性缺陷，作為leader的無人機一旦不能正常運行，將導致整個系統(tǒng)的失控，因此極少有l(wèi)eader-follower法的單獨應用，一般以和其他控制方法復合應用為主?；谛袨榉ㄔO計的控制器是一系列基本動作組成的合集，一般包括隊形生成、隊形保持、避障、聚集等，基于行為法能靈活地改變編隊隊形，針對不同場景適應力較強，同時可擴展性強，編隊可以進一步擴大，但是行為控制的模型復雜，難以用數(shù)學方法分析，同時對系統(tǒng)的穩(wěn)定性分析難度較大，這是基于行為法的缺點所在?；谝恢滦苑ň褪峭ㄟ^圖論中的圖來表示編隊個體之間的相互關系，通過圖論和控制理論對編隊控制問題進行分析研究；基于一致法應用的關鍵步驟為通過鄰接矩陣、Laplacian矩陣等進行狀態(tài)變換或矩陣分解，將要研究的問題轉化為一致性問題，再選擇合適的一致性理論對問題進行處理，在編隊控制的實際應用中也有著廣泛應用[9,10]，該方法是下一階段編隊發(fā)展的重點方向。

根據(jù)多無人機編隊的內部通信方式，一般將編隊控制分為集中式和分布式[11]。集中式控制是在編隊中選取一個或多個leader，通過leader對編隊中的每個成員發(fā)送指令，從而實現(xiàn)整個編隊的控制。分布式控制通過編隊中相鄰個體之間的信息交互實現(xiàn)控制目標，既能減少編隊的通信壓力，又能具有極強的容錯性和適應性，因此分布式控制逐漸成為編隊控制的熱門控制策略[12]，如文獻[13]提出了一種大規(guī)模固定翼無人機集群分組分層編隊控制的方案，將無人機分為2層，分別提出不同的控制律，實現(xiàn)了百架規(guī)模的無人機集群在控制約束和風擾條件下的編隊控制，為大規(guī)模編隊的實際作戰(zhàn)應用提供了一種實用解決方案。

相對旋翼無人機，固定翼無人機有著速度快、載重大、作戰(zhàn)范圍大等特點，對于大型復雜任務的成功執(zhí)行有著較大優(yōu)勢[14]。固定翼無人機是典型的具有非完整性約束的非線性運動體，且存在最大/最小空速以及最大航向角速率等限制，其編隊控制問題更具有挑戰(zhàn)性[15]。

本文以采用leader-follower法的固定翼無人機編隊為研究對象，在編隊拓撲網(wǎng)絡中引入分布式結構，改善編隊通信拓撲結構。根據(jù)長機和僚機不同的任務要求設計不同的編隊控制器，并對僚機編隊控制器進行優(yōu)化，在實際飛行中長機主要追蹤期望軌跡，僚機跟蹤長機的指定狀態(tài)以達成初步一致，并在此基礎上采用附加限幅PID方法實現(xiàn)精確編隊，該方法針對小型固定翼無人機，具有易于實現(xiàn)、可擴展性強等優(yōu)點；并根據(jù)僚機速度大小和飛行路徑的變化，動態(tài)調整控制器中的期望距離參數(shù)，以實現(xiàn)更精準、平滑的控制；依靠拓撲網(wǎng)絡中的分布式結構實現(xiàn)相鄰僚機的信息交互，以此實時比對僚機之間的相對距離，確定控制狀態(tài)，適時切換常態(tài)控制方法和臨時避撞控制方法來實現(xiàn)無人機編隊的避撞控制。最后，針對不同的控制情景，通過數(shù)據(jù)仿真進行模擬實驗，對控制算法和避撞策略的可行性和有效性進行驗證。

2 編隊模型建立

在leader-follower結構的固定翼無人機編隊中，長機跟蹤預設軌跡飛行，僚機根據(jù)長機位置以及隊形要求獲得目標位置，通過跟蹤目標位置實現(xiàn)對長機的跟蹤控制，最終達到對長機的跟蹤控制從而實現(xiàn)編隊飛行。下面針對編隊實際需要分別建立數(shù)學模型和網(wǎng)絡拓撲結構。

2.1 數(shù)學模型建立

首先對固定翼無人機的動力學特性和運動學特性進行分析，以建立合適的數(shù)學模型進行算法分析。固定翼無人機的空間運動可以分為質心運動和繞質心的轉動2種，需要6個自由度；選用型號的固定翼無人機的操縱機構包括副翼、方向舵、升降舵以及螺旋槳發(fā)電機，并配有飛控系統(tǒng)。用無人機3個線坐標和3個角坐標來表示無人機的空間運動，包括無人機質心的位移(線運動：飛行速度的增減運動，升降運動和側移運動)和繞質心的轉動(角運動：俯仰角運動、偏航角運動和滾轉角運動)。

慣性坐標系采用北東地(NED)坐標系。以長機航向方向為X軸正向，X軸右向(俯視視角)為Y軸正向，Z軸在無人機的對稱平面上，與X軸垂直指向機身下方，以此建立機體坐標系。將無人機的質心運動直接在機體坐標系進行投影，質心運動則從慣性坐標系轉換到機體坐標系，從而得出以下方程[16, 17]。

狀態(tài)變量：

XT=[u,v,w,φ,θ,φ,p,q,r,x,y,z]

(1)

控制變量：

UT=[δT,δe,δa,δr]

(2)

式(1)～(2)中：u、v、w分別為機體坐標系下沿縱軸、橫軸和豎軸的速度分量；φ、θ、φ分別為滾轉角、俯仰角、偏航角；p、q、r分別為滾轉、俯仰、偏航角速度；x、y、z分別為慣性坐標系下的位置分量；對無人機的控制輸入而言；δT為固定翼無人機氣門大小的控制量；δe、δa、δr分別為升降舵偏角、副翼偏角和方向舵偏角。

固定翼無人機的動力學特性如式(3)、式(4)所示，主要在控制器的內環(huán)控制考慮。

(3)

(4)

運動學特性如式(5)、式(6)所示，主要在控制器的外環(huán)進行控制。

(5)

(6)

針對本文具體情況對相關模型進行簡化。

由飛行動力學可知，當無人機在空中保持水平飛行或其高度變化比較平緩時，即θ≈0。在此條件下，無人機的垂直受力情況如圖1所示，F(xiàn)L為固定翼無人機在飛行過程中產(chǎn)生的升力。

根據(jù)圖1所示，此時側向加速度as和滾轉角φ的關系為：

mgtanφ=mas

(7)

圖1 無人機的垂直受力情況示意圖

本文在以下假設條件下進行相關設計：

1) 無人機的高度基本保持穩(wěn)定，即θ≈0，q≈0，w=0。

在此條件下，由式(6)、式(7)可以得出：

(8)

(9)

在飛行過程中，固定翼無人機因其固有特性，上述參數(shù)都必須在一定范圍內變化，以滿足機動性和飛行性能的限制要求。約束條件如下：

(10)

以4架固定翼無人機構成的菱形編隊為研究對象，期望編隊隊形和編隊內各無人機坐標如圖2所示。

圖2 無人機編隊模型坐標示意圖

2.2 編隊網(wǎng)絡拓撲結構

無人機編隊在飛行中，使用通信網(wǎng)絡拓撲中的通信鏈路實現(xiàn)信息交互，從而實現(xiàn)飛行任務。在飛行過程中，長機向僚機發(fā)送經(jīng)緯度、高度、速度、航向等信息，但并不獲取僚機信息，長機只需維持自身按預期航線飛行。僚機一方面獲取長機的位置和姿態(tài)信息，通過長機和僚機的相對距離確定自身期望位置；另一方面通過獲取相鄰僚機的位置信息以避免碰撞。本文在編隊內采用的拓撲結構為強連通有向圖，具體如圖3所示。該拓撲結構的好處在于僚機之間可以通過僚機間的通信鏈路實現(xiàn)信息交互，克服了通過長機間接獲取僚機位置信息導致的傳遞迭代誤差，避免不必要的誤差修正過程。

圖3 無人機編隊拓撲結構示意圖

3 編隊控制律設計

編隊控制可以解耦為內環(huán)控制和外環(huán)控制，內環(huán)控制確定姿態(tài)，外環(huán)控制則驅動無人機到達期望的編隊位置。本文在控制律設計時主要針對外環(huán)控制，內環(huán)控制只涉及到具體指令的補償改善。

選取菱形編隊中1號機作為編隊的長機，其余無人機作為僚機，分別對長機和僚機設計控制律。

3.1 長機控制律設計

長機以勻速按照預期航線進行飛行，并實時將信息傳遞給編隊中的僚機，因此長機只需保證能平滑地跟蹤預期航線即可。

根據(jù)需要設計長機控制律，長機能控制律的基本原理就是在目標航線上找到一個與無人機相距距離L1的目標點，利用該目標點牽引無人機飛行。具體方法是在目標航線一側作一段圓弧與無人機速度向量相切，圓弧與目標航線的交點為目標點，如圖4所示[19]。假設目標航路點與長機的距離為L1，圓弧半徑為R1，長機與目標航路點的方位角為ψ。

圖4 長機跟蹤模型目標點示意圖

根據(jù)幾何關系可得：

(11)

將式(11)代入向心加速度公式可得[19]：

(12)

由式(5)可知，橫向加速度需要獲得長機航速v1與向量L1之間的夾角ψ1，計算如下：

(13)

通過文獻[7]可以對長機控制律的穩(wěn)定性進行驗證。

3.2 僚機控制律設計

對于僚機而言，其目標位置與實際位置之間的關系是通過長機的機體坐標系確定的橫向相對距離和縱向相對距離來描述。機體坐標系和慣性坐標的關系如圖5所示。

圖5 僚機與長機位置關系示意圖

兩者坐標的轉換關系(i=2、3、4)為：

(14)

從式(14)得，僚機與長機的實時相對距離為lXi=|Xi|；lYi=|Yi|(=2、3、4)。

根據(jù)長機坐標和編隊任務需要，可得到長機與僚機的期望相對距離LXi、LYi。僚機的實時相對距離和期望相對距離的誤差ΔXi、ΔYi為：

(15)

每架僚機與長機的期望相對距離LXi、LYi固定，通過期望相對距離LXi、LYi和長機期望點P1實時確定僚機目標點Pi，從而實時引導僚機跟蹤長機飛行，形成期望編隊隊形，僚機目標點示意圖如圖6。

Li值是控制僚機跟蹤控制的關鍵參數(shù)，是確定僚機目標位置的距離參數(shù)。與長機不同，僚機的Li值不是一個指定的參數(shù)，而是根據(jù)實時生成的僚機目標點位置Pi和僚機的實時位置機計算所得。

由于僚機沒有固定的目標點，其飛行指令直接根據(jù)長機的飛行狀態(tài)來確定，長機與僚機的聯(lián)系最為緊密，且僚機的指令能根據(jù)長機的位置姿態(tài)變化而立刻做相應變化，因此該方法的編隊效果較好。

圖6 僚機目標點示意圖

當僚機的期望位置與實際位置不一致時，通過控制僚機的飛行速度和橫向加速度是無人機以平滑軌跡向無人機向目標點運動。給定僚機的速度為：

vi=v1-k1ΔXi

(16)

式(16)中，k1為比例因子，一般為常數(shù)。

僚機對長機的跟蹤控制實質上是對期望位置的跟蹤，與長機控制模式不同之處在于僚機是對動態(tài)期望位置的跟蹤，而長機是對于固定航線的跟蹤控制，因此取僚機的橫向加速度取為：

(17)

3.3 避撞策略

為了避免編隊中的無人機在飛行過程中相互碰撞，對僚機的控制方法增加避撞策略。僚機通過拓撲結構可知其余僚機的位置，將其余僚機所在區(qū)域整體簡化為以僚機為圓心的圓形威脅區(qū)域[20]，通過避免飛過其余僚機所在的圓形威脅區(qū)域實現(xiàn)避撞。

由于長機與僚機的理想相對距離為固定值，因此可以通過長機的位置推算出僚機的理想位置。僚機的動態(tài)理想位置構成理想航線與威脅區(qū)域相交時，僚機進入避撞控制；當理想位置不與威脅區(qū)域相交時，退出避撞控制。根據(jù)編隊中長機和僚機的位置分別確定理想位置和避撞威脅區(qū)域，由于編隊中無人機位置是動態(tài)變化的，因此避撞控制的具體參數(shù)也在實時變化。這正是與固定航線避障策略的區(qū)別所在。避撞策略的基本流程如圖7所示。

圖7 僚機避撞策略的基本流程框圖

當該僚機的期望位置開始與其余僚機所在的圓形威脅區(qū)域相接時，開始采取臨時避撞控制方法對僚機進行控制，避撞飛行示意如圖8所示。避撞結束后，僚機采取常規(guī)方法對長機進行跟蹤控制。

圖8 僚機避撞飛行示意圖

Rij為僚機之間的距離，計算公式為：

rj為其余僚機的威脅區(qū)域半徑，夾角γi和ηi計算公式為：

此時僚機速度vi取固定值v0，Li值取適宜的基礎值L0。橫向加速度ai為：

(18)

編隊的避撞飛行類似于對于圓弧形航跡的跟蹤控制，但是不同之處在于是對動態(tài)區(qū)域的跟蹤控制，而不是對于靜態(tài)圓弧形軌跡的跟蹤。僚機避撞分析如圖9所示。

圖9 僚機避撞分析示意圖

在如圖9所示的情景下，可以假設：

η1≈0,η2≈0,η3?0

(19)

并在此時的條件下，可知ψi=η，η2為當前航速方向和僚機j產(chǎn)生的圓形路線的切線之間產(chǎn)生的夾角。

角度之間的關系式為：

(20)

將式(19)代入到式(12)中，可得：

(21)

定義

(22)

又因為η2的極小特性可知：

(23)

(24)

在式(19)條件下，可得：

(25)

因此可得：

(26)

在式(12)、式(19)和η=η1+η2+η3下，可得：

(27)

(28)

將式(26)和式(28)代入式(12)可得：

(29)

4 仿真驗證

由于固定翼無人機的運動學特性限制，取φmin=-30°，φmax=30°，vmin= 20 m/s，vmax=75 m/s。

由于在實際飛行中，存在一些難以消除的實際誤差，如無人機無法保持水平飛行、傳感器誤差或模型不準確等，因此在算法應用時需要對編隊中各無人機的橫向加速度指令進行一定的誤差補償，具體為：

(30)

式(30)中，a1_last、ai_last分別為長機和僚機上一時刻的加速度指令。又由式(7)可得：

(31)

由于在實際飛行中俯仰角難于完全消除，俯仰角存在時俯仰角會偏小，因此對俯仰角進行補償，即：

(32)

式(32)中，k4∈[1.0,2.0)。

下面對編隊的隊形保持進行可行性實驗。

初始條件參數(shù)如表1所示。

表1 初始條件參數(shù)

此時編隊中各無人機(UAV1～UAV4)的初始位置分別為(0,80)、(30,130)、(-20,130)(10,180)，初始位置的編隊隊形即為期望編隊隊形，編隊中僚機的初始速度與長機相同，均以45 m/s。

首先令長機跟蹤一個邊長為1 000 m的正方形軌跡(如圖10中H0所示)，編隊中各無人機的飛行軌跡如圖10所示。

圖10 編隊飛行軌跡示意圖

由圖10可知，無人機編隊在直線飛行中能實現(xiàn)良好的編隊保持，在協(xié)調轉彎過程中編隊個體間的相對距離會產(chǎn)生一定誤差，編隊隊形會變差，但在恢復直線飛行后會快速消除誤差，恢復理想隊形。

然后,對避撞策略的有效性和可行性進行驗證。取v1=v0=40 m/s，威脅區(qū)域半徑r2=r3=r4=30 m， UAV1～UAV4各自初始坐標為(220,260)、(180,60)、(80,20)、(30,20)，初始航向角為φ1=45°，φ2=30°，φ3=φ4=60°。此時模擬的情景是編隊中UAV2位置誤差較大，在跟蹤控制過程中如不進行避撞控制,會與UAV3產(chǎn)生碰撞，具體情況如圖11、圖12所示。

圖11 編隊飛行軌跡曲線

圖12 編隊滾轉角變化曲線

圖11中，1、2、3時刻分別為開始避撞控制、結束避撞控制、飛行結束時刻的各無人機位置，分別在開始和結束避撞控制時刻以UAV2位置為圓心做圓，并同時顯示了UAV3不進行了避撞控制的飛行路徑和避撞控制的飛行軌跡，證明了避撞控制的必要性。由圖11、圖12可以看出，UAV3通過避撞控制，避開了以UAV2為圓心的威脅區(qū)域，實現(xiàn)了避撞控制的目的，從而證明通過對編隊控制算法增加避撞策略可以有效地預防編隊飛行的碰撞風險，并且在控制過程中滾轉角符合限幅要求，證明了本文方法的可行性。

5 結論

1) 針對長機和僚機分別提出的編隊控制律基本完成跟蹤預定航線和跟蹤動態(tài)目標的任務，編隊在直線飛行中能實現(xiàn)良好的編隊隊形保持，在協(xié)調轉彎中隊形不能較好保持，但是在恢復直線飛行后能快速消除編隊誤差，基本實現(xiàn)了編隊協(xié)同控制。

2) 在編隊通信網(wǎng)絡引入了分布式結構，減少了編隊內通信壓力，提高了編隊內相鄰無人機的定位精度，并通過增加避撞策略解決了編隊內無人機容易互相碰撞的問題，實現(xiàn)了編隊飛行過程中對威脅鄰機的實時規(guī)避，增強了編隊控制算法的實用性、可行性性和有效性,后續(xù)研究可以通過增加事件觸發(fā)機制減小通信壓力。

3) 控制算法內具體參數(shù)的選擇及其精確度至關重要，編隊控制算法的精確度和可靠性還需進一步提升。