王祝， 劉莉,*， 龍騰， 溫永祿

1.飛行器動力學(xué)與控制教育部重點實驗室， 北京 100081 2.北京理工大學(xué) 宇航學(xué)院， 北京 100081

基于罰函數(shù)序列凸規(guī)劃的多無人機軌跡規(guī)劃

王祝1,2， 劉莉1,2,*， 龍騰1,2， 溫永祿1,2

1.飛行器動力學(xué)與控制教育部重點實驗室， 北京 100081 2.北京理工大學(xué) 宇航學(xué)院， 北京 100081

多無人機(UAVs)軌跡規(guī)劃是具有非線性運動約束和非凸路徑約束的最優(yōu)控制問題。引入序列凸規(guī)劃思想，將非凸最優(yōu)控制問題近似為一系列凸優(yōu)化子問題，并利用成熟的凸優(yōu)化算法進行求解，以更好地權(quán)衡最優(yōu)性和時效性。首先，建立了多無人機協(xié)同軌跡規(guī)劃的非凸最優(yōu)控制模型。然后，利用離散化和凸近似方法將其轉(zhuǎn)換為凸優(yōu)化問題，包括對無人機運動模型的線性化，以及對威脅規(guī)避約束和無人機碰撞約束的凸化。同時，提出了一種離散點間的威脅規(guī)避方法，保證無人機在離散軌跡點間的飛行安全。在凸優(yōu)化模型的基礎(chǔ)上，給出了基于罰函數(shù)序列凸規(guī)劃求解多無人機軌跡規(guī)劃的具體框架。最后，通過數(shù)值仿真驗證了方法的有效性，結(jié)果表明該方法在多機軌跡規(guī)劃結(jié)果的最優(yōu)性和時效性都要優(yōu)于偽譜法，而且優(yōu)勢隨編隊數(shù)量的增加而增大。

無人機； 軌跡規(guī)劃； 碰撞規(guī)避； 最優(yōu)控制； 凸規(guī)劃

多無人機協(xié)同能夠提高任務(wù)完成效能和擴展任務(wù)執(zhí)行能力[1]，多無人機軌跡規(guī)劃是實現(xiàn)多機協(xié)同的關(guān)鍵技術(shù)，已成為當(dāng)前研究的熱點。目前，無人機(Unmanned Aerial Vehicle， UAV)軌跡規(guī)劃研究從運動方程的建模形式上可分為兩類，本文研究屬于第2類。第1類是基于轉(zhuǎn)彎角、航段長度建立代數(shù)運動方程，相應(yīng)的規(guī)劃方法主要有圖搜索、樹搜索、勢場法和數(shù)學(xué)優(yōu)化方法[2]；第2 類是基于加速度或過載等控制量建立微分運動方程，將軌跡規(guī)劃建模為最優(yōu)控制問題，求解方法主要包括偽譜法[3-4]、混合整數(shù)規(guī)劃[5-6]以及現(xiàn)代智能算法[7-8]，但上述方法應(yīng)用于多機規(guī)劃時，由于問題規(guī)模增大，規(guī)劃時間顯著增長，規(guī)劃效率難以滿足應(yīng)用需求[5]。

隨著凸優(yōu)化方法的完善和計算機技術(shù)的發(fā)展，大規(guī)模凸優(yōu)化問題已能夠在有限時間內(nèi)獲得最優(yōu)解[9]，因此凸優(yōu)化方法近年來得到廣泛應(yīng)用。序列凸規(guī)劃[10](Sequnential Convex Programmming， SCP)方法通過對非凸問題進行序列凸近似，能夠快速得到非凸優(yōu)化問題的近似最優(yōu)解，已開始應(yīng)用于飛行器軌跡規(guī)劃領(lǐng)域。飛行器軌跡規(guī)劃本質(zhì)上是非凸最優(yōu)控制問題，通過參數(shù)化方法轉(zhuǎn)化為非凸優(yōu)化問題后，則可以應(yīng)用SCP方法實現(xiàn)求解。Liu和Lu[11-14]近年來針對軌跡規(guī)劃的凸優(yōu)化建模與方法開展了深入研究。文獻[11]給出了基于凸優(yōu)化求解一類非凸最優(yōu)控制的方法，并將其應(yīng)用到交會對接、軌道轉(zhuǎn)移和運載器上升段問題中；文獻[12]基于圓錐優(yōu)化實現(xiàn)了航天器交匯對接任務(wù)的軌跡規(guī)劃；文獻[13]基于二階錐規(guī)劃，并結(jié)合序列線性化和松弛技術(shù)，完成了飛行器再入軌跡優(yōu)化；在此基礎(chǔ)上，文獻[14]結(jié)合低通濾波器和線性搜索，實現(xiàn)了最大化側(cè)向航程的再入軌跡規(guī)劃。Morgan和Chung[15]利用分散式SCP和模型預(yù)測控制實現(xiàn)了航天器集群的編隊重構(gòu)軌跡優(yōu)化。在無人機軌跡規(guī)劃方面，Augugliaro等[16]首次將SCP方法用于生成四旋翼無人機的避障軌跡，數(shù)值仿真和飛行試驗表明SCP方法能夠在很短時間內(nèi)完成軌跡規(guī)劃。在此基礎(chǔ)上，Chen等[17]針對非凸空間約束下，SCP方法會陷入不可行子問題而導(dǎo)致求解不收斂問題，提出了約束遞增SCP方法，提高了非凸空間內(nèi)SCP軌跡規(guī)劃的可行概率。然而，文獻[16-17]主要針對四旋翼無人機的軌跡規(guī)劃，考慮的是簡單線性離散運動學(xué)，難以求解非線性的無人機運動模型。同時，上述文獻基于凸優(yōu)化進行軌跡規(guī)劃時，僅考慮了在軌跡離散點處的威脅規(guī)避約束，無法保證離散點區(qū)間內(nèi)的飛行安全。

