過娟, 褚晶, 閆杰

(1.西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安　710072; 2.西北工業(yè)大學(xué) 航空學(xué)院, 陜西 西安　710072)

?

基于控制向量參數(shù)化和迭代凸規(guī)劃的無人機(jī)編隊(duì)飛行的建立

過娟1, 褚晶2, 閆杰1

(1.西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安710072; 2.西北工業(yè)大學(xué) 航空學(xué)院, 陜西 西安710072)

摘要：多個無人機(jī)組成的編隊(duì)飛行可以作為很多應(yīng)用中現(xiàn)有技術(shù)的一種高效、低成本的替代方案。研究了能滿足最終編隊(duì)隊(duì)形約束的且能量最優(yōu)的無人機(jī)編隊(duì)飛行的建立。首先，將編隊(duì)建立問題建模成一個受非線性動力學(xué)約束的能量最優(yōu)控制問題。其次，采用直接法來求解該最優(yōu)控制問題。在采用的直接法中，利用控制向量參數(shù)化方法將原始問題轉(zhuǎn)化成一系列待求解的凸優(yōu)化問題；然后，運(yùn)用迭代凸規(guī)劃(SCP)技術(shù)求得其最優(yōu)解。在求解過程中，SCP迭代的每一步本質(zhì)上都是一個二次型規(guī)劃問題；由于該規(guī)劃問題的約束是線性約束，因此可以高效地進(jìn)行求解。最后，將提出的方法運(yùn)用到3個無人機(jī)的V型編隊(duì)建立中，并使用了免費(fèi)的開源求解器CVX進(jìn)行求解。與MATLAB中提供的全局優(yōu)化技術(shù)相比，文中提出的算法能夠快速地收斂到全局最小值。

關(guān)鍵詞：無人機(jī)編隊(duì)飛行；最優(yōu)控制；控制向量參數(shù)化；迭代凸規(guī)劃

編隊(duì)飛行中的多個無人機(jī)(UAVs)可以通過相互協(xié)作實(shí)現(xiàn)單個無人機(jī)無法完成的復(fù)雜任務(wù)。因此，對現(xiàn)存的很多技術(shù)而言，多個無人機(jī)的編隊(duì)飛行，特別是小型無人機(jī)的編隊(duì)飛行，是一個前景廣闊且成本很低的替代方案，能夠在很多領(lǐng)域得到廣泛應(yīng)用，例如國土測繪、海洋勘探、高速公路/石油/天然氣/電力線路巡檢、氣象/環(huán)境監(jiān)測、警用巡邏、災(zāi)情防護(hù)、應(yīng)急救援、公共安全、通信中繼等[1]。

然而，在許多應(yīng)用中，為了能實(shí)現(xiàn)之后的協(xié)同操作，多無人機(jī)首先需要建立起所需的編隊(duì)。例如，一組無人機(jī)從一個小型跑道起飛后，通常會形成一個直線編隊(duì)[2]。然而為了執(zhí)行某個偵察任務(wù)，則要求一個V型編隊(duì)以降低被敵方雷達(dá)發(fā)現(xiàn)是多架飛機(jī)的概率。因此，這組無人機(jī)需要根據(jù)當(dāng)前的飛行狀態(tài)建立起任務(wù)所需的V型編隊(duì)。該編隊(duì)的建立過程通常被要求是能量最優(yōu)的，使得全局能量消耗最?。换蛘呤菚r間最優(yōu)的，使得系統(tǒng)快速具備空中作業(yè)的能力，例如，快速實(shí)現(xiàn)對森林火災(zāi)的監(jiān)控為消防單位提供寶貴的災(zāi)情信息。此外，編隊(duì)的建立還需滿足各種約束。這里主要有2種類型的約束。一種是局部約束，例如動力學(xué)約束、狀態(tài)約束、控制輸入約束、避障約束等；另外一種是耦合約束，這種約束與編隊(duì)中的其他無人機(jī)相關(guān)聯(lián)，例如無人機(jī)之間的避撞約束、通信網(wǎng)絡(luò)拓?fù)浼s束、最終編隊(duì)隊(duì)形約束等。

