劉祖均，何 明，馬子玉，顧凌楓

陸軍工程大學(xué) 指揮控制工程學(xué)院，南京 210007

1 引言

單架無人機(jī)執(zhí)行大型復(fù)雜任務(wù)時(shí)，效率低，成功率低。從時(shí)間上看，單架無人機(jī)續(xù)航較短。對于一些需要大規(guī)模搜索的復(fù)雜任務(wù)（如自然災(zāi)害巡邏和森林救援），其最大行程將受到限制；從空間上看，單架無人機(jī)的活動半徑有限，將嚴(yán)重縮小大規(guī)模軍事偵察任務(wù)范圍，降低完成效率；從任務(wù)層面來看，單架無人機(jī)抗干擾能力較弱，探測能力和負(fù)載能力有限，增加了任務(wù)失敗的概率。相比之下，多架無人機(jī)的協(xié)同編隊(duì)可以解決時(shí)間、空間和任務(wù)層面的沖突。當(dāng)任務(wù)比較復(fù)雜，飛行區(qū)域比較大時(shí)，整個(gè)任務(wù)可以劃分為一些簡單的小任務(wù)。每架無人機(jī)攜帶不同的探測設(shè)備完成自己的任務(wù)，使總?cè)蝿?wù)可以一次完成，從而大大提高了任務(wù)的效率。

編隊(duì)是指兩架或多架飛機(jī)以合作關(guān)系形成一定秩序的飛行。無人機(jī)群協(xié)同編隊(duì)控制的研究已經(jīng)得到了廣泛的研究，研究方法主要有：人工勢場法[1]、主機(jī)-從機(jī)[2]、一致性理論[3]、行為方法[4]等。

編隊(duì)協(xié)同控制方式大致可分為集中式控制和分布式控制。集中式控制是指以隊(duì)長的身份對整個(gè)編隊(duì)的一個(gè)成員進(jìn)行監(jiān)控，并對每個(gè)成員下達(dá)控制指令，實(shí)現(xiàn)對整個(gè)編隊(duì)的控制。分布式控制沒有系統(tǒng)控制的中心點(diǎn)，通過系統(tǒng)中相鄰個(gè)體之間的相互協(xié)商完成信息交換，最終完成整體的編隊(duì)行為。由于沒有控制中心，分布式控制系統(tǒng)具有較高的靈活性，可以動態(tài)地改變控制網(wǎng)絡(luò)的結(jié)構(gòu)。

Boid模型是著名的生物團(tuán)簇運(yùn)動模型，由Reynolds于1986 年提出。在仿真模型中，集群中的每只鳥都能在一定范圍內(nèi)獲得相鄰的個(gè)體飛行信息，并遵循對齊、內(nèi)聚、分離三個(gè)基本行為規(guī)則來進(jìn)行飛行決策?；跓o人機(jī)編隊(duì)飛行與生物集群社會行為的相似性，文獻(xiàn)[5-7]分析了多架無人機(jī)仿生自主編隊(duì)飛行的機(jī)理。然而，基于Reynolds 框架的無人機(jī)編隊(duì)控制無法實(shí)現(xiàn)嚴(yán)格的避障，仿生方法實(shí)現(xiàn)自主編隊(duì)[8]尚需要更深入的技術(shù)研究。傳統(tǒng)基于人工勢場的控制方法存在局部極小化問題，難以考慮實(shí)際約束。

