一致性算法和領(lǐng)航跟隨法結(jié)合的多機(jī)編隊(duì)控制*

劉志江 宋 友 李國(guó)賓 屈 瑞 鄧 麗

1. 北京航空航天大學(xué)軟件學(xué)院，北京 100191 2. 中國(guó)科學(xué)院國(guó)家空間科學(xué)中心復(fù)雜航天系統(tǒng)電子信息技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100190

0 引言

無(wú)人機(jī)編隊(duì)飛行可以使得多臺(tái)無(wú)人機(jī)協(xié)作完成更復(fù)雜多樣的飛行任務(wù)，在軍事領(lǐng)域、民用領(lǐng)域、科研領(lǐng)域等各個(gè)領(lǐng)域都有廣泛應(yīng)用[1-2]，成為了近些年的熱門(mén)研究領(lǐng)域[3]。無(wú)人機(jī)編隊(duì)飛行控制技術(shù)主要有基于領(lǐng)航跟隨法的編隊(duì)飛行控制方法、基于行為的編隊(duì)飛行控制方法、基于虛擬結(jié)構(gòu)的編隊(duì)飛行控制方法、基于一致性算法的編隊(duì)飛行控制方法。

Radaideh等在有環(huán)境風(fēng)影響的條件下對(duì)領(lǐng)航跟隨法進(jìn)行了優(yōu)化[4]。Hao等基于組的層次結(jié)構(gòu)實(shí)現(xiàn)了大規(guī)模無(wú)人機(jī)編隊(duì)控制[5]。Chen等使用downwash模型研究了無(wú)人機(jī)編隊(duì)中無(wú)人機(jī)產(chǎn)生的氣流對(duì)相鄰無(wú)人機(jī)的影響[6]。Balch等最早提出了基于行為的編隊(duì)飛行控制算法[7]。Sun等通過(guò)引入虛擬電子為多智能體分布式集群控制提供了一個(gè)通用框架[8]。劉金龍等基于自適應(yīng)通信拓?fù)浣Y(jié)構(gòu)，設(shè)計(jì)了一種防碰撞的分布式協(xié)同控制器[9]。張令等通過(guò)模仿寒鴉配對(duì)飛行行為機(jī)制，減小了無(wú)人機(jī)編隊(duì)飛行控制的通信負(fù)載[10]。王平等提出了一種基于混合算法的多無(wú)人機(jī)航跡規(guī)劃方法[11]。Huang等提出了一種基于并行觸發(fā)機(jī)制的控制策略，以解決存在障礙物和受限條件下的飛行穩(wěn)定性問(wèn)題[12]。Askari等基于逆動(dòng)力學(xué)提出了一種新的基于虛擬結(jié)構(gòu)的編隊(duì)控制方法，適用于編隊(duì)中無(wú)人機(jī)數(shù)量較多的情況[13]。任偉等在2007年首次將一致性理論引入到多智能體編隊(duì)控制系統(tǒng)中[14]。田靜等基于一致性理論，提高了編隊(duì)系統(tǒng)通信能力的魯棒性[15]。董朝陽(yáng)等針對(duì)時(shí)變編隊(duì)問(wèn)題提出了抗干擾編隊(duì)控制[16]。

但是，上述編隊(duì)飛行控制方法都不能同時(shí)兼顧好多無(wú)人機(jī)在執(zhí)行編隊(duì)飛行任務(wù)時(shí)在時(shí)間、空間、姿態(tài)多維度的協(xié)同配合。多機(jī)協(xié)同探測(cè)的編隊(duì)飛行控制中，風(fēng)力等級(jí)、電磁環(huán)境和網(wǎng)絡(luò)延遲等因素都會(huì)對(duì)無(wú)人機(jī)編隊(duì)飛行過(guò)程中時(shí)間、空間、姿態(tài)的協(xié)同配合帶來(lái)影響。

本文采用一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法。通過(guò)多無(wú)人機(jī)開(kāi)展飛行實(shí)驗(yàn)，得到并分析無(wú)人機(jī)飛行數(shù)據(jù)，驗(yàn)證了采用一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法控制無(wú)人機(jī)執(zhí)行編隊(duì)飛行任務(wù)時(shí)在時(shí)間、空間、姿態(tài)的協(xié)同性上都滿足協(xié)作要求。使無(wú)人機(jī)在執(zhí)行多機(jī)協(xié)同探測(cè)編隊(duì)飛行任務(wù)時(shí)對(duì)準(zhǔn)角度誤差小于通信天線的通信波束角，為使用無(wú)人機(jī)模擬衛(wèi)星來(lái)測(cè)試星載有效載荷設(shè)備提供了支持。

