動態(tài)優(yōu)先級解耦的無人機集群軌跡分布式序列凸規(guī)劃

徐廣通,王祝,曹嚴(yán),孫景亮,*,龍騰

1.北京理工大學(xué) 宇航學(xué)院,北京 100081

2.飛行器動力學(xué)與控制教育部重點實驗室,北京 100081

3.華北電力大學(xué) 河北省發(fā)電過程仿真與優(yōu)化控制技術(shù)創(chuàng)新中心,保定 071003

近年來,無人機集群智能技術(shù)在軍事與民用領(lǐng)域得到了愈來愈廣泛的研究。無人機集群快速軌跡規(guī)劃是實現(xiàn)集群智能化的一項瓶頸技術(shù)。集群軌跡規(guī)劃是指在有限空間維度下為大量無人機規(guī)劃出連續(xù)平滑的協(xié)同軌跡,滿足機間避碰、威脅規(guī)避及無人機動力學(xué)等約束,實現(xiàn)無人機之間高效的任務(wù)協(xié)作。然而,嚴(yán)格的威脅規(guī)避約束、耦合的機間避碰約束以及非線性無人機動力學(xué)導(dǎo)致了集群軌跡規(guī)劃難以快速求解,無法滿足集群在線軌跡規(guī)劃要求。因此,如何通過處理機間耦合關(guān)系來提升求解效率是快速軌跡規(guī)劃的關(guān)鍵。

優(yōu)先級規(guī)劃作為解耦規(guī)劃的主要分支,可使無人機按照優(yōu)先級次序依次進行規(guī)劃。該方法因其降低了集群軌跡規(guī)劃的耦合度,提升了集群軌跡規(guī)劃效率,得到了國內(nèi)外學(xué)者的廣泛研究。Van Den Berg和Dvermars提出一種基于運動距離的啟發(fā)式優(yōu)先級確定規(guī)則,能夠在15 s內(nèi)完成包含24個智能體的幾何航路規(guī)劃,但未考慮智能體非線性動力學(xué)約束。Robinson 等使用串行優(yōu)先級解耦機制與偽譜法求解異構(gòu)旋翼機協(xié)同軌跡規(guī)劃問題,32 架機軌跡規(guī)劃耗時累計為112 s。然而,該方法使用預(yù)設(shè)式優(yōu)先級生成方法,導(dǎo)致解耦規(guī)劃易于陷入不可行狀態(tài)。Prasanna等提出了同步并行優(yōu)先級解耦機制,并在理想通信條件下完成了算法的收斂性證明,仿真結(jié)果表明240個智能體的并行迭代一般可在20次以內(nèi)收斂,相比串行方式可以大幅減少規(guī)劃耗時。2015年,Cap等提出了改進的并行優(yōu)先級規(guī)劃方法,進一步節(jié)省了同步規(guī)劃方法中個體相互等待時間,更加充分地利用集群計算資源,相比于同步規(guī)劃方法,25個智能體軌跡規(guī)劃效率提升了1倍。但是,上述并行優(yōu)先級規(guī)劃方法均未考慮多智能體的時間協(xié)同約束,難以實現(xiàn)編隊形成與變換等任務(wù)。

除了高效的解耦機制外,數(shù)值優(yōu)化方法也是制約集群軌跡規(guī)劃效率的關(guān)鍵之一。近年序列凸規(guī)劃(SCP)開始應(yīng)用于多智能體協(xié)同軌跡規(guī)劃問題。蘇黎世聯(lián)邦理工學(xué)院D’Andrea團隊于2012年首次利用SCP 方法以耦合規(guī)劃方式實現(xiàn)了12架無人機的避撞軌跡規(guī)劃,每個迭代求解子問題是關(guān)于集群軌跡的二次規(guī)劃問題。文獻[11]結(jié)合罰函數(shù)與耦合的SCP方法,能夠在40 s內(nèi)為7架固定翼無人機規(guī)劃出編隊集結(jié)軌跡。為緩解耦合規(guī)劃時效性不足問題,麻省理工學(xué)院Chen等使用串行優(yōu)先級解耦規(guī)劃機制與SCP 方法,依據(jù)優(yōu)先級次序利用SCP逐個求解每架無人機軌跡,能夠?qū)?0架四旋翼編隊軌跡規(guī)劃耗時由40 s降到4 s。Chung等針對固定時間航天器集群軌跡規(guī)劃問題,提出并行解耦SCP 方法,降低了計算復(fù)雜度,將航天器集群無障礙環(huán)境下軌跡規(guī)劃時間縮短到幾十秒。在此基礎(chǔ)上,Wang等使用并行解耦SCP 求解多無人機時間最優(yōu)軌跡規(guī)劃問題,能夠在13.5 s內(nèi)生成7架機編隊集結(jié)軌跡,滿足動力學(xué)、障礙規(guī)避與機間避碰約束。然而,文獻[14]所提并行解耦SCP處理機間避碰約束可能陷入迭代振蕩,難以獲得可行解。另外,該研究未給出并行SCP求解集群軌跡規(guī)劃問題的時間一致理論分析。Lu針對非線性等式約束提出凹凸分解的SCP方法,將一個等式約束轉(zhuǎn)化為3個不等式約束,但是該方法難以處理非線性動力學(xué)約束。Foust等提出一種飛行器非線性動力學(xué)修正的SCP方法,以犧牲計算效率為代價提升了SCP的求解精度,且僅涉及小規(guī)模無人機的協(xié)同軌跡規(guī)劃。