對此，本文考慮具有非線性運動方程的無人機(例如固定翼無人機)軌跡規(guī)劃問題，建立多無人機軌跡規(guī)劃的非凸最優(yōu)控制模型。在此基礎(chǔ)上，利用凸化和離散化方法，將其轉(zhuǎn)換為關(guān)于基準(zhǔn)軌跡的凸優(yōu)化模型。同時，通過在凸優(yōu)化模型中增加約束項，保證了離散點間軌跡的飛行安全。另外，采用罰函數(shù)SCP方法對凸優(yōu)化模型進行迭代求解，可避免出現(xiàn)不可行子問題而導(dǎo)致SCP迭代不收斂。最后，通過仿真試驗，對本文提出的無人機協(xié)同軌跡規(guī)劃方法進行了驗證。

1 多無人機軌跡規(guī)劃建模

本節(jié)建立多無人機協(xié)同軌跡規(guī)劃問題的最優(yōu)控制模型，包括狀態(tài)方程、控制量、性能指標(biāo)和約束條件。

1.1 無人機運動方程

多無人機最優(yōu)控制的狀態(tài)方程包括各無人機的運動方程。無人機定高等速飛行的運動方程[18]為

(1)

式中：i∈{1,2,…,N}為無人機在編隊中的序號，N為編隊中無人機的數(shù)量；xi和yi為位置；ψi為航向角；Vi為無人機飛行速度；ui為法向加速度。為了便于描述，下文中用向量p代表位置[xiyi]T二元組。

1.2 控制量與性能指標(biāo)

選取各無人機的法向加速度ui作為控制量，設(shè)ui,max為無人機i的可用最大加速度，則容許控制約束為

|ui|≤ui,max

(2)

以最小化燃油為性能指標(biāo)，可等價表示為

(3)

式中：t0和tf分別為軌跡的初始時間和終端時間，本文中終端時間為已知的固定值。

1.3 約束條件

除了式(2)容許控制約束外，軌跡規(guī)劃還需考慮初始條件、終端條件和路徑約束。路徑約束，包括無人機飛行軌跡對已知威脅的規(guī)避和無人機之間的碰撞規(guī)避。

無人機軌跡規(guī)劃的初始位置[x0,iy0,i]與速度方向ψ0,i和終端位置[xf,iyf,i]與方向ψf,i均固定，對應(yīng)的初始與終端狀態(tài)約束為

xi(t0)=x0,i,yi(t0)=y0,i,ψi(t0)=ψ0,i

(4)

xi(tf)=xf,i,yi(tf)=yf,i,ψi(tf)=ψf,i

(5)

假設(shè)環(huán)境中有M個威脅(文中考慮圓形靜態(tài)威脅)，為保證無人機編隊的飛行安全，要求每個無人機在任意時刻都位于威脅區(qū)域外部，即?m∈{1,2,…,M}，?i∈{1,2,…,N}，有

(6)

式中：pm=[xmym]T為第m個威脅的位置；rm為對應(yīng)的威脅半徑。

無人機間避碰約束則要求兩架無人機間的距離時刻大于安全距離R，即?i,j∈{1,2,…,N}，i

(7)

1.4 最優(yōu)控制模型

綜上，建立多無人機軌跡規(guī)劃對應(yīng)的固定終端最優(yōu)控制問題(P1)模型為

s.t. 式(1)～式(7)

(8)

式中：約束包括3N個運動方程微分等式約束，3N個初始狀態(tài)代數(shù)等式約束，3N個末端狀態(tài)代數(shù)等式約束，M·N+N(N-1)個路徑不等式約束，以及N個控制不等式約束。