本文研究了典型的局部約束和耦合約束下，能量最優(yōu)的無人機(jī)編隊(duì)飛行的建立。在本文的研究中，局部約束包括狀態(tài)約束、控制約束和非線性動力學(xué)約束。與線性動力學(xué)約束相比，非線性動力學(xué)約束更加復(fù)雜，這是因?yàn)榫€性動力學(xué)的解可以通過解析推導(dǎo)獲得[3]。同時，對于典型耦合約束，本文考慮了最終隊(duì)形約束。在本文中，最終隊(duì)形約束不僅約束無人機(jī)的最終狀態(tài)，同時也存在著待確定的未知參數(shù)。例如，為了建立一個V字型編隊(duì)，最終隊(duì)形約束只設(shè)定了無人機(jī)之間的最終相對位置，然而在空間什么位置建立起所需編隊(duì)卻作為待優(yōu)化的未知參數(shù)進(jìn)行求解。

一般地，可以將編隊(duì)的建立建模為開環(huán)最優(yōu)控制問題，然后采用直接法或間接法進(jìn)行求解[4-5]。直接法將最優(yōu)控制問題轉(zhuǎn)換為參數(shù)優(yōu)化問題進(jìn)行求解，而間接法利用由變分法推導(dǎo)出的解析必要條件進(jìn)行求解。然而，對于存在終端約束的編隊(duì)建立(例如，最終編隊(duì)隊(duì)形約束)，間接法不再適用，這是因?yàn)榇藭r由最優(yōu)一階必要條件得到的兩點(diǎn)邊值問題(TPBVP)的解很難求得[4-5]。相反地，直接法更加適合求解此類問題，因?yàn)榫庩?duì)建立問題被直接轉(zhuǎn)換為參數(shù)優(yōu)化問題后，可由各種成熟的非線性規(guī)劃(NLP)求解器求解。

本文的目的便是為多無人機(jī)的編隊(duì)建立設(shè)計(jì)一個基于直接法的最優(yōu)控制算法。為實(shí)現(xiàn)這一目標(biāo)，我們利用控制向量參數(shù)化方法將原問題轉(zhuǎn)化為參數(shù)優(yōu)化問題。在控制向量參數(shù)化方法中，時間域被劃分成有限個子區(qū)間，且在每個子區(qū)間中，控制量被近似為一個分段常值參數(shù)[6]。之后可利用諸如龍格-庫塔法進(jìn)行非線性動力學(xué)方程的求解。這樣，原始最優(yōu)控制問題就可以被轉(zhuǎn)換為一個非線性規(guī)劃問題，而非線性規(guī)劃問題可通過多種技術(shù)求解。本文采用迭代凸規(guī)劃(SCP)技術(shù)對其進(jìn)行求解，這主要是因?yàn)镾CP簡單、高效且是免費(fèi)開源的[7]。而在運(yùn)用SCP之前，需要推導(dǎo)出狀態(tài)約束和最終編隊(duì)約束相對于優(yōu)化變量的梯度。本文采用靈敏度法求得上述梯度，主要原因是靈敏度法精度較高并且便于實(shí)現(xiàn)。在本文提出的方法中，SCP迭代的每一步本質(zhì)上是一個受線性約束的二次型規(guī)劃問題。對于這種問題，如果采用定制的而非標(biāo)準(zhǔn)的優(yōu)化算法，求解速度可以提高2～3個數(shù)量級[8]。

1編隊(duì)建立問題的建模

在本節(jié)中，將受非線性動力學(xué)約束，且能量最優(yōu)的多無人機(jī)編隊(duì)的建立建模成一個最優(yōu)控制問題。在問題建立之前，首先設(shè)定本文研究的背景。

1.1研究背景

假設(shè)現(xiàn)在有n個無人機(jī)在三維空間中自由飛行。而由于任務(wù)要求的改變,需要這n個無人機(jī)建立一個編隊(duì),且該編隊(duì)需滿足最終幾何約束,即最終隊(duì)形約束。這些幾何約束可以由1組未知參考參數(shù)和相應(yīng)設(shè)定的差量確定。例如,在V型編隊(duì)約束中,參考參數(shù)是編隊(duì)建立的空中位置,而相應(yīng)差量是每個無人機(jī)相對于該位置的相對位置。因此,只需要對參考參數(shù)進(jìn)行優(yōu)化。除了參考參數(shù)的優(yōu)化之外,編隊(duì)的建立還需要在規(guī)定的時間內(nèi)完成并盡可能少地消耗能量。另外,在此過程中,無人機(jī)的狀態(tài)變量必須滿足非線性動力學(xué)約束,控制輸入也要滿足無人機(jī)平臺的控制輸入約束,并且每個無人機(jī)的狀態(tài)變量應(yīng)該始終保持在它們的規(guī)定范圍域內(nèi)。