近年來，許多學(xué)者致力于基于分布式信息交互的協(xié)同控制算法，取得了一致性算法的研究成果[9-10]。文獻(xiàn)[11-12]對一致性算法進(jìn)行了深入研究，并基于一致性理論研究了多機(jī)器人系統(tǒng)的隊(duì)形控制問題。朱旭提出了一種具有高階、非線性和時(shí)滯特性的一致性判據(jù)，研究了基于一致性控制的無人機(jī)編隊(duì)保持算法以及編隊(duì)重構(gòu)[13]策略。Kuriki Y 等人充分考慮了無人機(jī)編隊(duì)的防撞問題，結(jié)合防撞算法和一致性控制算法，有效解決了無人機(jī)編隊(duì)中無人機(jī)的碰撞現(xiàn)象[14]。在利用虛擬導(dǎo)航儀研究分布式無人機(jī)編隊(duì)控制方法時(shí)，文獻(xiàn)[15]引入了虛擬力的概念和基于無向圖的控制算法，將加速度轉(zhuǎn)化為徑向速度和角速度的更新，實(shí)現(xiàn)自主編隊(duì)并跟蹤虛擬導(dǎo)航儀的航跡。

本文綜合了相關(guān)方法，采用主機(jī)-從機(jī)的控制方案，在分布式和一致性的基礎(chǔ)上，引入一種相對通用、簡單的多無人機(jī)編隊(duì)協(xié)同方法。

2 機(jī)理建模

2.1 控制系統(tǒng)模型

無人機(jī)編隊(duì)協(xié)同控制由協(xié)同飛行控制系統(tǒng)和協(xié)同航跡控制系統(tǒng)兩部分組成。協(xié)同飛行控制系統(tǒng)是一個(gè)控制飛行姿態(tài)的內(nèi)環(huán)；協(xié)同航跡控制系統(tǒng)是一個(gè)控制飛行軌跡的外環(huán)，外環(huán)的輸出是內(nèi)環(huán)的輸入。協(xié)同航跡控制系統(tǒng)根據(jù)期望的移動位置，計(jì)算出相應(yīng)的姿態(tài)信息，如俯仰角、偏航角、速度等，并傳送給協(xié)同飛行控制系統(tǒng)。飛行控制系統(tǒng)收到指令后，通過新的姿態(tài)信息計(jì)算出旋翼推力并發(fā)送電機(jī)指令控制運(yùn)動。如果不提前設(shè)定航路，編隊(duì)就可以自由飛行，航跡控制的任務(wù)就會變得簡單，只需要在飛行中注意避碰。在本文無人機(jī)編隊(duì)協(xié)同控制中，設(shè)計(jì)了一種基于一致性算法的反饋控制律來完成編隊(duì)和姿態(tài)協(xié)調(diào)任務(wù)。

各無人機(jī)的控制系統(tǒng)結(jié)構(gòu)如圖1 所示。外環(huán)的主要任務(wù)是控制每架無人機(jī)的位置、速度。基于外回路產(chǎn)生的控制指令，內(nèi)回路完成飛行姿態(tài)控制。

圖1 無人機(jī)控制系統(tǒng)模型

為了實(shí)現(xiàn)控制目標(biāo)，設(shè)計(jì)了速度控制子系統(tǒng)，獲取所需的時(shí)變推力大小和方向，將所需的姿態(tài)信息發(fā)送給姿態(tài)控制子系統(tǒng)進(jìn)行跟蹤。

2.2 分布式結(jié)構(gòu)

與傳統(tǒng)的集中式協(xié)同編隊(duì)控制理論相比，基于分布式結(jié)構(gòu)的協(xié)同編隊(duì)控制方法具有通信控制框架靈活、個(gè)體數(shù)量不限、計(jì)算量小、易于工程實(shí)現(xiàn)等優(yōu)點(diǎn)。無人機(jī)之間的通信是雙向的，雙向通信有利于無人機(jī)編隊(duì)協(xié)同飛行完成各種任務(wù)。同時(shí)，當(dāng)編隊(duì)無人機(jī)數(shù)量增加到3架以上時(shí)，可提高穩(wěn)定性和可靠性。圖2展示了多架無人機(jī)的分布式結(jié)構(gòu)，其中每架無人機(jī)都在通信距離R的范圍內(nèi)。

圖2 分布式結(jié)構(gòu)

（1）無人機(jī)指揮員

