基于一致性理論和S-MPC的四旋翼編隊協同避障

胡樹欣, 張 安, 孫嫚憶, 李銘浩

(西北工業大學航空學院, 陜西 西安 710072)

0 引 言

多機編隊協同控制研究已成為近年來無人機系統領域的一個前沿課題,文獻[1-6]中提出一些具有代表性的編隊控制方法,包括虛擬結構、行為和基于領航-跟隨的編隊控制方法。編隊協同控制不僅可以發揮單架無人機具有的機動性能優勢,還可以使多架無人機協同合作共同完成較為復雜的任務[7-12],是一種典型的多智能體系統[13],而這些協同控制任務的實現均建立在多智能體系統的一致性問題研究框架內。一致性理論的基本思想是編隊中每架無人機都可以與其鄰居無人機進行通信,通過設計一種分布式的算法使得無人機的狀態變量達成一致[14],以此來實現無人機間的合作。

在無人機編隊飛行中,安全問題是首要考慮的關鍵因素之一,避障研究就顯得尤為重要。和單機避障相比,編隊飛行不僅要考慮外部障礙,還需考慮內部的避碰、避障完成后隊形的重構等問題[15]。文獻[16]采用人工勢場法,通過構造避障懲罰函數解決了多智能體的避障問題,并證明了算法的全局漸進穩定性和最優性,最后通過數值仿真實驗驗證了算法的有效性。但其沒有考慮無人機的狀態、輸入約束等。文獻[17]以一致性協議為基礎構造時變編隊,并通過設置虛擬領導者引導編隊移動,避障方法采用人工勢場法,通過數值仿真驗證了時變編隊飛行過程中避障算法的有效性。文獻[18]基于速度障礙法將避碰、避障等問題轉化為凸優化問題的約束條件,通過構建代價函數來求解最優控制輸入,最終實現了三維環境中的多無人機局部運動規劃、避碰和避障。文獻[19]采用人工勢場法,當兩架無人機之間的距離小于給定值時,將人工勢場力作為修正項添加到一致性算法中,在保證無人機內部避碰的前提下使一致性算法仍能收斂。但在使用人工勢場法時,存在算法陷入局部最優解的問題,且在改變機群的編隊隊形時需要調整原有勢場函數的參數大小,靈活性較低。文獻[20]基于改進一致性算法設計無人機編隊,在避障問題中分別引入粒子群算法和模型預測控制(model predictive control, MPC)算法來處理靜態和動態障礙物,通過數值仿真驗證了算法的有效性,但該算法缺少相關的穩定性和收斂性證明。

本文受文獻[21-27]啟發,針對無人機編隊的隊形生成、保持及靜態障礙物避障問題,以一致性協議為基礎構造編隊控制器,通過設置領航者引導編隊移動,避障方法采用安全攸關MPC(safety-critical MPC, S-MPC)算法,使得無人機能夠在距離障礙物一定距離前對其進行規避,可有效避免在實際飛行過程中因反應時間不夠而造成危險。最后,通過數值仿真實驗驗證了無人機編隊飛行過程中躲避障礙物的能力。

1 問題描述

1.1 四旋翼無人機動力學模型

考慮存在一個由1架領航無人機和n架跟隨無人機組成的四旋翼無人機編隊系統,其中每架四旋翼無人機具有相同的運動特性,其結構形式如圖1所示。

圖1 四旋翼無人機結構圖Fig.1 Structure diagram of quadrotor unmanned aerial vehicle

為了簡化四旋翼無人機模型,做出如下假設:

假設 1四旋翼是剛體,其質量和轉動慣量不變,幾何中心與重心一致。忽略空氣阻力的影響。

假設 2螺旋槳對推力指令的響應足夠快,可以忽略從控制器向電機發出推力指令到螺旋槳產生實際推力的時間延遲。

假設 3在飛行過程中,偏航角始終不發生變化。

在以上假設的基礎上,為簡化系統模型,在定點懸停的平衡狀態下,將非線性四旋翼模型進行線性化處理。簡化后四旋翼系統動力學模型如下所示:

(1)

式中:k∈{1,2,3}分別代表x、y、z3個維度。3個方向上狀態向量ξk、狀態矩陣Ak、輸入矩陣Bk和控制輸入uk的具體形式如下:

(2)

(3)

(4)

(5)

表1給出了式(2)～式(5)的符號解釋。

表1 控制輸入和狀態符號定義

