孫昊,孫青林,*,滕海山,周朋,陳增強
1. 南開大學(xué) 人工智能學(xué)院,天津 300350 2. 北京空間機電研究所,北京 100094 3. 航天科技集團有限公司 航天進入減速與著陸技術(shù)實驗室,北京 100094
翼傘系統(tǒng)因其負載能力強、飛行穩(wěn)定、飛行方向可控等優(yōu)勢[1-3],在航天器回收、物資空投[4-6]等軍用、民用領(lǐng)域具有不可替代的重要作用。而在翼傘的主要應(yīng)用中,必須實現(xiàn)精確的歸航控制,確保翼傘可將負載中所搭載的物資安全投送至指定目標(biāo)位置[7]。因此,需要在軌跡規(guī)劃中計算出精確、可行的目標(biāo)軌跡。
已有學(xué)者針對多種控制目標(biāo)與約束條件,進行翼傘系統(tǒng)的軌跡規(guī)劃研究。如文獻[8]采用傳統(tǒng)高斯偽譜法,針對控制量最優(yōu)、精確著陸、迎風(fēng)雀降等多約束條件,進行了翼傘歸航任務(wù)中的軌跡規(guī)劃研究;文獻[9]所規(guī)劃的軌跡均設(shè)定為參數(shù)可變的多條貝塞爾曲線,并重點研究了如何應(yīng)用固定形狀的軌跡消除回收落點誤差的研究;文獻[10-11]基于質(zhì)點模型,采用蒙特卡洛仿真法計算大量規(guī)劃軌跡,并通過對比分析,從中選取回收精度最高的軌跡;文獻[12]則基于快速擴展隨機樹算法,在軌跡規(guī)劃中考慮了較大風(fēng)場的干擾;文獻[13]探索了翼傘系統(tǒng)的分段軌跡規(guī)劃策略,軌跡全部為標(biāo)準(zhǔn)直線或圓形;文獻[14-15]采用虛擬結(jié)構(gòu)法計算多個翼傘的目標(biāo)軌跡,并基于李雅普諾夫穩(wěn)定性設(shè)計引導(dǎo)策略,實現(xiàn)多翼傘編隊;文獻[16]采用一種線目標(biāo)的規(guī)劃策略,針對大型翼傘在空投回收中的分段軌跡規(guī)劃進行了研究;文獻[17]針對三自由度翼傘模型, 采用控制變量參數(shù)化與時間尺度變換相結(jié)合的優(yōu)化算法對其最優(yōu)控制問題進行數(shù)值求解。
通過分析現(xiàn)有研究,可以看出在翼傘軌跡規(guī)劃中所通用的傳統(tǒng)質(zhì)點模型難以體現(xiàn)翼傘真實控制量與系統(tǒng)速度間的非線性關(guān)系。但真實翼傘通過柔性傘衣提供升力,尤其在傘衣后緣下偏的水平控制狀態(tài)下,傘衣的柔性形變更加明顯,導(dǎo)致系統(tǒng)存在復(fù)雜氣動特性與非線性特征[18-20],在飛行過程中其水平速度與垂直速度均會受到環(huán)境、控制量狀態(tài)影響,導(dǎo)致其運動軌跡存在時變的動力學(xué)約束,而這種動力學(xué)約束難以通過質(zhì)點模型進行表述,所規(guī)劃軌跡常與翼傘的實際運動狀態(tài)產(chǎn)生較大差異,無法實現(xiàn)精確的歸航控制。因此,在軌跡規(guī)劃中采用高自由度動力學(xué)模型是計算滿足系統(tǒng)動力學(xué)約束的目標(biāo)軌跡的必然趨勢。但高自由度動力學(xué)模型的復(fù)雜度高、非線性明顯。因此,若仍沿用傳統(tǒng)算法,即使在不考慮地形干擾的情況下都難以保證規(guī)劃軌跡平滑、穩(wěn)定。但在翼傘進行實際飛行任務(wù)中,其軌跡規(guī)劃還必須針對實際任務(wù)中的復(fù)雜地形干擾進行考慮,實現(xiàn)精確的地形規(guī)避。
針對上述問題,本文在實現(xiàn)控制量最優(yōu)、落點精確、地形規(guī)避等傳統(tǒng)多約束條件的基礎(chǔ)上,在軌跡規(guī)劃中引入了翼傘系統(tǒng)的動力學(xué)約束。本文將建立翼傘系統(tǒng)的精確6-DOF(Degree of Freedom)動力學(xué)模型,通過對翼傘的6-DOF模型進行推導(dǎo),將動力學(xué)模型引入到系統(tǒng)的狀態(tài)更新矩陣中,計算翼傘偏航角的變化率與實際控制量(傘衣后緣下偏量)的非線性關(guān)系[21],通過其狀態(tài)更新矩陣中的非線性動力學(xué)方程計算翼傘在飛行中的運動狀態(tài),確保所規(guī)劃出的飛行軌跡也就可以滿足翼傘的動力學(xué)約束。隨后,針對高自由度動力學(xué)模型與改進傳統(tǒng)高斯偽譜法,設(shè)計基于分段節(jié)點規(guī)劃、離散點初次規(guī)劃、離散點自重構(gòu)的三階軌跡規(guī)劃策略,最大限度地保證規(guī)劃軌跡的穩(wěn)定性。最后,在仿真實驗中與傳統(tǒng)軌跡規(guī)劃算法相對比,驗證所提出算法的有效性與先進性。本文第1節(jié)介紹了翼傘6-DOF動力學(xué)模型;第2節(jié)介紹了基于改進高斯偽譜法的軌跡規(guī)劃策略;第3節(jié)為仿真實驗驗證與分析,第4節(jié)為結(jié)論。
如圖1所示,翼傘由傘體與負載兩部分組成。目前已有大量學(xué)者針對翼傘系統(tǒng)的建模進行了相關(guān)研究[22-23],而本文中為確保規(guī)劃軌跡滿足翼傘的動力學(xué)約束,有必要在軌跡規(guī)劃中考慮翼傘的角速度變化率,尤其是偏航角的角加速度[24-25]。
圖1 翼傘系統(tǒng)及相關(guān)坐標(biāo)系Fig.1 Parafoil system and related coordinates
同時,由于高自由度動力學(xué)模型更加復(fù)雜、計算量大,在選取模型時還需考慮有限時間內(nèi)的算法收斂。綜上所述,本項目將采用翼傘的6-DOF動力學(xué)模型。
在翼傘的6-DOF模型中,存在傘體坐標(biāo)系與大地坐標(biāo)系。在建模中,大地坐標(biāo)系與傘體坐標(biāo)系下的運動速度之間的關(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)
式(4)中,系統(tǒng)的質(zhì)量矩陣可表示為
A11=msI3×3+ma
(5)
(6)
(7)
(8)
而式(4)中的系統(tǒng)受力可表示為
(9)
(10)
式(9)中,傘體與負載的氣動力計算最為復(fù)雜。在傘體的氣動力計算中一般將傘衣沿展向分為8個部分,并將其視為剛體,分別計算氣動力后再進行合并。傘體與負載的氣動力可表示為
(11)
(12)
式中:Cd為負載的氣動阻力系數(shù);ρ為空氣密度;Sw為負載的特征面積;i=1,2,…,8表示傘衣分片;k表示不同傘衣分片的氣動系數(shù);Cli表示傘衣各分片的升力系數(shù);Cdi表示傘衣各分片的阻力系數(shù);Si表示各分片的特征面積。
如圖2所示,翼傘通過傘衣后緣下偏,改變的傘體氣動力,并以此控制其飛行方向。式(12)中,傘體的升阻力系數(shù)由傘衣后緣下偏量、迎角等因素推導(dǎo)得出,可表示為
(13)
式中:α表示迎角;ul、ur分別表示翼傘的左右下偏量。
結(jié)合式(1)~式(13),即可建立翼傘的6-DOF動力學(xué)模型。在動力學(xué)模型中,翼傘的翻滾、俯仰以及偏航角速度被視為系統(tǒng)狀態(tài),由系統(tǒng)受力推導(dǎo)得出。此外,基于式(12)~式(13),也可推導(dǎo)出翼傘控制量與傘體氣動力的非線性關(guān)系,可精確計算傘衣后緣下偏狀態(tài)下的偏航角變化率。
圖2 傘衣后緣下偏Fig.2 Flap deflection of canopy
翼傘的傳統(tǒng)質(zhì)點模型可表述為
(14)
式中:vxy、vz表示翼傘的水平和垂直速度;[vwx,vwy]表示水平方向上的風(fēng)場信息。
從式(14)中可以看出,質(zhì)點模型中系統(tǒng)只存在水平、垂直速度與偏航角速度這4個狀態(tài),控制量與偏航角間為簡單的線性關(guān)系,難以表述翼傘復(fù)雜的氣動特性,因此根據(jù)質(zhì)點模型所規(guī)劃出的軌跡也就無法滿足翼傘的非線性動力學(xué)約束。因此,本文將采用翼傘6-DOF動力學(xué)模型進行軌跡規(guī)劃,與質(zhì)點模型不同,在規(guī)劃中系統(tǒng)存在12個狀態(tài):
X=
(15)
狀態(tài)包含大地坐標(biāo)系下的系統(tǒng)位置[x,y,z]、歐拉角度[φ,θ,ψ]、體坐標(biāo)系下三軸的速度與角速度。而系統(tǒng)的狀態(tài)更新矩陣可設(shè)計為
(16)
式中:T表示采樣頻率。
而式(16)中,大地坐標(biāo)系下的系統(tǒng)狀態(tài)可由式(1)~式(3)推導(dǎo)得出,可表示為
(17)
(18)
高斯偽譜法可將復(fù)雜環(huán)境下的翼傘軌跡規(guī)劃問題轉(zhuǎn)化為一系列非線性參數(shù)規(guī)劃問題[26],并進行離散求解,該方法也被廣泛地應(yīng)用于各類航天器的軌跡規(guī)劃[27-28]。因此,本文將基于高斯偽譜法,針對翼傘的高自由度動力學(xué)模型進行軌跡規(guī)劃。在高斯偽譜法中,一個規(guī)劃問題可被表示為
(19)
系統(tǒng)的狀態(tài)更新需要滿足其動態(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)離散于多個離散點處,并計算離散點處的系統(tǒng)狀態(tài),以進行軌跡規(guī)劃。為此,將引入時間變量,把高斯偽譜法中的所規(guī)劃的離散點分布從實際時間[t0,tf]轉(zhuǎn)換于[-1,1]:
(23)
通過式(23),即可將離散點的分布從真實時間[t0,tf]轉(zhuǎn)換至[-1,1],并將離散點的位置設(shè)計為N階勒讓德-高斯配點(LG配點),即N階勒讓德多項式的零點。假設(shè)離散點為[τ1,τ2,…,τk,…,τN],則勒讓德多項式可表示為
(24)
式中:d為勒讓德多項式系數(shù)。
如式(19)~式(24)所示,傳統(tǒng)軌跡規(guī)劃算法首先計算各離散點處的系統(tǒng)狀態(tài),并將所有離散點相連,獲得對應(yīng)的規(guī)劃軌跡。而在傳統(tǒng)算法所通用序列二次規(guī)劃法中,首先將基于式(24)在所規(guī)劃的整體時間范圍內(nèi)選取一定數(shù)量的離散點,并通過模型推導(dǎo)計算此類離散點處的系統(tǒng)狀態(tài)。隨后將采用內(nèi)點法、插值法等方法直接插入第2階段的離散點及其所處位置的系統(tǒng)狀態(tài),最后將所有離散點連接在一起,形成規(guī)劃軌跡。該方法廣泛應(yīng)用于傳統(tǒng)質(zhì)點模型的軌跡規(guī)劃,但在采用高自由度動力學(xué)模型后,第2階段的離散點無法滿足翼傘的動力學(xué)約束,易令優(yōu)化軌跡出現(xiàn)震蕩,導(dǎo)致規(guī)劃失敗。因此,有必要針對高自由度動力學(xué)模型在軌跡規(guī)劃中進行針對化設(shè)計[29-30],最大限度地保證規(guī)劃軌跡的穩(wěn)定。
如圖3所示,本文將針對翼傘的高自由度動力學(xué)模型,設(shè)計一種三階的軌跡規(guī)劃策略,3個階段分別是:分段點規(guī)劃、離散點初次規(guī)劃以及離散點自重構(gòu)。
首先,將翼傘的整體運動軌跡分為多個段落,設(shè)定較少的離散點作為分段節(jié)點,計算多個分段節(jié)點處的系統(tǒng)狀態(tài)信息,先得到一條粗略、平穩(wěn)的目標(biāo)軌跡,防止出現(xiàn)軌跡震蕩、錯亂等問題。隨后,在各個分段中分別根據(jù)式(24)選取LG配點,基于高自由度動力學(xué)模型進行初次規(guī)劃,在確保軌跡滿足系統(tǒng)動力學(xué)約束的基礎(chǔ)上,同時實現(xiàn)地形規(guī)避、精確歸航等多控制目標(biāo)。最后,根據(jù)初次規(guī)劃中的系統(tǒng)運動狀態(tài)在各個分段中進行離散點自重構(gòu),在環(huán)境復(fù)雜、控制量波動大時提高離散點個數(shù),平滑優(yōu)化軌跡,以減少規(guī)劃軌跡與實際之間的誤差。針對上述結(jié)構(gòu),本文將設(shè)計分段軌跡優(yōu)化策略,令優(yōu)化軌跡同時滿足多約束條件。
圖3 針對高自由度動力學(xué)模型的軌跡優(yōu)化策略Fig.3 Trajectory optimization methodology for high-order dynamic model
1) 初、終值約束
首先,本文將所規(guī)劃軌跡分為多個分段,首先推導(dǎo)各分段點的位置,計算規(guī)劃軌跡的粗略狀態(tài)。各分段節(jié)點的狀態(tài)可表示為
(25)
式中:Pi表示第i個分段;t0表示初始時間;X0表示初始狀態(tài);t1表示第1分段的終止時間;X1表示第1分段終止時的狀態(tài);[ti-1,f,ti,f]和[Xi-1,f,Xi,f]表示各節(jié)點的起始、終止時刻及其系統(tǒng)狀態(tài);tf表示終止時間。如式(25)所示,每個階段的終止節(jié)點即為下一個階段的初始節(jié)點。
此外,優(yōu)化節(jié)點間的位置距離還需小于一定降落高度下系統(tǒng)滑降比所允許的最大飛行距離:
(zi-1,0-zi,0)×fgr,max
(26)
式中:fgr,max為翼傘的最大滑降比。
設(shè)定歸航控制的目標(biāo)點為大地坐標(biāo)系的坐標(biāo)原點,而翼傘的終值約束可表示為
(27)
2)控制約束
在采用6-DOF動力學(xué)模型后,軌跡規(guī)劃中的系統(tǒng)控制量為翼傘的真實控制量,即為傘衣后緣下偏量,其控制量限制可表示為
-10≤u≤10
(28)
式(28)中,負值表示左側(cè)傘衣后緣下偏,正值表示右側(cè)。
3)地形規(guī)避
路徑中的系統(tǒng)動力學(xué)已經(jīng)在2.1節(jié)中進行考慮,本節(jié)中將山體設(shè)為禁飛區(qū),著重于實現(xiàn)翼傘歸航控制中的地形規(guī)避。首先,山體將被設(shè)計為一系列同心圓,可表示為
(29)
(30)
式中:p表示山體編號;[xp,yp]表示山體圓心的水平位置;hp表示山體的最大高度;δ表示山體的傾斜角度;Rp表示山體的半徑。
基于地形規(guī)避的軌跡實時約束需滿足:
(31)
4)目標(biāo)函數(shù)
在目標(biāo)函數(shù)的設(shè)計中,主要考慮系統(tǒng)的控制量最優(yōu),可表示為
(32)
該目標(biāo)函數(shù)的判定標(biāo)準(zhǔn)為令目標(biāo)函數(shù)J的數(shù)值最小,該設(shè)計還可最小化系統(tǒng)的控制量震蕩,提高軌跡的穩(wěn)定性。
此外,在軌跡優(yōu)化中需針對控制量過大的階段進行離散點自重構(gòu),即在軌跡波動較大時增加離散點數(shù)量,進行二次規(guī)劃。設(shè)定Jmax為軌跡波動的最大閾值,當(dāng)
(33)
對第i段軌跡進行離散點自重構(gòu),平滑優(yōu)化軌跡。
本節(jié)給出了基于不同地形條件下的仿真實例,并通過與傳統(tǒng)高斯偽譜法相對比,驗證所提出算法的有效性。所選用傘形的展弦比為1.73,傘繩長度為3.7 m,傘衣面積為22 m2,負載質(zhì)量為80 kg,負載的阻力特征面積為0.5 m2。在仿真實例中,翼傘初始水平位置為[2 000, 2 000] m,初始高度為1 500 m,目標(biāo)落點設(shè)定為大地坐標(biāo)系原點,即[0,0,0] m。在實例1,翼傘的初始水平飛行速度設(shè)定為10 m/s,垂直速度為2 m/s,初始航向角為-145°;實例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動力學(xué)模型的軌跡優(yōu)化結(jié)果如圖4所示。在傳統(tǒng)軌跡規(guī)劃中,僅給出系統(tǒng)的初、終值要求,并忽略了地形約束。從結(jié)果中可以看到針對高自由度動力學(xué)模型,傳統(tǒng)算法難以獲得穩(wěn)定、可行的規(guī)劃軌跡,更無法滿足翼傘的動力學(xué)約束。雖然翼傘最終到達了目標(biāo)位置,然而所規(guī)劃軌跡存在明顯震蕩,甚至存在明顯的規(guī)劃錯誤。
圖5~圖7中則給出了基于改進后高斯偽譜法與翼傘6-DOF模型的仿真結(jié)果。在仿真實例1中,存在兩個山峰地形,山峰的高度為1 500 m,山峰地形的圓心為[1 000,1 000] m和[1 700,1 700] m,山峰最大半徑為250 m和300 m。從圖5(a)和圖5(b)可以看出,改進后軌跡規(guī)劃策略可在滿足系統(tǒng)動力學(xué)約束的基礎(chǔ)上,計算出一條較為平滑、穩(wěn)定的規(guī)劃軌跡,軌跡穩(wěn)定性相比于傳統(tǒng)算法有著較大的提高,還可針對不同高度下的禁飛區(qū)(山峰)半徑實現(xiàn)有效的地形規(guī)避。從圖5(c)和圖5(d)中也可以看出在進行著陸與地形規(guī)避時外,翼傘控制量可保持穩(wěn)定,并實現(xiàn)有效的偏航角控制。
在初次規(guī)劃后,離散點在一些波動較大的軌跡分段內(nèi)的數(shù)量較少,導(dǎo)致目標(biāo)軌跡的偏航角變化率過大,不夠平滑。但由于翼傘存在大慣性的特征,難以跟蹤角速度變化率過大的轉(zhuǎn)彎軌跡。而如圖6所示,通過分析初次規(guī)劃的軌跡后,可在偏航角變化率過大的分段內(nèi)進行離散點自重構(gòu),優(yōu)化離散點分布,增加軌跡波動較頻繁的區(qū)域的離散點數(shù)量,進行二次規(guī)劃,提高優(yōu)化軌跡的平滑度。
圖7則給出了翼傘在體坐標(biāo)系下的角速度信息,系統(tǒng)的角速度均由6-DOF動力學(xué)模型推導(dǎo)得出,在軌跡規(guī)劃中基于系統(tǒng)的角速度與速度信息計算規(guī)劃軌跡,可確保規(guī)劃軌跡滿足翼傘的非線性動力學(xué)約束。實例2中設(shè)計了5個山峰,圓心分別為[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)場方向為x軸負方向,持續(xù)時間10 s。
圖5 仿真實例1Fig.5 Case 1
圖6 離散點自重構(gòu)后的規(guī)劃軌跡Fig.6 Planning trajectory with discrete point self-reconfiguration
圖7 翼傘系統(tǒng)的角速度狀態(tài)Fig.7 Angular velocity of parafoil system
仿真實例2實驗結(jié)果如圖8所示。從仿真實驗結(jié)果中可以看出所提算法可以有效地針對復(fù)雜地形進行軌跡規(guī)劃,所得軌跡較為平滑。同時,圖8(a)中黑框處即為突風(fēng)干擾下的規(guī)劃軌跡,可以看到由于突風(fēng)干擾,傘體在一定時間內(nèi)出現(xiàn)了向左的偏轉(zhuǎn),但很快便通過控制量變化恢復(fù)穩(wěn)定,具備一定的魯棒性。此外,由于采用了6-DOF動力學(xué)模型,系統(tǒng)可通過控制量變化、轉(zhuǎn)彎等因素調(diào)整其滑降比,因此無需通過盤旋消耗其贅余高度。同時,為確保在采用6-DOF模型后還能維持飛行軌跡穩(wěn)定,在本文目標(biāo)函數(shù)的設(shè)計中也期望能最小化系統(tǒng)偏航角與控制量的波動,因此在一些復(fù)雜的地形下本文所提算法也可通過平滑、穩(wěn)定的飛行軌跡到達目標(biāo)位置。
圖8 仿真實例2Fig.8 Case 2
針對翼傘系統(tǒng)的歸航控制,本文提出了一種考慮系統(tǒng)動力學(xué)約束的軌跡規(guī)劃策略。相比于3-DOF簡單質(zhì)點模型,本文不僅在軌跡規(guī)劃中引入翼傘的6-DOF動力學(xué)模型,還針對傳統(tǒng)軌跡規(guī)劃算法難以計算出基于高自由度動力學(xué)模型的平穩(wěn)軌跡的問題,對傳統(tǒng)算法進行改進,提出了一種多階段的改進后高斯偽譜法,將軌跡規(guī)劃分為3個階段,即分段點優(yōu)化、離散點初次優(yōu)化以及離散點自重構(gòu),最大限度地保證優(yōu)化軌跡的平滑、穩(wěn)定,并結(jié)合翼傘的6-DOF動力學(xué)模型,在實現(xiàn)精確歸航、地形規(guī)避等約束條件的基礎(chǔ)上,確保規(guī)劃軌跡可以滿足翼傘的非線性動力學(xué)約束,提高規(guī)劃軌跡在真實飛行任務(wù)中的可實現(xiàn)性。
目前,由于模型非線性強且實驗室所用計算機的計算能力尚有提升空間等原因,規(guī)劃軌跡仍不夠平滑,可能出現(xiàn)一些轉(zhuǎn)角過大的拐點。因此,如何徹底消除這些拐點,計算出更加平滑的軌跡,也是下一階段研究的重點研究方向。