在本文研究中,無人機(jī)飛行的非線性動力學(xué)方程表示如下[9]

(1)

式中,Xi=[xi,yi,zi,θi,ψi]T是第i個無人機(jī)的狀態(tài)變量,Ui=[ui1,ui2]T是控制向量,(xi,yi,zi)是第i個無人機(jī)的坐標(biāo),Vi是該無人機(jī)的飛行速度,設(shè)定為常值,θi和ψi分別是第i個無人機(jī)的攻角和方向角,ui1和ui2分別控制第i個無人機(jī)的攻角和方向角。

1.2編隊(duì)建立問題的建模

將能量最優(yōu)的無人機(jī)編隊(duì)飛行的建立建模為最優(yōu)控制問題。目標(biāo)函數(shù)為最小化系統(tǒng)總能量的消耗

(2)

該最優(yōu)控制問題受到的約束有:(1)式中所示的動力學(xué)約束

(3)

控制輸入約束

(4)

初始邊界條件約束

(5)

狀態(tài)約束

(6)

以及最終編隊(duì)隊(duì)形約束

(7)

2編隊(duì)建立的最優(yōu)控制

如同之前所闡述的,能量最優(yōu)問題的建立是采用直接法的第一步。在本節(jié)中,首先將之前建立的最優(yōu)控制問題轉(zhuǎn)換成一個非線性規(guī)劃問題,這一步采用控制向量參數(shù)化方法實(shí)現(xiàn);其次,確定狀態(tài)約束和最終編隊(duì)約束相對于所有優(yōu)化變量的梯度;最后將能量最優(yōu)控制問題近似為SCP問題進(jìn)行求解。

2.1控制向量參數(shù)化

為了求解1.2節(jié)中建立的問題,根據(jù)標(biāo)準(zhǔn)的控制向量參數(shù)化方法將控制向量近似成一個分段常值函數(shù)。首先,將時間區(qū)間[t0,tf]分成N個相等的子區(qū)間,其中各個時刻滿足

之后,在每一個時間段,將第i個無人機(jī)的控制向量Ui做如下的近似

(8)

(9)

控制參數(shù)化后,可以重新建立能量最優(yōu)控制問題。將(9)式帶入(3)式,動力學(xué)方程可以寫為

(10)

初始條件在(5)式中給出。類似地,(4)式中顯示的控制約束可轉(zhuǎn)變成對σi的約束,即

(11)

對于第i個無人機(jī),將滿足(11)式的所有σi的集合記作Ξi。對每個無人機(jī)而言,當(dāng)給定一個σi∈Ξi,就可以利用(5)式中給出的初始條件求解(10)式,從而得到Xi(τ)。實(shí)質(zhì)上,這是一個初始值問題(IVP),而求解該問題的計(jì)算方法已經(jīng)相當(dāng)成熟[10]。大部分方法可以被歸納為單步法,例如龍格-庫塔(RK)方法,或者多步法,例如求解非剛性問題的Adams-Bashforth法,以及求解剛性問題的向后差分法(BDF)。另外一類求解初始值問題的方法是基于配置法,它通過一個分段多項(xiàng)式近似每個子區(qū)間的狀態(tài)變量[11]。一旦計(jì)算得到無人機(jī)飛行軌跡的解,就可以重新建立最終編隊(duì)隊(duì)形約束,如下

(12)

式中,Xi(tf|σi)是給定N個子區(qū)間對應(yīng)的分段常值控制參數(shù)后求得的第i個無人機(jī)的最終狀態(tài)解;顯然,它是σi的函數(shù)。

至此,可以將編隊(duì)建立問題重新寫成一種簡單形式。該形式便于處理狀態(tài)約束和最終隊(duì)形約束:

(13)

2.2梯度計(jì)算