2 多機軌跡規(guī)劃模型凸化與離散化

問題模型P1包括非線性狀態(tài)方程、非凸路徑約束，是一個非凸最優(yōu)控制問題。而凸優(yōu)化要求目標(biāo)函數(shù)和不等式約束函數(shù)均為凸函數(shù)，等式約束函數(shù)是仿射函數(shù)。因此，為了利用凸優(yōu)化實現(xiàn)求解，本節(jié)建立問題P1的凸化近似模型，同時通過離散化將最優(yōu)控制轉(zhuǎn)變?yōu)閰?shù)優(yōu)化問題。對比上述模型與凸優(yōu)化需求，模型凸化需對運動方程進行線性化，以及對威脅規(guī)避和無人機間避碰約束進行凸化，而目標(biāo)函數(shù)和其他約束已滿足凸優(yōu)化框架的要求。

2.1 無人機運動方程線性化與離散化

記無人機i的狀態(tài)量Xi=[xiyiψi]T，則系統(tǒng)運動方程式(1)可以等價地以矩陣形式表述為

(9)

(10)

式中：

(11)

(12)

式中：A、B和c為依賴于基準(zhǔn)軌跡，而不依賴于當(dāng)前軌跡的狀態(tài)量和控制量，在計算時可認(rèn)為是常數(shù)，從而實現(xiàn)了運動方程的線性化。序列凸規(guī)劃迭代求解過程中，基準(zhǔn)軌跡選取為前一次迭代得到的軌跡。

在線性化運動方程的基礎(chǔ)上，對運動方程進行離散化。離散按等采樣周期進行，控制量認(rèn)為只在采樣時刻發(fā)生變化，在相鄰兩采樣時刻之間，通過零階保持器保持不變，則得到離散時間系統(tǒng)運動方程為

Xi(k+1)=Gi(k)Xi(k)+Hi(k)·ui(k)+Ci(k)

(13)

式中：k=0,1,…,K為離散時刻；K為離散時刻數(shù)量，且K·Δt=tf-t0，Δt為離散步長；系數(shù)Gi、Hi和Ci的表達式分別為

(14)

式中：A、B和c分別由式(11)、式(9)、式(12)根據(jù)基準(zhǔn)軌跡相應(yīng)時刻狀態(tài)值確定，從而不同時刻的Gi、Hi、Ci可以根據(jù)式(14)計算得到。

綜上，通過對無人機運動方程的線性化和離散化，選定基準(zhǔn)狀態(tài)軌跡后，基于已知的初始狀態(tài)，利用式(13)以及遞歸思想，可將后續(xù)離散時刻的無人機狀態(tài)用控制量序列的仿射函數(shù)表示為

Xi(k)=Pi(k)·Xi(0)+

Qi(k)·[ui(0)ui(1) …ui(K-1)]T+

Ri(k)k=1,2，…,K

(15)

式中：

(16)

2.2 威脅規(guī)避與無人機間避碰約束凸化

威脅規(guī)避約束和無人機間避碰約束函數(shù)都是凹函數(shù)，參考文獻[15-17]的凸近似方法，將其轉(zhuǎn)化為仿射函數(shù)。盡管該方法實現(xiàn)約束凸化后，減小了問題可行域，但結(jié)合SCP的序列迭代能夠很好地緩解約束處理的保守性[15]。

2.2.1 圓形威脅規(guī)避約束凸化

軌跡離散化后，威脅規(guī)避約束函數(shù)可以表示為，?i∈{1,2,…,N}，?m∈{1,2,…,M}，

(17)

(18)

同時根據(jù)式(15)，狀態(tài)量可以用控制量的仿射函數(shù)表征，因此威脅規(guī)避約束可表示為控制量的仿射函數(shù)，即實現(xiàn)了威脅約束條件的凸化。

上述圓形威脅規(guī)避約束的凸化可以從幾何上直觀理解。如圖1(a)所示，威脅規(guī)避約束的真實可行域為圖中圓形威脅外的所有區(qū)域，為一個非凸集，而凸化后的可行域為圖中右上方區(qū)域，該區(qū)域為一個半平面(凸集)，其邊界與威脅圓相切且和威脅中心與基準(zhǔn)點連線垂直。規(guī)避多個威脅的情況如圖1(b)所示，凸化后的可行域為兩個半平面的交集，仍然是一個凸集。

圖1 威脅規(guī)避約束凸化示意圖[15]Fig.1 Convexification of threat avoidance constraint[15]

2.2.2 無人機間避碰約束凸化

基于離散軌跡點描述的無人機間避碰約束函數(shù)可表示為?i,j∈{1,2,…,N},i

(19)

