復(fù)雜環(huán)境下翼傘系統(tǒng)的組合式航跡規(guī)劃

李宇輝，趙敏,3,*，陳奇，姚敏,3，何紫陽

1. 南京航空航天大學(xué) 自動(dòng)化學(xué)院，南京 210016

2. 淮陰工學(xué)院 電子信息工程學(xué)院，淮安 223003

3. 高速交通設(shè)施無損檢測與監(jiān)控技術(shù)-工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室，南京 210016

翼傘系統(tǒng)是一種具有高負(fù)載及良好控制性能的柔性飛行器。相比于圓形降落傘系統(tǒng)只能隨風(fēng)而動(dòng)，無法對(duì)其施以有效控制，翼傘系統(tǒng)在繼承了圓形降落傘負(fù)載能力強(qiáng)的優(yōu)點(diǎn)的同時(shí)，還可以通過控制左右后緣襟翼的偏轉(zhuǎn)量來控制翼傘系統(tǒng)的飛行方向及下滑角，實(shí)現(xiàn)精確空投。利用翼傘空投進(jìn)行物資投送及航天器回收是一個(gè)非常不錯(cuò)的選擇。國內(nèi)外眾多學(xué)者都進(jìn)行了翼傘系統(tǒng)的相關(guān)研究[1]。實(shí)現(xiàn)翼傘精確空投的基礎(chǔ)是為翼傘系統(tǒng)規(guī)劃合理的航跡，常見的翼傘系統(tǒng)航跡規(guī)劃方法有分段航跡規(guī)劃及最優(yōu)控制航跡規(guī)劃，這兩種方法的前提是空間內(nèi)不存在障礙物的影響。而實(shí)際翼傘系統(tǒng)歸航途中可能會(huì)遇到高山或者高大建筑物的阻礙，此時(shí)就要考慮如何合理地規(guī)劃航跡，以使翼傘系統(tǒng)可以繞開這些障礙物，從而順利實(shí)現(xiàn)歸航。

在無障礙環(huán)境下，翼傘系統(tǒng)航跡規(guī)劃的方法主要包括分段航跡規(guī)劃及最優(yōu)控制航跡規(guī)劃。分段航跡規(guī)劃是一種較為經(jīng)典的方式，其基本思想是將翼傘系統(tǒng)航跡設(shè)計(jì)為螺旋曲線和直線的組合，要滿足由初始狀態(tài)和目標(biāo)狀態(tài)構(gòu)成的幾何約束，追求落點(diǎn)精度及逆風(fēng)著陸。熊菁等[2]研究了分段歸航方式下的翼傘系統(tǒng)航跡設(shè)計(jì)問題，并采用遺傳算法(GA)對(duì)分段歸航的目標(biāo)函數(shù)進(jìn)行求解。陶金等[3-4]分別采用粒子群算法(PSO)及量子遺傳算法進(jìn)行了翼傘系統(tǒng)分段歸航航跡規(guī)劃目標(biāo)函數(shù)的求解，均取得了不錯(cuò)的效果。趙志豪等[5]對(duì)常規(guī)的分段歸航方式進(jìn)行了改進(jìn)，舍棄了翼傘系統(tǒng)由向心歸航段到能量控制段之間的過渡階段，改為由切點(diǎn)切向進(jìn)入能量控制段，并采用改進(jìn)型魚群算法對(duì)目標(biāo)函數(shù)進(jìn)行求解。高峰等[6]建立了翼傘系統(tǒng)高精度9自由度動(dòng)力學(xué)模型，通過數(shù)值分析得到穩(wěn)態(tài)滑翔比及轉(zhuǎn)彎角速率限值。以耗能最小為目標(biāo)函數(shù)，耗時(shí)和轉(zhuǎn)彎半徑為輸入變量，建立了5段歸航軌跡優(yōu)化模型。將5段歸航軌跡作為初始條件，利用偽譜法進(jìn)行軌跡優(yōu)化，給出歸航最優(yōu)參考路徑，并對(duì)比分析了5段歸航與直接歸航過程中的能量損耗。除了分段歸航方式之外，最優(yōu)控制歸航也是一種常見的航跡規(guī)劃方式。相比于分段歸航方式，最優(yōu)控制歸航方式還額外追求控制能耗最小。熊菁[7]做了開創(chuàng)性的研究，采用間接法分析和研究了翼傘系統(tǒng)最優(yōu)控制航跡規(guī)劃問題。高海濤等[8-9]分別采用偽譜法進(jìn)行目標(biāo)函數(shù)的求解，其基本思想是將航跡規(guī)劃問題轉(zhuǎn)化為非線性規(guī)劃問題。此外，梁海燕等[10]將控制量參數(shù)化，將最優(yōu)控制問題簡化為參數(shù)優(yōu)化問題，降低了目標(biāo)函數(shù)的求解難度。

