王 祝 徐廣通 龍 騰

多無人機(Unmanned aerial vehicle,UAV)協(xié)同通過功能互補和能力疊加等方式可以提高任務(wù)完成效能,已成為無人機應(yīng)用的重要發(fā)展趨勢[1],協(xié)同軌跡規(guī)劃(Cooperative trajectory planning,CTP)則是系統(tǒng)關(guān)鍵技術(shù)之一.

針對軌跡規(guī)劃存在的非線性、約束多、變量維度高等計算復(fù)雜難題,近年來序列凸優(yōu)化(Sequential convex programming,SCP)方法受到廣泛關(guān)注[2].SCP 利用凸優(yōu)化算法具有迭代計算上界和多項式時間復(fù)雜度[3]的特點,通過求解一系列凸優(yōu)化子問題[4],可快速獲得原非凸問題的局部最優(yōu)解,其優(yōu)勢已在不同軌跡規(guī)劃領(lǐng)域得到驗證.相比基于采樣的軌跡規(guī)劃方法,SCP 具有更強的高維求解能力且無需進(jìn)行光滑處理[5-6];相比偽譜法,SCP 能夠在不降低優(yōu)化性能的條件下極大地提高求解效率[7-8];相比啟發(fā)式算法,SCP 具有更好的尋優(yōu)和收斂能力[9-10].

在協(xié)同軌跡規(guī)劃研究方面,文獻(xiàn)[11] 首次將SCP 應(yīng)用于四旋翼飛行器協(xié)同軌跡規(guī)劃;文獻(xiàn)[12]進(jìn)一步提出增量SCP 以提高算法收斂速度;Morgan 等[13]提出分散SCP 以快速生成集群重構(gòu)軌跡,并基于四旋翼完成了實驗驗證;針對固定翼無人機,文獻(xiàn)[14]提出一種罰函數(shù)SCP 方法實現(xiàn)了多機協(xié)同軌跡規(guī)劃;文獻(xiàn)[15]提出一種解耦SCP 方法,通過并行求解降低了算法耗時隨無人機數(shù)量的增長程度.

雖然SCP 相比于傳統(tǒng)軌跡規(guī)劃方法具有效率優(yōu)勢,但固定翼無人機編隊軌跡規(guī)劃仍需數(shù)十秒[14-16],難以滿足動態(tài)規(guī)劃需求.對此,本文在SCP 基礎(chǔ)上,從子問題求解性能角度開展研究,通過定制凸優(yōu)化算法降低子問題優(yōu)化耗時,從而提高規(guī)劃效率.

凸優(yōu)化子問題一般采用SeDuMi[17]和Mosek[18]等工具包求解,但工具包為保證穩(wěn)定性需增加額外運算成本,同時難以針對具體問題設(shè)計高效求解過程.已有的內(nèi)點法定制研究表明,通過算法定制能夠顯著提高凸優(yōu)化效率.Dueri 等[19]對火箭回收軌跡規(guī)劃進(jìn)行內(nèi)點法定制,實現(xiàn)了軌跡實時規(guī)劃,并完成了實驗驗證.Xu 等[20]通過定制稀疏內(nèi)點法,將單個四旋翼軌跡規(guī)劃耗時控制在1 s 以內(nèi).但是,定制算法不具有廣泛通用性,例如針對火箭或四旋翼軌跡規(guī)劃的定制內(nèi)點法無法直接應(yīng)用于固定翼無人機.根據(jù)文獻(xiàn)調(diào)研,尚無針對單架或多架固定翼無人機軌跡規(guī)劃的定制凸優(yōu)化算法.

固定翼無人機的動力學(xué)呈非線性,同時多無人機系統(tǒng)還需考慮機間協(xié)同約束,這些因素都增加了軌跡規(guī)劃復(fù)雜度.對此,本文在解耦SCP[15]的架構(gòu)下,研究子問題求解的定制內(nèi)點法,通過減少單次凸優(yōu)化耗時,進(jìn)一步提升多無人機協(xié)同軌跡規(guī)劃效率.

1 問題建模

多無人機協(xié)同軌跡規(guī)劃需要根據(jù)每架無人機的起始點和目標(biāo)點,考慮運動方程、終端條件、邊界條件、避障和避碰等約束,以最小化時間或能量等為性能指標(biāo),為所有無人機生成可行的飛行軌跡.

