復(fù)雜環(huán)境下考慮動(dòng)力學(xué)約束的翼傘軌跡規(guī)劃

孫昊,孫青林,*,滕海山,周朋,陳增強(qiáng)

1. 南開大學(xué) 人工智能學(xué)院,天津 300350 2. 北京空間機(jī)電研究所,北京 100094 3. 航天科技集團(tuán)有限公司 航天進(jìn)入減速與著陸技術(shù)實(shí)驗(yàn)室,北京 100094

翼傘系統(tǒng)因其負(fù)載能力強(qiáng)、飛行穩(wěn)定、飛行方向可控等優(yōu)勢(shì)[1-3],在航天器回收、物資空投[4-6]等軍用、民用領(lǐng)域具有不可替代的重要作用。而在翼傘的主要應(yīng)用中,必須實(shí)現(xiàn)精確的歸航控制,確保翼傘可將負(fù)載中所搭載的物資安全投送至指定目標(biāo)位置[7]。因此,需要在軌跡規(guī)劃中計(jì)算出精確、可行的目標(biāo)軌跡。

已有學(xué)者針對(duì)多種控制目標(biāo)與約束條件,進(jìn)行翼傘系統(tǒng)的軌跡規(guī)劃研究。如文獻(xiàn)[8]采用傳統(tǒng)高斯偽譜法,針對(duì)控制量最優(yōu)、精確著陸、迎風(fēng)雀降等多約束條件,進(jìn)行了翼傘歸航任務(wù)中的軌跡規(guī)劃研究；文獻(xiàn)[9]所規(guī)劃的軌跡均設(shè)定為參數(shù)可變的多條貝塞爾曲線,并重點(diǎn)研究了如何應(yīng)用固定形狀的軌跡消除回收落點(diǎn)誤差的研究；文獻(xiàn)[10-11]基于質(zhì)點(diǎn)模型,采用蒙特卡洛仿真法計(jì)算大量規(guī)劃軌跡,并通過對(duì)比分析,從中選取回收精度最高的軌跡；文獻(xiàn)[12]則基于快速擴(kuò)展隨機(jī)樹算法,在軌跡規(guī)劃中考慮了較大風(fēng)場(chǎng)的干擾；文獻(xiàn)[13]探索了翼傘系統(tǒng)的分段軌跡規(guī)劃策略,軌跡全部為標(biāo)準(zhǔn)直線或圓形；文獻(xiàn)[14-15]采用虛擬結(jié)構(gòu)法計(jì)算多個(gè)翼傘的目標(biāo)軌跡,并基于李雅普諾夫穩(wěn)定性設(shè)計(jì)引導(dǎo)策略,實(shí)現(xiàn)多翼傘編隊(duì)；文獻(xiàn)[16]采用一種線目標(biāo)的規(guī)劃策略,針對(duì)大型翼傘在空投回收中的分段軌跡規(guī)劃進(jìn)行了研究；文獻(xiàn)[17]針對(duì)三自由度翼傘模型, 采用控制變量參數(shù)化與時(shí)間尺度變換相結(jié)合的優(yōu)化算法對(duì)其最優(yōu)控制問題進(jìn)行數(shù)值求解。

通過分析現(xiàn)有研究,可以看出在翼傘軌跡規(guī)劃中所通用的傳統(tǒng)質(zhì)點(diǎn)模型難以體現(xiàn)翼傘真實(shí)控制量與系統(tǒng)速度間的非線性關(guān)系。但真實(shí)翼傘通過柔性傘衣提供升力,尤其在傘衣后緣下偏的水平控制狀態(tài)下,傘衣的柔性形變更加明顯,導(dǎo)致系統(tǒng)存在復(fù)雜氣動(dòng)特性與非線性特征[18-20],在飛行過程中其水平速度與垂直速度均會(huì)受到環(huán)境、控制量狀態(tài)影響,導(dǎo)致其運(yùn)動(dòng)軌跡存在時(shí)變的動(dòng)力學(xué)約束,而這種動(dòng)力學(xué)約束難以通過質(zhì)點(diǎn)模型進(jìn)行表述,所規(guī)劃軌跡常與翼傘的實(shí)際運(yùn)動(dòng)狀態(tài)產(chǎn)生較大差異,無法實(shí)現(xiàn)精確的歸航控制。因此,在軌跡規(guī)劃中采用高自由度動(dòng)力學(xué)模型是計(jì)算滿足系統(tǒng)動(dòng)力學(xué)約束的目標(biāo)軌跡的必然趨勢(shì)。但高自由度動(dòng)力學(xué)模型的復(fù)雜度高、非線性明顯。因此,若仍沿用傳統(tǒng)算法,即使在不考慮地形干擾的情況下都難以保證規(guī)劃軌跡平滑、穩(wěn)定。但在翼傘進(jìn)行實(shí)際飛行任務(wù)中,其軌跡規(guī)劃還必須針對(duì)實(shí)際任務(wù)中的復(fù)雜地形干擾進(jìn)行考慮,實(shí)現(xiàn)精確的地形規(guī)避。

