多無人機自適應編隊協同航跡規劃

許 洋 ，秦小林 *，劉 佳 ，張力戈

（1.中國科學院成都計算機應用研究所，成都610041； 2.中國科學院大學計算機與控制學院，北京100080）

（?通信作者電子郵箱qinxl2001@126.com）

0 引言

無 人 機（Unmanned Aerial Vehicle，UAV）航 跡 規 劃（trajectory planning）是指在初始狀態、任務目標、威脅區和一些已知環境信息的情況下獲得性能最優的規劃問題，是任務規劃系統的關鍵技術之一。目前單機航跡規劃技術已經相當成熟，然而在未來戰場，多無人機編隊協同執行任務將成為主要的作戰形式，而協同航跡規劃是提高無人機協同作戰效能，保證無人機協同作戰順利實施的關鍵。多無人機協同航跡規劃在為各無人機規劃出滿足約束條件的最優航跡時，還需考慮各無人機之間的空間協同性以及時間協同性［1］，其主要任務包括航跡規劃、碰撞避免、隊形形成/重構以及隊形保持等。

目前，針對多無人機協同航跡規劃問題，主要采用的算法包括人工勢場法（Artificial Potential Field，APF）［2-3］、粒子群優化（Particle Swarm Optimization，PSO）算法［4］、滑模控制［5］、PID（Proportion Integration Differentiation）［6-7］等。文獻［1］采用協同非支配排序進化算法，根據多架無人機的航行距離、安全性、時間以及空間的協同性進行規劃；文獻［8］采用混合粒子群優化算法與遺傳算法相結合的方法，實現了對多無人機編隊重構過程中參數最優化問題的求解；文獻［9］通過引入分布式結構和層次網絡，并基于由指揮官、虛擬領航者和跟隨者組成的多無人機系統建立無人機協同機制，實現了多無人機自主編隊和重組任務；文獻［10］采用任務分配和粒子群優化算法，實現了對無人機群編隊轉換的高效策略。以上幾種方法都是針對無障礙環境下多無人機的編隊協同控制問題。

為了確保復雜環境下多無人機編隊能夠安全、快速到達任務區域，需要設計滿足一定約束條件及性能指標最優的編隊航跡。文獻［11］利用共識的幾何優化方法，結合分布式策略，實現了在包含靜、動態障礙物的二、三維環境中，求解一組機器人協同控制的最優參數；文獻［12］通過建立多無人機系統的特殊正交群（Special Orthogonal group）SO（3）控制模型，實現了多無人機在姿態一致的狀態下以期望的隊形穩定飛行；文獻［13］采用神經網絡方法，自適應地逼近和在線補償動態逆誤差，提高了多無人機隊形的穩定性；文獻［3，14-15］采用了人工勢場法解決復雜環境下的編隊控制及避障問題，通過各無人機之間的斥力與引力相互作用，實現整個協同任務，然而復雜環境下的避障動作會在一定程度上擾亂無人機編隊隊形，影響編隊的穩定性。

基于滾動時域優化思想的模型預測控制（Model Predictive Control，MPC）策略能夠顯式地考慮各種約束條件，并滾動實施控制輸入，具有魯棒性較強等特點，因此在多無人機編隊控制等問題中得到了廣泛應用［16］；文獻［17］利用分布式模型預測控制（Distributed Model Predictive Control，DMPC）對計算得到的參考軌跡進行跟蹤，并對障礙物進行碰撞檢測，通過加入懲罰項實現動態航跡跟蹤及避障；文獻［18］采用DMPC方法將編隊重構問題轉化為滾動在線優化問題，并采用微分進化（Differential Evolution，DE）方法進行求解。

然而上述工作主要集中在單一編隊隊形控制、編隊飛行避障控制等方面，未考慮復雜環境下多無人機編隊隊形的適應性以及隊形轉換的高效性。