無人機(jī)指揮員在無人機(jī)編隊(duì)中起著指揮和控制的作用，引導(dǎo)整個(gè)團(tuán)隊(duì)按照預(yù)先設(shè)定的航跡飛行。它與地面控制中心和其他無人機(jī)保持實(shí)時(shí)通信。任務(wù)流程預(yù)裝在每架無人機(jī)上，可以在領(lǐng)導(dǎo)者失敗時(shí)由地面控制站激活。

（2）無人機(jī)跟隨者

無人機(jī)追隨者跟隨他們的領(lǐng)導(dǎo)者。與地面控制中心保持聯(lián)系，不斷接受無人機(jī)指揮員的指揮。每兩個(gè)無人機(jī)跟隨者之間也可以進(jìn)行通信。

本文將“老兵規(guī)則”[15]與經(jīng)典的“領(lǐng)導(dǎo)者跟隨者”通信機(jī)制相結(jié)合，根據(jù)局部領(lǐng)導(dǎo)者與局部追隨者之間的距離來設(shè)計(jì)各種經(jīng)驗(yàn)值，然后可以很容易地獲得預(yù)期的拉普拉斯矩陣L。

一旦獲得編隊(duì)變化次序，就可以計(jì)算矩陣L。假設(shè)無人機(jī)群的數(shù)量為6，并且當(dāng)前位置為圖3所示的“T”形，當(dāng)隊(duì)形變換到八面體形時(shí)，則得到的通信拓?fù)淙鐖D4所示，矩陣L為：

圖3 當(dāng)前6個(gè)UAV位置

圖4 6個(gè)UAV通信拓?fù)?/p>

2.3 協(xié)同機(jī)制

根據(jù)上述建立的分布式結(jié)構(gòu)，結(jié)合智能群體理論和具體的實(shí)際情況，給出無人機(jī)編隊(duì)過程中的協(xié)同機(jī)制如下：

（1）一致性

在給定編隊(duì)結(jié)構(gòu)的情況下，無人機(jī)領(lǐng)導(dǎo)者朝給定位置飛行，將領(lǐng)導(dǎo)者的相關(guān)位姿信息分配給相應(yīng)的無人機(jī)追隨者，使其分別達(dá)到位置和速度的一致性。

（2）任務(wù)分配

無人機(jī)跟隨者從無人機(jī)領(lǐng)導(dǎo)者接收信息，每個(gè)無人機(jī)追隨者的首要任務(wù)就是飛向期望位置。

（3）避碰

預(yù)先設(shè)置安全距離D(0 ＜D＜R) ，如果將每兩個(gè)UAV 之間的最小距離減小為D，則會采取一些相應(yīng)的措施來避免碰撞。

以上協(xié)同機(jī)制的三個(gè)步驟是每個(gè)無人機(jī)需要遵循并完成的工作，將會在下一章協(xié)同編隊(duì)設(shè)計(jì)分步具體展開。

3 分布式UAV協(xié)同編隊(duì)設(shè)計(jì)

在本章中，介紹了一種分布式UAV 編隊(duì)控制的設(shè)計(jì)。設(shè)計(jì)了一致性控制器、任務(wù)分配策略和避障策略。將一致性控制器的輸出加速度發(fā)送到高層控制層，每個(gè)UAV 的避障模塊修改所需的加速度。最后，將期望的加速度發(fā)送到低層控制層，最終實(shí)現(xiàn)期望的編隊(duì)。

3.1 基于一致性的協(xié)同編隊(duì)控制器

考慮到一組N個(gè)UAV，每個(gè)UAV 在其動力學(xué)模型的基礎(chǔ)上可以被建模為一個(gè)積分器模型，則可以描述為：

這里輸入ai是UAVi的加速度，ξi和μi分別是UAVi的位置和速度狀態(tài)。所有的UAV 在m維空間中移動，在本文的模擬中m=3。目標(biāo)狀態(tài)是所有的UAV 達(dá)到一個(gè)給定的編隊(duì)模式。編隊(duì)控制器如下：

