基于分布式一致性的無人機編隊控制方法

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

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

1 引言

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

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

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

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

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

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

2 機理建模

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

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

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

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

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

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

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

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

（1）無人機指揮員

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

（2）無人機跟隨者

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

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

一旦獲得編隊變化次序，就可以計算矩陣L。假設(shè)無人機群的數(shù)量為6，并且當前位置為圖3所示的“T”形，當隊形變換到八面體形時，則得到的通信拓撲如圖4所示，矩陣L為：

圖3 當前6個UAV位置

圖4 6個UAV通信拓撲

2.3 協(xié)同機制

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

（1）一致性

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

（2）任務(wù)分配

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

（3）避碰

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

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

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

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

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

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

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

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

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

對于n架無人機，定義其通信拓撲結(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]T，系統(tǒng)（3）可以寫為：

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

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

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

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

3.2 任務(wù)分配

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

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

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

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

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

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

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

3.3 避障策略

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

圖6 避障策略

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

算法1避碰策略

loop

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

whilei＜d米范圍內(nèi)無人機的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é)同編隊仿真

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

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

4.1 仿真架構(gòu)

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

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

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

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

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

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

4.2 仿真實驗

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

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

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

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

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

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

圖9 6UAV在Gazebo中的仿真

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

5 結(jié)束語

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

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