復雜環境下考慮動力學約束的翼傘軌跡規劃

孫昊,孫青林,*,滕海山,周朋,陳增強

1. 南開大學 人工智能學院,天津 300350 2. 北京空間機電研究所,北京 100094 3. 航天科技集團有限公司 航天進入減速與著陸技術實驗室,北京 100094

翼傘系統因其負載能力強、飛行穩定、飛行方向可控等優勢[1-3],在航天器回收、物資空投[4-6]等軍用、民用領域具有不可替代的重要作用。而在翼傘的主要應用中,必須實現精確的歸航控制,確保翼傘可將負載中所搭載的物資安全投送至指定目標位置[7]。因此,需要在軌跡規劃中計算出精確、可行的目標軌跡。

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

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

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

1 翼傘系統建模

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

圖1 翼傘系統及相關坐標系Fig.1 Parafoil system and related coordinates

同時,由于高自由度動力學模型更加復雜、計算量大,在選取模型時還需考慮有限時間內的算法收斂。綜上所述,本項目將采用翼傘的6-DOF動力學模型。

1.1 坐標系轉換與系統狀態

在翼傘的6-DOF模型中,存在傘體坐標系與大地坐標系。在建模中,大地坐標系與傘體坐標系下的運動速度之間的關系可以表示為

(1)

式中：[x,y,z]表示系統在大地坐標系三軸下的位置；[Vx,Vy,Vz]表示體坐標系三軸下的速度；Td-s表示大地坐標系到傘體坐標系下的轉換矩陣,該轉換矩陣可表示為

(2)

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

而系統在大地坐標系下的三軸角速度[φ,θ,ψ]可表示為

(3)

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

而在6-DOF模型中,體坐標系下的翼傘系統狀態將由其受力推導獲得,可表示為

(4)

1.2 系統的質量矩陣與受力分析

式(4)中,系統的質量矩陣可表示為

A11=msI3×3+ma

(5)

(6)

(7)

(8)

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

(9)

(10)

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

(11)

(12)

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

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

(13)

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

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

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

2 軌跡規劃策略

2.1 基于動力學模型的狀態更新矩陣推導

翼傘的傳統質點模型可表述為

(14)

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

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

X=

(15)

狀態包含大地坐標系下的系統位置[x,y,z]、歐拉角度[φ,θ,ψ]、體坐標系下三軸的速度與角速度。而系統的狀態更新矩陣可設計為

(16)

式中：T表示采樣頻率。

而式(16)中,大地坐標系下的系統狀態可由式(1)～式(3)推導得出,可表示為

(17)

(18)

2.2 高斯偽譜法

高斯偽譜法可將復雜環境下的翼傘軌跡規劃問題轉化為一系列非線性參數規劃問題[26],并進行離散求解,該方法也被廣泛地應用于各類航天器的軌跡規劃[27-28]。因此,本文將基于高斯偽譜法,針對翼傘的高自由度動力學模型進行軌跡規劃。在高斯偽譜法中,一個規劃問題可被表示為

(19)

系統的狀態更新需要滿足其動態方程：

(20)

系統的狀態與控制量還需滿足邊界等式約束與路徑不等式約束：

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

(21)

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

(22)

隨后,將連續的系統狀態離散于多個離散點處,并計算離散點處的系統狀態,以進行軌跡規劃。為此,將引入時間變量,把高斯偽譜法中的所規劃的離散點分布從實際時間[t0,tf]轉換于[-1,1]：

(23)

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

(24)

式中:d為勒讓德多項式系數。

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

2.3 分段軌跡規劃策略與多約束條件

如圖3所示,本文將針對翼傘的高自由度動力學模型,設計一種三階的軌跡規劃策略,3個階段分別是：分段點規劃、離散點初次規劃以及離散點自重構。