ωij是鄰接矩陣W的元素，當(dāng)且僅當(dāng)ωij>0 時(shí)，表示UAVj與UAVi通信。δi為無人機(jī)i的編隊(duì)偏移量，由最終期望的編隊(duì)構(gòu)型及其期望位置決定。γi是一個(gè)可調(diào)參數(shù)，關(guān)系到系統(tǒng)的穩(wěn)定性和收斂性。

由于系統(tǒng)的一致性與編隊(duì)的穩(wěn)定性是等價(jià)的，所以本文系統(tǒng)可以在文獻(xiàn)[16]條件下達(dá)到編隊(duì)的穩(wěn)定性。

對于n架無人機(jī)，定義其通信拓?fù)浣Y(jié)構(gòu)為Gn，L為Gn相應(yīng)的拉普拉斯矩陣，令L=[lij]∈RN×N，它的元素定義為：

令c=[c1,c2,…,cN]T，ξ=[ξ1,ξ2,…,ξN]T，δ=[δ1,δ2,…,δN]T，μ=[μ1,μ2,…,μN(yùn)]T，系統(tǒng)（3）可以寫為：

圖5 KM算法的匹配和篩選結(jié)果

這里Im=[1,1,…,1]T∈Rm，在本文仿真中m=3。

考慮到參數(shù)γ的設(shè)置，根據(jù)文獻(xiàn)[15]，當(dāng)且僅當(dāng)以下成立，系統(tǒng)達(dá)到穩(wěn)定：

λi是 -L的第i個(gè)特征值，Re(λi)和 Im(λi)是λi的實(shí)數(shù)和虛數(shù)部分。很明顯，原滿足條件式（7）的參數(shù)在改變矩陣L后是脆弱的，這可能會大大降低仿真平臺的可擴(kuò)展性。因此，對于任何數(shù)量的無人機(jī)，都非常需要通用的通信拓?fù)湟?guī)則和參數(shù)γ。

3.2 任務(wù)分配

在執(zhí)行任務(wù)時(shí)，考慮環(huán)境影響以及無人機(jī)自身續(xù)航能力，不同的無人機(jī)設(shè)置在不固定的編隊(duì)位置，將編隊(duì)變換視為一種任務(wù)分配，優(yōu)化目標(biāo)是獲得編隊(duì)任務(wù)點(diǎn)分配的最小代價(jià)，以使編隊(duì)在最短的理想總距離范圍和時(shí)間內(nèi)收斂。為了降低分布式任務(wù)分配的難度，并結(jié)合仿真平臺的特點(diǎn)，采用基于Kuhn-Munkres 算法的變種算法進(jìn)行任務(wù)分配。

考慮n個(gè)無人機(jī)當(dāng)前所在頂點(diǎn)位置與變換后頂點(diǎn)位置為兩組頂點(diǎn)集合，組內(nèi)任意兩個(gè)頂點(diǎn)間沒有邊相連，只有兩個(gè)集合之間存在邊，設(shè)其為二部圖A。本節(jié)任務(wù)目標(biāo)要完成二部圖的完美匹配，即兩集合存在n！個(gè)雙射匹配組合。A中每個(gè)元素aij的值表示前編隊(duì)偏移量和當(dāng)前的編隊(duì)偏移量兩點(diǎn)之間的距離dij，即這條邊的權(quán)重或代價(jià)。算法流程如下：

（1）初始化每個(gè)無人機(jī)可行路線的權(quán)重。

（2）使用匈牙利最大匹配算法判斷是否有符合條件的增廣路。

（3）若找到符合的增廣路則修改當(dāng)前可行路線的值。

（4）重復(fù)（2）、（3）直到找到最佳匹配為止。

本算法獲得的分配是最佳的分組，總代價(jià)最小[17]。例如，在簡化模擬器中將編隊(duì)配置“T”形變換為正八面體時(shí)，然后KM算法的匹配和篩選結(jié)果如圖5所示。

3.3 避障策略

