代飛揚(yáng),周紹磊,肖支才,祁亞輝,劉玄冰
(海軍航空大學(xué),山東 煙臺 264000)
單架無人機(jī)(unmanned aerial vehicle,UAV)由于其續(xù)航時間短、活動半徑小及抗干擾能力差等固有缺點(diǎn),在執(zhí)行大型復(fù)雜任務(wù)時效率較差、成功率低[1]。相對而言,多無人機(jī)編隊能解決執(zhí)行大型復(fù)雜任務(wù)存在的時間、空間和任務(wù)層面上的沖突,從而大大提高任務(wù)效率[2]。協(xié)同控制技術(shù)是多無人機(jī)作戰(zhàn)系統(tǒng)實(shí)現(xiàn)戰(zhàn)斗力融合發(fā)展的基礎(chǔ),得到了廣泛的重視和發(fā)展,近年來涌現(xiàn)出豐富的研究成果。編隊控制作為協(xié)同控制的基礎(chǔ)性研究之一,也取得了長足的發(fā)展[3]。
現(xiàn)在編隊控制的主要方法有l(wèi)eader-follower法[4]、虛擬結(jié)構(gòu)法[5]、人工勢場法[6]、基于行為法、預(yù)測控制法[7]及基于一致性的編隊控制法[8]等方法。leader-follower法具有控制簡單、較為直觀以及便于誤差分析的優(yōu)勢。近年來,leader-follower法因其優(yōu)點(diǎn)越發(fā)成為編隊控制的基礎(chǔ)性方法,研究人員針對leader-follower法和其他控制方法的糅合應(yīng)用進(jìn)行了大量研究。如文獻(xiàn)[4]利用滑膜控制理論設(shè)計了一種分布式控制律,實(shí)現(xiàn)了非完整機(jī)器人編隊的領(lǐng)導(dǎo)跟隨控制,保證了機(jī)器人方向的有界性。但是leader-follower法具有基礎(chǔ)性缺陷,作為leader的無人機(jī)一旦不能正常運(yùn)行,將導(dǎo)致整個系統(tǒng)的失控,因此極少有l(wèi)eader-follower法的單獨(dú)應(yīng)用,一般以和其他控制方法復(fù)合應(yīng)用為主?;谛袨榉ㄔO(shè)計的控制器是一系列基本動作組成的合集,一般包括隊形生成、隊形保持、避障、聚集等,基于行為法能靈活地改變編隊隊形,針對不同場景適應(yīng)力較強(qiáng),同時可擴(kuò)展性強(qiáng),編隊可以進(jìn)一步擴(kuò)大,但是行為控制的模型復(fù)雜,難以用數(shù)學(xué)方法分析,同時對系統(tǒng)的穩(wěn)定性分析難度較大,這是基于行為法的缺點(diǎn)所在?;谝恢滦苑ň褪峭ㄟ^圖論中的圖來表示編隊個體之間的相互關(guān)系,通過圖論和控制理論對編隊控制問題進(jìn)行分析研究;基于一致法應(yīng)用的關(guān)鍵步驟為通過鄰接矩陣、Laplacian矩陣等進(jìn)行狀態(tài)變換或矩陣分解,將要研究的問題轉(zhuǎn)化為一致性問題,再選擇合適的一致性理論對問題進(jìn)行處理,在編隊控制的實(shí)際應(yīng)用中也有著廣泛應(yīng)用[9,10],該方法是下一階段編隊發(fā)展的重點(diǎn)方向。
根據(jù)多無人機(jī)編隊的內(nèi)部通信方式,一般將編隊控制分為集中式和分布式[11]。集中式控制是在編隊中選取一個或多個leader,通過leader對編隊中的每個成員發(fā)送指令,從而實(shí)現(xiàn)整個編隊的控制。分布式控制通過編隊中相鄰個體之間的信息交互實(shí)現(xiàn)控制目標(biāo),既能減少編隊的通信壓力,又能具有極強(qiáng)的容錯性和適應(yīng)性,因此分布式控制逐漸成為編隊控制的熱門控制策略[12],如文獻(xiàn)[13]提出了一種大規(guī)模固定翼無人機(jī)集群分組分層編隊控制的方案,將無人機(jī)分為2層,分別提出不同的控制律,實(shí)現(xiàn)了百架規(guī)模的無人機(jī)集群在控制約束和風(fēng)擾條件下的編隊控制,為大規(guī)模編隊的實(shí)際作戰(zhàn)應(yīng)用提供了一種實(shí)用解決方案。
相對旋翼無人機(jī),固定翼無人機(jī)有著速度快、載重大、作戰(zhàn)范圍大等特點(diǎn),對于大型復(fù)雜任務(wù)的成功執(zhí)行有著較大優(yōu)勢[14]。固定翼無人機(jī)是典型的具有非完整性約束的非線性運(yùn)動體,且存在最大/最小空速以及最大航向角速率等限制,其編隊控制問題更具有挑戰(zhàn)性[15]。
本文以采用leader-follower法的固定翼無人機(jī)編隊為研究對象,在編隊拓?fù)渚W(wǎng)絡(luò)中引入分布式結(jié)構(gòu),改善編隊通信拓?fù)浣Y(jié)構(gòu)。根據(jù)長機(jī)和僚機(jī)不同的任務(wù)要求設(shè)計不同的編隊控制器,并對僚機(jī)編隊控制器進(jìn)行優(yōu)化,在實(shí)際飛行中長機(jī)主要追蹤期望軌跡,僚機(jī)跟蹤長機(jī)的指定狀態(tài)以達(dá)成初步一致,并在此基礎(chǔ)上采用附加限幅PID方法實(shí)現(xiàn)精確編隊,該方法針對小型固定翼無人機(jī),具有易于實(shí)現(xiàn)、可擴(kuò)展性強(qiáng)等優(yōu)點(diǎn);并根據(jù)僚機(jī)速度大小和飛行路徑的變化,動態(tài)調(diào)整控制器中的期望距離參數(shù),以實(shí)現(xiàn)更精準(zhǔn)、平滑的控制;依靠拓?fù)渚W(wǎng)絡(luò)中的分布式結(jié)構(gòu)實(shí)現(xiàn)相鄰僚機(jī)的信息交互,以此實(shí)時比對僚機(jī)之間的相對距離,確定控制狀態(tài),適時切換常態(tài)控制方法和臨時避撞控制方法來實(shí)現(xiàn)無人機(jī)編隊的避撞控制。最后,針對不同的控制情景,通過數(shù)據(jù)仿真進(jìn)行模擬實(shí)驗(yàn),對控制算法和避撞策略的可行性和有效性進(jìn)行驗(yàn)證。
在leader-follower結(jié)構(gòu)的固定翼無人機(jī)編隊中,長機(jī)跟蹤預(yù)設(shè)軌跡飛行,僚機(jī)根據(jù)長機(jī)位置以及隊形要求獲得目標(biāo)位置,通過跟蹤目標(biāo)位置實(shí)現(xiàn)對長機(jī)的跟蹤控制,最終達(dá)到對長機(jī)的跟蹤控制從而實(shí)現(xiàn)編隊飛行。下面針對編隊實(shí)際需要分別建立數(shù)學(xué)模型和網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)。
首先對固定翼無人機(jī)的動力學(xué)特性和運(yùn)動學(xué)特性進(jìn)行分析,以建立合適的數(shù)學(xué)模型進(jìn)行算法分析。固定翼無人機(jī)的空間運(yùn)動可以分為質(zhì)心運(yùn)動和繞質(zhì)心的轉(zhuǎn)動2種,需要6個自由度;選用型號的固定翼無人機(jī)的操縱機(jī)構(gòu)包括副翼、方向舵、升降舵以及螺旋槳發(fā)電機(jī),并配有飛控系統(tǒng)。用無人機(jī)3個線坐標(biāo)和3個角坐標(biāo)來表示無人機(jī)的空間運(yùn)動,包括無人機(jī)質(zhì)心的位移(線運(yùn)動:飛行速度的增減運(yùn)動,升降運(yùn)動和側(cè)移運(yùn)動)和繞質(zhì)心的轉(zhuǎn)動(角運(yùn)動:俯仰角運(yùn)動、偏航角運(yùn)動和滾轉(zhuǎn)角運(yùn)動)。
慣性坐標(biāo)系采用北東地(NED)坐標(biāo)系。以長機(jī)航向方向?yàn)閄軸正向,X軸右向(俯視視角)為Y軸正向,Z軸在無人機(jī)的對稱平面上,與X軸垂直指向機(jī)身下方,以此建立機(jī)體坐標(biāo)系。將無人機(jī)的質(zhì)心運(yùn)動直接在機(jī)體坐標(biāo)系進(jìn)行投影,質(zhì)心運(yùn)動則從慣性坐標(biāo)系轉(zhuǎn)換到機(jī)體坐標(biāo)系,從而得出以下方程[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分別為機(jī)體坐標(biāo)系下沿縱軸、橫軸和豎軸的速度分量;φ、θ、φ分別為滾轉(zhuǎn)角、俯仰角、偏航角;p、q、r分別為滾轉(zhuǎn)、俯仰、偏航角速度;x、y、z分別為慣性坐標(biāo)系下的位置分量;對無人機(jī)的控制輸入而言;δT為固定翼無人機(jī)氣門大小的控制量;δe、δa、δr分別為升降舵偏角、副翼偏角和方向舵偏角。
固定翼無人機(jī)的動力學(xué)特性如式(3)、式(4)所示,主要在控制器的內(nèi)環(huán)控制考慮。
(3)
(4)
運(yùn)動學(xué)特性如式(5)、式(6)所示,主要在控制器的外環(huán)進(jìn)行控制。
(5)
(6)
針對本文具體情況對相關(guān)模型進(jìn)行簡化。
由飛行動力學(xué)可知,當(dāng)無人機(jī)在空中保持水平飛行或其高度變化比較平緩時,即θ≈0。在此條件下,無人機(jī)的垂直受力情況如圖1所示,F(xiàn)L為固定翼無人機(jī)在飛行過程中產(chǎn)生的升力。
根據(jù)圖1所示,此時側(cè)向加速度as和滾轉(zhuǎn)角φ的關(guān)系為:
mgtanφ=mas
(7)
圖1 無人機(jī)的垂直受力情況示意圖
本文在以下假設(shè)條件下進(jìn)行相關(guān)設(shè)計:
1) 無人機(jī)的高度基本保持穩(wěn)定,即θ≈0,q≈0,w=0。
在此條件下,由式(6)、式(7)可以得出:
(8)
(9)
在飛行過程中,固定翼無人機(jī)因其固有特性,上述參數(shù)都必須在一定范圍內(nèi)變化,以滿足機(jī)動性和飛行性能的限制要求。約束條件如下:
(10)
以4架固定翼無人機(jī)構(gòu)成的菱形編隊為研究對象,期望編隊隊形和編隊內(nèi)各無人機(jī)坐標(biāo)如圖2所示。
圖2 無人機(jī)編隊模型坐標(biāo)示意圖
無人機(jī)編隊在飛行中,使用通信網(wǎng)絡(luò)拓?fù)渲械耐ㄐ沛溌穼?shí)現(xiàn)信息交互,從而實(shí)現(xiàn)飛行任務(wù)。在飛行過程中,長機(jī)向僚機(jī)發(fā)送經(jīng)緯度、高度、速度、航向等信息,但并不獲取僚機(jī)信息,長機(jī)只需維持自身按預(yù)期航線飛行。僚機(jī)一方面獲取長機(jī)的位置和姿態(tài)信息,通過長機(jī)和僚機(jī)的相對距離確定自身期望位置;另一方面通過獲取相鄰僚機(jī)的位置信息以避免碰撞。本文在編隊內(nèi)采用的拓?fù)浣Y(jié)構(gòu)為強(qiáng)連通有向圖,具體如圖3所示。該拓?fù)浣Y(jié)構(gòu)的好處在于僚機(jī)之間可以通過僚機(jī)間的通信鏈路實(shí)現(xiàn)信息交互,克服了通過長機(jī)間接獲取僚機(jī)位置信息導(dǎo)致的傳遞迭代誤差,避免不必要的誤差修正過程。
圖3 無人機(jī)編隊拓?fù)浣Y(jié)構(gòu)示意圖
編隊控制可以解耦為內(nèi)環(huán)控制和外環(huán)控制,內(nèi)環(huán)控制確定姿態(tài),外環(huán)控制則驅(qū)動無人機(jī)到達(dá)期望的編隊位置。本文在控制律設(shè)計時主要針對外環(huán)控制,內(nèi)環(huán)控制只涉及到具體指令的補(bǔ)償改善。
選取菱形編隊中1號機(jī)作為編隊的長機(jī),其余無人機(jī)作為僚機(jī),分別對長機(jī)和僚機(jī)設(shè)計控制律。
長機(jī)以勻速按照預(yù)期航線進(jìn)行飛行,并實(shí)時將信息傳遞給編隊中的僚機(jī),因此長機(jī)只需保證能平滑地跟蹤預(yù)期航線即可。
根據(jù)需要設(shè)計長機(jī)控制律,長機(jī)能控制律的基本原理就是在目標(biāo)航線上找到一個與無人機(jī)相距距離L1的目標(biāo)點(diǎn),利用該目標(biāo)點(diǎn)牽引無人機(jī)飛行。具體方法是在目標(biāo)航線一側(cè)作一段圓弧與無人機(jī)速度向量相切,圓弧與目標(biāo)航線的交點(diǎn)為目標(biāo)點(diǎn),如圖4所示[19]。假設(shè)目標(biāo)航路點(diǎn)與長機(jī)的距離為L1,圓弧半徑為R1,長機(jī)與目標(biāo)航路點(diǎn)的方位角為ψ。
圖4 長機(jī)跟蹤模型目標(biāo)點(diǎn)示意圖
根據(jù)幾何關(guān)系可得:
(11)
將式(11)代入向心加速度公式可得[19]:
(12)
由式(5)可知,橫向加速度需要獲得長機(jī)航速v1與向量L1之間的夾角ψ1,計算如下:
(13)
通過文獻(xiàn)[7]可以對長機(jī)控制律的穩(wěn)定性進(jìn)行驗(yàn)證。
對于僚機(jī)而言,其目標(biāo)位置與實(shí)際位置之間的關(guān)系是通過長機(jī)的機(jī)體坐標(biāo)系確定的橫向相對距離和縱向相對距離來描述。機(jī)體坐標(biāo)系和慣性坐標(biāo)的關(guān)系如圖5所示。
圖5 僚機(jī)與長機(jī)位置關(guān)系示意圖
兩者坐標(biāo)的轉(zhuǎn)換關(guān)系(i=2、3、4)為:
(14)
從式(14)得,僚機(jī)與長機(jī)的實(shí)時相對距離為lXi=|Xi|;lYi=|Yi|(=2、3、4)。
根據(jù)長機(jī)坐標(biāo)和編隊任務(wù)需要,可得到長機(jī)與僚機(jī)的期望相對距離LXi、LYi。僚機(jī)的實(shí)時相對距離和期望相對距離的誤差ΔXi、ΔYi為:
(15)
每架僚機(jī)與長機(jī)的期望相對距離LXi、LYi固定,通過期望相對距離LXi、LYi和長機(jī)期望點(diǎn)P1實(shí)時確定僚機(jī)目標(biāo)點(diǎn)Pi,從而實(shí)時引導(dǎo)僚機(jī)跟蹤長機(jī)飛行,形成期望編隊隊形,僚機(jī)目標(biāo)點(diǎn)示意圖如圖6。
Li值是控制僚機(jī)跟蹤控制的關(guān)鍵參數(shù),是確定僚機(jī)目標(biāo)位置的距離參數(shù)。與長機(jī)不同,僚機(jī)的Li值不是一個指定的參數(shù),而是根據(jù)實(shí)時生成的僚機(jī)目標(biāo)點(diǎn)位置Pi和僚機(jī)的實(shí)時位置機(jī)計算所得。
由于僚機(jī)沒有固定的目標(biāo)點(diǎn),其飛行指令直接根據(jù)長機(jī)的飛行狀態(tài)來確定,長機(jī)與僚機(jī)的聯(lián)系最為緊密,且僚機(jī)的指令能根據(jù)長機(jī)的位置姿態(tài)變化而立刻做相應(yīng)變化,因此該方法的編隊效果較好。
圖6 僚機(jī)目標(biāo)點(diǎn)示意圖
當(dāng)僚機(jī)的期望位置與實(shí)際位置不一致時,通過控制僚機(jī)的飛行速度和橫向加速度是無人機(jī)以平滑軌跡向無人機(jī)向目標(biāo)點(diǎn)運(yùn)動。給定僚機(jī)的速度為:
vi=v1-k1ΔXi
(16)
式(16)中,k1為比例因子,一般為常數(shù)。
僚機(jī)對長機(jī)的跟蹤控制實(shí)質(zhì)上是對期望位置的跟蹤,與長機(jī)控制模式不同之處在于僚機(jī)是對動態(tài)期望位置的跟蹤,而長機(jī)是對于固定航線的跟蹤控制,因此取僚機(jī)的橫向加速度取為:
(17)
為了避免編隊中的無人機(jī)在飛行過程中相互碰撞,對僚機(jī)的控制方法增加避撞策略。僚機(jī)通過拓?fù)浣Y(jié)構(gòu)可知其余僚機(jī)的位置,將其余僚機(jī)所在區(qū)域整體簡化為以僚機(jī)為圓心的圓形威脅區(qū)域[20],通過避免飛過其余僚機(jī)所在的圓形威脅區(qū)域?qū)崿F(xiàn)避撞。
由于長機(jī)與僚機(jī)的理想相對距離為固定值,因此可以通過長機(jī)的位置推算出僚機(jī)的理想位置。僚機(jī)的動態(tài)理想位置構(gòu)成理想航線與威脅區(qū)域相交時,僚機(jī)進(jìn)入避撞控制;當(dāng)理想位置不與威脅區(qū)域相交時,退出避撞控制。根據(jù)編隊中長機(jī)和僚機(jī)的位置分別確定理想位置和避撞威脅區(qū)域,由于編隊中無人機(jī)位置是動態(tài)變化的,因此避撞控制的具體參數(shù)也在實(shí)時變化。這正是與固定航線避障策略的區(qū)別所在。避撞策略的基本流程如圖7所示。
圖7 僚機(jī)避撞策略的基本流程框圖
當(dāng)該僚機(jī)的期望位置開始與其余僚機(jī)所在的圓形威脅區(qū)域相接時,開始采取臨時避撞控制方法對僚機(jī)進(jìn)行控制,避撞飛行示意如圖8所示。避撞結(jié)束后,僚機(jī)采取常規(guī)方法對長機(jī)進(jìn)行跟蹤控制。
圖8 僚機(jī)避撞飛行示意圖
Rij為僚機(jī)之間的距離,計算公式為:
rj為其余僚機(jī)的威脅區(qū)域半徑,夾角γi和ηi計算公式為:
此時僚機(jī)速度vi取固定值v0,Li值取適宜的基礎(chǔ)值L0。橫向加速度ai為:
(18)
編隊的避撞飛行類似于對于圓弧形航跡的跟蹤控制,但是不同之處在于是對動態(tài)區(qū)域的跟蹤控制,而不是對于靜態(tài)圓弧形軌跡的跟蹤。僚機(jī)避撞分析如圖9所示。
圖9 僚機(jī)避撞分析示意圖
在如圖9所示的情景下,可以假設(shè):
η1≈0,η2≈0,η3?0
(19)
并在此時的條件下,可知ψi=η,η2為當(dāng)前航速方向和僚機(jī)j產(chǎn)生的圓形路線的切線之間產(chǎn)生的夾角。
角度之間的關(guān)系式為:
(20)
將式(19)代入到式(12)中,可得:
(21)
定義
(22)
又因?yàn)棣?的極小特性可知:
(23)
(24)
在式(19)條件下,可得:
(25)
因此可得:
(26)
在式(12)、式(19)和η=η1+η2+η3下,可得:
(27)
(28)
將式(26)和式(28)代入式(12)可得:
(29)
由于固定翼無人機(jī)的運(yùn)動學(xué)特性限制,取φmin=-30°,φmax=30°,vmin= 20 m/s,vmax=75 m/s。
由于在實(shí)際飛行中,存在一些難以消除的實(shí)際誤差,如無人機(jī)無法保持水平飛行、傳感器誤差或模型不準(zhǔn)確等,因此在算法應(yīng)用時需要對編隊中各無人機(jī)的橫向加速度指令進(jìn)行一定的誤差補(bǔ)償,具體為:
(30)
式(30)中,a1_last、ai_last分別為長機(jī)和僚機(jī)上一時刻的加速度指令。又由式(7)可得:
(31)
由于在實(shí)際飛行中俯仰角難于完全消除,俯仰角存在時俯仰角會偏小,因此對俯仰角進(jìn)行補(bǔ)償,即:
(32)
式(32)中,k4∈[1.0,2.0)。
下面對編隊的隊形保持進(jìn)行可行性實(shí)驗(yàn)。
初始條件參數(shù)如表1所示。
表1 初始條件參數(shù)
此時編隊中各無人機(jī)(UAV1~UAV4)的初始位置分別為(0,80)、(30,130)、(-20,130)(10,180),初始位置的編隊隊形即為期望編隊隊形,編隊中僚機(jī)的初始速度與長機(jī)相同,均以45 m/s。
首先令長機(jī)跟蹤一個邊長為1 000 m的正方形軌跡(如圖10中H0所示),編隊中各無人機(jī)的飛行軌跡如圖10所示。
圖10 編隊飛行軌跡示意圖
由圖10可知,無人機(jī)編隊在直線飛行中能實(shí)現(xiàn)良好的編隊保持,在協(xié)調(diào)轉(zhuǎn)彎過程中編隊個體間的相對距離會產(chǎn)生一定誤差,編隊隊形會變差,但在恢復(fù)直線飛行后會快速消除誤差,恢復(fù)理想隊形。
然后,對避撞策略的有效性和可行性進(jìn)行驗(yàn)證。取v1=v0=40 m/s,威脅區(qū)域半徑r2=r3=r4=30 m, UAV1~UAV4各自初始坐標(biāo)為(220,260)、(180,60)、(80,20)、(30,20),初始航向角為φ1=45°,φ2=30°,φ3=φ4=60°。此時模擬的情景是編隊中UAV2位置誤差較大,在跟蹤控制過程中如不進(jìn)行避撞控制,會與UAV3產(chǎn)生碰撞,具體情況如圖11、圖12所示。
圖11 編隊飛行軌跡曲線
圖12 編隊滾轉(zhuǎn)角變化曲線
圖11中,1、2、3時刻分別為開始避撞控制、結(jié)束避撞控制、飛行結(jié)束時刻的各無人機(jī)位置,分別在開始和結(jié)束避撞控制時刻以UAV2位置為圓心做圓,并同時顯示了UAV3不進(jìn)行了避撞控制的飛行路徑和避撞控制的飛行軌跡,證明了避撞控制的必要性。由圖11、圖12可以看出,UAV3通過避撞控制,避開了以UAV2為圓心的威脅區(qū)域,實(shí)現(xiàn)了避撞控制的目的,從而證明通過對編隊控制算法增加避撞策略可以有效地預(yù)防編隊飛行的碰撞風(fēng)險,并且在控制過程中滾轉(zhuǎn)角符合限幅要求,證明了本文方法的可行性。
1) 針對長機(jī)和僚機(jī)分別提出的編隊控制律基本完成跟蹤預(yù)定航線和跟蹤動態(tài)目標(biāo)的任務(wù),編隊在直線飛行中能實(shí)現(xiàn)良好的編隊隊形保持,在協(xié)調(diào)轉(zhuǎn)彎中隊形不能較好保持,但是在恢復(fù)直線飛行后能快速消除編隊誤差,基本實(shí)現(xiàn)了編隊協(xié)同控制。
2) 在編隊通信網(wǎng)絡(luò)引入了分布式結(jié)構(gòu),減少了編隊內(nèi)通信壓力,提高了編隊內(nèi)相鄰無人機(jī)的定位精度,并通過增加避撞策略解決了編隊內(nèi)無人機(jī)容易互相碰撞的問題,實(shí)現(xiàn)了編隊飛行過程中對威脅鄰機(jī)的實(shí)時規(guī)避,增強(qiáng)了編隊控制算法的實(shí)用性、可行性性和有效性,后續(xù)研究可以通過增加事件觸發(fā)機(jī)制減小通信壓力。
3) 控制算法內(nèi)具體參數(shù)的選擇及其精確度至關(guān)重要,編隊控制算法的精確度和可靠性還需進(jìn)一步提升。