針對高維強耦合的集群軌跡規(guī)劃問題,本文結(jié)合優(yōu)先級解耦機制與SCP方法,發(fā)展動態(tài)優(yōu)先級解耦的集群軌跡分布式規(guī)劃方法,實現(xiàn)集群軌跡并行規(guī)劃,改善軌跡迭代收斂速度。本文主要貢獻為：①提出了動態(tài)優(yōu)先級解耦的序列凸規(guī)劃方法(DPD-SCP),將連續(xù)時間集群軌跡規(guī)劃問題轉(zhuǎn)化為一組單機凸規(guī)劃子問題,降低問題求解復(fù)雜度。②發(fā)展了飛行時間驅(qū)動的動態(tài)優(yōu)先級解耦機制,解決迭代振蕩問題,改善集群軌跡解耦規(guī)劃的收斂性；定制時間一致約束更新準(zhǔn)則,調(diào)整飛行時間下邊界約束,驅(qū)使集群同時抵達終端位置,形成既定編隊構(gòu)型。③理論證明了所提方法能夠獲得滿足協(xié)同約束的集群軌跡；數(shù)值仿真結(jié)果表明,DPD-SCP相比耦合SCP、串行優(yōu)先級解耦SCP以及并行解耦SCP具有明顯的效率優(yōu)勢。

1 無人機集群軌跡解耦規(guī)劃問題建模

集群軌跡解耦規(guī)劃是指每架無人機根據(jù)自身任務(wù)、他機軌跡、威脅等信息,并行規(guī)劃出最小飛行時間協(xié)同軌跡。圖1 為集群軌跡解耦規(guī)劃示意,每架無人機基于共享的軌跡信息規(guī)劃本機軌跡,通過不斷的軌跡迭代確保集群時空一致性。

圖1 無人機集群軌跡規(guī)劃示意圖Fig.1 Illustration of trajectory planning for UAV swarms

1.1 無人機動力學(xué)建模

考慮存在架具有相同飛行性能的固定翼無人機,基于歐拉角建立三自由度無側(cè)滑動力學(xué)模型,如式(1)所示：

式中：

無人機動力學(xué)狀態(tài)變量s ∈R包括無人機三維空間位置p =[p ,p ,p ]、飛行速度V 、航向角χ以及航跡傾角γ,即s =[p ,p ,,p ,V ,χ,γ]?？刂谱兞?span id="j5i0abt0b" class="emphasis_italic">u ∈R包括切向過載n 、法向過載n 與垂直過載n ,即u =[n ,n ,n ]。表示重力加速度,下標(biāo)為無人機索引,代表第架無人機。

1.2 解耦的非凸軌跡規(guī)劃問題建模

利用配點法離散化狀態(tài)與控制變量,將軌跡時間[,]拆分成等步長的個區(qū)間,離散步長為Δ=(-)/。使用離散處的+1個點近似表示軌跡,其中t =+·Δ,=0,1,…,。解耦的軌跡規(guī)劃問題設(shè)計變量X 包含單機狀態(tài)與控制變量及時間步長Δt,即X =[ ,Δt],其中z =[s [],u []],=0,1,…,。無人機考慮的初始與終端狀態(tài)[s ,s ]、狀態(tài)與控制邊界[,,,]、動力學(xué)以及威脅規(guī)避約束,均不涉及他機軌跡信息,分別如式(3)～式(6)所示。

針對無人機碰撞規(guī)避問題,建立如式(7)所示的機間避碰約束,其中表示機間安全距離限制,確保無人機安全性。