針對(duì)上述問題,本文在實(shí)現(xiàn)控制量最優(yōu)、落點(diǎn)精確、地形規(guī)避等傳統(tǒng)多約束條件的基礎(chǔ)上,在軌跡規(guī)劃中引入了翼傘系統(tǒng)的動(dòng)力學(xué)約束。本文將建立翼傘系統(tǒng)的精確6-DOF(Degree of Freedom)動(dòng)力學(xué)模型,通過對(duì)翼傘的6-DOF模型進(jìn)行推導(dǎo),將動(dòng)力學(xué)模型引入到系統(tǒng)的狀態(tài)更新矩陣中,計(jì)算翼傘偏航角的變化率與實(shí)際控制量(傘衣后緣下偏量)的非線性關(guān)系[21],通過其狀態(tài)更新矩陣中的非線性動(dòng)力學(xué)方程計(jì)算翼傘在飛行中的運(yùn)動(dòng)狀態(tài),確保所規(guī)劃出的飛行軌跡也就可以滿足翼傘的動(dòng)力學(xué)約束。隨后,針對(duì)高自由度動(dòng)力學(xué)模型與改進(jìn)傳統(tǒng)高斯偽譜法,設(shè)計(jì)基于分段節(jié)點(diǎn)規(guī)劃、離散點(diǎn)初次規(guī)劃、離散點(diǎn)自重構(gòu)的三階軌跡規(guī)劃策略,最大限度地保證規(guī)劃軌跡的穩(wěn)定性。最后,在仿真實(shí)驗(yàn)中與傳統(tǒng)軌跡規(guī)劃算法相對(duì)比,驗(yàn)證所提出算法的有效性與先進(jìn)性。本文第1節(jié)介紹了翼傘6-DOF動(dòng)力學(xué)模型；第2節(jié)介紹了基于改進(jìn)高斯偽譜法的軌跡規(guī)劃策略；第3節(jié)為仿真實(shí)驗(yàn)驗(yàn)證與分析,第4節(jié)為結(jié)論。

1 翼傘系統(tǒng)建模

如圖1所示,翼傘由傘體與負(fù)載兩部分組成。目前已有大量學(xué)者針對(duì)翼傘系統(tǒng)的建模進(jìn)行了相關(guān)研究[22-23],而本文中為確保規(guī)劃軌跡滿足翼傘的動(dòng)力學(xué)約束,有必要在軌跡規(guī)劃中考慮翼傘的角速度變化率,尤其是偏航角的角加速度[24-25]。

圖1 翼傘系統(tǒng)及相關(guān)坐標(biāo)系Fig.1 Parafoil system and related coordinates

同時(shí),由于高自由度動(dòng)力學(xué)模型更加復(fù)雜、計(jì)算量大,在選取模型時(shí)還需考慮有限時(shí)間內(nèi)的算法收斂。綜上所述,本項(xiàng)目將采用翼傘的6-DOF動(dòng)力學(xué)模型。

1.1 坐標(biāo)系轉(zhuǎn)換與系統(tǒng)狀態(tài)

在翼傘的6-DOF模型中,存在傘體坐標(biāo)系與大地坐標(biāo)系。在建模中,大地坐標(biāo)系與傘體坐標(biāo)系下的運(yùn)動(dòng)速度之間的關(guān)系可以表示為

(1)

式中：[x,y,z]表示系統(tǒng)在大地坐標(biāo)系三軸下的位置；[Vx,Vy,Vz]表示體坐標(biāo)系三軸下的速度；Td-s表示大地坐標(biāo)系到傘體坐標(biāo)系下的轉(zhuǎn)換矩陣,該轉(zhuǎn)換矩陣可表示為