將式(19)在無人機i和j的兩條基準(zhǔn)軌跡對應(yīng)離散點處進行線性化，可得關(guān)于兩無人機狀態(tài)量的仿射函數(shù)，從而實現(xiàn)無人機間避碰約束凸化。

(20)

幾何上，無人機間避碰約束凸化示意圖如圖2所示。準(zhǔn)確的避碰約束只需兩無人機間距離不小于安全距離，而凸化后的約束變?yōu)閮蔁o人機不能同時位于寬度為R的帶狀區(qū)域，該區(qū)域與兩無人機離散參考點連線垂直，而且區(qū)域位置并不固定，可以沿參考點連線方向移動。

圖2 無人機間避碰約束凸化示意圖[17]Fig.2 Convexification of inter-UAVs collision avoidance[17]

2.3 多機軌跡規(guī)劃的凸優(yōu)化模型

在運動方程、威脅規(guī)避和避撞約束凸化與離散化的基礎(chǔ)上，將控制量及其約束和性能指標(biāo)離散化?？刂屏侩x散后的允許控制約束可表示為

ui(k)≤ui,maxk=1,2,…,K

(21)

將性能指標(biāo)離散化，并結(jié)合前述凸化模型，可得到如下基于基準(zhǔn)軌跡的多機軌跡規(guī)劃凸優(yōu)化問題(P2)，其中優(yōu)化變量為無人機編隊的所有時刻控制量序列的集合U，U=[u1(k)u2(k) …uN(k)]T，k=1,2,…,K。

s.t.Aeq·U=beq

Ain·U≤bin

(22)

式中：等式約束Aeq·U=beq僅包括等式(5)；而不等式約束Ain·U=bin包括不等式(18)、式(20)和式(21)。需要注意的是，式(4)和式(15)的作用是將狀態(tài)量用控制量的仿射函數(shù)表征，通過將其代入其他約束條件表達式中可顯式滿足。

3 離散區(qū)間內(nèi)的威脅規(guī)避

基于離散化方法將軌跡參數(shù)化后，通過在離散點處增加威脅規(guī)避約束不等式，能夠保證在離散軌跡點處的安全，但是離散點之間的軌跡可能穿越威脅。

針對離散點間軌跡的威脅規(guī)避問題研究較少。其中，Maia和Galvao[19]在混合整數(shù)規(guī)劃求解障礙規(guī)避軌跡問題的框架下，通過限制二進制變量在離散點的變化方式，實現(xiàn)了離散點間軌跡的障礙規(guī)避。但該方法在原問題中新增了大量二進制變量，導(dǎo)致問題規(guī)模增大，求解效率降低。對此，Richards和Turnbull[20]提出了一種新的約束添加方法，即限制相鄰離散點間的障礙滿足模式，以確保離散點之間的軌跡不會與障礙發(fā)生碰撞。上述方法都是在混合整數(shù)優(yōu)化的框架下，通過增加二進制變量和對二進制變量增加約束的方式實現(xiàn)離散點間的障礙規(guī)避，并不能在序列凸優(yōu)化框架中應(yīng)用。本文針對SCP軌跡規(guī)劃框架，引入約束增加思想，給出了一種保守而有效的離散點間軌跡的威脅規(guī)避方法。

根據(jù)上述思想，離散區(qū)間內(nèi)威脅規(guī)避約束可表示為：?i∈{1,2,…,N}，?m∈{1,2,…,M}，有

(23)

圖3 離散點間威脅規(guī)避約束示意圖Fig.3 Inter-sample threat avoidance constraint

上述離散區(qū)間內(nèi)威脅規(guī)避約束為仿射的不等式約束，通過在凸優(yōu)化問題P2中增加該約束，即可實現(xiàn)離散點間軌跡對威脅的規(guī)避。

4 基于罰函數(shù)的序列凸規(guī)劃方法

基于上述建立的多機軌跡規(guī)劃凸優(yōu)化模型，選取一組初始軌跡作為基準(zhǔn)，即可利用凸優(yōu)化方法實現(xiàn)求解。然而，僅一次優(yōu)化的方法對基準(zhǔn)軌跡非常敏感，初值選取不當(dāng)則難以得到可行解，而且不能保證狀態(tài)方程線性化精度。對此，利用序列凸規(guī)劃進行迭代求解可以避免上述問題。

4.1 序列凸規(guī)劃

(24)

另外，為保證運動方程線性化的有效性和序列迭代的收斂性，序列求解時增加如下的信賴域約束：?i∈{1,2,…,N}，有

(25)

式中：Lq為第q次迭代對應(yīng)的信賴域半徑。上述信賴域約束為狀態(tài)量的仿射函數(shù)，因此不影響子問題的凸性。為了確保迭代收斂，可采用逐漸縮減的信賴域半徑。

