李宇輝,趙敏,3,*,陳奇,姚敏,3,何紫陽
1. 南京航空航天大學(xué) 自動(dòng)化學(xué)院,南京 210016
2. 淮陰工學(xué)院 電子信息工程學(xué)院,淮安 223003
3. 高速交通設(shè)施無損檢測(cè)與監(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ū)域航跡的銜接問題。
常見的翼傘系統(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)軌跡是一條具有恒定下滑角的曲線。
在實(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)行求解。
根據(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í)要考慮兩段航跡之間的順利銜接。
當(dāng)前用于復(fù)雜環(huán)境路徑規(guī)劃的常見算法主要有禁忌算法、GA、人工勢(shì)場(chǎng)法、模糊邏輯算法、RRT算法及貪心算法等。其中RRT算法是一種基于采樣的路徑規(guī)劃算法,相比于其他路徑規(guī)劃算法具有求解速度快的優(yōu)勢(shì),常用于移動(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ù)兩條樹枝相切的圓的半徑。
翼傘系統(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)。
在完成障礙區(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)方向飛行以便消耗自身高度勢(shì)能,當(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ì)能得到釋放的同時(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)行求解。
本文復(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ī)劃完成。
設(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)勢(shì)。從航跡規(guī)劃的效果來看,本文提出的翼傘系統(tǒng)組合式航跡規(guī)劃策略,能夠解決復(fù)雜環(huán)境下的翼傘系統(tǒng)的航跡規(guī)劃問題,規(guī)劃得到的航跡能夠滿足翼傘空投需要。
本文提出了一種復(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)一步的研究。