在復(fù)雜環(huán)境下的翼傘系統(tǒng)航跡規(guī)劃領(lǐng)域，本文作者[11]提出了一種基于梯度下降法的翼傘系統(tǒng)最優(yōu)分段航跡規(guī)劃方法，該方法將控制變量參數(shù)化，將逆風(fēng)精確著陸、控制能耗小、能實(shí)現(xiàn)避障等多目標(biāo)優(yōu)化問題轉(zhuǎn)化為加權(quán)單目標(biāo)優(yōu)化問題，并通過梯度下降法求解得到分段常值最優(yōu)歸航航跡。孫昊等[12]針對(duì)傳統(tǒng)質(zhì)點(diǎn)模型所規(guī)劃的目標(biāo)軌跡難以滿足復(fù)雜環(huán)境下的系統(tǒng)動(dòng)力學(xué)約束問題，將翼傘系統(tǒng)六自由度動(dòng)力學(xué)模型引入翼傘系統(tǒng)歸航的航跡規(guī)劃中，并通過改進(jìn)高斯偽譜法，設(shè)計(jì)了一種基于分段點(diǎn)規(guī)劃、離散點(diǎn)初次規(guī)劃、離散點(diǎn)自重構(gòu)的三階軌跡優(yōu)化策略。

實(shí)際翼傘空投環(huán)境一般較為復(fù)雜，但往往具有這樣的特點(diǎn)，即翼傘系統(tǒng)歸航過程中會(huì)遇到高山或者高大建筑物阻礙，只有目標(biāo)點(diǎn)周圍的小范圍內(nèi)沒有障礙物。此時(shí)，難以單獨(dú)采用分段歸航或者最優(yōu)控制歸航方式來設(shè)計(jì)翼傘系統(tǒng)的航跡。必須采取相應(yīng)方法，規(guī)劃一條能夠繞開障礙物并最終到達(dá)目標(biāo)點(diǎn)，滿足能量控制及逆風(fēng)著陸的要求，同時(shí)也要滿足一定的落點(diǎn)精度要求的翼傘系統(tǒng)航跡。

為了解決上述復(fù)雜環(huán)境下的翼傘系統(tǒng)航跡規(guī)劃問題，本文考慮實(shí)際空投任務(wù)中的環(huán)境特點(diǎn)，將空間劃分為障礙區(qū)和著陸區(qū)，在障礙區(qū)域內(nèi)采用經(jīng)過適用性改進(jìn)的快速搜索隨機(jī)樹(RRT)算法搜索可行路徑。并且采用相應(yīng)的處理，以生成平滑的參考軌跡?？紤]到RRT算法方向隨機(jī)，難以直接實(shí)現(xiàn)逆風(fēng)對(duì)準(zhǔn)著陸且在避障過程中不易進(jìn)行能量控制的問題，在翼傘系統(tǒng)進(jìn)入著陸區(qū)內(nèi)，采用分段歸航的方式設(shè)計(jì)航跡，實(shí)現(xiàn)翼傘系統(tǒng)能量控制及逆風(fēng)著陸。同時(shí)考慮了兩個(gè)區(qū)域航跡的銜接問題。

1 翼傘系統(tǒng)質(zhì)點(diǎn)模型

常見的翼傘系統(tǒng)模型有質(zhì)點(diǎn)模型、被視作剛體的六自由度模型、考慮傘體與載荷之間相對(duì)運(yùn)動(dòng)的九自由度或八自由度模型等。對(duì)于翼傘系統(tǒng)航跡規(guī)劃問題，主要是研究翼傘系統(tǒng)整體的運(yùn)動(dòng)，翼傘系統(tǒng)的姿態(tài)變化及系統(tǒng)各結(jié)構(gòu)之間的相對(duì)運(yùn)動(dòng)不是航跡規(guī)劃所要考慮的主要因素，因此，參考翼傘系統(tǒng)質(zhì)點(diǎn)模型即可滿足要求。

考慮穩(wěn)態(tài)情況下，翼傘系統(tǒng)滑翔比和速度保持不變，翼傘系統(tǒng)的質(zhì)點(diǎn)模型可以表示為

(1)

式中：(x,y,z)為翼傘系統(tǒng)在大地坐標(biāo)系下的位置；V為翼傘系統(tǒng)和速度；φ為翼傘系統(tǒng)航向角；u為控制量，與非對(duì)稱襟翼偏轉(zhuǎn)量存在對(duì)應(yīng)關(guān)系；γ為翼傘系統(tǒng)下滑角。

穩(wěn)定狀態(tài)下，翼傘系統(tǒng)運(yùn)動(dòng)可以分解為水平方向的勻速運(yùn)動(dòng)及豎直方向的勻速運(yùn)動(dòng)，翼傘系統(tǒng)滑翔比k定義為

(2)

則下滑角γ可以表示為

(3)