為實現(xiàn)無人機集群同時抵達終端位置,無人機之間需要分享彼此飛行時間信息,并設(shè)定離散時間步長下邊界約束,如式(8)所示。通過延長飛行時間短無人機的時間步長,或調(diào)整無人機的飛行速度,使得所有無人機飛行時間趨于一致,保證無人機集群形成預(yù)設(shè)的編隊隊形。

式中：Δ表示集群軌跡規(guī)劃的時間步長下邊界值,具體取值方法詳見2.4節(jié)。

考慮機間避碰與時間協(xié)同約束,建立無人機-的非凸軌跡規(guī)劃問題模型P1,如式(9)所示：

2 動態(tài)優(yōu)先級解耦的序列凸規(guī)劃方法

本節(jié)首先建立集群軌跡凸規(guī)劃子問題,然后設(shè)計分布式軌跡規(guī)劃架構(gòu),詳細(xì)描述動態(tài)優(yōu)先級解耦機制與時間一致約束更新準(zhǔn)則,最后給出動態(tài)優(yōu)先級解耦的序列凸規(guī)劃算法具體流程。

2.1 解耦的凸規(guī)劃子問題建模

使用一階泰勒展開將非線性動力學(xué)與非凸威脅規(guī)避約束凸化為式(10)與式(11)的仿射約束。

另外,針對耦合的非凸機間避碰約束,基于軌跡凍結(jié)思想,在單機軌跡規(guī)劃每一步迭代中將其他無人機軌跡視為固定不變,每架無人機僅需規(guī)避他機的凍結(jié)軌跡點位置,即上一步迭代結(jié)果。由于凍結(jié)軌跡在每次規(guī)劃時已知,因此避碰約束僅包含無人機-的狀態(tài)變量,實現(xiàn)避碰約束解耦。在此基礎(chǔ)上,使用一階泰勒展開凸化解耦機間避碰約束,得到式(12)所示的仿射形式。

引入信賴域約束,通過非凸約束凸化,建立集群軌跡的解耦凸規(guī)劃子問題,如式(13)所示,其中=[ρ,ρ,ρ,ρ,ρ,ρ]為信賴域半徑。

2.2 分布式軌跡規(guī)劃架構(gòu)

分布式體系架構(gòu)中無人機處于同等地位,相互之間不存在主從關(guān)系。分布式架構(gòu)依賴無人機之間的通信,通過迭代求解實現(xiàn)機間協(xié)同。因此,本節(jié)給出理想通信假設(shè)(假設(shè)1)：集群個體間保持兩兩通信,能夠有效獲取彼此的軌跡信息,不考慮通信數(shù)量、通信距離、通信帶寬、通信延遲等約束。

無人機集群分布式軌跡規(guī)劃本質(zhì)上是一個計算平臺并行迭代求解方法,架構(gòu)如圖2所示。在每一輪迭代中,各無人機考慮本機動力學(xué)、威脅規(guī)避與解耦的機間避碰等約束,構(gòu)建各自的單機軌跡規(guī)劃凸規(guī)劃子問題P2。針對耦合的機間避碰約束,每架無人機僅考慮規(guī)避其他無人機上一步迭代的軌跡,不考慮當(dāng)前迭代步他機軌跡的更新,通過軌跡暫時凍結(jié)實現(xiàn)機間避碰與時間一致約束解耦。每次迭代中,無人機獨立求解本機軌跡規(guī)劃問題,無人機之間沒有信息交互,計算較快無人機需等待他機完成本輪軌跡解算。在每一輪迭代結(jié)束后,無人機之間通過通信傳輸彼此最新的軌跡信息,為下一輪迭代提供凍結(jié)軌跡信息。無人機根據(jù)獲取的他機軌跡信息,更新時間步長下邊界約束,建立解耦的機間避碰約束,進行新一輪計算,直到獲得滿足飛行約束的軌跡。

圖2 無人機集群軌跡分布式規(guī)劃框架Fig.2 Framework of distributed trajectory planning for UAV swarms