本文針對以上問題，提出了一種基于自適應分布式模型預測控制的快速粒子群優化（Adaptive Distributed Model Predictive Controller-Fast Particle Swarm Optimization，ADMPCFPSO）方法。該方法利用虛擬結構法與領航跟隨法相結合的編隊策略，構造出自適應虛擬編隊引導點，并設計基于最小代價函數的優化目標；采用分布式模型預測控制方法將其轉化為滾動在線優化問題，通過快速粒子群優化（Fast Particle Swarm Optimization，FPSO）算法對問題進行求解。仿真結果表明，該方法可以高效解決復雜環境下的多無人機協同航跡規劃問題，與基于人工勢場法的編隊協同控制方案［3］相比，航跡規劃的平均消耗時間降低了31.6%。

1 相關工作

1.1 模型預測控制

模型預測控制是一種基于滾動時域的在線優化控制策略，其基本思想是，假設當前時間點為k，在有限時域［k，k+j］，通過使某一性能指標最優化來確定其未來的控制作用，選擇第一個最優控制輸入作為當前的控制輸入，在下一時刻k+1，計算［k+1，k+j+1］的控制輸出，重復以上過程，直到任務完成。其原理如圖1所示。

圖1 模型預測控制原理框圖Fig.1 Functional block diagramof model predictivecontrol

1.2 粒子群優化

粒子群優化（PSO）算法［4］是由美國 Kennedy和Elberhart于1995年，受到鳥群覓食的行為啟發而提出的，是一種模擬鳥類飛行過程中群聚行為的智能優化算法。

由m個粒子組成的群體在超維空間內以一定的速度飛行，每個粒子表示問題的一個解。粒子在優化過程中，跟隨兩個極值實現自身的迭代。一個是粒子自身找到的歷史最優解，稱為個體極值；另一個是整個群體找到的最優解，稱為全局極值。粒子的速度和位置更新公式分別為：

式（1）是PSO的速度更新公式，其中表示第i個粒子在第k次迭代時的速度；c1和c2為學習因子；r1和r2是[0，1]區間內均勻分布的隨機數；ω為慣性權重，表示粒子對原有速度繼承的程度；式（2）是位置更新公式，表示第i個粒子在第k次迭代時的位置；式（3）為慣性權重的更新公式，其中ωmin為最小權重；ωmax為最大權重；itermax為最大迭代次數。

2 問題描述

假定在某次任務中，由n（n＞1）架無人機組成的編隊從某一起始區域飛往目標區域執行任務。以多旋翼無人機為例，討論其由初始狀態（初始區域）出發，以最小代價到達目標區域。在此過程中，需要綜合考慮飛行環境中的威脅、無人機自身性能以及編隊控制等約束條件，為各無人機實時規劃航跡。因此，提出以下兩個假設。

假設1 在航跡規劃過程中，將單個無人機看作質點，多無人機編隊需考慮編隊寬度；線性編隊屬于特例，不考慮寬度。

假設2 多無人機在寬闊區域，保持特定的編隊飛行，增大偵察范圍，從而減小可能存在的風險；在狹窄區域，無需考慮偵察范圍，以線性編隊飛行，保證多無人機安全通過該區域。

2.1 協同控制架構

多無人機協同控制架構包括全局航跡規劃系統和自適應編隊協同系統，如圖2所示。全局航跡規劃系統主要根據部分已知環境信息，預先計算全局代價圖，在滿足相應的約束條件（自身動力學約束、編隊隊形約束、環境約束等）下，利用ADMPC-FPSO動態規劃航跡，引導整個編隊以最低代價從初始區域前往目標區域。

圖2 多無人機協同控制架構Fig.2 Framework of multi-UAV cooperative control

自適應編隊協同系統是以虛擬編隊引導點為主的編隊控制策略。無人機編隊通過感知環境（通道寬度），選取合適隊形，結合虛擬編隊引導點與ADMPC-FPSO，在滿足所有約束條件下，動態規劃最優航跡并保持特定的隊形。規劃過程中，編隊隊形之間的變換通過改變虛擬編隊引導點實現。

2.2 編隊飛行中全局航跡規劃問題

為滿足假設2，在航跡規劃過程中，若只考慮某一固定編隊，可能因部分狹窄區域只允許線性編隊通過，而造成該區域被忽略，大大增加全局代價，導致最終無法得到全局最優航跡。