首先,將翼傘的整體運動軌跡分為多個段落,設定較少的離散點作為分段節點,計算多個分段節點處的系統狀態信息,先得到一條粗略、平穩的目標軌跡,防止出現軌跡震蕩、錯亂等問題。隨后,在各個分段中分別根據式(24)選取LG配點,基于高自由度動力學模型進行初次規劃,在確保軌跡滿足系統動力學約束的基礎上,同時實現地形規避、精確歸航等多控制目標。最后,根據初次規劃中的系統運動狀態在各個分段中進行離散點自重構,在環境復雜、控制量波動大時提高離散點個數,平滑優化軌跡,以減少規劃軌跡與實際之間的誤差。針對上述結構,本文將設計分段軌跡優化策略,令優化軌跡同時滿足多約束條件。

圖3 針對高自由度動力學模型的軌跡優化策略Fig.3 Trajectory optimization methodology for high-order dynamic model

1) 初、終值約束

首先,本文將所規劃軌跡分為多個分段,首先推導各分段點的位置,計算規劃軌跡的粗略狀態。各分段節點的狀態可表示為

(25)

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

此外,優化節點間的位置距離還需小于一定降落高度下系統滑降比所允許的最大飛行距離：

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

(26)

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

設定歸航控制的目標點為大地坐標系的坐標原點,而翼傘的終值約束可表示為

(27)

2)控制約束

在采用6-DOF動力學模型后,軌跡規劃中的系統控制量為翼傘的真實控制量,即為傘衣后緣下偏量,其控制量限制可表示為

-10≤u≤10

(28)

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

3)地形規避

路徑中的系統動力學已經在2.1節中進行考慮,本節中將山體設為禁飛區,著重于實現翼傘歸航控制中的地形規避。首先,山體將被設計為一系列同心圓,可表示為

(29)

(30)

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

基于地形規避的軌跡實時約束需滿足：

(31)

4)目標函數

在目標函數的設計中,主要考慮系統的控制量最優,可表示為

(32)

該目標函數的判定標準為令目標函數J的數值最小,該設計還可最小化系統的控制量震蕩,提高軌跡的穩定性。

此外,在軌跡優化中需針對控制量過大的階段進行離散點自重構,即在軌跡波動較大時增加離散點數量,進行二次規劃。設定Jmax為軌跡波動的最大閾值,當

(33)

對第i段軌跡進行離散點自重構,平滑優化軌跡。

3 仿真實驗

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

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

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

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

在初次規劃后,離散點在一些波動較大的軌跡分段內的數量較少,導致目標軌跡的偏航角變化率過大,不夠平滑。但由于翼傘存在大慣性的特征,難以跟蹤角速度變化率過大的轉彎軌跡。而如圖6所示,通過分析初次規劃的軌跡后,可在偏航角變化率過大的分段內進行離散點自重構,優化離散點分布,增加軌跡波動較頻繁的區域的離散點數量,進行二次規劃,提高優化軌跡的平滑度。

圖7則給出了翼傘在體坐標系下的角速度信息,系統的角速度均由6-DOF動力學模型推導得出,在軌跡規劃中基于系統的角速度與速度信息計算規劃軌跡,可確保規劃軌跡滿足翼傘的非線性動力學約束。實例2中設計了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。并在仿真中引入突風干擾,突風速度為2 m/s,風場方向為x軸負方向,持續時間10 s。

圖5 仿真實例1Fig.5 Case 1

圖6 離散點自重構后的規劃軌跡Fig.6 Planning trajectory with discrete point self-reconfiguration

圖7 翼傘系統的角速度狀態Fig.7 Angular velocity of parafoil system

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

圖8 仿真實例2Fig.8 Case 2

4 結 論

針對翼傘系統的歸航控制,本文提出了一種考慮系統動力學約束的軌跡規劃策略。相比于3-DOF簡單質點模型,本文不僅在軌跡規劃中引入翼傘的6-DOF動力學模型,還針對傳統軌跡規劃算法難以計算出基于高自由度動力學模型的平穩軌跡的問題,對傳統算法進行改進,提出了一種多階段的改進后高斯偽譜法,將軌跡規劃分為3個階段,即分段點優化、離散點初次優化以及離散點自重構,最大限度地保證優化軌跡的平滑、穩定,并結合翼傘的6-DOF動力學模型,在實現精確歸航、地形規避等約束條件的基礎上,確保規劃軌跡可以滿足翼傘的非線性動力學約束,提高規劃軌跡在真實飛行任務中的可實現性。

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