基于一致性的多無人機編隊算法研究

高鵬程，劉小雄，黃劍雄，梁 晨

(西北工業大學 自動化學院，西安 710072)

0 引言

從無人機的出現到無人機技術逐漸趨于完善，已經走過了幾十年的歷程。隨著技術的不斷完善，對無人機提出的要求也在不斷提高，任務的復雜程度也在不斷上升，單個無人機的性能逐漸無法滿足當前任務的需求。比如由于無人機的動力限制，它無法攜帶大量的偵察設備或者多種傳感器，這可能會導致無人機功能有較大的局限性[1-2]；單個無人機執行任務的效率較低，一旦遭遇極端天氣或者復雜地形等意外情況，可能會導致無法完成任務；另外如果需要對某一地區或物體進行測繪拍照，由于條件的限制，可能需要多次才能完成全部任務，這就會大大降低使用的效率，提高了運營成本。

針對以上種種情況，學者們研究通過多無人機編隊飛行(Formation Flight)來解決上述問題[3]。通過多無人機編隊飛行，可以大大提高無人機完成任務的效率，拓寬無人機的應用領域，從而實現安全、高效地執行任務。目前多無人機協同編隊在軍事和民用領域都有廣泛的應用。

目前主流的編隊控制算法包括長-僚機，虛擬結構法，人工勢場法[4]，一致性算法[5]以及基于行為的蟻群蜂群等智能算法[6-7]。其中一致性理論由于其實用性強且成本低廉，易于進行工程實現，于是受到學者的廣泛關注。1987年，Reynolds等人首次提出Boid模型[8]，設計了三條規則來模擬鳥群運動；1995年，Vicsek在Boid模型的基礎上提出了“Vicsek”模型[9]；2004年，Olfati-Saber和Murry給出了連續時間內多智能體一致性問題的理論框架[5]，為未來的一致性編隊奠定了方向；2007年，Li研究了有限時間多智能體的一致性編隊情況，并應用于編隊控制和避障的實驗中[10]；2013年，Jakovetic對通訊時延和故障[11]做了深入研究。2019年，侯岳奇建立覆蓋分布地圖描述任務環境，提出了一種以覆蓋率為搜索獎勵的的集群協同區域搜索算法[12]。2020年，張佳龍針對“長機-僚機”編隊模型提出了自適應隊形保持的方法，減弱風場干擾的影響，實現編隊穩定飛行[13]。

上述文獻對無人機編隊飛行控制方法進行了深入研究，基于上述研究基礎，針對一致性算法能夠實現狀態以及行為一致的特點，改進了通信拓撲結構和由于加入了歐氏距離導致的系統不穩定狀況，在此基礎上設計了目標點跟蹤的一致性算法，通過分段控制無人機加減速模態，從而保證無人機編隊飛行的控制效果。

1 基于一致性的無人機編隊建模

基于圖論的設計理論，參考了基本一致性算法原理[14]，對一致性變量的邊界問題進行平滑設計以及限幅處理，并在控制協議中加入了對目標點控制，通過分段規劃獲得最佳的控制效果，設計了一種基于一致性協議的分布式編隊控制算法，實現無人機一致性的穩定控制。

1.1 圖論基本原理

在理想情況下，無人機通訊鏈路就類似于一張網絡，這張網絡可以有方向，也可以無方向。而在數學中，就有專門以圖或者網絡為研究對象的學科，就是圖論，它主要用來描述圖中多個體的拓撲關系。

一個完整的圖包含許多的概念和元素，而通常來說，一個由頂點集V(G)和邊集E(G)組成的二元組就可以稱之為一個圖，一般將其命名為G。其中頂點集V=V(G)，包括圖G中所有的頂點，|V|是圖G的階數。而邊集E(G)中的無序數對表示圖G中連接兩個頂點的邊。此外，邊的權重W也是一個非常重要的圖論的概念。在圖論中邊的長度一般不代表他的實際長度，而是由他的權重W來決定。邊的權重代表這條邊在實際中的長度或者實際含義，一般為正數，但也可以存在負數或者零。