1 多機(jī)協(xié)同探測(cè)的編隊(duì)飛行控制

1.1 編隊(duì)隊(duì)形和協(xié)作要求

為了測(cè)試星載有效載荷設(shè)備的性能，使用無(wú)人機(jī)模擬衛(wèi)星，攜帶星載有效載荷設(shè)備執(zhí)行編隊(duì)飛行任務(wù)。無(wú)人機(jī)從機(jī)攜帶的有效載荷設(shè)備對(duì)地面進(jìn)行探測(cè)的同時(shí)，與無(wú)人機(jī)主機(jī)攜帶的有效載荷設(shè)備進(jìn)行通信并將探測(cè)數(shù)據(jù)實(shí)時(shí)傳輸給主機(jī)攜帶的有效載荷設(shè)備。有效載荷設(shè)備上的窄波束通信天線，具備高速率的數(shù)據(jù)傳輸能力，但天線波束較窄，對(duì)通信天線對(duì)準(zhǔn)提出了高的要求。通信天線搭載在無(wú)人機(jī)上，因此進(jìn)一步對(duì)無(wú)人機(jī)在執(zhí)行編隊(duì)飛行任務(wù)過(guò)程中時(shí)間、空間、姿態(tài)上的協(xié)同配合提出了高的要求。

在空間維度，多臺(tái)無(wú)人機(jī)距離地面的高度相同，多機(jī)協(xié)同探測(cè)時(shí)以相同的角速度繞同一圓心以不同半徑進(jìn)行圓形繞飛。在姿態(tài)維度，無(wú)人機(jī)的機(jī)頭方向始終指向主機(jī)的機(jī)尾，對(duì)準(zhǔn)角度誤差不能超過(guò)無(wú)人機(jī)攜帶的窄波束通信天線的通信波束角。在時(shí)間維度，無(wú)人機(jī)到達(dá)協(xié)同探測(cè)開(kāi)始點(diǎn)后，同時(shí)開(kāi)始探測(cè)和無(wú)人機(jī)間的通信傳輸工作。圖1為無(wú)人機(jī)編隊(duì)進(jìn)行圓形繞飛的隊(duì)形設(shè)計(jì)圖。無(wú)人機(jī)A為主機(jī)，無(wú)人機(jī)B、C為從機(jī)。從機(jī)機(jī)頭方向?qū)崟r(shí)對(duì)準(zhǔn)主機(jī)的機(jī)尾，將探測(cè)數(shù)據(jù)傳輸至主機(jī)攜帶的有效載荷設(shè)備。無(wú)人機(jī)B、C的機(jī)頭方向和無(wú)人機(jī)A的機(jī)尾的對(duì)準(zhǔn)角度誤差需要小于有效載荷設(shè)備的通信波束角(6°)。無(wú)人機(jī)的運(yùn)動(dòng)軌跡為：

圖1 隊(duì)形設(shè)計(jì)圖

(1)

式中，x,y,z是無(wú)人機(jī)在慣性坐標(biāo)系下的坐標(biāo)，ri是無(wú)人機(jī)的繞飛半徑，h是無(wú)人機(jī)的繞飛高度，α是無(wú)人機(jī)繞飛角速度，t是時(shí)間。

為實(shí)現(xiàn)無(wú)人機(jī)編隊(duì)飛行控制，本文采用了一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法。具體流程如圖2所示，以無(wú)人機(jī)A為示例，飛行初始調(diào)整時(shí)，無(wú)人機(jī)飛行路徑為A0→A1→A2→A3。首先從地面出發(fā)點(diǎn)A0垂直上升至A1點(diǎn)(20 m高度)。而后，飛向A2點(diǎn)，在A2點(diǎn)旋轉(zhuǎn)調(diào)姿，使得無(wú)人機(jī)機(jī)頭指向繞飛圓心；而后保持該姿態(tài)指向到達(dá)協(xié)同探測(cè)的開(kāi)始點(diǎn)A3。無(wú)人機(jī)姿態(tài)變化如圖2中俯視圖所示。以此類(lèi)推，無(wú)人機(jī)B、C同時(shí)完成飛行初始調(diào)整。

圖2 編隊(duì)隊(duì)形組織策略流程圖