因此，穩(wěn)態(tài)下翼傘系統(tǒng)的運(yùn)動(dòng)軌跡是一條具有恒定下滑角的曲線。

2 復(fù)雜環(huán)境下翼傘系統(tǒng)航跡規(guī)劃問題描述

在實(shí)際翼傘空投任務(wù)中，翼傘系統(tǒng)在歸航途中會(huì)遇到高山或者高大建筑物阻礙，需要繞開這些障礙物，僅在著陸點(diǎn)附近存在范圍有限的空曠區(qū)域，即為目標(biāo)著陸區(qū)域。圖1為復(fù)雜環(huán)境下的翼傘空投示意圖。圖1中：φ0為初始航向角。大地坐標(biāo)系原點(diǎn)為空投的目標(biāo)著陸點(diǎn)，以目標(biāo)點(diǎn)為中心，周圍半徑為Rth的圓形區(qū)域?yàn)闆]有障礙物的著陸區(qū)。而在空投的初始點(diǎn)與著陸區(qū)之間則是存在高山或者高大建筑物的障礙區(qū)，要實(shí)現(xiàn)定點(diǎn)空投，必須合理地規(guī)劃航跡，以使翼傘系統(tǒng)繞過障礙物，并最終到達(dá)目標(biāo)點(diǎn)。

圖1 復(fù)雜環(huán)境下空投示意圖

根據(jù)上述有障礙時(shí)翼傘空投的特點(diǎn)，將復(fù)雜環(huán)境下翼傘系統(tǒng)的航跡規(guī)劃要求總結(jié)為以下幾點(diǎn)：

1) 系統(tǒng)能夠躲避障礙物，不發(fā)生碰撞。

2) 航跡應(yīng)平滑，可被跟蹤。

3) 滿足逆風(fēng)著陸。

4) 落點(diǎn)精度盡可能高。

采用數(shù)學(xué)的方法來描述復(fù)雜環(huán)境翼傘系統(tǒng)航跡規(guī)劃問題，假設(shè)翼傘系統(tǒng)初始狀態(tài)為

(4)

式中：(x0,y0,z0)為空投初始時(shí)刻t0時(shí)翼傘系統(tǒng)的位置。翼傘系統(tǒng)在穿出障礙區(qū)，亦即進(jìn)入著陸區(qū)瞬間的狀態(tài)為

(5)

式中：ts為穿越障礙區(qū)的最終時(shí)刻，也是進(jìn)入著陸區(qū)域的初始時(shí)刻。翼傘系統(tǒng)到達(dá)目標(biāo)點(diǎn)時(shí)的狀態(tài)為

(6)

式中：tf為著陸時(shí)刻；φf為著陸時(shí)翼傘系統(tǒng)的航向角，此時(shí)φf應(yīng)該為著陸點(diǎn)風(fēng)向的反方向。

在障礙區(qū)內(nèi)，翼傘系統(tǒng)軌跡表示為

(7)

令Xobs表示障礙物占據(jù)的空間區(qū)域，因此，復(fù)雜環(huán)境下翼傘系統(tǒng)航跡規(guī)劃實(shí)現(xiàn)避障及航跡應(yīng)平滑的要求可表示為

(8)

式中：σ為軌跡變化率的最大值，其值越小表示軌跡越光滑。

為了方便分析計(jì)算，假定目標(biāo)點(diǎn)為大地坐標(biāo)系原點(diǎn)，風(fēng)向沿著X軸正向。此時(shí)，對(duì)逆風(fēng)著陸及落點(diǎn)精度要求,可以通過式(9)目標(biāo)函數(shù)表示：

(9)

對(duì)于述(9)目標(biāo)函數(shù)可以通過加權(quán)的方式，將多目標(biāo)函數(shù)求解問題轉(zhuǎn)化為單目標(biāo)函數(shù)求解問題，即

J=k1J1+k2J2

(10)

式中：k1、k2為權(quán)重，可以根據(jù)實(shí)際空投任務(wù)的側(cè)重點(diǎn)選擇，再借助GA或PSO等優(yōu)化算法進(jìn)行求解。

3 翼傘系統(tǒng)組合式航跡規(guī)劃策略

根據(jù)有障礙物存在時(shí)的翼傘空投任務(wù)要求，特提出復(fù)雜環(huán)境下翼傘系統(tǒng)的組合式航跡規(guī)劃策略，如圖2所示。

圖2 復(fù)雜環(huán)境翼傘系統(tǒng)組合式航跡規(guī)劃原理示意圖

其核心思想是將空投區(qū)域分為有障礙物的障礙區(qū)和無障礙物的著陸區(qū)。在障礙區(qū)內(nèi)采用進(jìn)行了適用性改進(jìn)的RRT算法搜索可行路徑，完成避障任務(wù)。而當(dāng)翼傘系統(tǒng)進(jìn)入著陸區(qū)，采用分段的方式設(shè)計(jì)航跡，完成能量控制及逆風(fēng)著陸任務(wù)。同時(shí)要考慮兩段航跡之間的順利銜接。