序列凸規(guī)劃通過序列求解對應(yīng)于不同基準(zhǔn)解的凸近似模型，相比單純的凸優(yōu)化方法，降低了求解結(jié)果對初值的依賴性。序列凸規(guī)劃應(yīng)用于多無人機協(xié)同軌跡規(guī)劃的具體步驟如下所示。

步驟1給定無人機編隊的一組初始猜測軌跡X0i,i=1,2,…,N，作為序列凸規(guī)劃第一次迭代求解的基準(zhǔn)軌跡。

步驟3在信賴域約束式(25)下，利用凸優(yōu)化算法求解所構(gòu)造的凸優(yōu)化子問題，獲得當(dāng)前迭代的最優(yōu)控制序列。

步驟5判斷是否滿足收斂條件，若滿足，則輸出當(dāng)前的軌跡結(jié)果；若不滿足，根據(jù)式(24)更新基準(zhǔn)軌跡，并返回步驟2。

上述SCP求解協(xié)同軌跡規(guī)劃步驟中的收斂條件為：當(dāng)前迭代的軌跡結(jié)果滿足所有約束(包括威脅規(guī)避、無人機間避碰約束、允許控制約束、信賴域約束)，并且連續(xù)兩次迭代的軌跡結(jié)果在一定誤差范圍內(nèi)相同。軌跡迭代誤差的滿足條件可表示為：?i∈{1,2,…,N}，有

(26)

式中：e為迭代收斂誤差限，文中取為0.001。

4.2 罰函數(shù)SCP

序列凸規(guī)劃求解多無人機軌跡規(guī)劃問題時，由于約束條件眾多，而且構(gòu)建子問題時進行了保守凸近似，從而導(dǎo)致在SCP迭代中存在子問題不可行的情況，特別對于有終端等式約束的問題。盡管通過序列迭代，SCP具有從不可行解中恢復(fù)的能力，但也存在迭代陷入不可行解空間而無法收斂的情況[17]。另外，對于不可行凸優(yōu)化問題，當(dāng)前一些成熟的凸優(yōu)化工具包(例如CVX[21-22])不會返回尋優(yōu)過程中的不可行解，而是返回?zé)o效數(shù)據(jù)，這將導(dǎo)致SCP無法繼續(xù)迭代，也無法從不可行解中恢復(fù)。

針對上述問題，本文采用罰函數(shù)方法處理凸優(yōu)化子問題中的約束。文中將終端等式約束、威脅規(guī)避約束和無人機間避碰約束以懲罰項形式加到性能指標(biāo)中，得到如下凸優(yōu)化子問題(P3)?；诹P函數(shù)SCP求解時，只需簡單修改4.1節(jié)給出的算法步驟，即在步驟2中不再構(gòu)建問題P2，而是構(gòu)建問題P3，進行序列求解。

|ui(k)|≤ui,max

k=1,2,…,K；i=1,2,…,N

(27)

式中：ωeq和ωin分別為等式約束和不等式約束的罰系數(shù)。注意到等式約束僅在終端時刻存在，而不等式約束是過程約束，存在于每個離散時刻，因此罰系數(shù)取為ωeq=K·ωin。

5 仿真驗證與結(jié)果分析

本節(jié)針對多無人機協(xié)同軌跡規(guī)劃的一類典型問題，即編隊集結(jié)任務(wù)，通過仿真試驗驗證本文方法的有效性，并與偽譜法進行性能對比。

仿真實驗在Windows XP中的MATLAB 2013b環(huán)境下進行，硬件環(huán)境為Intel Core i5-2310 2.90Ghz處理器和3.24 GB內(nèi)存的PC機。凸優(yōu)化子問題采用CVX[21-22]和SDPT3[23]求解，偽譜法軌跡規(guī)劃基于GPOPS-II工具包[24]實現(xiàn)。

編隊集結(jié)任務(wù)仿真想定為：多架無人機分別從各自規(guī)劃初始狀態(tài)抵達集結(jié)區(qū)域，并形成雁形編隊。編隊的初始狀態(tài)和終端狀態(tài)如表1所示。

以軌跡規(guī)劃起點對應(yīng)時間為0 s，無人機編隊在80 s時到達指定集結(jié)位置，無人機速度都為20 m·s-2，最大法向加速度ui,max都為5 m·s-2。在集結(jié)過程中，無人機編隊需避免相互碰撞，同時需規(guī)避環(huán)境中的已知威脅。其中，無人機間安全距離設(shè)為50 m，環(huán)境中存在8個已知威脅，威脅半徑都為100 m，威脅坐標(biāo)位置分別為(500,600)、(500,200)、(500 ,-200)、(500,-600)、(1 700,500)、(1 700,100)、(1 700,-300)和(1 700,-700) m。

表1 無人機編隊信息Table 1 Information of UAV formation