利用“領航者-跟隨者”結構（后文，將該結構中的“領航者”統稱為“領航無人機”，而“跟隨者”統稱為“跟隨無人機”），在全局航跡規劃過程中，主要對領航無人機進行動態航跡規劃。領航無人機處于編隊的中間位置，在非線性編隊的情況下，規劃過程必須考慮無人機編隊寬度約束，即領航無人機與任一障礙物的距離至少保持編隊寬度的一半。當無人機進入狹窄區域，編隊變換為線性編隊，此時可不考慮編隊寬度，從而盡可能減少全局代價。

在上述約束條件下，可將其看作單獨地對領航無人機的航跡規劃問題。

2.3 基于虛擬編隊引導點的編隊變換及控制

在本文中，主要考慮線性編隊和三角編隊之間的變換，如圖3所示。在自適應編隊變換過程中，首先根據領航無人機的狀態信息，選取合適編隊，并計算虛擬編隊引導點。而后跟隨無人機前往其對應的虛擬編隊引導點，實現編隊變換。

在形成虛擬編隊引導點后，對各跟隨無人機來說，與2.2節中描述的問題類似，只需將虛擬編隊引導點作為各對應跟隨無人機的臨時目標位置即可。

各無人機的規劃相互獨立，通過建立通信保證實時信息交互，以實現對各無人機的分布式控制。

圖3 多無人機編隊變換示意圖Fig.3 Schematic diagramof multi-UAVformationtransformation

3 數學模型

3.1 無人機動力學模型

在飛行過程中，假定無人機在短時間內保持在同一高度飛行，其二維質心運動方程［17］為：

式中：（xi，yi）為無人機i的二維坐標；V表示飛行速度；ψ?i為航向角；ui為對應的控制量。控制量與航向角約束為：

3.2 航跡規劃目標函數模型設計

多無人機系統中，對各無人機建立的數學模型一致，區別在于目標位置的選取，其線性時不變系統動力學可統一描述為：

式中：x(k)∈ RNx是狀態向量，u(k)∈ RNu是輸入向量；式（5）為無人機的狀態空間模型，式（6）為狀態和輸入需要滿足的約束條件；其中γ(k)為k時刻需要滿足的約束條件，輸出向量y(k)∈ RNy。

一般的目標函數，采用如下形式：

式中：l(?)是代價函數，χF是目標集。

在航跡規劃過程中，當控制輸入被執行，形成新的狀態，基于該狀態進行下一步優化，產生新的控制輸入，循環執行直到任務結束。采用MPC算法，基于滾動時域框架的優化描述為

3.3 自適應虛擬編隊引導點

本文采用虛擬編隊引導點方法完成自適應編隊飛行控制任務，如圖4所示，以三個無人機為例，UAVleader為領航無人機，UAVfollower#1和UAVfollower#2為跟隨無人機所對應的虛擬編隊引導點，引導點沿無人機運動方向對稱分布，領航無人機輸入向量為（ulx，uly），UAVfollower#i的坐標為(xfi，yfi)，編隊半徑為rf。

圖4 虛擬編隊引導點坐標系Fig.4 Virtual formation guidepoint coordinatesystem

由于本文針對多旋翼無人機飛行控制，在飛行過程中，領航無人機可能會出現轉彎角度較大的情況，為避免出現大幅度甩尾情況，對ld做如下處理：

式中cld為減幅權重。

結合式（11）、（12）、（13），求出虛擬編隊引導點中UAVfollower#1和UAVfollower#2的坐標。

式中(xfi，yfi)為第i個虛擬引導點的坐標。

通過自適應調節rf值，可在一定程度上提高復雜環境下編隊的適應度。假設檢測前方通道寬度為Rdet，那么：

若Rdet＜Rmin，則變換為線性編隊。Rmin為無人機之間的最小安全距離；φ為保證無人機飛行安全的一個極小值。

3.4 多無人機編隊通信拓撲模型

本文采用圖論對無人機之間的通信拓撲關系進行描述，設G（V，E，A）表示一個加權無向圖，其中：V是圖的頂點集，E是圖的邊集，A=(aij)N×N稱為圖的鄰接矩陣。在多旋翼編隊飛行問題中，將每一個無人機看作圖的一個頂點，無人機之間的信息交互連接就可以用圖G相應的邊來表示。若aij＞0，則表示邊(j，i)∈E，即第i個無人機能夠從第j個無人機獲得所需信息，否則aij=0。由此可以定義頂點i的鄰居集為Ni={j∈V|(j，i)∈E}。