3.1 基本RRT算法

當(dāng)前用于復(fù)雜環(huán)境路徑規(guī)劃的常見算法主要有禁忌算法、GA、人工勢場法、模糊邏輯算法、RRT算法及貪心算法等。其中RRT算法是一種基于采樣的路徑規(guī)劃算法，相比于其他路徑規(guī)劃算法具有求解速度快的優(yōu)勢，常用于移動(dòng)機(jī)器人路徑規(guī)劃，適合解決高維空間和復(fù)雜約束下的路徑規(guī)劃問題[13-15]。其基本思想是以產(chǎn)生隨機(jī)點(diǎn)的方式通過一個(gè)步長向目標(biāo)點(diǎn)搜索前進(jìn)。能夠有效躲避障礙物，避免陷入局部最優(yōu)，且收斂速度快。

RRT算法在狀態(tài)空間內(nèi)隨機(jī)采樣到一個(gè)節(jié)點(diǎn)時(shí)，首先在當(dāng)前樹中尋找與該采樣點(diǎn)距離最近的鄰節(jié)點(diǎn)，然后連接采樣節(jié)點(diǎn)和鄰節(jié)點(diǎn)，生成新的節(jié)點(diǎn)。記隨機(jī)采樣點(diǎn)為Prand,鄰節(jié)點(diǎn)為Pnearest,新節(jié)點(diǎn)為Pnew,搜索步長為Step。則新節(jié)點(diǎn)的表達(dá)式為

(11)

式(11)的含義是當(dāng)采樣節(jié)點(diǎn)與鄰節(jié)點(diǎn)距離大于或者等于搜索步長Step時(shí)，在采樣節(jié)點(diǎn)與鄰節(jié)點(diǎn)的連線上距離鄰節(jié)點(diǎn)一個(gè)搜索步長的位置生成新節(jié)點(diǎn)。而當(dāng)采樣節(jié)點(diǎn)與鄰節(jié)點(diǎn)距離小于搜索步長時(shí)，直接將臨節(jié)點(diǎn)作為新節(jié)點(diǎn)。式中，sign(·)為符號(hào)函數(shù)，其表達(dá)式為

(12)

如果相鄰節(jié)點(diǎn)與新節(jié)點(diǎn)之間的路徑與障礙物發(fā)生碰撞，則刪除該新節(jié)點(diǎn)并重新進(jìn)行采樣，反之將新節(jié)點(diǎn)插入樹中，即完成隨機(jī)搜索樹的一次生長過程。通常情況下RRT算法采樣點(diǎn)的生成遵照以下規(guī)則：

(13)

式中：λ∈[0,1],即有λ的概率，隨機(jī)產(chǎn)生采樣點(diǎn)，其余1-λ的概率直接將目標(biāo)點(diǎn)作為采樣點(diǎn)。通常取λ=0.5。

RRT算法隨機(jī)搜索過程相鄰兩步之間的幾何關(guān)系如圖3所示，記矢量Pn-1Pn的方向?yàn)楣?jié)點(diǎn)Pn的搜索方向，其與X軸夾角為φn，相鄰兩節(jié)點(diǎn)的搜索方向角度差為β，則存在以下幾何關(guān)系：

圖3 隨機(jī)搜索幾何關(guān)系示意圖

(14)

式中：R為與連續(xù)兩條樹枝相切的圓的半徑。

3.2 基于RRT算法翼傘系統(tǒng)航跡規(guī)劃

翼傘系統(tǒng)在障礙區(qū)內(nèi)的航跡規(guī)劃借鑒于平面機(jī)器人路徑規(guī)劃，并根據(jù)翼傘系統(tǒng)運(yùn)動(dòng)特性，將其擴(kuò)展到三維空間。翼傘系統(tǒng)由于自身的特性，其運(yùn)動(dòng)軌跡在平面的投影為直線或者圓弧，RRT算法產(chǎn)生的路徑為樹狀，為折線路徑，顯然與翼傘系統(tǒng)運(yùn)動(dòng)軌跡存在一定的差距。因此采用RRT算法進(jìn)行翼傘系統(tǒng)航跡規(guī)劃的同時(shí)也必須考慮路徑平滑處理問題。本文基于RRT的翼傘系統(tǒng)航跡規(guī)劃策略如圖4所示。

圖4 基于RRT算法翼傘系統(tǒng)航跡規(guī)劃示意圖