本文采用靈敏度法計(jì)算狀態(tài)約束和最終隊(duì)形約束相對于ξ的梯度。由于狀態(tài)約束和最終隊(duì)形約束都是ξ的隱函數(shù),使得梯度的計(jì)算更加復(fù)雜。在文獻(xiàn)中,這些梯度通常被稱作敏感變量。

對于每個無人機(jī)i,給定一個σi∈Ξi,對(10)式積分計(jì)算即可求得任意τ∈[τk-1,τk)時的狀態(tài)變量。

(14)

(15)

(16)

(17)

這樣,在區(qū)間[τk-1,τk)內(nèi),可以通過(15)～(17)式對τ求導(dǎo)從而可以得到計(jì)算?Xi(τ|σi)/?σi的輔助方程如下

(18)

為了計(jì)算?Xi(tf|σi)/?σi,需要對(18)式在整個時間區(qū)間[0,tf]進(jìn)行積分,且積分初始狀態(tài)為

(19)

在本文研究中運(yùn)用龍格-庫塔法來求解推導(dǎo)出的初始值問題,這僅僅是因?yàn)樵摲椒ㄔ诮鉀Q后面算例時的高效性和準(zhǔn)確性,然而對于特定的問題可以選擇特定的方法。因此,可以采用更加合適的方法來解決具體的剛性或非剛性問題。注意,在對(18)式積分的過程中,需要用到求解(10)式得到的狀態(tài)信息,因此需要聯(lián)立進(jìn)行求解。由于同時求解狀態(tài)方程和輔助方程,因此可以對狀態(tài)變量和敏感變量同時施加一個局部控制誤差以提高解的精度。

2.3迭代凸規(guī)劃方法

一旦目標(biāo)約束和最終編隊(duì)隊(duì)形約束相對于優(yōu)化變量的梯度已知,在文獻(xiàn)中通常采用NLP技術(shù)中的SQP或者內(nèi)點(diǎn)法求解問題(13)。然而,在本文中,選擇SCP技術(shù)是因?yàn)樗唵?、高效且是免費(fèi)開源的。

SCP的基本思想是通過迭代求解原始問題的凸逼近序列來求解問題(13)。在問題(13)中,只有狀態(tài)約束和最終隊(duì)形約束需要進(jìn)行近似,而由于它們相對于優(yōu)化變量的梯度可以積分求得,因此可以采用一階泰勒展開進(jìn)行近似。對于第k個序列,利用下面給出的問題(20)以及優(yōu)化變量ξ來近似問題(13)。明顯地,問題(20)是一個凸優(yōu)化問題。在問題(20)中,所有的偏微分以及所有無人機(jī)狀態(tài)Xi,k都可以利用前面小節(jié)中推導(dǎo)出的方程計(jì)算得到。在問題(20)中,為了保證初始猜測值任意選擇時的全局收斂性,需要將一個信任域(trustregion)施加在ξ上。然而,由于只有優(yōu)化變量中的控制參數(shù)出現(xiàn)在目標(biāo)函數(shù)中,信任域只需要設(shè)定在所有控制參數(shù)當(dāng)前點(diǎn)的周圍即可,如(21)式所示。

(20)

(21)

3多無人機(jī)編隊(duì)建立的優(yōu)化算法

至此,基于控制向量參數(shù)化方法和SCP技術(shù),可以構(gòu)建求解能量最優(yōu)控制問題的算法,如算法1所示。在算法1中,步驟2利用控制參數(shù)化方法,而步驟3采用了SCP方法。在SCP方法中,優(yōu)化變量{ξk}k≥1一直迭代直到滿足預(yù)先設(shè)定的收斂條件ε>0為止。在該算法中,可運(yùn)用開源軟件CVX求解問題(20)[12],同時也可以求得相對于最終隊(duì)形約束的拉格朗日算子。一旦計(jì)算得到能量最優(yōu)控制向量,對(10)式積分即可得到建立編隊(duì)的飛行軌跡。

算法1求解能量最優(yōu)控制問題的SCP算法

1:迭代開始

3:運(yùn)用CVX求解問題(20),得到ξk的一個解。

4:k=k+1。

5:直到‖ξk-ξk-1‖≤ε時迭代結(jié)束。

輸出:ξ

4V型編隊(duì)建立的仿真算例