(2)

式中：[φ,θ,ψ]分別表示大地坐標(biāo)系下的翻滾、俯仰與偏航角；cx≡cosx；sx≡sinx。

而系統(tǒng)在大地坐標(biāo)系下的三軸角速度[φ,θ,ψ]可表示為

(3)

式中：[Wx,Wy,Wz]表示體坐標(biāo)下的三軸角速度；tx≡tanx。

而在6-DOF模型中,體坐標(biāo)系下的翼傘系統(tǒng)狀態(tài)將由其受力推導(dǎo)獲得,可表示為

(4)

1.2 系統(tǒng)的質(zhì)量矩陣與受力分析

式(4)中,系統(tǒng)的質(zhì)量矩陣可表示為

A11=msI3×3+ma

(5)

(6)

(7)

(8)

而式(4)中的系統(tǒng)受力可表示為

(9)

(10)

式(9)中,傘體與負(fù)載的氣動(dòng)力計(jì)算最為復(fù)雜。在傘體的氣動(dòng)力計(jì)算中一般將傘衣沿展向分為8個(gè)部分,并將其視為剛體,分別計(jì)算氣動(dòng)力后再進(jìn)行合并。傘體與負(fù)載的氣動(dòng)力可表示為

(11)

(12)

式中：Cd為負(fù)載的氣動(dòng)阻力系數(shù)；ρ為空氣密度；Sw為負(fù)載的特征面積；i=1,2,…,8表示傘衣分片；k表示不同傘衣分片的氣動(dòng)系數(shù)；Cli表示傘衣各分片的升力系數(shù)；Cdi表示傘衣各分片的阻力系數(shù)；Si表示各分片的特征面積。

如圖2所示,翼傘通過傘衣后緣下偏,改變的傘體氣動(dòng)力,并以此控制其飛行方向。式(12)中,傘體的升阻力系數(shù)由傘衣后緣下偏量、迎角等因素推導(dǎo)得出,可表示為

(13)

式中：α表示迎角；ul、ur分別表示翼傘的左右下偏量。

結(jié)合式(1)～式(13),即可建立翼傘的6-DOF動(dòng)力學(xué)模型。在動(dòng)力學(xué)模型中,翼傘的翻滾、俯仰以及偏航角速度被視為系統(tǒng)狀態(tài),由系統(tǒng)受力推導(dǎo)得出。此外,基于式(12)～式(13),也可推導(dǎo)出翼傘控制量與傘體氣動(dòng)力的非線性關(guān)系,可精確計(jì)算傘衣后緣下偏狀態(tài)下的偏航角變化率。

圖2 傘衣后緣下偏Fig.2 Flap deflection of canopy

2 軌跡規(guī)劃策略

2.1 基于動(dòng)力學(xué)模型的狀態(tài)更新矩陣推導(dǎo)

翼傘的傳統(tǒng)質(zhì)點(diǎn)模型可表述為

(14)

式中：vxy、vz表示翼傘的水平和垂直速度；[vwx,vwy]表示水平方向上的風(fēng)場(chǎng)信息。

從式(14)中可以看出,質(zhì)點(diǎn)模型中系統(tǒng)只存在水平、垂直速度與偏航角速度這4個(gè)狀態(tài),控制量與偏航角間為簡(jiǎn)單的線性關(guān)系,難以表述翼傘復(fù)雜的氣動(dòng)特性,因此根據(jù)質(zhì)點(diǎn)模型所規(guī)劃出的軌跡也就無法滿足翼傘的非線性動(dòng)力學(xué)約束。因此,本文將采用翼傘6-DOF動(dòng)力學(xué)模型進(jìn)行軌跡規(guī)劃,與質(zhì)點(diǎn)模型不同,在規(guī)劃中系統(tǒng)存在12個(gè)狀態(tài)：

X=

(15)

狀態(tài)包含大地坐標(biāo)系下的系統(tǒng)位置[x,y,z]、歐拉角度[φ,θ,ψ]、體坐標(biāo)系下三軸的速度與角速度。而系統(tǒng)的狀態(tài)更新矩陣可設(shè)計(jì)為

(16)

式中：T表示采樣頻率。

而式(16)中,大地坐標(biāo)系下的系統(tǒng)狀態(tài)可由式(1)～式(3)推導(dǎo)得出,可表示為