將RRT算法的相鄰兩個(gè)搜索節(jié)點(diǎn)的中點(diǎn)作為軌跡節(jié)點(diǎn)。相鄰的軌跡節(jié)點(diǎn)之間以相切于相鄰兩條樹枝的圓弧作為過渡。通過這樣的處理方式，解決了RRT算法產(chǎn)生的軌跡存在棱角的問題，使得產(chǎn)生的軌跡為光滑曲線。隨機(jī)搜索樹的根節(jié)點(diǎn)與第一個(gè)軌跡節(jié)點(diǎn)之間的線段及最后一個(gè)軌跡節(jié)點(diǎn)與隨機(jī)搜索終止節(jié)點(diǎn)之間的線段與各軌跡節(jié)點(diǎn)之間的若干圓弧過渡段共同組成了障礙區(qū)內(nèi)的翼傘系統(tǒng)的軌跡。

(15)

式中:Step為搜索步長；θ為樹枝延伸方向，其計(jì)算公式為

(16)

由于受到翼傘系統(tǒng)的運(yùn)動(dòng)特性的限制，軌跡要光滑，且要滿足最小轉(zhuǎn)彎半徑限制?；诖?，令連續(xù)兩個(gè)相鄰搜索節(jié)點(diǎn)的中點(diǎn)作為翼傘系統(tǒng)軌跡節(jié)點(diǎn)。則第n深度的翼傘系統(tǒng)軌跡節(jié)點(diǎn)水平投影坐標(biāo)為

(17)

式(15)和式(16)描述了路徑節(jié)點(diǎn)在水平面投影的更新規(guī)律，翼傘系統(tǒng)的航跡為三維軌跡，需要將二維平面RRT算法推廣到三維空間。根據(jù)圖4 的幾何關(guān)系，同時(shí)，還需要滿足翼傘系統(tǒng)質(zhì)點(diǎn)模型的約束，確保生成的航跡具有恒定的下滑角。結(jié)合式(14)，則軌跡節(jié)點(diǎn)Xn-1與Xn之間的軌跡可以通過式(18)表示:

?∈[0,βn)

(18)

式中：?表示角度自變量；sign(·)為取符號(hào)函數(shù)，符號(hào)為正表示逆時(shí)針旋轉(zhuǎn)，反之則為順時(shí)針旋轉(zhuǎn)。

上述RRT算法的適用性改進(jìn)是在水平方向根據(jù)平面RRT算法來生成軌跡節(jié)點(diǎn)，之后在水平坐標(biāo)及翼傘系統(tǒng)的滑翔比的約束下生成翼傘系統(tǒng)的三維軌跡，以此來確保翼傘系統(tǒng)的航跡具有穩(wěn)定的下滑角。本研究中改進(jìn)型RRT算法的實(shí)質(zhì)是水平方向隨機(jī)，而垂直方向并非隨機(jī)。

對(duì)于復(fù)雜環(huán)境下翼傘空投任務(wù)，本文建立的空間地形模型的表達(dá)式為

Xobs=f(x,y)=

(19)

式中：x∈[xmin,xmax]、y∈[ymin,ymax]表示空投區(qū)域的范圍。路徑可行性的判斷依據(jù)為

(20)

式中：d為安全距離，可根據(jù)實(shí)際需要進(jìn)行設(shè)置。

翼傘系統(tǒng)由于控制能力有限，翼傘系統(tǒng)轉(zhuǎn)向運(yùn)動(dòng)的角速率具有一定的上限，對(duì)應(yīng)翼傘系統(tǒng)轉(zhuǎn)彎半徑R的下限Rmin。為了適應(yīng)翼傘系統(tǒng)實(shí)際運(yùn)動(dòng)特性，需要對(duì)隨機(jī)搜索樹的搜索方向施加以相應(yīng)的約束，根據(jù)式(14)，可以得出：

(21)

搜索步長的值不宜設(shè)置過大，否則不利于搜索到可行路徑。同時(shí)Step的取值也不宜過小，否則會(huì)降低算法求解速度。取

Step=Rmin

(22)

則式(21)可進(jìn)一步簡化為

(23)

只有當(dāng)βn滿足約束條件進(jìn)行搜索時(shí)，才可以得到能夠滿足翼傘系統(tǒng)歸航需要的軌跡。否則可能導(dǎo)致軌跡曲率過大，無法被翼傘系統(tǒng)跟蹤。

隨機(jī)搜索的初始狀態(tài)為式(4)，終止?fàn)顟B(tài)為式(5)，隨機(jī)搜索終止的判斷判斷依據(jù)為

(24)

式中：O為目標(biāo)點(diǎn)；PnO|xy為搜索節(jié)點(diǎn)與目標(biāo)點(diǎn)構(gòu)成的矢量在水平面的投影矢量；Rth為著陸區(qū)域的半徑。在障礙區(qū)內(nèi)翼傘系統(tǒng)航跡的水平距離為

(25)

翼傘系統(tǒng)下降的高度為

(26)

當(dāng)滿足隨機(jī)搜索終止條件時(shí)，表明翼傘進(jìn)入著陸區(qū)域內(nèi)，但此時(shí)翼傘系統(tǒng)還未到達(dá)目標(biāo)點(diǎn)。