本文以時間最短為目標(biāo),將無人機協(xié)同軌跡規(guī)劃建模為下述最優(yōu)控制問題P1[15]:

其中,下標(biāo)i表示無人機編號;下標(biāo)0 表示變量初始時刻值;下標(biāo)f表示變量終端時刻值;N為無人機數(shù)量;t為飛行時間;si表示無人機i的狀態(tài)量且si=(xi,yi,hi,vi,χi,γi)T;smax和smin為狀態(tài)量上下限;ui表示無人機i的控制量且ui=(nx,i,ny,i,nz,i)T;umax和umin為控制量上下限;pobs,m和rm為障礙m的位置和半徑;M表示障礙數(shù)量;C=[I2×2,04×4];R為機間避碰安全距離.

圖1 給出了多無人機協(xié)同軌跡規(guī)劃問題及部分參數(shù)的示意圖.

圖1 多無人機協(xié)同軌跡規(guī)劃問題示意圖Fig.1 Illustration of multi-UAV cooperative trajectory planning problems

問題P1 中各子式的意義可參見文獻(xiàn)[15-16].其中,運動方程=f(si,ui,t)的表達(dá)式[15]如式(2)所示.其中,(x,y)為水平位置;h為高度;v為速度;χ為航向角;γ為航跡傾角;g為重力加速度值;nx,ny,nz分別為無人機在切向、水平、鉛垂方向的過載.

2 解耦SCP 方法

針對式(1)所述的最優(yōu)控制問題,文獻(xiàn)[15]給出了一種解耦SCP 求解架構(gòu).本文在該架構(gòu)的基礎(chǔ)上,聚焦凸優(yōu)化子問題的定制內(nèi)點法研究.考慮文章完整性,本節(jié)對解耦SCP 方法進(jìn)行簡述.

基于SCP 的無人機軌跡規(guī)劃步驟可概括為:1)采用配點法將最優(yōu)控制問題轉(zhuǎn)化為非凸優(yōu)化問題;2)利用約束凸化構(gòu)建一系列凸優(yōu)化子問題;3)通過迭代求解凸優(yōu)化子問題,獲得非凸優(yōu)化問題的解,即最優(yōu)控制問題的數(shù)值近似解.

在SCP 基礎(chǔ)上,解耦SCP 進(jìn)一步針對協(xié)同規(guī)劃問題,利用軌跡凍結(jié)思想和協(xié)調(diào)約束機制,將關(guān)于多機軌跡的一個優(yōu)化問題解耦為一組優(yōu)化問題,解耦后每個問題的優(yōu)化變量僅為一架無人機的軌跡,且解耦問題支持并行求解,提高了協(xié)同軌跡規(guī)劃效率.解耦SCP 方法的求解架構(gòu)如圖2 所示.

圖2 解耦SCP 方法架構(gòu)Fig.2 Frame of decoupled SCP method

針對式(1)所述的問題P1,利用梯形積分和約束凸化,并引入罰函數(shù)和信賴域,可構(gòu)建解耦SCP方法中需多無人機并行迭代求解的凸優(yōu)化子問題P2[15],如式(3)所示.

3 子問題最優(yōu)性條件

為了開展內(nèi)點法定制研究,本節(jié)首先構(gòu)建子問題的一種等價描述形式,然后基于最優(yōu)化理論構(gòu)建該形式下的結(jié)果最優(yōu)性條件.

3.1 凸優(yōu)化子問題的等價描述形式

為了方便表述和推導(dǎo),將問題P2 罰函數(shù)中的所有等式約束統(tǒng)一表示為式(13),包括運動約束式(5)以及終端約束式(6)和式(7),其中zi簡記為z.

將不等式約束統(tǒng)一表示為式(14),包括邊界約束式(8)～(10)、避障約束式(11)和避碰約束式(12),即

由于凸優(yōu)化子問題P2 的約束式(6)～(12)均具有線性特征,因此上述表述是可行的.在此約束表示形式下,凸優(yōu)化子問題P2 可表示為子問題P3

其中,下標(biāo)l表示取矩陣或向量第l行,且

則凸優(yōu)化子問題可進(jìn)一步等價表示為下述僅含有不等式約束的線性規(guī)劃子問題P4.

其中,變量X維度nX和約束數(shù)量nA為

3.2 凸優(yōu)化子問題的最優(yōu)性條件

針對式(20)所示的不等式約束線性規(guī)劃問題P4,其拉格朗日函數(shù)L(X,λ)可表示為