如圖5所示，在無人機的整個通信拓撲結構中，各無人機只與距離其半徑r以內的無人機建立通信，完成信息交換，滿足以下公式：

式中：d ij為i與j之間的距離，r為無人機的通信半徑。

圖5 多無人機編隊通信拓撲示意圖Fig.5 Multi-UAV formation communication topology

4 ADMPC-FPSO自適應協同航跡規劃

多無人機協同航跡規劃主要分為全局航跡規劃和自適應編隊協同控制兩部分。全局航跡規劃可簡單看作對領航無人機的航跡規劃過程，與單個無人機不同，這里必須考慮編隊寬度的約束。對此，本文通過擴充障礙物的方法實現可視點遷移，以此滿足編隊協同控制的相應約束條件。

編隊全局航跡規劃過程主要包括兩個階段：第1階段為代價評估階段，根據當前環境生成代價圖，利用計算得到的代價圖來表示航跡端點到目標點的代價估計；第2階段為實時航跡規劃階段，在問題描述中，已經將航跡規劃表示成一個優化問題，在此主要是基于ADMPC-FPSO實時航跡規劃的一個優化過程。

自適應編隊協同控制主要包括兩個階段：第1個階段為虛擬編隊引導點的形成，根據無人機所處環境與自身狀態，自適應選取并計算最優虛擬編隊引導點的位置，為編隊隊形的形成、保持及變換提供前提條件；第2階段為各無人機在整個多無人機系統中，基于ADMPC-FPSO協同完成編隊隊形的形成、保持及變換任務。

4.1 代價圖

根據第1章可知，從航跡規劃全局看，粒子群優化有可能陷入局部最優。因此，需要預先進行合理的代價轉換，如文獻［18］中提到的代價圖方法，利用可視圖［19］和Dijkstra［20］單源最短路徑算法，搜索從目標點到各個點的最短路徑。

對于領航無人機，由第3章可知，航跡端點為x(k+Hp+1)，因此基于代價圖的終端懲罰項f(?)可以表示如下：

式中：xvis為基于代價圖的可視點；d(x(k+Hp+1)，xvis)為航跡端點到可視點的代價；Cvis為可視點到目標點的代價。

對于跟隨無人機，在滿足相應的約束條件，其規劃過程實際上是局部的，因此終端懲罰項如下：

式中：xvfgp為對應的虛擬編隊引導點；d(x(k+Hp+1)，xvfgp)為航跡端點到對應虛擬編隊引導點的代價。

4.2 基于ADMPC-FPSO協同航跡規劃

協同航跡規劃是復雜約束條件下的最優化問題，采用粒子群優化算法進行航跡規劃時，迭代初期，控制輸入和狀態可能存在違反約束的情況，算法需要迭代若干次才能產生滿足要求的輸入，并在此基礎上進一步尋優。為提高算法的效率，本文根據粒子違反約束的程度來更新粒子。

在1.1節中對MPC進行了介紹，實際應用過程中，MPC分為集中式和分布式兩種。集中式在多無人機編隊飛行控制中，隨著無人機數量的增加，其求解復雜度、通信量急劇增長。實際上，各無人機的動力學特性互相解耦［21］，無人機可以通過各自的約束和狀態條件進行獨立控制，故適于采用DMPC結構進行協同控制。

4.2.1 評價函數設計

評價函數用于粒子群中每個個體計算適應度。在文獻［18］中，對任意輸入，可采用下面的評價函數：

式中：C（u）為式（8）計算的航跡代價；BigM為一個比較大的數；C為約束違背量，采用式（19）計算如下：

式中：f(li)表示航跡段穿越禁飛區的約束違背量；g(vi)和h(ai)表示速度和加速度的約束違背量；w1、w2、w3為權系數。

4.2.2 粒子群優化求解