無人機集群協(xié)同飛行的密集特征與動態(tài)特性給算法快速求解可行軌跡帶來了更加嚴(yán)苛的條件。由于無人機規(guī)避他機凍結(jié)軌跡,在密集高動態(tài)集群環(huán)境下,基于相互避碰機制的軌跡迭代可能出現(xiàn)振蕩而難以收斂的問題。為此,本文設(shè)計了飛行時間驅(qū)動的動態(tài)優(yōu)先級解耦機制(具體見2.3節(jié)),改善軌跡迭代的收斂性?；谲壽E凍結(jié)思想,無人機規(guī)避的他機凍結(jié)軌跡與實際軌跡的偏差在信賴域的約束范圍內(nèi),且隨著迭代進行信賴域半徑逐漸縮小,凍結(jié)軌跡與實際軌跡的偏差降低。當(dāng)序列迭代收斂時,凍結(jié)軌跡與實際軌跡趨于一致。因此,基于軌跡凍結(jié)思想的分布式規(guī)劃能夠獲得滿足約束的集群軌跡。

2.3 飛行時間驅(qū)動的動態(tài)優(yōu)先級解耦機制

在基于軌跡凍結(jié)思想的解耦規(guī)劃框架下,不合理的機間避碰約束處理機制可能導(dǎo)致集群軌跡規(guī)劃難以收斂。在軌跡迭代過程中,各無人機通信僅得到他機上一步迭代的軌跡信息,而未獲取他機當(dāng)前的軌跡信息。在無人機相互規(guī)避機制下,軌跡迭代可能出現(xiàn)振蕩情況。圖3為機間避碰反復(fù)振蕩示例,在第次迭代時,由于無人機-考慮規(guī)避無人機-第-1次迭代的位置,無人機-在虛線所示的安全區(qū)域外飛行,逐漸靠近無人機-當(dāng)前位置。在下一輪+1次迭代時,相互避碰機制將會保守的驅(qū)使兩架無人遠離彼此。后續(xù)迭代中兩架無人機可能反復(fù)靠近與遠離,難以獲取滿足避碰約束的軌跡。

圖3 碰撞規(guī)避振蕩現(xiàn)象示意圖Fig.3 Illustration of collision-avoidance oscillation

為了處理上述問題,使用優(yōu)先級驅(qū)動的單向避碰規(guī)則,給無人機分配高低優(yōu)先級,低優(yōu)先級無人機進行軌跡規(guī)劃時,需規(guī)避高優(yōu)先級無人機,而高優(yōu)先級無人機無需考慮對低優(yōu)先級個體的規(guī)避,從而避免無人機相互避碰而引起的軌跡迭代振蕩問題。圖4給出了6架無人機優(yōu)先級次序示例,其中P 表示無人機優(yōu)先級,下標(biāo)=1,2,…,為無人機編號,上標(biāo)=1,2,…,為優(yōu)先級等級。例如,P表示無人機-1優(yōu)先級等級為3,需要規(guī)避無人機-2、無人機-5與無人機-6；P表示無人機-3優(yōu)先級最低；P表示無人機-5具有最高的優(yōu)先級,無需考慮機間避碰約束。

圖4 避碰優(yōu)先級示意圖Fig.4 Illustration of collision-avoidance priority

不同的優(yōu)先級生成方法對于集群軌跡規(guī)劃收斂速度具有重要影響,合適的優(yōu)先級可以提高集群解耦軌跡規(guī)劃效率。本文提出飛行時間驅(qū)動的動態(tài)優(yōu)先級生成方法,在每次迭代結(jié)束后,根據(jù)無人機凍結(jié)軌跡對應(yīng)飛行時間,自適應(yīng)調(diào)整無人機優(yōu)先級次序,改善軌跡規(guī)劃的收斂性。無人機-的優(yōu)先級評價指標(biāo)κ取為無人機-從起始點到目標(biāo)點的飛行時間·Δt,如式(14)所示：

由于飛行時間短無人機具有更多的剩余飛行空間,其成功規(guī)避他機的概率更高,因此對飛行時間短的無人機分配低的優(yōu)先級,充分利用該類無人機的剩余飛行空間進行軌跡調(diào)整。若2架無人機飛行時間相同,則隨機分配其優(yōu)先級次序。

根據(jù)飛行時間·Δt確定集群優(yōu)先級關(guān)系,將無人機-所需規(guī)避的無人機集合Π表示為

根據(jù)優(yōu)先級確定的無人機規(guī)避集合,集群內(nèi)無人機的避碰約束可表示為

在每一輪迭代求解結(jié)束后,動態(tài)優(yōu)先級生成機制根據(jù)新生成的軌跡信息,對無人機優(yōu)先級次序進行調(diào)整,如式(17)所示：