根據(jù)凸優(yōu)化理論的Karush-Kuhn-Tucker 條件[3],子問題P4 的最優(yōu)性條件可表示為

引入輔助變量s=-(A·X-b),可將上述最優(yōu)性條件等價表示為

針對式(26)給出的子問題最優(yōu)性條件,利用數(shù)值算法對該非線性方程組進(jìn)行迭代求解,即可獲得問題P4 的最優(yōu)解,即凸優(yōu)化子問題P2 的最優(yōu)解.

4 內(nèi)點法定制

本節(jié)針對凸優(yōu)化子問題的最優(yōu)性條件方程組,定制高效的內(nèi)點法以降低子問題優(yōu)化的計算復(fù)雜度,進(jìn)而提高原協(xié)同軌跡規(guī)劃問題的求解效率.

4.1 預(yù)測-校正原對偶內(nèi)點法框架

本文定制內(nèi)點法的框架沿用文獻(xiàn)[21]提出的預(yù)測-校正原對偶內(nèi)點法,其流程如圖3 所示,包括初始化、預(yù)測步、校正步、搜索點更新和收斂判斷等.

圖3 原對偶內(nèi)點法計算框架Fig.3 Frame of primal-dual interior-point method

在預(yù)測-校正原對偶內(nèi)點法的每次迭代中,搜索點的更新需考慮預(yù)測步和校正步.其中,預(yù)測步通過求解牛頓系統(tǒng)計算仿射方向和步長;校正步則利用仿射搜索信息對牛頓系統(tǒng)進(jìn)行高階修正,然后求解獲得校正搜索方向和步長,以實現(xiàn)沿中心路徑快速搜索最優(yōu)解.其中,在確定搜索方向后,搜索步長h根據(jù)Wolfe 準(zhǔn)則計算得到.

中心路徑為嚴(yán)格可行點構(gòu)成的一條曲線[21],其定義為

其中,τ為中心路徑參數(shù).F(X,λ,s)可根據(jù)式(26)表示為

其中,Λ=diag{λ},S=diag{s},e=[1,1,···,1]T.

4.2 原對偶搜索方向計算公式

在預(yù)測-校正原對偶內(nèi)點法中,搜索方向ΔX由預(yù)測步搜索方向 ΔXaff和中心校正方向ΔXcen構(gòu)成,如式(29)所示[21].

其中,上標(biāo)aff 表示預(yù)測步的參數(shù)取值,上標(biāo)cen 表示校正步的參數(shù)取值.ΔXaff和 ΔXcen的計算方法如下所述.

1)預(yù)測步搜索方向

在預(yù)測步,直接利用牛頓法計算迭代方向,即計算函數(shù)F(X,λ,s)的雅可比矩陣J(X,λ,s),構(gòu)建非線性方程組的牛頓系統(tǒng),具體為

其中,下標(biāo)q表示內(nèi)點法中當(dāng)前的迭代步數(shù).

因此,預(yù)測步的搜索方向 ΔXaff可根據(jù)式(31)計算得到:

其中,Δλ和 Δs表示拉格朗日乘子λ和輔助變量s對應(yīng)的搜索方向;右端項的對偶?xì)埐顁d,原殘差rp和互補殘差rsλ可將第q次迭代步的值(Xq,λq,sq)代入式(28)計算得到:

2)校正步搜索方向

預(yù)測搜索方向是對非線性系統(tǒng)線性化計算得到的仿射方向,為了使迭代能夠貼近中心路徑和加快迭代收斂,增加校正步以修正仿射搜索方向.

由于式(26)的前兩項均為仿射方程,因此校正步僅需對最優(yōu)條件的第3 項進(jìn)行校正,校正后的互補殘差可表示為

其中,σ∈(0,1)為中心校正因子,μ為互補條件均值,其表達(dá)式為

基于校正后的殘差值,校正步的搜索方向ΔXcen可根據(jù)式(35)進(jìn)行計算:

4.3 原對偶搜索方向快速計算方法

為了獲得原對偶搜索方向,預(yù)測步和校正步需要分別求解式(31)和式(35)所示的非線性方程組,本節(jié)針對方程特點,定制搜索方向的快速計算方法.

1)逐次消元求解搜索方向

若直接對方程組的左端矩陣進(jìn)行求逆,由于矩陣非對稱且不正定,迭代過程中容易發(fā)生矩陣奇異,導(dǎo)致內(nèi)點法迭代難以收斂.