針對上述無人機編隊集結(jié)想定，分別求解無人機數(shù)量N為1到7的編隊軌跡規(guī)劃，以對比SCP和偽譜法在不同問題規(guī)模下的性能。例如，無人機編隊數(shù)量為3時，表示編隊由表中的UAV-1、UAV-2和UAV-3組成。

具體求解時，GPOPS軟件包中設(shè)置網(wǎng)格區(qū)間系數(shù)為0.1，區(qū)間配點數(shù)為4，其他參數(shù)為默認(rèn)值。同時，SCP求解框架中將離散時刻數(shù)K取為40，使兩方法具有相同離散點數(shù)量，便于算法性能對比。另外，不等式約束罰系數(shù)ωin為100，等式約束罰系數(shù)ωeq=K·ωin，即4 000。

基于SCP和偽譜法分別求解不同編隊數(shù)量情況下的無人機集結(jié)軌跡規(guī)劃，得到的對比結(jié)果如圖4所示。

由圖4(a)最優(yōu)性能指標(biāo)結(jié)果可見，單機軌跡規(guī)劃時，SCP方法和偽譜法所得結(jié)果的最優(yōu)性相當(dāng)；對于多機編隊軌跡規(guī)劃，SCP獲得的最優(yōu)值優(yōu)于偽譜法，而且無人機數(shù)量越多，SCP的優(yōu)勢越顯著。另一方面，根據(jù)圖4(b)規(guī)劃時間對比結(jié)果，SCP規(guī)劃多機編隊軌跡的耗時都要低于偽譜法。特別是隨著無人機編隊數(shù)量的增加，SCP求解耗時近似呈線性增長，而偽譜法則超線性增長，SCP方法的效率優(yōu)勢愈發(fā)明顯。這主要是由于偽譜法中采用非線性規(guī)劃求解一般的參數(shù)優(yōu)化問題，其耗時隨著問題維度的增加而顯著增加；序列凸規(guī)劃中求解的則是一類特殊的參數(shù)優(yōu)化問題，即凸優(yōu)化問題，而成熟的凸優(yōu)化方法已經(jīng)可以在較短時間內(nèi)完成大規(guī)模問題求解，盡管其求解時間也隨維度增大而增加，但其增長程度要低于一般非凸優(yōu)化。

圖4 軌跡規(guī)劃結(jié)果對比Fig.4 Comparison of trajectory planning results

圖5給出了SCP和偽譜法對7架無人機編隊集結(jié)任務(wù)所規(guī)劃的軌跡結(jié)果。圖中，方形表示無人機的規(guī)劃起點，五角星為各自的集結(jié)點，軌跡外的圓為威脅，飛行軌跡上的小圓分別為各無人機在0、10、20和30 s的位置。

圖5 7架無人機編隊的規(guī)劃軌跡Fig.5 Planned trajectories of formation with 7 UAVs

由圖5可見，兩種求解方案都能夠得到規(guī)避威脅和避免碰撞的可行飛行軌跡。對比規(guī)劃軌跡結(jié)果可見，偽譜法和SCP方法得到的UAV-4到UAV-7軌跡雖然不同，但兩種方法得到的軌跡都是繞著威脅邊界的局部最優(yōu)軌跡。

綜上，仿真對比實驗結(jié)果表明，針對多無人機編隊集結(jié)軌跡規(guī)劃，SCP方法和偽譜法都能獲得安全可行的飛行軌跡，同時SCP方法比偽譜法具有更好的最優(yōu)性和效率，而且SCP方法的優(yōu)勢隨著編隊數(shù)量的增加而增大。

6 結(jié) 論

1) 建立了多無人機協(xié)同軌跡規(guī)劃的非凸最優(yōu)控制模型，其中考慮了威脅規(guī)避和無人機間避碰約束；在此基礎(chǔ)上，基于運動方程線性化和約束凸化方法，將問題轉(zhuǎn)換為凸優(yōu)化問題。

2) 給出了一種離散點間軌跡的威脅規(guī)避方法，從理論上說明了該方法能夠保證軌跡安全。

3) 給出罰函數(shù)SCP方法求解多機協(xié)同軌跡規(guī)劃問題的框架，并通過具體算例的數(shù)值仿真和對比驗證了方法的有效性。

[1] 沈林成, 牛軼峰, 朱華勇, 等. 多無人機自主協(xié)同控制理論與方法[M]. 北京: 國防工業(yè)出版社, 2013: 1-9.

SHEN L C, NIU Y F, ZHU H Y, et al. Theories and methods of autonomous cooperative control for multiple UAVs[M]. Beijing: National Defense Industry Press, 2013: 1-9 (in Chinese).