在本節(jié)中,將提出的算法運(yùn)用到3個無人機(jī)的V型編隊(duì)建立中,并且將仿真結(jié)果與運(yùn)用Matlab中全局優(yōu)化方法得到的結(jié)果進(jìn)行比較,其中,該全局優(yōu)化方法采用了多個起始點(diǎn)。首先,假設(shè)3個無人機(jī)當(dāng)前分別以200 m/s、100 m/s和100 m/s的速度飛行,它們的初始狀態(tài)如下所示

假設(shè)上面的這3個無人機(jī)需要在50 s內(nèi)建立1個V型編隊(duì)。在最終編隊(duì)隊(duì)形中,編隊(duì)建立將要到達(dá)的位置和編隊(duì)建立時的飛行攻角以及方向角一起被定義為參考參數(shù)。對于V型編隊(duì),這3個無人機(jī)的最終狀態(tài)與參考參數(shù)之間的差值設(shè)置如下

在本文的仿真中,將時間區(qū)間劃分為60個相等的子區(qū)間。在算法實(shí)現(xiàn)中,并沒有設(shè)定停止準(zhǔn)則來結(jié)束該仿真;相反,只限制了迭代次數(shù)(例如,進(jìn)行50次迭代)。對于每一次迭代,它的最優(yōu)控制問題都使用免費(fèi)開源軟件CVX進(jìn)行求解。當(dāng)仿真結(jié)果收斂時,編隊(duì)建立過程中消耗的總能量是0.254 6,計(jì)算得到的參考參數(shù)及各個無人機(jī)的最終狀態(tài)為

圖1顯示了總能量消耗隨著迭代次數(shù)收斂的曲線。圖2顯示編隊(duì)建立過程中,所有無人機(jī)的三維軌跡,而圖3顯示了X-Y平面內(nèi)的二維軌跡。

圖1　V型編隊(duì)建立過程中的總能量消耗　圖2　V型編隊(duì)建立的3-D軌跡　圖3　X-Y平面內(nèi)V型編隊(duì)建立的2-D軌跡

因?yàn)闋顟B(tài)約束和最終編隊(duì)隊(duì)形約束的梯度方程可以視為已知,所以也可以采用SQP和內(nèi)點(diǎn)法來求解能量最優(yōu)的編隊(duì)建立問題。在研究中,內(nèi)點(diǎn)法直接通過Matlab提供的fmincon函數(shù)實(shí)現(xiàn)。如果將之前的仿真設(shè)置再次用到內(nèi)點(diǎn)法中,將會得到與運(yùn)用SCP法相似的結(jié)果。然而,內(nèi)點(diǎn)法需要進(jìn)行67次迭代才能收斂,而本文提出的方法僅需11次迭代就可收斂,如圖1所示。另外,除了將SCP法與上述局部優(yōu)化方法進(jìn)行比較,本文還采用多個初始點(diǎn)的全局優(yōu)化技術(shù)來尋找全局最小值。具體采用的是Matlab中的MultiStart求解器,它利用隨機(jī)分布的初始點(diǎn)尋找多個局部最小值。對于上述能量最優(yōu)的編隊(duì)建立算例,在優(yōu)化變量ξ允許的范圍內(nèi),總共產(chǎn)生了20個隨機(jī)初始點(diǎn)。對于每個初始點(diǎn),MultiStart求解器采用內(nèi)點(diǎn)法求解相應(yīng)的優(yōu)化問題。最終計(jì)算結(jié)果表明,能量消耗0.254 6是全局最小值。

5結(jié)論

本文提出了一個簡單高效的算法可實(shí)現(xiàn)多無人機(jī)能量最優(yōu)的編隊(duì)建立。該方法利用了控制向量參數(shù)化方法以及迭代凸規(guī)劃技術(shù)的優(yōu)點(diǎn),且可以基于一個免費(fèi)開源的凸優(yōu)化求解器進(jìn)行算法的實(shí)現(xiàn)。該方法可應(yīng)用于存在非線性動力學(xué)約束和非凸局部約束和/或耦合約束的編隊(duì)建立問題。本方法在直接法的基礎(chǔ)上先將最優(yōu)控制問題近似為非線性規(guī)劃問題,再進(jìn)一步使用迭代凸規(guī)劃技術(shù)(SCP)將其近似為一系列凸規(guī)劃問題。SCP迭代的每一步本質(zhì)上是一個受線性約束的二次型規(guī)劃問題,可以高效地進(jìn)行求解。與Matlab中提供的全局優(yōu)化技術(shù)相比,采用本文算法能夠非常迅速地收斂到全局最小值。