在水平方向x,y上,模型組合后可表示為

(6)

式中:ξh和τ分別為水平面上的組合狀態向量和組合控制輸入向量,ξh的具體形式如下:

ξh=[s(0)T,s(1)T,s(2)T,s(3)T]T

(7)

式中:s(k)定義為組合向量,其表達式為

(8)

(9)

(10)

同樣,用水平面上縱向控制輸入τθ和橫向控制輸入τφ定義組合輸入向量τ:

(11)

狀態矩陣Ach和控制輸入矩陣Bch分別表示為

(12)

式中:?表示Kronecker積;IN∈RN×N表示N維單位矩陣。

同時,高度方向狀態空間模型可表示為如下形式:

(13)

式中:ξv=[z(0)z(1)]T=[z-w]T;f=ftotal/m;狀態矩陣Acv和控制輸入矩陣Bcv分別表示為

(14)

式(6)和式(13)中連續時間模型可離散化表示為

ξh(k+1)=Ahξh(k)+Bhτ(k)

(15)

ξv(k+1)=Avξv(k)+Bvf(k)

(16)

式中:k是一個非負整數,表示時間步長;t時刻仿真時間,表示為t=kΔt,Δt為仿真步長,同時:

(17)

(18)

1.2 圖論基礎

無人機編隊系統的通信拓撲結構可以用圖G=(V,E)來描述,其中V={1,2,…,n}表示圖G中的節點集合;E?V×V表示邊(i,j)的集合,其中i∈V,j∈V。用圖G中的節點i代表編隊系統的第i架無人機,邊(i,j)∈E代表節點i能夠獲取節點j的狀態信息,且稱節點j是節點i的鄰居。用Ni={j|(i,j)∈E}表示節點i的鄰居集。若無人機i與j之間可雙向通信,圖G為無向圖。其拉普拉斯矩陣定義為L=D-A。其中,D=diag{di}表示其入度矩陣,di為向節點i傳輸信息的節點數目,其形式如下:

(19)

A=[aij]表示其鄰接矩陣;aij為邊(i,j)的權值,可表示為

(20)

1.3 編隊問題描述及求解框架

在本文中,考慮以下控制任務:n架無人機在三維空間中跟隨其領航者編隊飛行,同時避免碰撞。圖2顯示了4架無人機跟隨領機編隊飛行。其中,Leader表示領航無人機,Quadrotor 1～Quadrotor 4表示4架四旋翼無人機。

圖2 編隊控制問題描述Fig.2 Description of formation control problem