[2] 丁明躍, 鄭昌文, 周成平, 等. 無人飛行器航跡規(guī)劃[M]. 北京: 電子工業(yè)出版社, 2009: 6-15.

DING M Y, ZHENG C W, ZHOU C P, et al. Route planning for unmanned aerial vehicles[M]. Beijing: Publishing House of Electronics industry, 2009: 6-15 (in Chinese).

[3] 張煜, 張萬鵬, 陳璟, 等. 基于Gauss偽譜法的UCAV對地攻擊武器投放軌跡規(guī)劃[J]. 航空學(xué)報, 2011, 32(7): 1240-1251.

ZHANG Y, ZHANG W P, CHEN J, et al. Air-to-ground weapon delivery trajectory planning for UCAVs using Gauss pseudospectral method[J]. Acta Aeronautica et Astronautica Sinica, 2011, 32(7): 1240-1251 (in Chinese).

[4] 白瑞光, 孫鑫, 陳秋雙, 等. 基于Gauss偽譜法的多UAV協(xié)同航跡規(guī)劃[J]. 宇航學(xué)報, 2014, 35(9): 1022-1029.

BAI R G, SUN X, CHEN Q S, et al. Multiple UAV cooperative trajectory planning based on Gauss pseudospectral method[J]. Journal of Astronautics, 2014, 35(9): 1022-1029 (in Chinese).

[5] RICHARDS A, HOW J P. Aircraft trajectory planning with collision avoidance using mixed integer linear programming[C]//Proceedings of the American Control Conference. Reston: AIAA, 2002: 1936-1941.

[6] KUWATA Y, HOW J P. Cooperative distributed robust trajectory optimization using receding horizon MILP[J]. IEEE Transactions on System Technology, 2011, 19(2): 423-431.

[7] 熊偉, 陳宗基, 周銳. 運用混合遺傳算法的多機編隊重構(gòu)優(yōu)化方法[J]. 航空學(xué)報, 2008, 29(S1): 209-214.

XIONG W, CHEN Z J, ZHOU R. Optimization of multiple flight vehicle formation reconfiguration using hybrid genetic algorithm[J]. Acta Aeronautica et Astronautica Sinica, 2008, 29(S1): 209-214 (in Chinese).

[8] DUAN H B, LIU S Q, WU J. Novel intelligent water drops optimization approach to single UCAV smooth trajectory planning[J]. Aerospace Science and Technology, 2009, 13(8): 442-449.

[9] BOYD S, VANDENBERGHE L. Convex optimization[M]. New York: Cambridge University Press, 2004: 1-15.

[10] BYRD R H, GILBERT J C, NOCEDAL J. A trust region method based on interior point techniques for nonlinear programming[J]. Mathematical Programming, 2000, 89(1): 149-185.

[11] LIU X F, LU P. Solving nonconvex optimal control problems by convex optimization[J]. Journal of Guidance, Control, and Dynamics, 2014, 37(3): 750-765.

[12] LU P, LIU X F. Autonomous trajectory planning for rendezvous and proximity operations by conic optimization[J]. Journal of Guidance, Control, and Dynamics, 2013, 36(2): 375-389.

[13] LIU X F, SHEN Z J, LU P. Entry trajectory optimization by second-order cone programming[J]. Journal of Guidance, Control, and Dynamics, 2016, 39(2): 227-241.

[14] LIU X F, SHEN Z J, LU P. Solving the maximum-crossrange problem via successive second-order cone programming with a line search[J]. Aerospace Science and Technology, 2015, 47: 10-20.

[15] MORGAN D, CHUNG S. Model predictive control of swarm of spacecraft using sequential convex programming[J]. Journal of Guidance, Control, and Dynamics, 2014, 37(6): 1725-1740.