根據第1章中相關工作的介紹，假設粒子群種群數為N，每個粒子的空間維數為D，第i個粒子的輸入向量和速度為u i=(ui，1，ui，2，…，ui，D)，v i=(vi，1，vi，2，…，vi，D)。第i個粒子最優解 為P i=(pi，1，pi，2，…，pi，D)，粒 子 群 最 優 解 為G=(g1，g2，…，gD)。根據粒子群優化策略，按照式（1）、（2）、（3）更新粒子。在本文中，式（3）中的ωmax不再是一成不變的，而是根據粒子的適應值來更新，如果粒子是可行解，則按照ωk更新，如果粒子是不可行解，則按照式（20）更新：

式中，Fk，i(u)為第k次迭代第i個粒子的適應度。基于ωmax改進的快速粒子群優化（FPSO）算法與經典粒子群優化算法相比，優化速度表現出明顯的提升。

4.2.3 粒子群優化算法流程

根據上述描述，以下部分給出了粒子群優化算法的基本流程。

初始化粒子群P，包括粒子的位置和速度，全局最優值以及局部最優值

4.2.4 ADMPC滾動優化

在每一個采樣時刻，MPC通過求解一個有限時域的優化問題，得到系統當前時刻的最優控制序列，從而實現在整個控制時域內系統的在線閉環控制。多無人機協同航跡規劃ADMPC滾動優化過程如圖6所示。

圖6 多無人機協同航跡規劃ADMPC滾動優化過程Fig.6 Flowchart of multi-UAV cooperativetrajectory planning ADMPCrolling optimization

其中，根據環境約束及自身狀態，多無人機編隊采取必要的隊形變換或避障等動作。通過ADMPC方法對無人機狀態進行預測，得到滾動在線優化問題，通過4.2.3節中提到的FPSO算法對其進行優化求解，計算其最優適應度，得到最優控制序列并執行，完成整個自適應編隊控制任務過程。

4.3 多無人機協同航跡規劃流程

步驟1 首先初始化無人機參數，設置最大速度、最大加速度等限制，設置模型預測控制參數，參數設置如表1所示。

步驟2 根據實時仿真環境，自適應選擇多無人機編隊寬度rf或隊形，計算并更新虛擬編隊引導點及與跟隨無人機的對應關系，并根據式（15）建立各無人機間的通信關系。

步驟3 利用粒子群優化算法，分別對領航無人機和各跟隨無人機進行航跡規劃問題求解。

步驟4 取最優結果的第一個控制輸入模型中，即代入式（5），計算無人機下一時刻的狀態。

步驟5 如果到達目標點，完成協同航跡規劃，任務結束；否則轉步驟2。

5 仿真實驗分析

為了驗證方法的有效性，本文在Matlab 2016環境下進行了仿真實驗。仿真實驗將空間的障礙抽象為大小不一的矩形。領航無人機的預定起始位置為（25，0），目標位置為（-28，-6）。

多旋翼無人機其動力學可以由具有速度和加速度約束的質點動力學模型近似：

實驗主要針對三架無人機在不同編隊形式下的協同航跡規劃，包括線性、三角、線性-三角編隊（二者可根據環境情況相互變換），同時對三種編隊形式下的飛行總航程進行了對比。另外，對九架無人機也做了簡單的仿真實驗，進一步展示了文中方法的靈活性和可擴展性。

表1為仿真實驗參數，包括模型預測控制、多旋翼無人機基本參數、粒子群以及約束等基礎參數。

表1 仿真實驗參數Tab.1 Simulation experiment parameters

實驗一 三架無人機編隊飛行。

圖7展示了三架無人機協同航跡規劃的預測過程，每次預測未來6個航跡點，并將第1個控制輸入作為實際的輸入值，剩下的預測航跡點作為參考航跡點。在此實驗中，跟隨無人機的初始位置分別為P1（26，0）和P2（27，0）。

圖7 多無人機協同航跡規劃預測示意圖（三架無人機）Fig.7 Prediction of multi-UAV cooperative trajectory planning（three UAVs）

如圖8所示，實線為將每次預測值的第1個控制輸入作為實際輸入后規劃的領航無人機完整航跡，6個虛線框則分別展示了不同環境下三架無人機的編隊情況。在經過狹窄區域時，多無人機以線性編隊飛行，而在經過空曠區域時，則以三角編隊飛行。可以看出，在滿足所有約束條件的前提下，規劃出一條能保證多無人機編隊安全穿過障礙區的實際可飛航跡。