3.3 著陸區(qū)內(nèi)航跡規(guī)劃

在完成障礙區(qū)航跡規(guī)劃任務(wù)后，翼傘系統(tǒng)進(jìn)入著陸區(qū)，此時(shí)的初始狀態(tài)即為隨機(jī)搜索的終止?fàn)顟B(tài)。著陸區(qū)內(nèi)航跡規(guī)劃旨在實(shí)現(xiàn)翼傘系統(tǒng)能量控制及逆風(fēng)著陸。在無障礙的環(huán)境中可以借助分段航跡規(guī)劃或者最優(yōu)控制航跡規(guī)劃的方式設(shè)計(jì)航跡，以實(shí)現(xiàn)能量控制和逆風(fēng)著陸。在采用最優(yōu)控制歸航方式時(shí)，翼傘系統(tǒng)會(huì)先向遠(yuǎn)離目標(biāo)點(diǎn)方向飛行以便消耗自身高度勢能，當(dāng)能量消耗到合適的程度后掉轉(zhuǎn)方向，向著目標(biāo)點(diǎn)靠近，并進(jìn)行逆風(fēng)對(duì)準(zhǔn)著陸，因此需要較大的空間。而分段歸航的方式則可以人為地將翼傘系統(tǒng)的航跡限制在一個(gè)有限的范圍內(nèi)。因此，對(duì)于范圍有限的著陸區(qū)，采用分段航跡規(guī)劃的方式設(shè)計(jì)航跡更為合適[16-18]。著陸區(qū)內(nèi)分段航跡規(guī)劃原理如圖5所示。

圖5 著陸區(qū)航跡規(guī)劃示意圖

(27)

C1點(diǎn)坐標(biāo)為

(28)

AB段長度為

(29)

定義一個(gè)符號(hào)變量s，設(shè)定順時(shí)針轉(zhuǎn)彎時(shí)s=-1，逆時(shí)針轉(zhuǎn)彎時(shí)s=1。則C1B與X軸正向夾角為

(30)

則第1個(gè)過渡圓弧的圓心角α1為

α1=2π+s(θep-∠C1BX)∈[0,2π]

(31)

∠C2OE的計(jì)算公式為

(32)

能量控制段轉(zhuǎn)彎圓弧對(duì)應(yīng)圓心角α2為

α2=2π+s(-s·∠C2OE-θep)=

2π-∠C2OE-sθep∈[0,2π]

(33)

第2個(gè)過渡圓弧的圓心角α3為

(34)

EO段距離為

(35)

建立目標(biāo)函數(shù)：

(36)

式中：k為穩(wěn)定狀態(tài)下的翼傘系統(tǒng)滑翔比；zA為點(diǎn)A高度；zf為目標(biāo)著陸點(diǎn)的海拔高度。

從本研究所針對(duì)的翼傘空投環(huán)境的特點(diǎn)來看，能量控制段轉(zhuǎn)彎半徑Rep不能大于著陸區(qū)半徑Rth，即

Rep≤Rth

(37)

否則可能撞到障礙物。因此目標(biāo)函數(shù)可進(jìn)一步建立為

τ=0,1,2，…

(38)

參數(shù)τ的值表示翼傘系統(tǒng)盤旋的圈數(shù)，目的是通過合理選擇盤旋圈數(shù)，在確保翼傘系統(tǒng)高度勢能得到釋放的同時(shí)，也能確保將翼傘的運(yùn)動(dòng)范圍限制在著陸區(qū)內(nèi)，避免與障礙物發(fā)生碰撞。

進(jìn)入點(diǎn)A的高度與初始高度及在障礙區(qū)內(nèi)的路徑長度有關(guān)，滿足：

(39)

在進(jìn)入著陸區(qū)后，翼傘系統(tǒng)下降的總高度為

H2=zA-zf

(40)

若要滿足翼傘系統(tǒng)能夠最終到達(dá)目標(biāo)點(diǎn)，則需要滿足：

(41)

由于翼傘系統(tǒng)最小轉(zhuǎn)彎半徑的限制，著陸區(qū)半徑必須要滿足一定的條件。根據(jù)圖5所示的幾何關(guān)系，可以得出著陸區(qū)半徑Rth滿足：

Rth≥2Rmin

(42)

著陸區(qū)內(nèi)的航跡規(guī)劃目標(biāo)是求取參數(shù)Rep及進(jìn)入能量控制段的角度θep，可以借用GA或者PSO等優(yōu)化算法進(jìn)行求解。

3.4 算法流程

本文復(fù)雜環(huán)境下翼傘系統(tǒng)組合式航跡規(guī)劃算法流程如圖6所示，主要分為如下幾個(gè)步驟：

圖6 復(fù)雜環(huán)境下翼傘系統(tǒng)組合式航跡規(guī)劃算法流程圖