一條邊eij={(vi,vj)∈V}不僅可以有權重，也可以有方向。有方向的圖被稱為“有向圖”，而與之相對的便是“無向圖”。在“有向圖”中，邊eij表示無人機j可以獲得無人機i的信息，反之卻不行。而在“無向圖”中，表示無人機i和j都可以獲得對方的信息。此外，在“有向圖”中還存在父子節點的概念，形如eij的邊就表示，節點i是節點j的父節點，節點j是節點i的子節點。有向路徑也就是由一系列形式為(v1,v2)，(v2,v3)，(vi,vj)的邊所組成的，其中，vi,j∈V。在有向圖中，圓是一個起點和終點是同一節點的有向路徑。規定如果在有向圖中，從某一個節點出發到其他的任意一個節點都存在至少一條有向路徑，那么稱該有向圖為強連通圖。鄰接矩陣是表示圖中各節點之間關系的矩陣，根據圖論知識，給出有向圖的鄰接矩陣表達式：

A=[aij]∈Rn×n,aii=0,aij>0,(i≠j)

(1)

拉普拉斯矩陣表達式：

(2)

根據矩陣的定義能夠知道，矩陣L具有如下性質：

(3)

定義1n表示所有元素都為1的n×1維列向量，0n表示所有元素都為0的1維列向量。那么根據拉普拉斯矩陣的定義有如下等式成立：L1n=0n。例如給定圖形拓撲關系，如圖1所示。

圖1 圖形拓撲關系

根據上述定義，圖1的鄰接矩陣和拉普拉斯矩陣分別表示為：

(4)

1.2 一致性編隊控制協議設計

基于Reynolds等人提出的Boid模型，設計二階一致性控制協議如下：

(5)

(6)

在一致性協議中的rij是一個單位矢量，沿著qi到qj連線，

(7)

定義的新的范數σ-范數如下，

(8)

其中:k和b兩個參數的作用是調節位置控制量的變化快慢和響應速率，k∈(0,1)。該范數是向量先求范數之后進行了限幅的處理，無論無人機之間距離多遠，該參數大小都不會出現突變，能夠更好地調整位置控制量的大小和響應的穩定性。‖z‖σ用來構建多智能體編隊的平滑結構和運動軌跡。當z=0時，‖z‖σ=0。在不同的位置，會選擇不同的參數。

該范數由于參數選擇的不同，可以產生不同的影響方式和大小，從而使編隊控制的運動方式更加平滑高效。

為了平滑編隊軌跡，定義如下的分段函數:

(9)

圖2 fb(z)分段曲線

該函數被定義在[0,1]的范圍內，并且在邊界處漸進為零，在約束條件的作用下，一致性的協同變量可以在單位圓范圍內調整，不會出現極端狀況。因此將這個分段函數用于構建一致性協議的鄰接矩陣。

當h=1時，該分段函數變形為一個單純的脈沖函數，在[0,1]的區間內，函數值均為1，其余為0。利用該分段函數，定義鄰接矩陣中的每個元素為:

Aij(q)=fb(‖qj-qi‖/R),j≠i

(10)

該鄰接矩陣的作用范圍只在范圍為R的球形領域內，當二者的歐式距離超出該范圍則鄰接矩陣取零，而如果在范圍內則不取零，矩陣的元素在[0,1]的范圍內。對于任意兩個被控對象，如果i=j則Aij(q)=0。最后所有的元素組成的矩陣就是該智能體系統的空間鄰接矩陣，矩陣主對角線元素為0。

飛機的運動是依靠發動機的推力以及飛行時產生的空氣動力完成的，通過作用于機翼上的力和力矩來改變飛行的姿態，所以可以將其近似看成是一種具有二階微分的運動模態。綜合以上，單架無人機的運動狀態可以表示為：

(11)

之后將公式(5)改寫成如下形式:

(12)

所以二階動力學方程的矩陣形式為：

(13)

式中,

(14)

求Φ(t)的特征根為：

det(λ2In+(λ+ζ)Ln)

(15)

(16)

其中:ηi是拉普拉斯矩陣Ln(t)的特征值。

當Φ(t)有且僅有兩個零特征根，并且其他非零特征根均具有負實部時，二階編隊系統達到一致性狀態[15]。

1.3 基于目標的一致性編隊算法設計