針對任意2架無人機,如果原高優(yōu)先級無人機飛行時間小于低優(yōu)先級無人機,且時間差大于閾值,則進行優(yōu)先級次序互換,否則優(yōu)先級次序不變。若低優(yōu)先級無人機飛行時間仍然小于高優(yōu)先級無人機,則優(yōu)先級次序保持不變?；谒O(shè)計的動態(tài)優(yōu)先級生成方法,只有無人機飛行時間變化超過一定范圍后才進行優(yōu)先級調(diào)整,避免微小變動引發(fā)的優(yōu)先級次序反復(fù)變化問題。

2.4 時間一致約束更新準(zhǔn)則

針對集群解耦軌跡規(guī)劃時間一致問題,增加了時間步長下邊界約束Δ?？紤]到凸規(guī)劃子問題中的動力學(xué)約束是線性近似的結(jié)果,無人機軌跡迭代過程中存在前后兩次迭代結(jié)果相差較大的情況,因此直接取所有無人機軌跡離散步長最大值作為時間步長下邊界,可能導(dǎo)致整體飛行時間的顯著增大。為了避免該現(xiàn)象引起集群飛行時間下限非正常增長的情況,在計算時間步長下限時,僅考慮連續(xù)兩次迭代中時間步長變化較小無人機的飛行時間,如式(18)所示：

式中：表示時間步長變化判斷閾值；上標(biāo)表示軌跡迭代索引。

2.5 動態(tài)優(yōu)先級解耦的序列凸規(guī)劃算法流程

DPD-SCP方法求解流程主要包括3個階段,初始直線軌跡生成的第1階段,不考慮避碰約束的第2階段和考慮所有約束的第3階段。第2和第3階段需要滿足式(19)所示的時間一致收斂條件,其中表示集群時間一致收斂閾值。

動態(tài)優(yōu)先級解耦的序列凸規(guī)劃偽代碼如算法1所示,具體流程如下所述。

算法1 動態(tài)優(yōu)先級解耦的序列凸規(guī)劃算法

初始化算法。初始化算法參數(shù),包括軌跡離散時刻數(shù)量、狀態(tài)收斂閾值∈R、初始信賴域半徑、時間一致收斂閾值、時間步長調(diào)整閾值、優(yōu)先級動態(tài)調(diào)整閾值。輸入無人機的初始與終端狀態(tài)[s ,s ]、狀態(tài)與控制邊界約束[,]和[,]、威脅集合等信息。

(第1～3行) 生成初始軌跡與初始優(yōu)先級。使用線性插值為每架無人機規(guī)劃出連接起始點與目標(biāo)點的直線軌跡,為后續(xù)軌跡迭代提供初值。基于式(15)所示的優(yōu)先級生成方法,根據(jù)初始軌跡時間步長,確定無人機所需規(guī)避的他機集合Π。

(第4～13行) 規(guī)劃滿足動力學(xué)與時間一致約束的軌跡。迭代過程中,使用式(18)計算時間下邊界值Δ,逐次建立并求解考慮無人機動力學(xué)、初始與終端狀態(tài)、狀態(tài)與控制邊界、時間一致約束的凸規(guī)劃子問題。根據(jù)上一步迭代的軌跡信息,使用式(17)更新無人機規(guī)避集合Π。該階段不斷迭代求解,直到生成滿足收斂條件(20)的軌跡,其中表示狀態(tài)收斂閾值。

(第14～25行) 規(guī)劃滿足全部約束的協(xié)同軌跡。本階段重新給信賴域賦初始值。在步驟3生成軌跡的基礎(chǔ)上,考慮碰撞規(guī)避約束,使用所提動態(tài)優(yōu)先級解耦機制,迭代規(guī)劃出滿足動力學(xué)、碰撞規(guī)避、時間一致等約束及收斂條件(20)的集群軌跡。

3 集群軌跡分布式序列凸規(guī)劃收斂性分析

本節(jié)首先給出理論分析所依賴的基本假設(shè),然后分析DPD-SCP求解集群軌跡規(guī)劃問題的收斂性,并給出定理1及其證明。

假設(shè)2(集群非凸軌跡規(guī)劃問題解存在性)：無人機集群非凸軌跡規(guī)劃問題(P1)至少存在一個可行解,滿足初始與終端狀態(tài)、狀態(tài)與控制邊界、動力學(xué)、威脅規(guī)避、機間避碰與時間一致約束。

在假設(shè)2解存在性的基礎(chǔ)上,針對轉(zhuǎn)化的集群軌跡規(guī)劃的凸規(guī)劃子問題P2,如果序列凸規(guī)劃方法生成有限或無限序列{X },則{X }能夠收斂到至少一個極值點,即序列凸規(guī)劃至少能夠求解得到問題P2的一個局部最優(yōu)解。