參考文獻(xiàn)：

[1]Burkle A, Segor F, Kollmann M. Towards Autonomous Micro UAV Swarms[J]. Journal of Intelligent Robotic Systems, 2011, 61(1): 339-353

[2]Navaravong L, Kan Z, Shea J M, et al. Formation Reconfiguration for Mobile Robots with Network Connectivity Constraints[J]. IEEE Trans on Network, 2012, 26(4): 18-24

[3]Raffard R L, Tomlin C L, Boyd S P. Distributed Optimization for Cooperative Agents: Application to Formation Flight[C]∥Proceedings of 43rd IEEE Conference on Decision and Control, 2004: 2453-2459

[4]Betts J T. Survey of Numerical Methods for Trajectory Optimization[J]. Journal of Guidance, Control, and Dynamics, 1998, 21(2): 193-207

[5]Conway B A. A Survey of Methods Available for the Numerical Optimization of Continuous Dynamic Systems[J]. Journal of Optimization Theory and Applications, 2012, 152(2): 271-306

[6]Teo K L, Goh C, Wong K. A Unified Computational Approach to Optimal Control Problems[M]. Longman Scientific and Technology, Essex, 1991

[7]Boyd S, Vandenberghe L. Convex Optimization[M]. Cambridge University Press, New York, 2009

[8]Mattingley J, Boyd S. Cvxgen: a Code Generator for Embedded Convex Optimization[J]. Optimization and Engineering, 2012, 13(1): 1-27

[9]Bai R, Sun X, Chen Q, et al. Multiple UAV Cooperative Trajectory Planning Based on Gauss Pseudospectral Method[J]. Journal of Astronautics, 2014, 35(9): 1022-1029

[10] Chachuat B. Nonlinear and Dynamic Optimization: from Theory to Practice[R]. Technical Report, 2007

[11] Rao A V. A Survey of Numerical Methods for Optimal Control[J]. Advances in the Astronautical Sciences, 2009, 135(1): 497-528

[12] Grant M, Boyd S. Graph Implementations for Nonsmooth Convex Programs[M]. Springer, Berlin, 2008, 95-110

Establishment of UAV Formation Flight Using Control Vector Parameterization and Sequential Convex Programming

Guo Juan1, Chu Jing2, Yan Jie1

1.School of Astronautics, Northwestern Polytechnical University, Xi′an 710072, China 2.School of Aeronautics, Northwestern Polytechnical University, Xi′an 710072, China

Abstract:Formation flight of multiple Unmanned Aerial Vehicles (UAVs) could provide an effective, efficient yet cost reduction alternative to existing technologies of various applications. This paper addresses the energy-optimal establishment of UAV formation flight that shall satisfy final configuration constraints. First, the problem of formation establishment is formulated as an energy-optimal control problem which is subject to the nonlinear dynamics constraints. Then, the optimal control problem is solved using a direct method. In this direct method the original problem is transcribed into a sequence of convex optimization problems via the control vector parameterization approach. After that, the sequential convex programming (SCP) technology is employed to derive the optimal solution. Essentially, each instance of SCP is a quadratic programming problem subject to linear constraints that can be solved very efficiently. In the end, the proposed method is applied to the establishment of V formation for three UAVs, where the free open source CVX is exploited. By comparison with the global optimization technique provided in MATLAB, the solution converges very fast to the global minimum.

Keywords:convex optimization, cost reduction, MATLAB, parameterization, unmanned aerial vehicles(UAVs), formation flight, optimal control, control vector parameterization, sequential convex programming

收稿日期：2016-03-25

作者簡介：過娟(1986—)，女，西北工業(yè)大學(xué)博士研究生，主要從事分布式優(yōu)化及制導(dǎo)技術(shù)的研究。

中圖分類號：TP391

文獻(xiàn)標(biāo)志碼：A

文章編號:1000-2758(2016)04-0607-07