(17)

(18)

2.2 高斯偽譜法

高斯偽譜法可將復(fù)雜環(huán)境下的翼傘軌跡規(guī)劃問題轉(zhuǎn)化為一系列非線性參數(shù)規(guī)劃問題[26],并進(jìn)行離散求解,該方法也被廣泛地應(yīng)用于各類航天器的軌跡規(guī)劃[27-28]。因此,本文將基于高斯偽譜法,針對(duì)翼傘的高自由度動(dòng)力學(xué)模型進(jìn)行軌跡規(guī)劃。在高斯偽譜法中,一個(gè)規(guī)劃問題可被表示為

(19)

系統(tǒng)的狀態(tài)更新需要滿足其動(dòng)態(tài)方程：

(20)

系統(tǒng)的狀態(tài)與控制量還需滿足邊界等式約束與路徑不等式約束：

φmin≤φ(x(t0),t0,x(tf))≤φmax

(21)

C(x(τ),u(τ),t0,tf)≤0

(22)

隨后,將連續(xù)的系統(tǒng)狀態(tài)離散于多個(gè)離散點(diǎn)處,并計(jì)算離散點(diǎn)處的系統(tǒng)狀態(tài),以進(jìn)行軌跡規(guī)劃。為此,將引入時(shí)間變量,把高斯偽譜法中的所規(guī)劃的離散點(diǎn)分布從實(shí)際時(shí)間[t0,tf]轉(zhuǎn)換于[-1,1]：

(23)

通過式(23),即可將離散點(diǎn)的分布從真實(shí)時(shí)間[t0,tf]轉(zhuǎn)換至[-1,1],并將離散點(diǎn)的位置設(shè)計(jì)為N階勒讓德-高斯配點(diǎn)(LG配點(diǎn)),即N階勒讓德多項(xiàng)式的零點(diǎn)。假設(shè)離散點(diǎn)為[τ1,τ2,…,τk,…,τN],則勒讓德多項(xiàng)式可表示為

(24)

式中:d為勒讓德多項(xiàng)式系數(shù)。

如式(19)～式(24)所示,傳統(tǒng)軌跡規(guī)劃算法首先計(jì)算各離散點(diǎn)處的系統(tǒng)狀態(tài),并將所有離散點(diǎn)相連,獲得對(duì)應(yīng)的規(guī)劃軌跡。而在傳統(tǒng)算法所通用序列二次規(guī)劃法中,首先將基于式(24)在所規(guī)劃的整體時(shí)間范圍內(nèi)選取一定數(shù)量的離散點(diǎn),并通過模型推導(dǎo)計(jì)算此類離散點(diǎn)處的系統(tǒng)狀態(tài)。隨后將采用內(nèi)點(diǎn)法、插值法等方法直接插入第2階段的離散點(diǎn)及其所處位置的系統(tǒng)狀態(tài),最后將所有離散點(diǎn)連接在一起,形成規(guī)劃軌跡。該方法廣泛應(yīng)用于傳統(tǒng)質(zhì)點(diǎn)模型的軌跡規(guī)劃,但在采用高自由度動(dòng)力學(xué)模型后,第2階段的離散點(diǎn)無法滿足翼傘的動(dòng)力學(xué)約束,易令優(yōu)化軌跡出現(xiàn)震蕩,導(dǎo)致規(guī)劃失敗。因此,有必要針對(duì)高自由度動(dòng)力學(xué)模型在軌跡規(guī)劃中進(jìn)行針對(duì)化設(shè)計(jì)[29-30],最大限度地保證規(guī)劃軌跡的穩(wěn)定。

2.3 分段軌跡規(guī)劃策略與多約束條件

如圖3所示,本文將針對(duì)翼傘的高自由度動(dòng)力學(xué)模型,設(shè)計(jì)一種三階的軌跡規(guī)劃策略,3個(gè)階段分別是：分段點(diǎn)規(guī)劃、離散點(diǎn)初次規(guī)劃以及離散點(diǎn)自重構(gòu)。