實驗二 線性、三角、線性-三角編隊航跡規劃對比。

圖9展示了線性、三角、線性-三角三種編隊形式下的飛行航跡對比情況。線性編隊在編隊航跡規劃中，可簡單地看作單個無人機并需滿足其相應約束條件；而三角編隊必須考慮編隊寬度的約束；線性-三角編隊表示整個編隊根據不同環境進行相應隊形轉換，使得規劃出的航跡在保證較低代價的同時，還滿足假設2的要求。

圖8 多無人機協同飛行過程Fig.8 Processof multi-UAV cooperativeflight

圖9 基于不同編隊形式的航跡規劃對比Fig.9 Trajectory planningcomparison of different formations

表2展示了在三種不同編隊形式下，三架無人機飛行的航程對比情況，可以看出，在以航程為代價的航跡規劃算法中，線性-三角編隊在三種編隊形式里，航跡規劃最優，這也充分說明了本文所提出方法的優勢所在。

表2 不同編隊形式航程比較 單位：mTab.2 Flight distancecomparison of different formations unit：m

實驗三 基于人工勢場法（APF）的編隊協同控制方案［3］與本文編隊協同控制方案的對比。

圖10是利用APF進行多無人機編隊協同控制的飛行效果，實線為虛擬領航無人機飛行航跡（航跡規劃算法與本文一致），4個虛線框展示了4個代表性階段的無人機編隊飛行情況，實線上的圓點為虛擬領航無人機在各階段的對應位置。可以看出，相較于本文的方案（如圖8），基于APF的方案，無人機在一定程度上保持了編隊飛行，但很難形成穩定的編隊隊形，且難以預測任一無人機的飛行情況。

圖11進一步展示了利用本文方案，在整個規劃過程中，其單步時間及平均時間都少于基于APF的編隊控制方案。APF平均時間為0.0918 s，本文方案平均時間為0.0609 s。

實驗四 無人機集群（九架）編隊協同航跡規劃。

無人機集群與三架無人機略有不同，其編隊控制必須考慮通信距離與通信拓撲關系。在本實驗中，增加的無人機以前三架無人機為參考，依據編隊情況，確定相互之間對應的位置關系以及飛行次序，從而建立合理的通信連接，以保證實時信息交換，并形成穩定的編隊隊形。

圖10 基于人工勢場法實現編隊協同控制效果Fig.10 Effect of formation cooperativecontrol based on artificial potential field method

圖11 協同航跡規劃方案時間對比Fig.11 Timecomparison of cooperativetrajectory planningschemes

圖12展示了無人機集群（九架）自適應編隊協同飛行過程圖，實線表示領航無人機飛行軌跡，兩個虛線框展示了兩個代表性階段的無人機協同編隊飛行情況。可以看出，在狹窄區域多無人機以線性編隊飛行，在寬闊區域則以形呈特定編隊飛行。通過此實驗進一步驗證了本文方法，有效解決了多無人機數量限制、通信拓撲結構約束等方面的難題。

6 結語

本文設計的基于自適應分布式模型預測控制的多無人機協同航跡規劃方案，采用模型預測控制思想，將整個控制過程轉化為基于滾動時域的在線優化問題；結合分布式策略，有效解決了多無人機在計算資源與通信鏈路等方面的約束限制問題，以及各無人機獨立避障的問題。采用虛擬結構法與領航跟隨法結合的策略，提出了虛擬編隊引導點，對多無人機進行自適應編隊控制，同時通過改進的粒子群優化算法對整個控制過程進行處理。仿真結果表明，利用本文提出的方案，有效解決了多無人機協同航跡規劃問題，同時在編隊隊形的保持與變換控制中也表現出較好的效果；另外，該方案的應用還極大地增加了多無人機控制的靈活性，并降低了整個控制過程中的代價總和。在今后的工作中，可以把該方案擴展到三維環境下的無人機集群自適應編隊協同航跡規劃問題，并進一步提高求解算法的效率。