問題P2中動力學(xué)約束為仿射等式形式,初始與終端狀態(tài)約束為線性等式形式,狀態(tài)與控制邊界、威脅規(guī)避、機間避碰以及信賴域約束均為仿射不等式形式,因此問題P2 所考慮的約束構(gòu)建了一個凸緊集。由于問題P2的目標(biāo)函數(shù)在凸緊集上是連續(xù)可微的,依據(jù)Bolzano-Weierstrass定理,在凸緊集中問題P2 至少存在一個極值點,即局部最優(yōu)解。

若解耦的非凸軌跡規(guī)劃問題P1包含可微目標(biāo)函數(shù)、如式(6)與式(7)所示的威脅規(guī)避與機間避碰凹不等式約束、如式(1)所示的無人機非線性動力學(xué)等式約束,序列凸規(guī)劃能夠通過迭代求解獲得原非凸軌跡規(guī)劃問題P1的局部最優(yōu)解。

引理1說明了序列凸規(guī)劃能夠獲得問題P2的局部最優(yōu)解。下面重點分析問題P2 的局部最優(yōu)解同樣是問題P1局部最優(yōu)解。

針對考慮凹不等式約束(機間避碰與威脅規(guī)避)的軌跡規(guī)劃問題,文獻[19]利用凹約束Hessian矩陣負(fù)定特性,證明了序列凸規(guī)劃能夠得到凹不等式約束下軌跡規(guī)劃問題的局部最優(yōu)解。

針對考慮無人機非線性動力學(xué)等式約束的軌跡規(guī)劃問題,文獻[20]基于罰函數(shù)方法,證明了序列凸規(guī)劃能夠得到非線性動力學(xué)約束下軌跡規(guī)劃問題的局部最優(yōu)解。

綜上分析可得序列凸規(guī)劃方法能夠得到非凸軌跡規(guī)劃問題P1的局部最優(yōu)解。

針對非凸軌跡規(guī)劃問題P1,在分布式軌跡規(guī)劃架構(gòu)下,通過設(shè)計飛行時間驅(qū)動的動態(tài)優(yōu)先級解耦機制,定制時間協(xié)同約束更新準(zhǔn)則,使得DPD-SCP能夠收斂到滿足機間避碰與時間一致約束的集群軌跡。

基于引理1與引理2,序列凸規(guī)劃能夠為每架無人機規(guī)劃出滿足初始與終端狀態(tài)、狀態(tài)與控制邊界、無人機動力學(xué)、威脅規(guī)避約束的局部最優(yōu)解。

針對機間避碰約束,DPD-SCP基于軌跡凍結(jié)思想實現(xiàn)問題解耦。隨著軌跡序列迭代過程進行,信賴域半徑逐漸縮減,由式(21)可知迭代的軌跡逐漸變得與凍結(jié)軌跡一致。

當(dāng)軌跡規(guī)劃收斂時,無人機規(guī)避的他機軌跡即為最新軌跡。因此,最終的集群軌跡結(jié)果能夠滿足機間避碰約束。

后續(xù)基于反證法重點分析DPD-SCP 能夠收斂到時間一致的集群軌跡。

不失一般性,第次迭代,無人機-以最小化飛行時間為目標(biāo)函數(shù),考慮時間下邊界約束(18),通過序列規(guī)劃得到滿足約束的飛行時間結(jié)果,如式(22)所示：

DPD-SCP求解得到每架無人機飛行時間為

考慮動力學(xué)約束與時間下邊界約束包含時間步長信息,為便于推導(dǎo),建立僅考慮動力學(xué)約束與時間下邊界約束的凸規(guī)劃子問題P3。

式中：(s ,u ,Δt)=0表示線性化后的動力學(xué)等式約束。推導(dǎo)P3 的Karush-Kuhn-Tucker(KKT)條件,如式(26)所示：

式中：h 為動力學(xué)約束求偏導(dǎo)后的常值；h 與凍結(jié)軌跡相關(guān),與當(dāng)前設(shè)計變量X 無關(guān)；λ與分別表示等式與不等式約束的拉格朗日乘子。

若第次迭代-Δt+Δ=0,則時間下邊界約束成為主動約束。若Δt＞Δ,根據(jù)KKT條件第1項與第4項,迭代結(jié)果需滿足式(27),才能獲得滿足KKT 條件的解。

由集群具有統(tǒng)一的飛行時間下邊界可得

考慮無人機數(shù)量有界,可得