首先,將翼傘的整體運(yùn)動(dòng)軌跡分為多個(gè)段落,設(shè)定較少的離散點(diǎn)作為分段節(jié)點(diǎn),計(jì)算多個(gè)分段節(jié)點(diǎn)處的系統(tǒng)狀態(tài)信息,先得到一條粗略、平穩(wěn)的目標(biāo)軌跡,防止出現(xiàn)軌跡震蕩、錯(cuò)亂等問題。隨后,在各個(gè)分段中分別根據(jù)式(24)選取LG配點(diǎn),基于高自由度動(dòng)力學(xué)模型進(jìn)行初次規(guī)劃,在確保軌跡滿足系統(tǒng)動(dòng)力學(xué)約束的基礎(chǔ)上,同時(shí)實(shí)現(xiàn)地形規(guī)避、精確歸航等多控制目標(biāo)。最后,根據(jù)初次規(guī)劃中的系統(tǒng)運(yùn)動(dòng)狀態(tài)在各個(gè)分段中進(jìn)行離散點(diǎn)自重構(gòu),在環(huán)境復(fù)雜、控制量波動(dòng)大時(shí)提高離散點(diǎn)個(gè)數(shù),平滑優(yōu)化軌跡,以減少規(guī)劃軌跡與實(shí)際之間的誤差。針對(duì)上述結(jié)構(gòu),本文將設(shè)計(jì)分段軌跡優(yōu)化策略,令優(yōu)化軌跡同時(shí)滿足多約束條件。

圖3 針對(duì)高自由度動(dòng)力學(xué)模型的軌跡優(yōu)化策略Fig.3 Trajectory optimization methodology for high-order dynamic model

1) 初、終值約束

首先,本文將所規(guī)劃軌跡分為多個(gè)分段,首先推導(dǎo)各分段點(diǎn)的位置,計(jì)算規(guī)劃軌跡的粗略狀態(tài)。各分段節(jié)點(diǎn)的狀態(tài)可表示為

(25)

式中：Pi表示第i個(gè)分段；t0表示初始時(shí)間；X0表示初始狀態(tài)；t1表示第1分段的終止時(shí)間;X1表示第1分段終止時(shí)的狀態(tài)；[ti-1,f,ti,f]和[Xi-1,f,Xi,f]表示各節(jié)點(diǎn)的起始、終止時(shí)刻及其系統(tǒng)狀態(tài);tf表示終止時(shí)間。如式(25)所示,每個(gè)階段的終止節(jié)點(diǎn)即為下一個(gè)階段的初始節(jié)點(diǎn)。

此外,優(yōu)化節(jié)點(diǎn)間的位置距離還需小于一定降落高度下系統(tǒng)滑降比所允許的最大飛行距離：

(zi-1,0-zi,0)×fgr,max

(26)

式中：fgr,max為翼傘的最大滑降比。

設(shè)定歸航控制的目標(biāo)點(diǎn)為大地坐標(biāo)系的坐標(biāo)原點(diǎn),而翼傘的終值約束可表示為

(27)

2)控制約束

在采用6-DOF動(dòng)力學(xué)模型后,軌跡規(guī)劃中的系統(tǒng)控制量為翼傘的真實(shí)控制量,即為傘衣后緣下偏量,其控制量限制可表示為

-10≤u≤10

(28)

式(28)中,負(fù)值表示左側(cè)傘衣后緣下偏,正值表示右側(cè)。

3)地形規(guī)避

路徑中的系統(tǒng)動(dòng)力學(xué)已經(jīng)在2.1節(jié)中進(jìn)行考慮,本節(jié)中將山體設(shè)為禁飛區(qū),著重于實(shí)現(xiàn)翼傘歸航控制中的地形規(guī)避。首先,山體將被設(shè)計(jì)為一系列同心圓,可表示為

(29)

(30)

式中：p表示山體編號(hào)；[xp,yp]表示山體圓心的水平位置；hp表示山體的最大高度；δ表示山體的傾斜角度；Rp表示山體的半徑。

基于地形規(guī)避的軌跡實(shí)時(shí)約束需滿足：

(31)

4)目標(biāo)函數(shù)

在目標(biāo)函數(shù)的設(shè)計(jì)中,主要考慮系統(tǒng)的控制量最優(yōu),可表示為

(32)

該目標(biāo)函數(shù)的判定標(biāo)準(zhǔn)為令目標(biāo)函數(shù)J的數(shù)值最小,該設(shè)計(jì)還可最小化系統(tǒng)的控制量震蕩,提高軌跡的穩(wěn)定性。

