基于定制內(nèi)點(diǎn)法的多無人機(jī)協(xié)同軌跡規(guī)劃

王 祝 徐廣通 龍 騰

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

針對(duì)軌跡規(guī)劃存在的非線性、約束多、變量維度高等計(jì)算復(fù)雜難題,近年來序列凸優(yōu)化(Sequential convex programming,SCP)方法受到廣泛關(guān)注[2].SCP 利用凸優(yōu)化算法具有迭代計(jì)算上界和多項(xiàng)式時(shí)間復(fù)雜度[3]的特點(diǎn),通過求解一系列凸優(yōu)化子問題[4],可快速獲得原非凸問題的局部最優(yōu)解,其優(yōu)勢(shì)已在不同軌跡規(guī)劃領(lǐng)域得到驗(yàn)證.相比基于采樣的軌跡規(guī)劃方法,SCP 具有更強(qiáng)的高維求解能力且無需進(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)軌跡,并基于四旋翼完成了實(shí)驗(yàn)驗(yàn)證;針對(duì)固定翼無人機(jī),文獻(xiàn)[14]提出一種罰函數(shù)SCP 方法實(shí)現(xiàn)了多機(jī)協(xié)同軌跡規(guī)劃;文獻(xiàn)[15]提出一種解耦SCP 方法,通過并行求解降低了算法耗時(shí)隨無人機(jī)數(shù)量的增長(zhǎng)程度.

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

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

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

1 問題建模

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

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

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

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

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

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

2 解耦SCP 方法

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

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

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

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

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

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

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

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

為了方便表述和推導(dǎo),將問題P2 罰函數(shù)中的所有等式約束統(tǒng)一表示為式(13),包括運(yùn)動(dòng)約束式(5)以及終端約束式(6)和式(7),其中zi簡(jiǎn)記為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)一步等價(jià)表示為下述僅含有不等式約束的線性規(guī)劃子問題P4.

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

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

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

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

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

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

4 內(nèi)點(diǎn)法定制

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

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

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

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

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

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

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

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

4.2 原對(duì)偶搜索方向計(jì)算公式

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

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

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

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

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

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

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

2)校正步搜索方向

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

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

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

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

4.3 原對(duì)偶搜索方向快速計(jì)算方法

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

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

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

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

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

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

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

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

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

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

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

其中,

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

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

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

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

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

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

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

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

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

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

5 仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2)算法性能對(duì)比測(cè)試

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)束語

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