圖6 為避障策略。假設(shè)無人機(jī)的信息感知范圍為圓球體，則R為無人機(jī)最大通信半徑，d為無人機(jī)最大安全避障半徑，r為兩UAV之間距離矢量，指向安全范圍內(nèi)障礙UAV，a為UAV1 的避障矢量，a′為UAV2 的避障矢量。a和a′方向相反，垂直于r。

圖6 避障策略

如算法1 所示，利用兩個(gè)輔助向量h1和h2來避免向量積結(jié)果趨近于零。kP是一個(gè)比例因子，lp是一個(gè)修正因子，根據(jù)兩飛機(jī)距離來修整正避障矢量的大小。如果在d米范圍內(nèi)有超過一個(gè)UAV，則會對避障矢量a進(jìn)行多次修改。最后的a從協(xié)同層添加到期望的加速度中，然后將期望的加速度發(fā)送到低層控制層。

算法1避碰策略

loop

Initialization：a0=[0 ,0,0]；h1=[1 , 0,0]；h2=[0 ,1,0]；i=0

whilei＜d米范圍內(nèi)無人機(jī)的do

ifri?h1＜ri?h2then

a0=a0+(kP*ri×h1)lp；

else

a0=a0+(kP*ri×h2)lp；

end if

end while

end loop

4 分布式UAV協(xié)同編隊(duì)仿真

如圖7 所示，在4.1 節(jié)中簡要介紹了仿真平臺的分布式架構(gòu)，五個(gè)層通過通信層相互通信。該架構(gòu)的靈感來自于文獻(xiàn)[18]，它是一個(gè)實(shí)用的UAV 集群的多層次分布式架構(gòu)。4.2 節(jié)進(jìn)行了簡化模擬器Matplotlib 和實(shí)景Gazebo 下的仿真實(shí)驗(yàn)，驗(yàn)證上述協(xié)同控制方法的有效性。

圖7 分布式UAV協(xié)同仿真平臺架構(gòu)

4.1 仿真架構(gòu)

通信層負(fù)責(zé)所有UAV和地面控制站之間的消息傳輸，該層是整個(gè)仿真平臺的基礎(chǔ)。ROS、MAVLink 和MAVROS在通信層中提供消息傳遞。

仿真層提供了無人機(jī)動態(tài)模型、傳感器、虛擬場景和一些其他類型的機(jī)器人，所提供的這些都是可以編程定制的，因此開發(fā)人員可以根據(jù)自己的需要修改動力學(xué)模型，或向無人機(jī)添加其他需要的傳感器。這一層主要的模擬器是Gazebo。此外，為了在早期階段快速開發(fā)算法，提供了一種基于Matplotlib的簡化模擬器[19]。

低層控制層是基于PX4 自駕儀的軟件在環(huán)仿真（SITL）實(shí)現(xiàn)，使用到的主要是官方的PX4 Firmware 固件包，其中包含狀態(tài)估計(jì)和基礎(chǔ)控制，例如位置控制、姿態(tài)控制和飛行模式。

高層控制層包含感知和運(yùn)動規(guī)劃。例如路徑規(guī)劃、避障等均在此層中。后期若需要研究視覺SLAM、目標(biāo)追蹤等算法，也應(yīng)在此層中進(jìn)行設(shè)計(jì)。對于無人機(jī)群，該層接收從協(xié)同層分配的任務(wù)，然后每個(gè)無人機(jī)完成任務(wù)。

協(xié)同層負(fù)責(zé)與用于任務(wù)協(xié)調(diào)的無人機(jī)之間的協(xié)商（例如，任務(wù)分配）有關(guān)的任務(wù)。它將總?cè)蝿?wù)劃分為不同的小任務(wù)，然后將它們發(fā)送到高層控制層。該層非常靈活，可以根據(jù)不同的任務(wù)和協(xié)同策略包含不同的模塊。在4.2 節(jié)仿真實(shí)驗(yàn)中，任務(wù)分配和一致性控制器位于此層中。