多機(jī)協(xié)同探測(cè)時(shí)，所有無(wú)人機(jī)收到執(zhí)行任務(wù)的控制指令后，以相同的繞飛圓心為中心進(jìn)行圓形繞飛。繞飛時(shí)，無(wú)人機(jī)A是領(lǐng)航機(jī)，無(wú)人機(jī)B和C是跟隨機(jī)。在保持繞飛高度、半徑不變的同時(shí)，跟隨機(jī)B和C始終保持在繞飛圓心O和領(lǐng)航機(jī)A組成的以O(shè)為頂點(diǎn)的射線上。本階段任務(wù)要求無(wú)人機(jī)B和C窄波束通信天線(機(jī)頭方向安裝)實(shí)時(shí)指向無(wú)人機(jī)A攜帶的窄波束通信天線(機(jī)尾方向安裝)，在探測(cè)的同時(shí)將數(shù)據(jù)實(shí)時(shí)傳輸至無(wú)人機(jī)A攜帶的有效載荷設(shè)備，即對(duì)準(zhǔn)角度誤差小于通信波束角，需要無(wú)人機(jī)編隊(duì)在時(shí)間、空間、姿態(tài)上的協(xié)同配合。

任務(wù)完成返航時(shí)，無(wú)人機(jī)接收控制系統(tǒng)的返航指令，按照A3→A1→A0的飛行路徑安全返航。

1.2 無(wú)人機(jī)運(yùn)動(dòng)學(xué)模型

假設(shè)p∈R3是無(wú)人機(jī)在慣性坐標(biāo)系下的位置，v∈R3是無(wú)人機(jī)在慣性坐標(biāo)系下的速度，φ,θ,ψ是無(wú)人機(jī)在機(jī)體坐標(biāo)系下的歐拉角。R∈R3×3是機(jī)體坐標(biāo)系轉(zhuǎn)換到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣

(2)

式中，s表示正弦函數(shù)sin(·)，c表示余弦函數(shù)cos(·)。無(wú)人機(jī)的運(yùn)動(dòng)學(xué)模型為：

(3)

式中，e3=(0 0 1)T，m是無(wú)人機(jī)的質(zhì)量，ω=(ω1ω2ω3)T∈R3是以無(wú)人機(jī)機(jī)體為參考的機(jī)體坐標(biāo)系下姿態(tài)變化角速度，g是重力加速度，f是無(wú)人機(jī)的旋翼產(chǎn)生的升力的和，S(ω)是ω的反對(duì)稱(chēng)矩陣。

(4)

1.3 控制器設(shè)計(jì)

設(shè)編隊(duì)中無(wú)人機(jī)數(shù)量為N。針對(duì)式(3)所述的無(wú)人機(jī)運(yùn)動(dòng)學(xué)模型，設(shè)計(jì)控制輸入μ(t)=[μ1(t)μ2(t) …μN(yùn)(t)]T，μi(t)是無(wú)人機(jī)i的控制輸入，i=1,2,…,N。一致性算法公式為：

(5)

對(duì)于N架無(wú)人機(jī)組成的編隊(duì)，使用G表示其通信拓?fù)浣Y(jié)構(gòu)，使用L∈RN×N表示G的拉普拉斯矩陣。lij是L的元素。

(6)

關(guān)于參數(shù)γ，當(dāng)-L的所有N-1個(gè)非零特征值均為負(fù)實(shí)部時(shí)

(7)

否則

(8)

2 無(wú)人機(jī)編隊(duì)飛行控制系統(tǒng)

基于一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法研發(fā)的無(wú)人機(jī)協(xié)同編隊(duì)飛行控制系統(tǒng)由電腦總控端和手持端兩部分組成，電腦總控端和手持端通過(guò)無(wú)線網(wǎng)絡(luò)通信，手持端通過(guò)無(wú)線電波直接控制與其連接的無(wú)人機(jī)。手持端的主要功能是向電腦總控端轉(zhuǎn)發(fā)無(wú)人機(jī)的控制指令，接收無(wú)人機(jī)狀態(tài)數(shù)據(jù)并向總控端轉(zhuǎn)發(fā)信息?？偪囟藢?duì)所有無(wú)人機(jī)進(jìn)行編隊(duì)控制。涉及的指令和數(shù)據(jù)交互過(guò)程如下。