無人機編隊能夠執行多種任務，實現多種控制指令，而最基礎的編隊任務即為指向目標的無人機編隊。

單純的一致性編隊只能保證多無人機能夠在一致性指令的控制下，在一定時間內完成無人機的一致性編隊，即位置和速度達到一致。而在添加了任務目標之后，重新設計了新的一致性控制協議：

(17)

(18)

qg表示目標點的位置，k和b是之前定義的控制參數。

(19)

(20)

dq表示無人機剩余的路程占總路程的百分比，當無人機還未到達該位置，他會先達到一致之后按照預設的速度前進；到達該位置后它會有一個減速階段。

綜合以上，加入了目標點的單架無人機的運動狀態可以表示為：

(21)

1.4 一致性編隊隊形設計

為了能夠令無人機編隊保持固定的隊形，需要設計一種利用幾何方法表示隊形的方式。本節利用了隊形矩陣表示無人機的幾何位置，以所有無人機的平均位置作為編隊的中心位置，設計了隊形矩陣。

無人機的中心位置為：

(22)

隊形的結構模型是一個3行N列隊形矩陣H:

(23)

N表示無人機的個數，第一行為無人機隊形的序號，第二行和第三行分別表示無人機與編隊中心位置X方向和Y方向之間的距離，以圖3為例。

圖3 菱形編隊隊形

該隊形的隊形矩陣為：

(24)

2 領航-跟隨控制邏輯

領航跟隨控制的基本方法是在編隊中設定一個或幾個領航者，其余成員作為跟隨者，在編隊航行的過程中領航者的作用是控制整個編隊的行進路線，作為跟隨者的成員與領航者之間保持一定的角度和距離便可以控制編隊隊形。其中領航者既可以是真實的個體成員，也可以是虛擬領航者。

圖4 領航跟隨示意圖

根據領航-跟隨的控制思想可知，在領航員的位置和航向角已知的情況下，只要設定跟隨者相對于領航者的期望距離和期望相對角度，那么跟隨者的位置期望值就唯一地被確定下來。無人機之間的距離以及相對角度可通過傳感器數據計算獲得，所以只要將兩機間的實際距離和相對角度限定在期望值所允許的誤差范圍內,即可達到隊形控制的目的。由于目前模型為質點模型，沒有姿態信息，所以只要考慮二者之間的距離即可。

‖qleader-qfollower‖∈[a,b]

(25)

之后設計簡單的L-F邏輯:

(26)

結合上文提出的一致性控制算法，設計了新的領航跟隨控制算法

首先定義Leader和Follower的實時距離lerr=ql-qi，

(27)

d是L-F結構中設計的安全距離，保證L和F能夠安全實現編隊。

之后設計L-F的減速階段，令跟隨者能夠和領航員保持一定的安全距離且速度保持一致:

(28)

B是一個交互矩陣，通過B來調整跟隨者的控制順序和信息是否交互。

上述公式就是領航-跟隨的速度控制項，當L與F之間的距離大于5時，會以預設速度一直前進同時靠近領航員；當二者距離小于5時，會進入減速階段，保證領航跟隨結構的安全性。

綜上，基于領航跟隨算法的一致性編隊控制算法如下：

(29)

領航跟隨編隊的具體步驟如下。

步驟1：設計領航員的控制方法和行進路徑，根據信息交互矩陣B，選擇合適的跟隨者與領航員進行信息交互，將該跟隨者命名為跟隨者α。

步驟2：跟隨者α基于領航跟隨控制算法開始跟蹤領航員，同時跟隨者之間信息交互，獲得彼此的位置和速度。多個跟隨者從不同位置出發，以跟隨者α為基準開始進行一致性編隊。

步驟3：根據隊形矩陣，分配給跟隨者不同的位置，跟隨者在跟蹤領航員的同時逐漸實現編隊一致性，速度和位置達到一致。

步驟4：當跟隨者α與領航員的距離到達安全距離時，跟隨者進入減速階段，保證領航員與跟隨者不會產生機間碰撞，消去跟隨者由于速度過大導致的超調。

步驟5：當領航員與跟隨者的距離達到預設距離時，編隊系統達到動態穩定的狀態，領航員與跟隨者之間的距離保持一致，速度基本相同。