對此,本研究通過逐次消元推導(dǎo),定制非線性方程組的求解步驟,避免求解過程出現(xiàn)非對稱矩陣求逆運算,而且變量逐次求解可以降低計算復(fù)雜性.

針對預(yù)測步搜索方向的計算,首先根據(jù)式(31)的第3 行,推導(dǎo)可得

將式(36)與式(31)第2 行結(jié)合,消去 Δsaff,可得

再將式(37)與式(31)第1 行結(jié)合,消去 Δλaff可得

通過上述推導(dǎo),將根據(jù)式(31)對變量同時求解的方式轉(zhuǎn)換為依次根據(jù)式(38)、(37)和(36)求解ΔXaff、Δλaff和 Δsaff.

采用相同的消元方式,可以得到校正步方向的計算公式,即

2)基于LDL 分解降低計算復(fù)雜度

依據(jù)前述的消元逐次求解方法,原對偶搜索方向的計算量主要在的求逆運算.由于S和 Λ 均為對角矩陣,所以S-1·Λ 為對角矩陣.結(jié)合式(22)中A的表達(dá)式,可將S-1·Λ 表示為

其中,

對于多無人機協(xié)同軌跡規(guī)劃問題,由式(5)～(12)可知,矩陣Aeq、Ain和Atr均為稀疏矩陣,其中Aeq為分塊雙對角與列向量組合的形式,如式(43)所示.

Ain中的避障約束、避碰約束、狀態(tài)邊界約束以及信賴域約束系數(shù)矩陣Atr均為分塊對角矩陣,例如信賴域約束的左端項系數(shù)矩陣形式如式(44)所示.

同時,考慮到矩陣D1,D2,···,D5均為對角矩陣,因此,為稀疏的對稱矩陣.對此,可以采用稀疏矩陣的LDL 分解[22],即

其中,P為排列矩陣,L為單位下三角矩陣,D為分塊對角陣.則式(38)可以轉(zhuǎn)化為

從而,可將求解搜索方向 ΔXaff轉(zhuǎn)化為求解一系列低計算成本的方程,計算過程如式(47)所示.而且計算校正搜索方向 ΔXcen時,可用同樣的方法降低計算成本.

為了說明定制方法計算搜索方向的優(yōu)勢,以浮點運算次數(shù)為統(tǒng)計指標(biāo),對下述3 種方式的計算復(fù)雜度進(jìn)行定量分析: 方法I (即定制方法)通過LDL分解計算 ΔX,再依次計算 Δλ和 Δs;方法II 以求逆方式求解式(38),再計算 Δλ和 Δs;方法III 直接以求逆方式求解式(31).3 種方法的計算成本如表1 所示,其中,nX和nA的表達(dá)式見式(23).

表1 原對偶搜索方向計算復(fù)雜度Table 1 Computation complexity for primal-dual search direction

圖4 給出了3 種搜索方向生成方法的計算復(fù)雜度隨變量維度nX和約束數(shù)量nA的變化情況.由表1和圖4 可見,本文提出的定制方法通過消元逐次計算(比較方法III 和方法II)和LDL 分解(比較方法II 和方法I)減少了原對偶搜索方向的計算復(fù)雜度,從而可以降低子問題優(yōu)化成本.

圖4 原對偶搜索方向計算復(fù)雜度變化圖Fig.4 Variations of computation complexity for primal-dual search direction

另外,上述定制算法利用數(shù)值特征降低子問題的計算成本,并不會改變原對偶內(nèi)點法或SCP 協(xié)同軌跡規(guī)劃的收斂性,其收斂性分析可參考文獻(xiàn)[4]和文獻(xiàn)[23-24].

5 仿真實驗

本節(jié)通過數(shù)值仿真實驗,在解耦SCP 協(xié)同軌跡規(guī)劃架構(gòu)下,驗證定制內(nèi)點法的有效性,并將定制內(nèi)點法與一般內(nèi)點法進(jìn)行對比,驗證定制內(nèi)點法的性能優(yōu)勢.

5.1 仿真環(huán)境與想定

數(shù)值仿真實驗在Windows7 中的MATLAB 2016a 下進(jìn)行,硬件環(huán)境為Intel Core i5-2310@2.90 GHz 處理器和4 GB 內(nèi)存的PC 機.凸優(yōu)化子問題分別利用定制內(nèi)點法和SeDuMi[17]進(jìn)行對比求解.SeDuMi 是廣泛應(yīng)用于MATLAB 環(huán)境的一種凸優(yōu)化工具包,其中采用了基本的預(yù)測-校正原對偶內(nèi)點法,因此將其作為對比算法以驗證本文方法的性能.