[16] AUGUGLIARO F, SCHOELLIG A P, D’ANDREA R. Generation of collision-free for a quadrocopter fleet: a sequential convex programming approach [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE Press, 2012: 1917-1922.

[17] CHEN Y F, CUTLER M, HOW J P. Decoupled multiagent path planning via incremental sequential convex programming[C]//IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2015: 5954-5961.

[18] KABAMBA P T, MEERKOV S M, ZEITZ F H. Optimal path planning for unmanned combat aerial vehicles to defeat radar tracking[J]. Journal of Guidance, Control, and Dynamics, 2006, 29(2): 279-288.

[19] MAIA M H, GALVAO R K H. On the use of mixed-integer linear programming for predictive control with avoidance constraints[J]. International Journal of Robust and Nonlinear Control, 2009, 19(7): 822-828.

[20] RICHARDS A, TURNBULL O. Inter-sample avoidance in trajectory optimizers using mixed-integer linear programming[J]. International Journal of Robust and Nonlinear Control, 2015, 25(4): 521-526.

[21] CVX Research Inc. CVX: Matlab software for disciplined convex programming, version 2.0[EB/OL]. (2011-06-17)[2015-10-09]. http://cvxr.com/cvx.

[22] GRANT M AND BOYD S. Graph implementations for nonsmooth convex programs[M]// Blondel V, Boyd S, and Kimura H. Recent Advances in Learning and Control. Berlin: Springer, 2008: 95-110.

[23] TUTUNCU R H, TOH K C, TODD M J. Solving semidefinite-quadratic-linear programs using SDPT3[J]. Mathematical Programming, Series B, 2003, 95(2): 189-217.

[24] PATTERSON M A, RAO A V. GPOPS-II: A matlab software for solving multiple-phase optimal control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming[J]. ACM Transactions on Mathematical Software, 2014, 41(1): 1-37.

王祝男， 博士研究生。主要研究方向： 無人機任務(wù)規(guī)劃、 無人機協(xié)同決策與控制。

Tel: 010-68913290

E-mail: wangzhubit@163.com

劉莉女， 博士， 教授， 博士生導(dǎo)師。主要研究方向： 飛行器總體設(shè)計、 飛行器結(jié)構(gòu)設(shè)計、 多學(xué)科設(shè)計優(yōu)化。

Tel: 010-68914534

E-mail: liuli@bit.edu.cn

URL：www.cnki.net/kcms/detail/11.1929.V.20160308.1305.004.html

Trajectoryplanningformulti-UAVsusingpenaltysequentialconvexpro-gramming

WANGZhu1,2,LIULi1,2,*,LONGTeng1,2,WENYonglu1,2

1.KeyLaboratoryofDynamicsandControlofFlightVehicle,MinistryofEducation,Beijing100081,China2.SchoolofAerospaceEngineering,BeijingInstituteofTechnology,Beijing100081,China

Trajectoryplanningofmultipleunmannedaerialvehicles(UAVs)isanoptimalcontrolproblemwhichsubjectstononlinearmotionandnonconvexpathconstraints.Basedonthesequentialconvexprogrammingapproach,suchnonconvexoptimalcontrolisapproximatedtobeaseriesofconvexoptimizationsubproblems,whichcanbesolvedbythestate-of-the-artconvexoptimizationalgorithm.Agoodbalancebetweensolutionqualityandcomputationaltractabilitycanthenbeachieved.Nonconvexoptimalcontrolmodelformulti-UAVtrajectoryplanningisformulatedfirst,andisthenapproximatedtobeaconvexoptimizationbydiscretizationandconvexificationmethods.Toconvexifythenonconvexmodel,equationsofmotionofUAVsarelinearized,andconstraintsofthreatavoidanceandinter-UAVscollisionavoidanceareconvexified.Meanwhile,aninter-samplethreatavoidancemethodisprovidedtoguaranteeUAVs’safetyatintervalsbetweendiscretetrajectorypoints.Basedonconvexoptimizationformulation,thedetailedprocedureofusingsequentialconvexprogrammingbasedonpenaltyfunctiontosolvemulti-UAVtrajectoryplanningisprovided.Numericalsimulationsareconductedtoshowtheeffectivenessoftheproposedmethod.Theresultsshowthatthemethodcanacquirethesolutionwithbetteroptimalityandefficiencythanthepseudospectralmethod,especiallyforlargerscaleUAVformation.

unmannedaerialvehicle;trajectoryplanning;collisionavoidance;optimalcontrol;convexprogramming

2015-11-11；Revised2016-01-22；Accepted2016-03-04；Publishedonline2016-03-081305

s：NationalNaturalScienceFoundationofChina(11372036;51105040);AeronauticalScienceFoundationofChina(2015ZA72004)

.Tel.：010-68914534E-mailliuli@bit.edu.cn

2015-11-11；退修日期2016-01-22；錄用日期2016-03-04； < class="emphasis_bold">網(wǎng)絡(luò)出版時間

時間：2016-03-081305

www.cnki.net/kcms/detail/11.1929.V.20160308.1305.004.html

國家自然科學(xué)基金 (11372036,51105040)； 航空科學(xué)基金 (2015ZA72004)

.Tel.：010-68914534E-mailliuli@bit.edu.cn

王祝， 劉莉， 龍騰， 等． 基于罰函數(shù)序列凸規(guī)劃的多無人機軌跡規(guī)劃J． 航空學(xué)報,2016,37(10):3149-3158.WANGZ,LIUL,LONGT,etal.Trajectoryplanningformulti-UAVsusingpenaltysequentialconvexprogrammingJ.ActaAeronauticaetAstronauticaSinica,2016,37(10):3149-3158.

http://hkxb.buaa.edu.cnhkxb@buaa.edu.cn

10.7527/S1000-6893.2016.0064

V279

A

1000-6893(2016)10-3149-10