步驟1初始化，給定翼傘系統(tǒng)初始點(diǎn)坐標(biāo)(x(t0),y(t0),z(t0))、航向角φ(t0)、目標(biāo)點(diǎn)位置(x(tf),y(tf),z(tf))、目標(biāo)點(diǎn)航向角φ(tf)、搜索步長Step等信息。

步驟2判斷翼傘系統(tǒng)位置是否在空投區(qū)域內(nèi)。若不在，則結(jié)束。反之，則進(jìn)行步驟3。

步驟3判斷翼傘系統(tǒng)位置是否已經(jīng)進(jìn)入著陸區(qū)域內(nèi)，即是否滿足式(24)。若翼傘系統(tǒng)在障礙區(qū)內(nèi)，如果是第一次循環(huán)則執(zhí)行步驟4。否則執(zhí)行步驟5，采用RRT算法進(jìn)行翼傘系統(tǒng)可行路徑搜索。若已進(jìn)入著陸區(qū)，則跳轉(zhuǎn)到步驟9。

步驟4先從初始投放點(diǎn)向初始航向的方向延伸一個(gè)搜索步長生成一個(gè)節(jié)點(diǎn)(默認(rèn)一個(gè)步長的圓形范圍內(nèi)無障礙物)，該步驟的目的在于確保規(guī)劃的航跡的初始方向與所給定的初始航向的方向相同。之后將該節(jié)點(diǎn)作為RRT算法的根節(jié)點(diǎn)。

步驟5隨機(jī)生成樣點(diǎn)，檢查搜索方向是否滿足式(23)的約束。若不滿足約束，則返回并重新隨機(jī)生成樣點(diǎn)。若滿足搜索方向的約束，則進(jìn)入步驟6。

步驟6檢查生成的采樣節(jié)點(diǎn)是否在空投區(qū)域內(nèi)，若不在空投區(qū)域內(nèi)則返回步驟5。若在空投區(qū)域內(nèi)，則進(jìn)行步驟7。

步驟7進(jìn)行路徑檢查，根據(jù)式(20)檢查新生成路徑是否與障礙物發(fā)生碰撞。若發(fā)生碰撞則刪除樣點(diǎn)，并返回步驟5。否則表明路徑可行，進(jìn)入步驟8。

步驟8根據(jù)式(18)更新路徑，將新的節(jié)點(diǎn)插入進(jìn)路徑。之后返回到步驟3，如果沒有進(jìn)入著陸區(qū)，則重復(fù)步驟5～步驟7。若進(jìn)入著陸區(qū)內(nèi)，則執(zhí)行步驟9。

步驟9進(jìn)行著陸區(qū)內(nèi)的航跡規(guī)劃。將進(jìn)入著陸區(qū)時(shí)的樣點(diǎn)信息作為著陸區(qū)內(nèi)分段航跡規(guī)劃的起點(diǎn)，調(diào)用優(yōu)化算法求解著陸區(qū)內(nèi)航跡參數(shù)。至此，復(fù)雜環(huán)境下翼傘系統(tǒng)航跡規(guī)劃完成。

4 仿真及分析

設(shè)置翼傘系統(tǒng)初始投放點(diǎn)的坐標(biāo)為(1 800，1 800， 1 300) m，為了方便顯示完整軌跡，目標(biāo)點(diǎn)坐標(biāo)設(shè)為(300,400,0) m。設(shè)置搜索步長Step=Rmin=50 m，滑翔比k=3，空投區(qū)域?yàn)? 000 m×2 000 m×2 000 m的立方體空間，著陸區(qū)半徑Rth=300 m,目標(biāo)點(diǎn)附近風(fēng)向與X軸平行，指向X軸正方向。值得注意的是，本文山峰模型的設(shè)置更具有一般性，即山峰模型的水平截面不必為標(biāo)準(zhǔn)圓，且山峰之間可以存在重疊。本文采用翼傘海拔高度與當(dāng)前水平坐標(biāo)下的地表海拔高度的相對(duì)大小作為判斷是否發(fā)生碰撞的依據(jù)。因此如果翼傘系統(tǒng)當(dāng)前的高度低于山峰之間的間隙的高度則很可能導(dǎo)致無法搜索到可行路徑。山峰之間的間隙的寬度及翼傘投放高度等因素都會(huì)影響航跡規(guī)劃的速度。山峰模型的參數(shù)可以根據(jù)實(shí)際情況進(jìn)行調(diào)整。本文以表1所示的山峰模型參數(shù)為例，來驗(yàn)證提出的航跡規(guī)劃算法的可行性。在MATLAB上運(yùn)行算法，航跡規(guī)劃結(jié)果如圖7所示。

表1 山峰模型參數(shù)