采用文獻(xiàn)[15]的仿真實驗算例,考慮7 架無人機所構(gòu)編隊的集結(jié)和重構(gòu)任務(wù),對本文提出的定制內(nèi)點法進(jìn)行測試驗證.無人機起始點、目標(biāo)點和障礙的水平位置見仿真結(jié)果圖,無人機的起始點和目標(biāo)點高度均設(shè)為400 m,障礙半徑均設(shè)為400 m.編隊飛行過程中,無人機間的防撞安全距離設(shè)為100 m.

無人機初始時刻和終端時刻的速度、航向角和航跡傾角分別設(shè)為35 m/s,0 rad 和0 rad.無人機狀態(tài)量和控制量的邊界約束為

針對上述編隊集結(jié)和重構(gòu)想定,SCP 算法相關(guān)參數(shù)設(shè)置為

5.2 仿真結(jié)果與分析

1)定制內(nèi)點法有效性測試

針對設(shè)定的7 架無人機編隊集結(jié)和重構(gòu)任務(wù),基于定制內(nèi)點法進(jìn)行軌跡規(guī)劃求解,軌跡結(jié)果分別如圖5 和圖6 以及圖7 和圖8 所示.由圖可知,定制內(nèi)點法生成的無人機協(xié)同軌跡能夠?qū)︼w行空間中的障礙進(jìn)行有效規(guī)避,并實現(xiàn)從初始點到給定集結(jié)點或隊形重構(gòu)點的平滑轉(zhuǎn)移.距離目標(biāo)點較近的無人機進(jìn)行了繞遠(yuǎn)飛行,以實現(xiàn)飛行時間協(xié)同.

圖5 編隊集結(jié)協(xié)同軌跡二維結(jié)果圖Fig.5 Cooperative trajectories in 2D for formation rendezvous

圖6 編隊集結(jié)協(xié)同軌跡規(guī)劃三維結(jié)果圖Fig.6 Cooperative trajectories in 3D for formation rendezvous

圖7 編隊重構(gòu)協(xié)同軌跡規(guī)劃二維結(jié)果圖Fig.7 Cooperative trajectories in 2D for formation reconfiguration

圖8 編隊重構(gòu)協(xié)同軌跡規(guī)劃三維結(jié)果圖Fig.8 Cooperative trajectories in 3D for formation reconfiguration

圖9 和圖10 給出了編隊飛行過程中無人機間最短距離的變化曲線.由圖可知,任意兩架無人機間的距離始終大于設(shè)定的最小安全距離,可見定制內(nèi)點法生成的協(xié)同軌跡能夠滿足機間避碰約束,實現(xiàn)空間協(xié)同.

圖9 編隊集結(jié)任務(wù)無人機間最短距離變化曲線Fig.9 Variation curve of minimum distance between UAVs for formation rendezvous

圖10 編隊重構(gòu)任務(wù)無人機間最短距離變化曲線Fig.10 Variation curve of minimum distance between UAVs for formation reconfiguration

因此,在解耦SCP 框架下,采用定制內(nèi)點法對凸優(yōu)化子問題進(jìn)行求解,能夠規(guī)劃生成滿足約束的無人機編隊協(xié)同軌跡.

2)算法性能對比測試

在驗證定制內(nèi)點法有效性的基礎(chǔ)上,開展算法對比仿真實驗,比較定制內(nèi)點法和SeDuMi 工具包中一般內(nèi)點法的軌跡規(guī)劃性能.針對編隊集結(jié)和重構(gòu)任務(wù),編隊無人機數(shù)量由1 逐漸增加至7,子問題求解分別采用定制內(nèi)點法和SeDuMi,對獲得的協(xié)同軌跡規(guī)劃結(jié)果和計算耗時進(jìn)行統(tǒng)計.

針對編隊任務(wù)完成時間即飛行時間,定制內(nèi)點法和SeDuMi 優(yōu)化得到的編隊集結(jié)時間如圖11 所示,編隊重構(gòu)時間如圖12 所示.不同編隊規(guī)模下,SeDuMi 獲得的集結(jié)時間均值為179.67 s,定制內(nèi)點法為186.11 s,兩者相差約3.6%;SeDuMi 獲得的重構(gòu)時間均值為187.34 s,定制內(nèi)點法為188.67 s,兩者相差約0.7%.如式(1)所示,本文以任務(wù)完成時間最短為優(yōu)化目標(biāo),因此定制內(nèi)點法的尋優(yōu)能力與SeDuMi 基本相當(dāng).

