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

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

陸軍工程大學 指揮控制工程學院，南京 210007

1 引言

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

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

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

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

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

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

2 機理建模

2.1 控制系統模型

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

各無人機的控制系統結構如圖1 所示。外環的主要任務是控制每架無人機的位置、速度。基于外回路產生的控制指令，內回路完成飛行姿態控制。

圖1 無人機控制系統模型

為了實現控制目標，設計了速度控制子系統，獲取所需的時變推力大小和方向，將所需的姿態信息發送給姿態控制子系統進行跟蹤。

2.2 分布式結構

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

圖2 分布式結構

（1）無人機指揮員

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

（2）無人機跟隨者

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

本文將“老兵規則”[15]與經典的“領導者跟隨者”通信機制相結合，根據局部領導者與局部追隨者之間的距離來設計各種經驗值，然后可以很容易地獲得預期的拉普拉斯矩陣L。

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

圖3 當前6個UAV位置

圖4 6個UAV通信拓撲

2.3 協同機制

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

（1）一致性

在給定編隊結構的情況下，無人機領導者朝給定位置飛行，將領導者的相關位姿信息分配給相應的無人機追隨者，使其分別達到位置和速度的一致性。

（2）任務分配

無人機跟隨者從無人機領導者接收信息，每個無人機追隨者的首要任務就是飛向期望位置。

（3）避碰

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

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

3 分布式UAV協同編隊設計

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

3.1 基于一致性的協同編隊控制器

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

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

ωij是鄰接矩陣W的元素，當且僅當ωij>0 時，表示UAVj與UAVi通信。δi為無人機i的編隊偏移量，由最終期望的編隊構型及其期望位置決定。γi是一個可調參數，關系到系統的穩定性和收斂性。

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

對于n架無人機，定義其通信拓撲結構為Gn，L為Gn相應的拉普拉斯矩陣，令L=[lij]∈RN×N，它的元素定義為：

令c=[c1,c2,…,cN]T，ξ=[ξ1,ξ2,…,ξN]T，δ=[δ1,δ2,…,δN]T，μ=[μ1,μ2,…,μN]T，系統（3）可以寫為：

圖5 KM算法的匹配和篩選結果

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

考慮到參數γ的設置，根據文獻[15]，當且僅當以下成立，系統達到穩定：

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

3.2 任務分配

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

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

（1）初始化每個無人機可行路線的權重。

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

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

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

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

3.3 避障策略

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

圖6 避障策略

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

算法1避碰策略

loop

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

whilei＜d米范圍內無人機的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協同編隊仿真

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

圖7 分布式UAV協同仿真平臺架構

4.1 仿真架構

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

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

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

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

協同層負責與用于任務協調的無人機之間的協商（例如，任務分配）有關的任務。它將總任務劃分為不同的小任務，然后將它們發送到高層控制層。該層非常靈活，可以根據不同的任務和協同策略包含不同的模塊。在4.2 節仿真實驗中，任務分配和一致性控制器位于此層中。

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

4.2 仿真實驗

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

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

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

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

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

圖8 在簡化的模擬器中實現6UAV的編隊

圖9 6UAV在Gazebo中的仿真

圖10 兩次編隊變換位置響應曲線

5 結束語

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

由于多機通信問題較為復雜，尤其在ROS 的仿真環境下實現困難，下一步研究工作將會探索如何優化基于ROS 的多機仿真，并且應進一步考慮通信時延環境下的無人機集群協同編隊問題。