人機(jī)交互層是開發(fā)人員用來控制和監(jiān)視UAV群的一組接口。常用的人機(jī)交互界面是地面控制站，如QGroundControl。QGroundControl 通過 MAVLink 與 PX4 SITL通信（如圖7中虛線箭頭所示），因此開發(fā)人員可以監(jiān)視和調(diào)整底層控制層的參數(shù)和目標(biāo)。此外，日志記錄對于分析算法和調(diào)試代碼至關(guān)重要，可以使用一些用戶友好的軟件分析ULog文件。

4.2 仿真實(shí)驗(yàn)

仿真主要包含兩個(gè)python 類，一個(gè)用于Leader，另一個(gè)用于Follower。開發(fā)人員可以繼承這兩個(gè)類來修改通信和控制方案。通過啟動多個(gè)ROS 節(jié)點(diǎn)，可以模擬多個(gè)UAV控制器。

（1）Leader.py：初始化了一個(gè)leader，follower跟隨其變化。

（2）Follower.py：編程設(shè)計(jì)每個(gè)follower無人機(jī)進(jìn)行編隊(duì)變換需要完成的一致性控制協(xié)議、任務(wù)點(diǎn)分配和避障策略的算法。

圖8 展示了在簡化模擬器中實(shí)現(xiàn)的6 個(gè)UAV 的協(xié)同編隊(duì)，圖9展示了在Gazebo中實(shí)現(xiàn)的6個(gè)UAV的協(xié)同編隊(duì)，從左到右完成三個(gè)構(gòu)型的變換：T形、正八面體和三角形。

為了驗(yàn)證本文分布式協(xié)同控制算法，從PX4 中獲取了飛行日志，并且對兩次編隊(duì)變換無人機(jī)群位置響應(yīng)數(shù)據(jù)進(jìn)行分析，曲線如圖10 所示。在PX4 日志中選擇記錄頻率為60 點(diǎn)位/s，即每幅圖的橫坐標(biāo)軸表示記錄位置頻率。其中，圖10（a）～（c）表示T 形編隊(duì)到正八面體編隊(duì)構(gòu)型的變換中六架無人機(jī)XYZ位置響應(yīng)；圖10（d）～（f）表示正八面體編隊(duì)到三角形編隊(duì)構(gòu)型的變換中六架無人機(jī)XYZ位置響應(yīng)；盡管在響應(yīng)曲線上可以看到一些過沖，但最終整個(gè)編隊(duì)都能夠很快地達(dá)到穩(wěn)定，從而驗(yàn)證了本文提出分布式協(xié)同控制方法的有效性。

圖8 在簡化的模擬器中實(shí)現(xiàn)6UAV的編隊(duì)

圖9 6UAV在Gazebo中的仿真

圖10 兩次編隊(duì)變換位置響應(yīng)曲線

5 結(jié)束語

本文針對多無人機(jī)協(xié)同編隊(duì)控制問題，分析了多無人機(jī)的協(xié)同機(jī)制，基于傳統(tǒng)的領(lǐng)航-跟隨結(jié)構(gòu)設(shè)計(jì)了一致性協(xié)同編隊(duì)控制器，利用匈牙利算法實(shí)現(xiàn)機(jī)群隊(duì)形變換中距離代價(jià)最小，為了使無人機(jī)移動過程中避免碰撞，提出一種簡單有效的避障算法。最后分別在簡易模擬器和基于ROS-Gazebo 的實(shí)景模擬器進(jìn)行仿真驗(yàn)證，實(shí)現(xiàn)了6架無人機(jī)協(xié)同編隊(duì)，驗(yàn)證了本文一致性算法和分布式控制方法的有效性。

由于多機(jī)通信問題較為復(fù)雜，尤其在ROS 的仿真環(huán)境下實(shí)現(xiàn)困難，下一步研究工作將會探索如何優(yōu)化基于ROS 的多機(jī)仿真，并且應(yīng)進(jìn)一步考慮通信時(shí)延環(huán)境下的無人機(jī)集群協(xié)同編隊(duì)問題。