指令交互過(guò)程包括9個(gè)環(huán)節(jié)、17次指令交互，如圖3所示。全部9個(gè)環(huán)節(jié)分別是：1)總控端上傳編隊(duì)飛行任務(wù)參數(shù)配置文件。2)總控端→手持端→無(wú)人機(jī)，發(fā)送編隊(duì)飛行任務(wù)參數(shù)配置，內(nèi)容包括探測(cè)起始點(diǎn)(經(jīng)度、緯度、高度)、繞飛圓心(經(jīng)度、緯度)、繞飛半徑、繞飛角速度、繞飛圈數(shù)等。3)無(wú)人機(jī)→手持端→總控端，反饋編隊(duì)飛行任務(wù)參數(shù)配置成功信息。4)總控端→手持端→無(wú)人機(jī)，若所有無(wú)人機(jī)任務(wù)參數(shù)均配置成功，發(fā)送“開(kāi)始執(zhí)行”指令。若出現(xiàn)部分無(wú)人機(jī)狀態(tài)反饋異常，可選擇對(duì)該無(wú)人機(jī)重新發(fā)送編隊(duì)飛行任務(wù)參數(shù)配置，或選擇部分無(wú)人機(jī)繼續(xù)執(zhí)行編隊(duì)飛行任務(wù)，或停止任務(wù)。5)無(wú)人機(jī)→手持端→總控端，各無(wú)人機(jī)到達(dá)協(xié)同探測(cè)的開(kāi)始點(diǎn)后，發(fā)送“到達(dá)協(xié)同探測(cè)起始點(diǎn)”指令?？偪囟说却袩o(wú)人機(jī)均已到達(dá)協(xié)同起始點(diǎn)，任務(wù)就緒。6)總控端→手持端→無(wú)人機(jī)，發(fā)送“執(zhí)行任務(wù)”指令，無(wú)人機(jī)開(kāi)始執(zhí)行編隊(duì)飛行任務(wù)，并實(shí)時(shí)將其狀態(tài)數(shù)據(jù)反饋給總控端。7)無(wú)人機(jī)→手持端→總控端，各無(wú)人機(jī)完成編隊(duì)飛行任務(wù)后，發(fā)送“任務(wù)完成”指令至總控端。8)總控端→手持端→無(wú)人機(jī)，總控端待收到“任務(wù)完成”指令后，發(fā)送“返航”指令到各無(wú)人機(jī)。若無(wú)人機(jī)在飛行過(guò)程中經(jīng)狀態(tài)數(shù)據(jù)判斷，出現(xiàn)異常，總控端也可以隨時(shí)向其發(fā)送“結(jié)束任務(wù)，返航”指令。9)無(wú)人機(jī)→手持端→總控端，無(wú)人機(jī)成功返航至指定點(diǎn)后，發(fā)送無(wú)人機(jī)“已返航”指令至總控端，編隊(duì)飛行任務(wù)結(jié)束。

圖3 指令及數(shù)據(jù)交互過(guò)程圖

數(shù)據(jù)交互過(guò)程主要為無(wú)人機(jī)傳輸至手持端的無(wú)人機(jī)狀態(tài)數(shù)據(jù)，和手持端轉(zhuǎn)發(fā)至主控端的無(wú)人機(jī)狀態(tài)數(shù)據(jù)。無(wú)人機(jī)狀態(tài)數(shù)據(jù)具體包括無(wú)人機(jī)的時(shí)間、位置(經(jīng)度、緯度、高度)、姿態(tài)(俯仰角、偏航角、橫滾角)、速度、飛行模式、任務(wù)執(zhí)行狀態(tài)。多臺(tái)無(wú)人機(jī)的狀態(tài)數(shù)據(jù)均通過(guò)無(wú)線電波傳輸至手持端，再由手持端通過(guò)無(wú)線網(wǎng)絡(luò)傳輸至總控端，在總控端實(shí)時(shí)顯示各無(wú)人機(jī)狀態(tài)數(shù)據(jù)，繪制無(wú)人機(jī)實(shí)時(shí)飛行軌跡3D圖和2D圖，并保存任務(wù)數(shù)據(jù)。

上述實(shí)施過(guò)程，采用一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法，為多無(wú)人機(jī)執(zhí)行編隊(duì)飛行任務(wù)時(shí)在時(shí)間、空間、姿態(tài)多維度協(xié)同配合提供了保障。

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)設(shè)置

在完成仿真實(shí)驗(yàn)后，進(jìn)行了外場(chǎng)飛行實(shí)驗(yàn)。實(shí)驗(yàn)使用3臺(tái)如圖4所示的大疆M600 Pro無(wú)人機(jī)，分別攜帶探測(cè)載荷低頻干涉成像譜儀，進(jìn)行編隊(duì)飛行，完成干涉成像實(shí)驗(yàn)。編隊(duì)需求：3臺(tái)無(wú)人機(jī)繞圓心飛行，保持無(wú)人機(jī)A-B-C實(shí)時(shí)共線，且A-B、A-C的對(duì)準(zhǔn)角度誤差小于有效載荷設(shè)備的通信波束角(6°)。

圖4 大疆M600 Pro