編隊控制問題求解框架如圖3所示,其中UAV1～UAVn表示第1架～第n架無人機,UAV(#1)～UAV(#n)表示無人機信息通信傳輸流。具體而言,每架無人機在避免碰撞的同時收斂到時變的期望位置。時變期望軌跡由頂層基于一致性理論的編隊控制器產生。為了實現這一控制目標,做出以下假設:

假設 4每個無人機都必須連接到網絡上的領導者,但并非所有無人機都與領導者有直接聯系。

假設 5所有網絡連接必須是雙向的。

假設 6領導者的行動必須獨立于無人機,也就是說,領導者的行動不受任何無人機的影響。

圖3 編隊控制求解框架Fig.3 Formation control solution’s framework

2 基于一致性理論的編隊控制器

在領航-跟隨控制策略下,基于一致性理論設計編隊控制器。該控制算法的優點是,所有無人機都不一定需要直接連接到領航者,如果網絡結構滿足編隊問題描述中的假設4～假設6,則無人機與其鄰居通信就足夠了。無人機i在水平面上編隊飛行的控制律由式(21)和式(22)給出:

i∈{1,2,…,n};j∈{1,2,…,n+1}

αk>0, ?k∈{0,1,2,3}

(21)

i∈{1,2,…,n};j∈{1,2,…,n+1}

βk>0,k∈{0,1}

(22)

以水平方向為例,為了得到各架無人機的期望狀態軌跡,采用基于一致性的協同控制算法,即式(21)計算下一階段的期望狀態,并通過重復該過程生成預測時域內的期望狀態。設Np∈R,Nc∈R分別為預測時域和控制時域。計算期望狀態軌跡的控制算法如下。

算法 1 期望軌跡生成算法 步驟 1 獲取當前狀態通過網絡獲取每架無人機的當前狀態ξhi(k)(i∈{1,2,…,n})。步驟 2 計算下一時刻的期望狀態通過無人機當前狀態,計算下一時刻水平方向上的期望狀態ξrefhi(k+1)(i∈{1,2,…,n})。 ξrefhi(k+1)=Ahξhi(k)+Bhτi(k), i∈{1,2,…,n}式中:τi(k)為通過式(21)獲得的水平方向的編隊飛行控制輸入。步驟 3 計算預測時域內的期望狀態軌跡將從步驟2中得到的所需狀態替換為當前狀態。返回到步驟2,并在接下來的時間步長中計算所需的狀態。重復這一過程,直到獲得(k+Np)時刻水平方向上的期望狀態。步驟 4 生成期望軌跡將預測時域內的期望狀態組合如下:ξ～refhi=[ξTrefhi(k+1),ξTrefhi(k+2),…,ξTrefhi(k+Np)]T,i∈{1,2,…,n}

算法1根據無人機當前狀態,生成各無人機預測時域內編隊飛行期望狀態軌跡。

3 基于S-MPC的避障控制

為了避免碰撞,通常采用的方法是為障礙物添加膨脹半徑,這種做法實際上是等同于縮小了障礙物可行集的范圍。這種基于歐幾里得范數的避障約束并不會影響無人機的運動,并不能使無人機在比較遠的距離就開始避障,在實際操作過程中常常會存在反應時間不夠而造成的危險。通過模型預測控制,在確保集合不變性的前提下,將限制無人機運動加入到優化過程中。通過將控制障礙函數(control barrier function, CBF)添加到模型預測控制器的優化約束中,以保證MPC算法的安全性。下面將介紹二者的結合使用。

3.1 MPC

MPC是一種基于模型的控制方法,通過對系統動態模型的預測來生成控制策略,從而實現對系統的控制[28]。MPC將系統的控制問題轉化為優化問題,通過對未來一段時間內的狀態和控制變量進行預測,計算出滿足約束條件的最優控制策略。隨著預測時間步長的不斷向前,MPC將不斷更新控制策略,以實現系統的穩定性和最優性能[29]。

無論結構和形式如何,MPC一般都具有如下特點:預測模型、滾動優化和反饋校正[30]。由于模型預測控制具備上述特點,基于該算法的控制器可以取得較好的控制效果并且具有很強的抗干擾性能。MPC的控制原理如圖4所示。

圖4 MPC原理Fig.4 Principle of MPC

(23)

(24)

(25)

第i架無人機通過解決以下局部子問題,來跟蹤水平方向上的期望狀態軌跡。子問題描述如下:

(ξhi(k+Np)-ξrefhi(k+Np))+

(ξhi(k+j)-ξrefhi(k+j))+

(26)

式中:下標i∈{1,2,…,n}表示第i架無人機;Qi為非負定矩陣;Ri為正定矩陣。令Ξ=Ah+BhKh,終端權重Pi必須滿足以下線性矩陣不等式(linear matrix inequality, LMI)以保證MPC的穩定性:

(27)

Pi>0

(28)

式中:Kh是一個狀態反饋控制增益矩陣,用于在水平方向穩定無人機系統。子問題對狀態和控制輸入沒有顯式的不等式約束,以盡可能快地計算求解優化問題。圖5是由4架無人機和領航者組成的多無人機系統通信網絡結構。

圖5 編隊通信網絡結構Fig.5 Formation communication network’s structure

每個無人機執行的編隊控制算法如算法2所示。

算法 2 編隊飛行軌跡跟蹤算法 步驟 1 當前狀態的獲取:獲取當前狀態ξhi(k)。步驟 2 信息傳輸:按照從領航者出發的通信網絡拓撲結構,將信息傳輸給直接連接的無人機,直到領航者獲得所有無人機的信息。某一無人機發送的信息由其自身的當前狀態和其接收的信息組成。步驟 3 期望狀態軌跡的生成:依據算法1的步驟2～步驟4,領航者計算每架無人機的期望狀態軌跡。步驟 4 信息傳輸:按照與步驟2相反的順序將信息傳遞給直接連接的無人機,該信息由期望狀態軌跡和所有無人機的當前狀態組成。步驟 5 優化求解:通過數值計算求解子問題的最優解τ～*i。步驟 6 施加最優控制輸入:將最優控制輸入序列τ～*i的第一個分量τ*i(k)作用于UAVi。步驟 7 重復步驟1～步驟6。

在步驟5中,每架無人機只解決自己的子問題以規劃自己的行動,并且與其他無人機并行解決問題,而不是按順序解決問題。這使得無人機的決策獨立于其他無人機的決策。步驟2和步驟4中的步驟減少了通信的數量。由于無人機內嵌無線通信模塊的可靠性較差,因此盡量減少無線通信的數量是非常重要的。

3.2 穩定性證明

定理 1假設一個多無人機系統包含n(n≥1)架無人機和一個領航者。同時,滿足假設4～假設6。當每架無人機執行算法2時,若終端權重矩陣Pi滿足式(27)和式(28),算法2中的MPC器將保證漸近穩定,無人機將收斂到水平方向編隊飛行的期望狀態。

證明由于子問題是一個凸二次規劃(quadratic programming, QP)問題,因此其在任何時間步長都存在解。另外,當滿足假設4～假設6時,領航者可以通過算法1生成編隊水平方向飛行的期望狀態軌跡。選用以下值函數作為Lyapunov函數:

(29)

(30)

因此,為了保證MPC的穩定性,需要有如下不等式成立:

ΔVi(k)=Vi(ξhi(k+1))-Vi(ξhi(k))<0

(31)

(32)

現在,考慮以下不是最優控制序列的控制輸入:

(33)

式中:τi(k+Nc)是使無人機系統在水平方向穩定的控制輸入。

當Kh是使無人機系統在水平方向穩定的狀態反饋增益矩陣時,考慮將以下控制輸入作為穩定的控制輸入之一:

(34)

上述控制輸入是虛擬的,并不將其作用于無人機系統。根據式(33)和式(34),式(32)可以改寫為

(35)

τi(k)滿足在預測時域內足夠小,則表明:

ξrefh(k+Np+1)=Ahξrefh(k+Np)

(36)

由式(6)和式(36)可知,離散時間模型中變量ξh(k+Np)可表示為

(37)

由式(37),可將式(35)改寫為

(38)

由于不等式的后兩項總是負的,為使ΔVhi(k)<0,必須滿足以下不等式:

(39)

Pi>0

(40)

系統的漸近穩定性可以用Lyapunov定理求解。因此,當k→∞時,UAVi的狀態滿足ξhi(k)→ξrefhi(k),這使得在水平方向上編隊飛行收斂到所需的狀態。

證畢

3.3 控制障礙函數

將離散時間CBF添加到MPC控制器的約束中處理安全性約束。對于安全性控制問題,定義在自由空間C(沒有障礙物)中一個連續可微的函數h(χ):X?Rn→R,其中C為安全集:

C={χ∈X:h(χ)≥0}

(41)

函數h(·)即為CBF,同時有?χ∈?C,?h/?χ≠0,并且存在一個擴展的κ∞類函數γ(·),使得對于控制系統,h(·)滿足:

(42)

該條件可以擴展到離散時間域,如下所示:

Δh(χk,uk)≥-γ(h(χk)), 0<γ≤1

(43)

式中:Δh(χk,uk)=h(χk+1)-h(χk),將其代入式(44),可以得到h(χk+1)≥(1-γ)h(χk),即控制障礙函數h(·)的下限隨著速率1-γ的值呈指數下降。

將CBF約束添加到優化問題中,得到:

(44)

式中:χ=[xy]T代表水平面上的位置向量。

4 仿真分析

4.1 避障控制仿真實驗

在提出S-MPC控制設計的基礎上,在二維平面內進行了仿真驗證,并分析了其特性,采用IPOPT求解器解決S-MPC中的優化問題。

對于離散時間控制障礙函數約束,選擇二次障礙函數:

(45)

式中:xobs、yobs、robs分別描述了障礙物的x、y的坐標和半徑,障礙物信息如表2所示,障礙物在仿真圖中用橙色圓圈所示。起始位置和目標位置分別為(-5,-5)和(0, 0),用黑色菱形標記。

表2 障礙物信息

(1) 參數γ對控制器的影響分析

在圖6中,γ=0.1,0.2,0.5,1.0的S-MPC控制器規劃的軌跡分別用紅色、綠色、藍色和紫色線表示。從圖6可以看出,隨著γ的降低,系統開始更早地避開障礙物,這意味著更小的安全集。圖7、圖8分別為不同γ參數值下S-MPC控制器每次求解優化問題的時間及其平均值。可以看出,γ參數并不會影響控制器的求解效率。

圖6 不同γ參數下的S-MPC規劃軌跡Fig.6 S-MPC planning trajectory with different γ parameters

圖7 不同γ參數下的S-MPC每次迭代計算時間Fig.7 Calculation time of each iteration of S-MPC with different γ parameters

圖8 不同γ參數下的S-MPC平均計算時間Fig.8 Average calculation time of S-MPC with different γ parameters

表3 參數γ影響分析

(2) 參數Np對控制器的影響分析

圖9為不同預測時域Np下S-MPC控制器規劃的軌跡。由圖9可知,Np值對系統開始規避障礙物的遠近并未產生較大影響。這是由于離散時間CBF強制了安全集的不確定性,它允許S-MPC具有較小的Np值,以實現與具有較大Np值的S-MPC相同的性能。圖10、圖11為每步的求解時間及其均值,可以看出,S-MPC的預測范圍越小,計算時間越短。

圖9 不同Np參數下的S-MPC規劃軌跡Fig.9 S-MPC planning trajectory with different Np parameters

圖10 不同Np參數下的S-MPC每次迭代計算時間Fig.10 Calculation time of each iteration of S-MPC with different Np parameters

圖11 不同Np參數下的S-MPC平均計算時間Fig.11 Average calculation time of S-MPC with different Np parameters

(3) S-MPC預測性能分析

圖12為存在3個障礙物的空間中,S-MPC控制器規劃出的軌跡,當其中一個障礙物由(3,2)位置移動到(5,2)位置時,最優軌跡從左側障礙物下方移動到左側障礙物上方,這充分體現了S-MPC預測控制的特點。

圖12 S-MPC軌跡規劃Fig.12 S-MPC planning trajectory

(4) S-MPC避障能力分析

圖13 S-MPC和APF規劃軌跡Fig.13 S-MPC and APF planning trajectories

4.2 編隊控制仿真實驗

實驗采用1架領航者(UAV5)和4架跟隨者的編隊機群,其通信拓撲結構如圖14所示。各無人機的初始位置如表4所示。各參數設置為

(46)

圖14 編隊通信拓撲結構Fig.14 Structure of formation communication topology

表4 無人機初始狀態

在一致性編隊控制協議下,通過S-MPC跟蹤頂層生成的期望軌跡,同時引導編隊避障。無人機編隊飛行軌跡如圖15所示。從圖15可以看出,仿真開始后,無人機編隊基本在1 s內形成了預期的隊形,并保持該隊形飛行。在距離障礙物一定距離時,領航機提前引導編隊對障礙物進行規避,且在過程中,編隊隊形只在小范圍內調整,基本維持不變。

圖15 無人機編隊飛行軌跡Fig.15 Unmanned aerial vehicle formation flying trajectory

圖16、圖17分別給出了5架無人機的位置和速度曲線,從圖16和圖17可以看出,5架無人機基本在2 s內跟蹤到了領航機的運動軌跡,形成了期望隊形,隨后保持編隊系統位置和速度的一致性,完成了避障任務。圖17中的速度曲線基本位于其約束集,滿足狀態約束。針對初始速度不滿足約束等特殊情況,在S-MPC優化問題有解的前提下,通過施加狀態軟約束使速度曲線基本在1 s前進入約束集內。

圖16 無人機編隊系統位置跟蹤一致性Fig.16 Position tracking consistency for unmanned aerial vehicle formation system

圖17 無人機編隊系統速度跟蹤一致性Fig.17 Velocity tracking consistency for unmanned aerial vehicle formation system

5 結 論

本文針對無人機編隊系統隊形保持和避障控制問題,提出了基于S-MPC和一致性理論的分布式多無人機協同避障控制算法。在領航跟隨策略下,設計基于二階和四階一致性的編隊控制算法,為每架無人機生成期望軌跡。進一步地,通過在S-MPC算法中引入CBF約束,實現了編隊系統對障礙物的較早規避,規劃軌跡滿足狀態約束,且更為平滑,使飛行能量損耗更少。最后,進行無人機編隊避障飛行仿真實驗。仿真結果表明,本文所提出的控制算法具有較好的控制效果,能夠有效規避外部障礙。

本文編隊飛行僅在二維平面進行障礙物規避,并未考慮三維環境下的協同避障問題。后續將結合無人機軌跡規劃設計的方法,實現無人機編隊的三維避障飛行控制。