在編隊任務的過程中，當領航員與跟隨者距離大于安全距離時，始終基于公式(28)的二式進行計算，跟隨者能夠基于設定的安全速度，盡快的縮短與領航員之間的距離。之后將步驟1、2、3進行多次迭代。

當領航員與跟隨著距離小于預設的安全距離時，會基于式(28)的一式進行編隊控制，之后將步驟1～4重復迭代，最終實現無人機的編隊。

3 仿真驗證與分析

為了驗證一致性控制協議的控制效果，進行一致性編隊仿真驗證，之后在此基礎上加入領航跟隨的思想，設計無人機從不同位置以不同速度出發，在運動過程中形成編隊。

在仿真環境中，搭建編隊協同控制模型，設計具體參數如下：k=0.5,b=1,c=15，d=2，k1=1.44，k2=0.32。

首先測試了基礎的一致性編隊算法，先采用了ode函數進行狀態分析，認為設計初始狀態，之后通過ode45函數求出對應時間的狀態值。在函數里，將目標當前狀態和時間區間(或步長)作為函數的輸入值，將狀態的微分作為函數的輸出。之后根據目標的狀態和鄰接矩陣Aij(q)做矩陣運算。

實驗中設置目標點為[10,10],無人機數量為4個，仿真時間為10 s，仿真步長為0.01，仿真曲線如圖5所示。

圖5 目標隨時間的軌跡變化

圖6 XY方向速度隨時間的變化

圖中“○”是旋翼無人機的初始位置，“☆”無人機的最終位置，在2 s時各無人機已經達到位置和速度一致，保持著1.5 m/s的速度前進，之后在5 s時開始陸續出現減速階段。出現這種情況是因為不同的無人機與目標點的距離不同導致。最后成功到達目標點附近且速度基本為0。

之后以菱形隊形作為編隊隊形進行仿真驗證，目標點為[22.5,25]，仿真時間和步長不變。

如圖7可知，無人機從不同的隨機位置出發，在經過一定時間后，完成的菱形的編隊隊形。之后一直以該隊形前進，仿真效果與預期基本保持一致。

圖7 目標隨時間的軌跡變化

最后設計了“一帶四”的領航跟隨任務，一個領航員，4個跟隨者。設置領航員初始坐標為[5.7,9.2](隨機生成)，用類正弦信號作為領航員的加速度輸入，設置仿真時間為20 s，步長仍為0.01，仿真曲線如下所示。

從圖8中可以看出，跟隨者能夠較好的跟蹤領航員的軌跡，實現領航跟隨的隊形保持，在10 s后領航員和跟隨者的距離基本保持在一定范圍內。而在圖10中，實線表示X方向的速度差，虛線表示Y方向的速度差，領航員與跟隨者的速度差從2 s開始就保持在很小的范圍內，且跟隨者速度保持一致；在第12 s左右領航員與跟隨者速度基本保持一致。

圖8 目標隨時間的軌跡變化

圖9 領航者與跟隨者位置誤差

圖10 領航員與跟隨者速度差隨時間的變化

4 結束語

基于一致性算法并對該算法進行了改進和平滑處理，從而設計編隊控制律和領航跟隨隊形保持控制律。

首先對一致性算法進行改進，加入了鄰域概念和區間分段函數，利用sigmod函數設計新的分段函數，使數據可以平滑過渡且在邊界處漸進為0。之后將分段函數利用到無人機的信息控制交互范圍內，設計了無人機信息交互鄰域，從而定義了新的鄰接矩陣Aij(q),當無人機之間的距離超出交互距離之后，無人機的通信交互逐漸過渡為0。在設計一致性編隊的速度匹配項和位置控制項的參數時，定義新的范數形式σ-范數，將各項控制量的參數大小控制在一定范圍內，保證編隊的平滑和魯棒性。最后基于領航跟隨的思路設計了領航跟隨隊形保持算法，編隊效果達到了預期目標，實現了一帶四的領航跟隨隊形，且速度和位置能夠保持高度的一致。由于尚未考慮編隊的工程實現問題，之后將重點考慮編隊無人機的工程實現可行性，開發工程樣機進行物理仿真。