基于罰函數序列凸規劃的多無人機軌跡規劃

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

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

基于罰函數序列凸規劃的多無人機軌跡規劃

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

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

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

無人機； 軌跡規劃； 碰撞規避； 最優控制； 凸規劃

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

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

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

1 多無人機軌跡規劃建模

本節建立多無人機協同軌跡規劃問題的最優控制模型，包括狀態方程、控制量、性能指標和約束條件。

1.1 無人機運動方程

多無人機最優控制的狀態方程包括各無人機的運動方程。無人機定高等速飛行的運動方程[18]為

(1)

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

1.2 控制量與性能指標

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

|ui|≤ui,max

(2)

以最小化燃油為性能指標，可等價表示為

(3)

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

1.3 約束條件

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

無人機軌跡規劃的初始位置[x0,iy0,i]與速度方向ψ0,i和終端位置[xf,iyf,i]與方向ψf,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)

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

(6)

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

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

(7)

1.4 最優控制模型

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

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

(8)

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

2 多機軌跡規劃模型凸化與離散化

問題模型P1包括非線性狀態方程、非凸路徑約束，是一個非凸最優控制問題。而凸優化要求目標函數和不等式約束函數均為凸函數，等式約束函數是仿射函數。因此，為了利用凸優化實現求解，本節建立問題P1的凸化近似模型，同時通過離散化將最優控制轉變為參數優化問題。對比上述模型與凸優化需求，模型凸化需對運動方程進行線性化，以及對威脅規避和無人機間避碰約束進行凸化，而目標函數和其他約束已滿足凸優化框架的要求。

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

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

(9)

(10)

式中：

(11)

(12)

式中：A、B和c為依賴于基準軌跡，而不依賴于當前軌跡的狀態量和控制量，在計算時可認為是常數，從而實現了運動方程的線性化。序列凸規劃迭代求解過程中，基準軌跡選取為前一次迭代得到的軌跡。

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

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

(13)

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

(14)

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

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

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 威脅規避與無人機間避碰約束凸化

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

2.2.1 圓形威脅規避約束凸化

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

(17)

(18)

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

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

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

2.2.2 無人機間避碰約束凸化

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

(19)

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

(20)

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

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

2.3 多機軌跡規劃的凸優化模型

在運動方程、威脅規避和避撞約束凸化與離散化的基礎上，將控制量及其約束和性能指標離散化。控制量離散后的允許控制約束可表示為

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

(21)

將性能指標離散化，并結合前述凸化模型，可得到如下基于基準軌跡的多機軌跡規劃凸優化問題(P2)，其中優化變量為無人機編隊的所有時刻控制量序列的集合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)的作用是將狀態量用控制量的仿射函數表征，通過將其代入其他約束條件表達式中可顯式滿足。

3 離散區間內的威脅規避

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

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

根據上述思想，離散區間內威脅規避約束可表示為：?i∈{1,2,…,N}，?m∈{1,2,…,M}，有

(23)

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

上述離散區間內威脅規避約束為仿射的不等式約束，通過在凸優化問題P2中增加該約束，即可實現離散點間軌跡對威脅的規避。

4 基于罰函數的序列凸規劃方法

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

4.1 序列凸規劃

(24)

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

(25)

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

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

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

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

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

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

(26)

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

4.2 罰函數SCP

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

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

|ui(k)|≤ui,max

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

(27)

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

5 仿真驗證與結果分析

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

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

編隊集結任務仿真想定為：多架無人機分別從各自規劃初始狀態抵達集結區域，并形成雁形編隊。編隊的初始狀態和終端狀態如表1所示。

以軌跡規劃起點對應時間為0 s，無人機編隊在80 s時到達指定集結位置，無人機速度都為20 m·s-2，最大法向加速度ui,max都為5 m·s-2。在集結過程中，無人機編隊需避免相互碰撞，同時需規避環境中的已知威脅。其中，無人機間安全距離設為50 m，環境中存在8個已知威脅，威脅半徑都為100 m，威脅坐標位置分別為(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

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

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

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

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

圖4 軌跡規劃結果對比Fig.4 Comparison of trajectory planning results

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

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

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

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

6 結 論

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

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

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

[1] 沈林成, 牛軼峰, 朱華勇, 等. 多無人機自主協同控制理論與方法[M]. 北京: 國防工業出版社, 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] 丁明躍, 鄭昌文, 周成平, 等. 無人飛行器航跡規劃[M]. 北京: 電子工業出版社, 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對地攻擊武器投放軌跡規劃[J]. 航空學報, 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協同航跡規劃[J]. 宇航學報, 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] 熊偉, 陳宗基, 周銳. 運用混合遺傳算法的多機編隊重構優化方法[J]. 航空學報, 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.

王祝男， 博士研究生。主要研究方向： 無人機任務規劃、 無人機協同決策與控制。

Tel: 010-68913290

E-mail: wangzhubit@163.com

劉莉女， 博士， 教授， 博士生導師。主要研究方向： 飛行器總體設計、 飛行器結構設計、 多學科設計優化。

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">網絡出版時間

時間：2016-03-081305

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

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

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

王祝， 劉莉， 龍騰， 等． 基于罰函數序列凸規劃的多無人機軌跡規劃J． 航空學報,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