此外,在軌跡優(yōu)化中需針對(duì)控制量過大的階段進(jìn)行離散點(diǎn)自重構(gòu),即在軌跡波動(dòng)較大時(shí)增加離散點(diǎn)數(shù)量,進(jìn)行二次規(guī)劃。設(shè)定Jmax為軌跡波動(dòng)的最大閾值,當(dāng)

(33)

對(duì)第i段軌跡進(jìn)行離散點(diǎn)自重構(gòu),平滑優(yōu)化軌跡。

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

本節(jié)給出了基于不同地形條件下的仿真實(shí)例,并通過與傳統(tǒng)高斯偽譜法相對(duì)比,驗(yàn)證所提出算法的有效性。所選用傘形的展弦比為1.73,傘繩長(zhǎng)度為3.7 m,傘衣面積為22 m2,負(fù)載質(zhì)量為80 kg,負(fù)載的阻力特征面積為0.5 m2。在仿真實(shí)例中,翼傘初始水平位置為[2 000, 2 000] m,初始高度為1 500 m,目標(biāo)落點(diǎn)設(shè)定為大地坐標(biāo)系原點(diǎn),即[0,0,0] m。在實(shí)例1,翼傘的初始水平飛行速度設(shè)定為10 m/s,垂直速度為2 m/s,初始航向角為-145°；實(shí)例2中翼傘的速度不變,初始航向角為180°。

圖4 基于傳統(tǒng)高斯偽譜法與6-DOF模型的軌跡規(guī)劃Fig.4 Trajectory optimization based on traditional Gauss pseudo-spectral method and 6-DOF dynamic model

首先,若仍沿用傳統(tǒng)高斯偽譜法,基于翼傘6-DOF動(dòng)力學(xué)模型的軌跡優(yōu)化結(jié)果如圖4所示。在傳統(tǒng)軌跡規(guī)劃中,僅給出系統(tǒng)的初、終值要求,并忽略了地形約束。從結(jié)果中可以看到針對(duì)高自由度動(dòng)力學(xué)模型,傳統(tǒng)算法難以獲得穩(wěn)定、可行的規(guī)劃軌跡,更無法滿足翼傘的動(dòng)力學(xué)約束。雖然翼傘最終到達(dá)了目標(biāo)位置,然而所規(guī)劃軌跡存在明顯震蕩,甚至存在明顯的規(guī)劃錯(cuò)誤。

圖5～圖7中則給出了基于改進(jìn)后高斯偽譜法與翼傘6-DOF模型的仿真結(jié)果。在仿真實(shí)例1中,存在兩個(gè)山峰地形,山峰的高度為1 500 m,山峰地形的圓心為[1 000,1 000] m和[1 700,1 700] m,山峰最大半徑為250 m和300 m。從圖5(a)和圖5(b)可以看出,改進(jìn)后軌跡規(guī)劃策略可在滿足系統(tǒng)動(dòng)力學(xué)約束的基礎(chǔ)上,計(jì)算出一條較為平滑、穩(wěn)定的規(guī)劃軌跡,軌跡穩(wěn)定性相比于傳統(tǒng)算法有著較大的提高,還可針對(duì)不同高度下的禁飛區(qū)(山峰)半徑實(shí)現(xiàn)有效的地形規(guī)避。從圖5(c)和圖5(d)中也可以看出在進(jìn)行著陸與地形規(guī)避時(shí)外,翼傘控制量可保持穩(wěn)定,并實(shí)現(xiàn)有效的偏航角控制。

在初次規(guī)劃后,離散點(diǎn)在一些波動(dòng)較大的軌跡分段內(nèi)的數(shù)量較少,導(dǎo)致目標(biāo)軌跡的偏航角變化率過大,不夠平滑。但由于翼傘存在大慣性的特征,難以跟蹤角速度變化率過大的轉(zhuǎn)彎軌跡。而如圖6所示,通過分析初次規(guī)劃的軌跡后,可在偏航角變化率過大的分段內(nèi)進(jìn)行離散點(diǎn)自重構(gòu),優(yōu)化離散點(diǎn)分布,增加軌跡波動(dòng)較頻繁的區(qū)域的離散點(diǎn)數(shù)量,進(jìn)行二次規(guī)劃,提高優(yōu)化軌跡的平滑度。