由此可證DPD-SCP能夠求解得到滿足機間避碰與時間一致約束的集群軌跡。

4 仿真試驗結(jié)果與分析

本節(jié)針對DPD-SCP 開展數(shù)值仿真試驗,在典型集群編隊集結(jié)想定下驗證DPD-SCP的合理性與時效性。通過對比耦合SCP(cSCP)、串行優(yōu)先級解耦SCP(spSCP)互換想定,進一步驗證DPD-SCP的有效性。針對單機軌跡凸規(guī)劃子問題P2,使用SeDu Mi與CVX進行求解?；贛ATLAB R2017a軟件平臺進行數(shù)值仿真運算,PC機硬件為Intel Core i7-6700 3.40 GHz處理器與8 GB運行內(nèi)存。

4.1 集群編隊想定與參數(shù)設(shè)置

編隊集結(jié)想定包含15架無人機,要求集群從分散位置形成雁形編隊。無人機初始與終端飛行速度均設(shè)置為35 m/s；初始與終端航跡傾角均為0°；初始與終端航向角分別為0°與90°；無人機之間的避碰距離設(shè)置為100 m；狀態(tài)與控制邊界約束取值如式(30)所示：

集群編隊互換想定包含100架無人機,初始與終端位置等間距分布在半徑為3 km 的圓上,初末速度為35 m/s,航跡傾角為0°。想定要求處于對角位置的無人機進行位置互換,初末航向角設(shè)置為初始位置朝圓心的方向角。機間安全距離設(shè)置為10 m,狀態(tài)與控制邊界如式(30)所示。

使用所提DPD-SCP方法求解編隊集結(jié)與編隊重構(gòu)軌跡規(guī)劃問題,軌跡離散數(shù)量設(shè)置為40。初始信賴域半徑、狀態(tài)收斂閾值、時間一致收斂閾值、優(yōu)先級調(diào)整閾值、飛行時間下邊界更新閾值等參數(shù)設(shè)置如式(31)所示。

cSCP與pdSCP的參數(shù)設(shè)置分別見文獻[11,14],其狀態(tài)收斂閾值與時間一致收斂閾值同DPD-SCP。

4.2 集群編隊集結(jié)軌跡規(guī)劃

4.2.1 編隊集結(jié)軌跡規(guī)劃結(jié)果

DPD-SCP規(guī)劃的編隊集結(jié)軌跡如圖5所示。從圖中結(jié)果可以看出,所生成軌跡能夠引導(dǎo)無人機集群從松散編隊形成較為緊密的雁形編隊,且集群無人機飛行時間均為174.4 s,滿足時間一致約束。三維視圖與水平投影視圖均表明集群飛行軌跡成功規(guī)避了環(huán)境中的威脅。

圖5 編隊集結(jié)想定下DPD-SCP的軌跡規(guī)劃結(jié)果Fig.5 Trajectory planning results by DPD-SCP on the formation rendezvous scenario

如圖6所示,在編隊集結(jié)想定下,所生成的集群軌跡滿足機間避碰約束,機間最小距離始終大于安全距離限制(100 m),驗證了動態(tài)優(yōu)先級解耦機制的有效性。綜上分析可得,DPD-SCP方法能夠生成滿足威脅規(guī)避、機間避碰與時間一致約束的飛行軌跡。

圖6 編隊集結(jié)想定下集群軌跡的機間最小距離Fig.6 Minimum distance among UAVs of the swarm trajectories on the formation rendezvous scenario

DPD-SCP分布式求解耗時結(jié)果如圖7所示,其中圖上方標(biāo)注表示軌跡迭代序列。針對編隊集結(jié)想定,DPD-SCP通過19次迭代求解得到滿足飛行約束與收斂條件的集群軌跡。DPD-SCP 每步軌跡迭代耗時大約為0.5 s。注意的是,每步軌跡迭代耗時取所有無人機當(dāng)前步迭代耗時的最大值。從結(jié)果中可以得出,優(yōu)先級解耦機制下分布式規(guī)劃通過并行求解將計算壓力分散至每架無人機,增強了方法的可擴展性。

圖7 編隊集結(jié)想定下DPD-SCP分布式求解耗時Fig.7 Runtime of distributed trajectory planning by DPDSCP on the formation rendezvous scenario

4.2.2 編隊集結(jié)軌跡規(guī)劃結(jié)果對比分析