圖7(a)和圖7(b)分別為RRT隨機(jī)搜索過程的3D及2D視角圖，可以看出，在進(jìn)行了多次隨機(jī)搜索后，當(dāng)有節(jié)點(diǎn)進(jìn)入著陸區(qū)內(nèi)，RRT隨機(jī)搜索過程結(jié)束，切換為分段航跡規(guī)劃方式。圖7(c)和圖7(d)分別為最終得到的航跡的3D及2D視角。由于本文對(duì)RRT算法進(jìn)行的改進(jìn)，使得生成的航跡較為平滑，且最終落點(diǎn)精度較高，并實(shí)現(xiàn)了逆風(fēng)著陸。圖7(e)為利用GA求解分段航跡規(guī)劃目標(biāo)函數(shù)的適應(yīng)度變化曲線，可以看出在迭代了20步左右的時(shí)候，粒子適應(yīng)度最優(yōu)值已經(jīng)達(dá)到0.000 342 21， 所有個(gè)體的平均適應(yīng)度達(dá)到0.008 195 2，這表明航跡誤差已基本收斂為0。

圖7 復(fù)雜環(huán)境下翼傘系統(tǒng)組合式航跡規(guī)劃

將投放點(diǎn)海拔高度增大，進(jìn)行高海拔空投翼傘系統(tǒng)的航跡規(guī)劃。設(shè)定翼傘系統(tǒng)初始點(diǎn)坐標(biāo)為(1 800，1 800，1 600) m，其余各條件與前一組仿真實(shí)驗(yàn)相同。航跡規(guī)劃結(jié)果如圖8所示?？梢钥闯霎?dāng)海拔高度增大后，通過采用盤旋的方式，實(shí)現(xiàn)了翼傘系統(tǒng)能量控制，同時(shí)將分段歸航段的軌跡限制在無障礙物的著陸區(qū)內(nèi)，從而避免和障礙物發(fā)生碰撞。

圖8 高海拔復(fù)雜環(huán)境下翼傘系統(tǒng)組合式航跡規(guī)劃

本文的算法在CPU為i7-9750H、內(nèi)存為16 G 的筆記本電腦上運(yùn)行，兩組仿真實(shí)驗(yàn)的算法運(yùn)行時(shí)間分別為16.67 s和18.33 s，求解速度較快，體現(xiàn)了RRT算法在路徑規(guī)劃方面求解速度的優(yōu)勢。從航跡規(guī)劃的效果來看，本文提出的翼傘系統(tǒng)組合式航跡規(guī)劃策略，能夠解決復(fù)雜環(huán)境下的翼傘系統(tǒng)的航跡規(guī)劃問題，規(guī)劃得到的航跡能夠滿足翼傘空投需要。

5 結(jié) 論

本文提出了一種復(fù)雜環(huán)境下的翼傘系統(tǒng)組合式航跡規(guī)劃策略，根據(jù)實(shí)際空投的環(huán)境特點(diǎn)，將翼傘空投區(qū)域分為障礙區(qū)和著陸區(qū)。在障礙區(qū)內(nèi)采用RRT算法快速搜索可行路徑，根據(jù)翼傘系統(tǒng)運(yùn)動(dòng)特性，對(duì)RRT算法進(jìn)行了適用性改進(jìn)，使其滿足于翼傘系統(tǒng)航跡規(guī)劃需要，在障礙區(qū)內(nèi)完成翼傘系統(tǒng)避障任務(wù)。又針對(duì)RRT算法搜索方向隨機(jī)，難以實(shí)現(xiàn)翼傘系統(tǒng)逆風(fēng)著陸及翼傘系統(tǒng)能量控制的問題，采用分段規(guī)劃的方式，借助GA對(duì)目標(biāo)函數(shù)進(jìn)行優(yōu)化求解，實(shí)現(xiàn)了著陸區(qū)內(nèi)的航跡規(guī)劃。在研究中考慮了兩段航跡的銜接問題，以實(shí)現(xiàn)航跡的順利過渡。得到的航跡能夠同時(shí)滿足避障、能量控制及逆風(fēng)著陸的要求。航跡較為平滑，能夠被跟蹤，且算法具有較快的求解速度。

本文的研究也存在一些局限性。首先，未考慮平均風(fēng)及由高大障礙物所引起的風(fēng)切變對(duì)翼傘航跡規(guī)劃的影響，這可能會(huì)給翼傘系統(tǒng)的軌跡跟蹤控制造成一定的困難。未來可以將平均風(fēng)及風(fēng)切變對(duì)于翼傘系統(tǒng)航跡規(guī)劃的影響作為一個(gè)重要的研究方向。其次，本文算法適用于著陸點(diǎn)周圍有較大的空曠區(qū)域的環(huán)境，當(dāng)空投環(huán)境更為復(fù)雜，即著陸點(diǎn)附近存在障礙物時(shí)，提出的策略則難以適用。改進(jìn)型的雙向RRT算法或許可以成為解決該問題的一個(gè)可行方法，但仍需進(jìn)行進(jìn)一步的研究。