圖7則給出了翼傘在體坐標(biāo)系下的角速度信息,系統(tǒng)的角速度均由6-DOF動(dòng)力學(xué)模型推導(dǎo)得出,在軌跡規(guī)劃中基于系統(tǒng)的角速度與速度信息計(jì)算規(guī)劃軌跡,可確保規(guī)劃軌跡滿足翼傘的非線性動(dòng)力學(xué)約束。實(shí)例2中設(shè)計(jì)了5個(gè)山峰,圓心分別為[1 500,1 500] m、[1 700,900] m、[800,1 100] m、[1 200,800] m、[500,500] m,半徑為400 m或350 m,山峰的高度均為1 500 m。并在仿真中引入突風(fēng)干擾,突風(fēng)速度為2 m/s,風(fēng)場(chǎng)方向?yàn)閤軸負(fù)方向,持續(xù)時(shí)間10 s。

圖5 仿真實(shí)例1Fig.5 Case 1

圖6 離散點(diǎn)自重構(gòu)后的規(guī)劃軌跡Fig.6 Planning trajectory with discrete point self-reconfiguration

圖7 翼傘系統(tǒng)的角速度狀態(tài)Fig.7 Angular velocity of parafoil system

仿真實(shí)例2實(shí)驗(yàn)結(jié)果如圖8所示。從仿真實(shí)驗(yàn)結(jié)果中可以看出所提算法可以有效地針對(duì)復(fù)雜地形進(jìn)行軌跡規(guī)劃,所得軌跡較為平滑。同時(shí),圖8(a)中黑框處即為突風(fēng)干擾下的規(guī)劃軌跡,可以看到由于突風(fēng)干擾,傘體在一定時(shí)間內(nèi)出現(xiàn)了向左的偏轉(zhuǎn),但很快便通過控制量變化恢復(fù)穩(wěn)定,具備一定的魯棒性。此外,由于采用了6-DOF動(dòng)力學(xué)模型,系統(tǒng)可通過控制量變化、轉(zhuǎn)彎等因素調(diào)整其滑降比,因此無需通過盤旋消耗其贅余高度。同時(shí),為確保在采用6-DOF模型后還能維持飛行軌跡穩(wěn)定,在本文目標(biāo)函數(shù)的設(shè)計(jì)中也期望能最小化系統(tǒng)偏航角與控制量的波動(dòng),因此在一些復(fù)雜的地形下本文所提算法也可通過平滑、穩(wěn)定的飛行軌跡到達(dá)目標(biāo)位置。

圖8 仿真實(shí)例2Fig.8 Case 2

4 結(jié) 論

針對(duì)翼傘系統(tǒng)的歸航控制,本文提出了一種考慮系統(tǒng)動(dòng)力學(xué)約束的軌跡規(guī)劃策略。相比于3-DOF簡(jiǎn)單質(zhì)點(diǎn)模型,本文不僅在軌跡規(guī)劃中引入翼傘的6-DOF動(dòng)力學(xué)模型,還針對(duì)傳統(tǒng)軌跡規(guī)劃算法難以計(jì)算出基于高自由度動(dòng)力學(xué)模型的平穩(wěn)軌跡的問題,對(duì)傳統(tǒng)算法進(jìn)行改進(jìn),提出了一種多階段的改進(jìn)后高斯偽譜法,將軌跡規(guī)劃分為3個(gè)階段,即分段點(diǎn)優(yōu)化、離散點(diǎn)初次優(yōu)化以及離散點(diǎn)自重構(gòu),最大限度地保證優(yōu)化軌跡的平滑、穩(wěn)定,并結(jié)合翼傘的6-DOF動(dòng)力學(xué)模型,在實(shí)現(xiàn)精確歸航、地形規(guī)避等約束條件的基礎(chǔ)上,確保規(guī)劃軌跡可以滿足翼傘的非線性動(dòng)力學(xué)約束,提高規(guī)劃軌跡在真實(shí)飛行任務(wù)中的可實(shí)現(xiàn)性。

目前,由于模型非線性強(qiáng)且實(shí)驗(yàn)室所用計(jì)算機(jī)的計(jì)算能力尚有提升空間等原因,規(guī)劃軌跡仍不夠平滑,可能出現(xiàn)一些轉(zhuǎn)角過大的拐點(diǎn)。因此,如何徹底消除這些拐點(diǎn),計(jì)算出更加平滑的軌跡,也是下一階段研究的重點(diǎn)研究方向。