為充分驗證DPD-SCP 方法的有效性,給出不同集群規(guī)模編隊集結(jié)想定下cSCP、spSCP、pdSCP與DPD-SCP目標(biāo)函數(shù)值(飛行時間)對比結(jié)果,如圖8所示。spSCP包含兩層迭代,即外層的協(xié)同迭代層和內(nèi)層的SCP 迭代層。spSCP 在內(nèi)層迭代收斂后,在外層迭代更新時間下邊界約束值,因此spSCP時間下邊界值更新存在一定滯后,易于出現(xiàn)時間下邊界值迅速增長的情況。pdSCP與DPD-SCP將內(nèi)外雙層迭代合并為一層,在每次軌跡迭代后立即進行時間下邊界值調(diào)整。因此,相比于spSCP,pdSCP與DPD-SCP能夠根據(jù)最新的迭代結(jié)果更加及時的進行飛行時間調(diào)整,避免飛行時間下邊界值迅速增長的問題,獲得更短的飛行時間。DPD-SCP 的目標(biāo)函數(shù)與cSCP、pdSCP 相當(dāng),最大偏差不超過3%,表明DPD-SCP規(guī)劃結(jié)果具有良好的時間最優(yōu)性。

圖8 cSCP、spSCP、pdSCP與DPD-SCP目標(biāo)函數(shù)值Fig.8 Objective function value of cSCP,spSCP,pd-SCP,and DPD-SCP

為進一步驗證DPD-SCP 方法的時效性,對比cSCP、spSCP、pdSCP 與DPD-SCP 的規(guī)劃耗時,如圖9所示,其中括號中數(shù)據(jù)表示迭代次數(shù)。由結(jié)果可得,DPD-SCP計算效率明顯高于cSCP、spSCP與pdSCP,且隨著無人機規(guī)模增大,DPDSCP效率優(yōu)勢相比于cSCP更加明顯。受益于動態(tài)優(yōu)先級解耦機制,DPD-SCP平均迭代次數(shù)相比于pdSCP降低了大約15%。15架無人機編隊集結(jié)想定下,DPD-SCP求解耗時相比cSCP、spSCP與pdSCP 分別減低了93.7%、88.3%與40.2%。綜上分析可得,DPD-SCP方法能夠降低序列迭代次數(shù),提升軌跡規(guī)劃方法的收斂速度,相比于其他3種方法具有更高的求解效率與更優(yōu)的可擴展性。

圖9 cSCP、spSCP、pdSCP與DPD-SCP求解耗時對比Fig.9 Runtime comparisons among cSCP,spSCP,pdSCP,and DPD-SCP

4.3 集群編隊互換軌跡規(guī)劃

100架無人機編隊互換軌跡規(guī)劃結(jié)果如圖10所示,機間最小距離如圖11所示。針對強耦合的大規(guī)模集群軌跡規(guī)劃問題,DPD-SCP 通過26次迭代規(guī)劃出滿足約束的集群編隊互換軌跡,集群飛行時間(目標(biāo)函數(shù)值)為209.8 s。DPD-SCP求解耗時9.9 s,然而對比方法cSCP、spSCP 與pdSCP難以在有限時間內(nèi)獲得可行的大規(guī)模集群編隊互換軌跡。相比于15架無人機編隊集結(jié)軌跡規(guī)劃,100 架無人機軌跡規(guī)劃耗時僅增加1.1 s,表明DPD-SCP具有良好的可擴展性。

圖10 編隊互換想定下DPD-SCP的軌跡規(guī)劃結(jié)果Fig.10 Trajectory planning results by DPD-SCP on the formation exchange scenario

圖11 編隊互換想定下集群軌跡的機間最小距離Fig.11 Minimum distance among UAVs of the swarm trajectories on the formation exchange scenario

5 結(jié) 論

1)提出了動態(tài)優(yōu)先級解耦的序列凸規(guī)劃方法,將集群軌跡規(guī)劃問題分解為一組單機分布式規(guī)劃問題,降低了問題維度與求解復(fù)雜度。

2)發(fā)展了飛行時間驅(qū)動的動態(tài)優(yōu)先級解耦機制,根據(jù)飛行時間動態(tài)調(diào)整無人機優(yōu)先級并更新時間一致約束,提高集群軌跡規(guī)劃收斂速度。

3)理論分析證明了DPD-SCP能夠生成滿足碰撞規(guī)避與時間一致約束的集群軌跡。數(shù)值仿真試驗驗證了DPD-SCP 的合理性與時效性,結(jié)果表明相比于cSCP、spSCP與pdSCP,DPD-SCP具備良好的時間最優(yōu)性與顯著的效率優(yōu)勢。