圖11 編隊集結(jié)任務(wù)完成時間優(yōu)化結(jié)果Fig.11 Optimized completion time for formation rendezvous

圖12 編隊重構(gòu)任務(wù)完成時間優(yōu)化結(jié)果Fig.12 Optimized completion time for formation reconfiguration

針對7 機編隊集結(jié)任務(wù),圖13 和圖14 給出了SeDuMi 和定制內(nèi)點法規(guī)劃結(jié)果對應(yīng)的速度、高度和過載變化曲線.根據(jù)7 機集結(jié)平均速度統(tǒng)計結(jié)果,SeDuMi 為34.27 m/s,定制內(nèi)點法為33.08 m/s.這與圖11 中的7 機集結(jié)時間結(jié)果相符,SeDuMi 所得集結(jié)時間(232.86 s)少于定制方法(256.63 s).

圖13 基于SeDuMi 的編隊集結(jié)軌跡規(guī)劃結(jié)果Fig.13 Trajectory planning results using SeDuMi for formation rendezvous

圖14 基于定制內(nèi)點法的編隊集結(jié)軌跡規(guī)劃結(jié)果Fig.14 Trajectory planning results using customized interior-point method for formation rendezvous

對無人機集結(jié)過程中速度和高度的標(biāo)準(zhǔn)差進(jìn)行計算,再統(tǒng)計獲得編隊的整體均值,其中SeDuMi 結(jié)果為1.78 m/s 和4.33 m,定制內(nèi)點法結(jié)果為1.49 m/s和2.36 m.可見定制內(nèi)點法生成軌跡的狀態(tài)變化稍平滑,更易于軌跡跟蹤控制,但兩者差距不大.

對無人機過載的絕對值之和進(jìn)行統(tǒng)計,該值可近似反映無人機控制能耗的大小關(guān)系.其中鉛垂方向以 (n2-1)進(jìn)行統(tǒng)計.根據(jù)統(tǒng)計,SeDuMi 所得協(xié)同軌跡的三軸過載絕對值之和為(3.77,33.04,0.91),定制內(nèi)點法為(3.66,37.30,0.97).可見,兩種方法所生成軌跡對應(yīng)的控制能耗大小相當(dāng),按定制內(nèi)點法結(jié)果飛行所需的控制能耗略高.

圖15 和圖16 分別給出了7 機編隊重構(gòu)想定下SeDuMi 和定制內(nèi)點法規(guī)劃軌跡的速度、高度和過載變化曲線.SeDuMi 生成軌跡的編隊平均速度為35.71 m/s,定制內(nèi)點法則為36.08 m/s.根據(jù)圖12的7 機編隊重構(gòu)時間結(jié)果,SeDuMi 獲得的重構(gòu)時間(191.91 s)相應(yīng)地也大于定制內(nèi)點法結(jié)果(191.24 s).

圖15 基于SeDuMi 的編隊重構(gòu)軌跡規(guī)劃結(jié)果Fig.15 Trajectory planning results using SeDuMi for formation reconfiguration

圖16 基于定制內(nèi)點法的編隊重構(gòu)軌跡規(guī)劃結(jié)果Fig.16 Trajectory planning results using customized interior-point method for formation reconfiguration

根據(jù)編隊無人機速度標(biāo)準(zhǔn)差和高度標(biāo)準(zhǔn)差的均值統(tǒng)計結(jié)果,SeDuMi 為1.02 m/s 和0.36 m,定制內(nèi)點法為1.64 m/s 和0.34 m.可見兩種方法生成重構(gòu)軌跡的平滑性相當(dāng),都變化不大,易于進(jìn)行軌跡跟蹤控制.

根據(jù)7 機編隊重構(gòu)軌跡的過載統(tǒng)計結(jié)果,Se-DuMi 生成軌跡對應(yīng)的三軸過載絕對值之和為(1.41,13.84,0.16),定制內(nèi)點法則為(2.72,18.60,0.33),可見定制內(nèi)點法所得軌跡對應(yīng)的控制能耗略高于SeDuMi 所得軌跡.