任務(wù)參數(shù)配置：繞飛圓心(經(jīng)度116.8990753°，緯度40.3543671°，高度100 m)，無(wú)人機(jī)起飛和返航速度為2 m/s，無(wú)人機(jī)A繞飛半徑為30 m，無(wú)人機(jī)B繞飛半徑為60 m，無(wú)人機(jī)C繞飛半徑為100 m，各無(wú)人機(jī)繞飛角速度均為2(°)/s，繞飛圈數(shù)為1圈，實(shí)驗(yàn)當(dāng)天風(fēng)力等級(jí)在2～4級(jí)之間波動(dòng)。從地面拍攝的飛行實(shí)驗(yàn)實(shí)景圖如圖5所示。

圖5 飛行實(shí)驗(yàn)實(shí)景圖

3.2 實(shí)驗(yàn)結(jié)果分析

根據(jù)無(wú)人機(jī)飛行數(shù)據(jù)繪制的無(wú)人機(jī)飛行軌跡3D圖像(包含飛行初始調(diào)整、多機(jī)協(xié)同探測(cè)兩個(gè)子行為)如圖6所示。3臺(tái)無(wú)人機(jī)垂直上升20 m；飛向預(yù)備點(diǎn)；調(diào)整姿態(tài)并飛向協(xié)同探測(cè)開(kāi)始點(diǎn)；執(zhí)行多機(jī)協(xié)同探測(cè)任務(wù)。從該軌跡圖可以看出，三臺(tái)無(wú)人機(jī)按照預(yù)期的飛行軌跡完成了編隊(duì)飛行任務(wù)。

圖6 無(wú)人機(jī)編隊(duì)飛行軌跡圖

飛行數(shù)據(jù)分析結(jié)果如圖7～10所示。俯仰角誤差圖如圖7所示，俯仰角誤差在0°到1.1°范圍內(nèi)波動(dòng)。俯仰角誤差對(duì)有效載荷設(shè)備對(duì)準(zhǔn)角度誤差的影響較小。橫滾角誤差如圖8所示，無(wú)人機(jī)在繞飛過(guò)程中機(jī)頭方向指向繞飛圓心，即無(wú)人機(jī)一直在橫向飛行，因此在繞飛起始時(shí)和繞飛結(jié)束時(shí)橫滾角誤差波動(dòng)較大。無(wú)人機(jī)B的繞飛半徑比無(wú)人機(jī)C的繞飛半徑小，因此大部分時(shí)候無(wú)人機(jī)A/B的橫滾角誤差比無(wú)人機(jī)A/C的橫滾角誤差小。橫滾角誤差對(duì)有效載荷設(shè)備對(duì)準(zhǔn)角度的影響幾乎可以忽略不記。偏航角誤差如圖9所示，偏航角誤差在0°到3°范圍內(nèi)波動(dòng)。造成無(wú)人機(jī)偏航角誤差的原因主要有繞飛起始時(shí)間不同步和環(huán)境風(fēng)的影響。偏航角誤差對(duì)有效載荷設(shè)備對(duì)準(zhǔn)角度誤差的影響較大。

圖7 俯仰角誤差

圖8 橫滾角誤差

圖9 偏航角誤差

無(wú)人機(jī)對(duì)準(zhǔn)角度誤差如圖10所示，在0°到4°范圍內(nèi)波動(dòng)，始終小于有效載荷設(shè)備的通信波束角(6°)，滿足有效載荷設(shè)備對(duì)準(zhǔn)角度的精度要求。

圖10 對(duì)準(zhǔn)角度誤差

結(jié)果表明，實(shí)驗(yàn)中，在網(wǎng)絡(luò)時(shí)延、2～4級(jí)環(huán)境風(fēng)力等因素影響下，一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法有效保障了多機(jī)協(xié)同探測(cè)的編隊(duì)飛行任務(wù)的實(shí)施。

4 結(jié)論與展望

無(wú)人機(jī)協(xié)同編隊(duì)飛行可以使得多臺(tái)無(wú)人機(jī)協(xié)作完成更復(fù)雜多樣的飛行和探測(cè)任務(wù)，尤其是分布式合成孔徑等機(jī)載應(yīng)用。本文采用一致性算法和領(lǐng)航跟隨法相結(jié)合的無(wú)人機(jī)編隊(duì)控制方法，使多無(wú)人機(jī)執(zhí)行編隊(duì)飛行任務(wù)時(shí)在時(shí)間、空間、姿態(tài)多維度協(xié)同配合。后續(xù)，將繼續(xù)提升無(wú)人機(jī)閉環(huán)控制能力。