綜合編隊集結(jié)和重構(gòu)的實驗結(jié)果及分析,在任務(wù)完成時間最優(yōu)性、軌跡可實現(xiàn)性和軌跡控制能耗方面,定制內(nèi)點法所得軌跡結(jié)果與SeDuMi 結(jié)果基本相當(dāng),即兩種方法在上述軌跡指標(biāo)中相互不存在明顯優(yōu)勢.

基于定制內(nèi)點法和SeDuMi 進(jìn)行編隊軌跡規(guī)劃的計算耗時統(tǒng)計結(jié)果如圖17 和圖18 所示.由圖可見,在所有想定下,定制內(nèi)點法的規(guī)劃效率都優(yōu)于SeDuMi.其中,對于7 機集結(jié)和重構(gòu)軌跡規(guī)劃問題,基于SeDuMi 的軌跡規(guī)劃耗時分別為19.0 s 和13.1 s,而基于定制內(nèi)點法的耗時僅為3.0 s 和2.4 s.不同規(guī)模下編隊集結(jié)和編隊重構(gòu)規(guī)劃結(jié)果的統(tǒng)計數(shù)據(jù)表明,基于定制內(nèi)點法的軌跡規(guī)劃平均耗時約為SeDuMi 求解耗時的1/7 和1/6.

圖17 編隊集結(jié)軌跡規(guī)劃耗時Fig.17 Computation time for formation rendezvous

圖18 編隊重構(gòu)軌跡規(guī)劃耗時Fig.18 Computation time for formation reconfiguration

同時,對于上述編隊集結(jié)和重構(gòu)軌跡規(guī)劃問題,單機解耦規(guī)劃時凸優(yōu)化的變量維度nX和約束數(shù)量nA約為103和2×103,結(jié)合表1給出的計算復(fù)雜度表達(dá)式,此時定制內(nèi)點法在子優(yōu)化問題求解時的計算復(fù)雜度約為一般內(nèi)點法的1/10.但除了子問題求解,軌跡規(guī)劃還需要完成子問題構(gòu)建、協(xié)同約束生成等計算,這些過程的計算耗時對于定制內(nèi)點法和一般內(nèi)點法是基本相同的.因此,定制內(nèi)點法完成協(xié)同軌跡規(guī)劃的耗時與一般內(nèi)點法耗時的比值會大于1/10,這與仿真實驗獲得的計算耗時統(tǒng)計結(jié)果是一致的.

另外,根據(jù)文獻(xiàn)[15]給出的偽譜法解耦協(xié)同軌跡規(guī)劃結(jié)果,上述編隊集結(jié)和重構(gòu)的平均計算耗時為100.7 s 和111.0 s.可見,相比于廣泛應(yīng)用的偽譜法,本文提出的定制內(nèi)點法可顯著提高軌跡規(guī)劃的效率.

根據(jù)上述軌跡結(jié)果分析和計算時效性對比可知,定制內(nèi)點法生成的編隊軌跡結(jié)果最優(yōu)性與一般內(nèi)點法相當(dāng),但采用定制內(nèi)點法可將協(xié)同軌跡規(guī)劃的計算耗時降低一個數(shù)量級,即從十秒級降低到秒級.對于7 機編隊軌跡規(guī)劃問題,在解耦SCP 規(guī)劃架構(gòu)下,定制內(nèi)點法可以在3 s 內(nèi)生成安全可行的編隊協(xié)同軌跡.

6 結(jié)束語

針對基于解耦SCP 的多機協(xié)同軌跡規(guī)劃中需迭代求解的凸優(yōu)化子問題,通過引入松弛變量構(gòu)建等價形式,并利用協(xié)同軌跡規(guī)劃問題的模型特征,定制子問題求解的高效內(nèi)點法,以降低子問題計算耗時,進(jìn)而提高協(xié)同軌跡規(guī)劃效率.在計算復(fù)雜度分析的基礎(chǔ)上,通過數(shù)值仿真對定制內(nèi)點法的有效性和性能優(yōu)勢進(jìn)行驗證.仿真結(jié)果表明,在軌跡尋優(yōu)能力、軌跡可實現(xiàn)性和軌跡控制能耗方面,定制內(nèi)點法可以獲得與SeDuMi 相當(dāng)?shù)能壽E結(jié)果,但定制內(nèi)點法完成軌跡規(guī)劃的計算耗時相比SeDuMi 可以降低一個數(shù)量級,即由十秒級降至秒級.