(海軍工程大學(xué)基礎(chǔ)部 武漢 430033)
隨著現(xiàn)代軍事科技的迅猛發(fā)展,軍民科技的深度融合,現(xiàn)代戰(zhàn)場已經(jīng)越來越趨向于無人化、精準(zhǔn)化、自動化。無人機(jī)以其目標(biāo)小、成本低、可遠(yuǎn)程操控與自主控制相結(jié)合的特點,逐漸在現(xiàn)代戰(zhàn)場中扮演著越來越重要的角色[1]。無人機(jī)在各類約束下尋求最優(yōu)或次優(yōu)的航跡規(guī)劃[1]也越發(fā)重要。同時,戰(zhàn)場環(huán)境瞬息萬變,無人機(jī)在不確定環(huán)境下的實時航跡規(guī)劃能力的提升尤為迫切,這就對航跡規(guī)劃算法的速度、效果提出了更高的要求。
傳統(tǒng)的航跡規(guī)劃算法在實時航跡規(guī)劃方面,都顯現(xiàn)出了一定的局限和不足。傳統(tǒng)的航跡規(guī)劃算法主要分為啟發(fā)式算法和數(shù)學(xué)優(yōu)化算法[2]。啟發(fā)式算法包括模擬退火[3]、蟻群算法[4]、遺傳算法[5]、A*算法[6]、粒子群優(yōu)化算法[7]等。常用的遺傳算法在環(huán)境復(fù)雜時收斂速度下降,尤其在接近最優(yōu)解時收斂速度會極低[5],不滿足實時航跡規(guī)劃的速度要求;A*算法由于不能每次都保證結(jié)果正確,且可能因為某一估計函數(shù)陷入漫長的搜索過程中[6],不滿足實時航跡規(guī)劃的速度要求;蟻群算法相比其他算法需要時間較長,且容易陷入局部最優(yōu)解[4],也不適用于實時航跡規(guī)劃。數(shù)學(xué)優(yōu)化算法包括最優(yōu)控制、動態(tài)規(guī)劃、牛頓法以及一些搜索方法如Dijkstra法[8]、Voronoi圖法[9]、快速擴(kuò)展隨機(jī)樹(RRT)法[10]等。Voronoi圖法由于通常需要結(jié)合遺傳算法等智能搜索算法搜索最優(yōu)路徑,耗時較長[9],不適用于實時航跡規(guī)劃;Dijkstra法盡管可以找到最優(yōu)路徑,但由于搜索效率較低[8],不適用于實時航跡規(guī)劃。
快速擴(kuò)展隨機(jī)樹(RRT)算法作為目前廣泛使用的實時航跡規(guī)劃算法,具有無需對環(huán)境建模,只需通過在狀態(tài)空間隨機(jī)擴(kuò)展節(jié)點,就可構(gòu)造出可行路徑,節(jié)點擴(kuò)展的隨機(jī)性保證了路徑可以避開威脅區(qū)域,且可以同時考慮無人機(jī)動力學(xué)約束等問題。目前,國內(nèi)外在路徑規(guī)劃方面已經(jīng)通過RRT算法取得了大量實踐與應(yīng)用[11]。但RRT算法的不足也很明顯,由于節(jié)點擴(kuò)展的隨機(jī)性較強(qiáng),往往需要大量次數(shù)迭代才可以找到可行航跡,消耗大量時間且航跡無法保證最優(yōu)。同時,由于擴(kuò)展步長的固定,在靠近威脅區(qū)域時,生長樹具有局限性,需耗費大量時間才可繞過威脅區(qū)域。而且,由于無人機(jī)自身存在動力學(xué)約束,節(jié)點生長無法完全隨機(jī)。
因此,本文對原始RRT算法予以改進(jìn)。首先,增加動態(tài)步長,通過計算當(dāng)前節(jié)點與最近威脅的距離合理確定步長;同時,結(jié)合人工勢場法中目標(biāo)引力的概念,引入自適應(yīng)目標(biāo)引力,將節(jié)點生長引導(dǎo)向目標(biāo)方向;最后,在節(jié)點選取時考慮無人機(jī)的動力學(xué)約束,使航跡規(guī)劃算法更加可執(zhí)行。實驗證明該改進(jìn)算法可以快速規(guī)劃出較優(yōu)航跡。
無人機(jī)在執(zhí)行一次任務(wù)期間,由于受到自身機(jī)動性能、燃油攜帶等限制,需要滿足如下約束:
1)最小飛行距離約束:最小飛行距離,是指無人機(jī)在改變飛行狀態(tài)之前需要保持直線飛行的最小距離,即當(dāng)無人機(jī)的直線飛行距離超過最小飛行距離之后,才可以改變飛行姿態(tài)[12]。圖1中總航程可表示為設(shè)定最小飛行距離為lmin,則最小飛行距離約束則表示為lm≥lmin。
圖1 無人機(jī)航跡示意圖
2)最大轉(zhuǎn)彎角約束:無人機(jī)在調(diào)整方向時由于慣性以及無人機(jī)自身性能約束,改變航向的角度需要小于最大轉(zhuǎn)彎角θ,如圖2所示。
圖2 最大轉(zhuǎn)彎角示意圖
3)最大航程約束:由于無人機(jī)攜帶燃油限制等原因,無人機(jī)執(zhí)行一次任務(wù)所飛行的總航程應(yīng)小于最大航程Lmax,則最大航程約束表示為。
由于無人機(jī)在飛行期間一般保持固定高度飛行,所以本文設(shè)定無人機(jī)飛行高度固定,不考慮俯沖角、爬升角等約束的影響,只涉及二維平面的航跡規(guī)劃。
無人機(jī)執(zhí)行任務(wù)時的航跡規(guī)劃,需要考慮各類威脅情況,主要包括雷達(dá)、防空武器、地形威脅、惡劣天氣等。不同種類的威脅對無人機(jī)的影響程度不同,對于不同的威脅,無人機(jī)的應(yīng)對方式也不同,下面分別對地形威脅、氣象威脅、雷達(dá)威脅、防空威脅進(jìn)行建模。
1)地形威脅:主要指山峰、高地等地形可能對飛行造成的威脅。以圓錐體近似表示山峰、高地等,在固定高度上,地形威脅可近似表示為一個威脅半徑為rT的圓周。無人機(jī)飛入地形威脅半徑rT時,認(rèn)為飛機(jī)損毀;飛出威脅半徑rT時,認(rèn)為對飛機(jī)無影響。
2)氣象威脅:主要指颶風(fēng)、雷暴等惡劣氣象條件對無人機(jī)造成的威脅。以圓柱體近似表示颶風(fēng)、雷暴等,在固定高度上,氣象威脅可近似表示為一個威脅半徑為rc的圓周。無人機(jī)飛入氣象威脅半徑rc時,認(rèn)為飛機(jī)損毀;飛出威脅半徑rc時,認(rèn)為對飛機(jī)無影響。
3)雷達(dá)威脅:主要指敵方雷達(dá)對我方無人機(jī)的探測威脅。由于現(xiàn)代軍事科技發(fā)展,雷達(dá)大部分為360度全向雷達(dá),在二維平面上,其探測范圍可以看似為圓周。由于無人機(jī)與雷達(dá)距離越近,被探測到的可能性越大[13],所以本文將雷達(dá)探測區(qū)域以不同的探測半徑,分為絕對探測區(qū)域和相對探測區(qū)域兩個同心圓,如圖3所示。當(dāng)無人機(jī)飛入絕對探測半徑rRmin時,認(rèn)為飛機(jī)被探測并損毀;當(dāng)飛入絕對探測半徑rRmin與相對探測半徑rRmax之間時,認(rèn)為可能被探測;當(dāng)飛出相對探測半徑rRmax時,認(rèn)為對飛機(jī)無影響。
4)防空威脅:主要指敵方防空導(dǎo)彈、高射火炮等防空武器對無人機(jī)造成的威脅。在二維平面上,防空武器的威脅半徑可以近似看做圓周[14~15],由于無人機(jī)與防空武器的距離越近,受到的打擊威脅越大,所以本文將防空威脅區(qū)域以不同的打擊半徑,分為絕對打擊區(qū)域和相對打擊區(qū)域兩個同心圓,如圖4所示。當(dāng)無人機(jī)飛入絕對打擊半徑rMmin時,認(rèn)為飛機(jī)被擊落;當(dāng)飛入絕對打擊半徑rMmin與相對打擊半徑rMmax之間時,認(rèn)為可能被擊落;當(dāng)飛出相對打擊半徑rMmax時,認(rèn)為對飛機(jī)無影響。
圖3 雷達(dá)威脅示意圖
圖4 防空威脅示意圖
由于要保證無人機(jī)成功執(zhí)行任務(wù),防止無人機(jī)出現(xiàn)損毀的可能,本文將雷達(dá)威脅和防空威脅的最大威脅范圍視為威脅范圍,即均為一個以最大威脅半徑為半徑的圓周,當(dāng)飛入威脅區(qū)域時,認(rèn)為無人機(jī)損毀。
圖5 動態(tài)步長示意圖
在傳統(tǒng)RRT算法進(jìn)行航跡規(guī)劃時,如果設(shè)定的步長較小,會導(dǎo)致隨機(jī)樹生長過慢,影響求解速度;如果設(shè)定的步長較大,在威脅環(huán)境較多的情況下,新節(jié)點會很難通過威脅檢測,同樣影響求解速度。為了提高航跡規(guī)劃效率,本文引入了動態(tài)步長的策略。如圖5所示,Pk為環(huán)境中第k個威脅源,qrand為隨機(jī)采樣點,qnear為隨機(jī)樹中距離qrand最近的節(jié)點,qgoal為目標(biāo)點,qini為起始點,qn為由起始點向外擴(kuò)展的第n個節(jié)點,qnew為新節(jié)點。ρ為搜索步長,ρmin'ρmax為設(shè)定好的最小、最大步長,Dthr為qnear與其最近威脅的邊緣之間的距離。
在確定qrand之后,搜索并計算距離qrand最近的威脅邊緣之間的距離 Dthr,并根據(jù)設(shè)定的ρmin'ρmax以及動態(tài)步長公式:
確定qrand的步長。通過這一方法確定的步長保持在最大與最小步長范圍之間,當(dāng)qrand距離威脅越近,步長越接近 ρmin,當(dāng)以qrand為圓心,ρmax為半徑范圍內(nèi)沒有威脅時,步長為ρmax。
圖6為靠近威脅的情況下,傳統(tǒng)步長與動態(tài)步長的區(qū)別??梢钥闯觯诳拷{的情況下,傳統(tǒng)步長可能會因為無法通過威脅檢測而進(jìn)行重新采樣,從而影響航跡規(guī)劃效率,而動態(tài)步長則可以通過縮小步長而通過威脅檢測。
圖6 靠近威脅情況下,傳統(tǒng)步長與動態(tài)步長區(qū)別示意圖
圖7 為遠(yuǎn)離威脅的情況下,傳統(tǒng)步長與動態(tài)步長的區(qū)別??梢钥闯觯谶h(yuǎn)離威脅的情況下,傳統(tǒng)步長由于步長相對較小,會導(dǎo)致隨機(jī)樹生長緩慢,從而影響航跡規(guī)劃效率,而動態(tài)步長則可以通過增大步長,提高隨機(jī)樹的生長速度,從而提高航跡規(guī)劃的效率。
圖7 遠(yuǎn)離威脅情況下,傳統(tǒng)步長與動態(tài)步長區(qū)別示意圖
在進(jìn)行航跡規(guī)劃時,我們既希望可以有一定的隨機(jī)性從而避開威脅區(qū)域,又希望可以盡可能地向目標(biāo)方向行進(jìn)?;綬RT算法由于在擴(kuò)展節(jié)點時從全局無威脅空間進(jìn)行采樣,隨機(jī)性較強(qiáng),得到的最終航跡往往不是最優(yōu)航跡,且算法耗時較長。因此,本文引入人工勢場法[16]中的引力思想,在保證隨機(jī)樹具有一定隨機(jī)性的同時,引導(dǎo)隨機(jī)樹向目標(biāo)方向生長,優(yōu)化航跡的同時縮短了規(guī)劃時間,提高了算法的實時性。同時根據(jù)與威脅區(qū)域的距離設(shè)定自適應(yīng)引力系數(shù),保證隨機(jī)樹可以準(zhǔn)確避開威脅區(qū)域的同時更快的向目標(biāo)區(qū)域生長。
改進(jìn)的核心思想是在每個節(jié)點qn都增加一個節(jié)點生長函數(shù)F(qn),表示為
其中,F(xiàn)(qn)為從節(jié)點qn到新節(jié)點的生長函數(shù),R(qn)為從節(jié)點qn到新節(jié)點的隨機(jī)生長函數(shù),G(qn)為從節(jié)點qn到新節(jié)點的目標(biāo)引力函數(shù)。
由人工勢場中的目標(biāo)對qnear的引力勢能[16]:
可以得到目標(biāo)對qnear的引力:
kp為引力系數(shù),‖qgoal-qnear‖ 為qgoal與qnear之間幾何距離的絕對值。根據(jù)式(4)可以構(gòu)造出節(jié)點qn到新節(jié)點的目標(biāo)引力函數(shù)G(qn):
在增加目標(biāo)引力思想之后,RRT算法生成新節(jié)點時,通過計算目標(biāo)引力函數(shù)來指導(dǎo)新節(jié)點在一定程度上向目標(biāo)生長。
在基本RRT算法中,從節(jié)點qn到新節(jié)點的隨機(jī)生長函數(shù)R(qn)為
將式(5)、(6)帶入式(2)中可以得到從節(jié)點 qn到新節(jié)點的生長函數(shù)F(qn)為
根據(jù)式(7)可以得到增加目標(biāo)引力思想后的新節(jié)點生長公式:
可以看出,引力系數(shù)的大小直接影響著新節(jié)點的生長方向。由于我們既希望隨機(jī)樹生長時可以有一定的隨機(jī)性來避開威脅區(qū)域,又希望可以盡可能地向目標(biāo)方向行進(jìn),所以本文引入了自適應(yīng)引力系數(shù):
即當(dāng)遠(yuǎn)離威脅的情況下,kp取較大值,減小隨機(jī)樹的隨機(jī)性,引導(dǎo)新節(jié)點盡可能向目標(biāo)生長,同時,結(jié)合動態(tài)步長的確定,進(jìn)一步提高航跡規(guī)劃的效率;當(dāng)靠近威脅的情況下,kp取較小值,使隨機(jī)樹可以避開威脅區(qū)域,向無威脅區(qū)域生長,同時,減小動態(tài)步長,以順利通過威脅檢測。
1)最小飛行距離約束:考慮2.1節(jié)中的最小飛行距離約束,結(jié)合提出的動態(tài)步長策略,可知最小步長ρmin應(yīng)不小于最小飛行距離Lmin,即
本文為加快求解速度,提高航跡規(guī)劃效率,設(shè)定 ρmin=Lmin。
2)最大轉(zhuǎn)彎角約束:考慮2.1節(jié)中的最大轉(zhuǎn)彎角約束,如圖8,令航跡段qnear_rootqnear和qnearqnew在二維平面的水平投影為
圖8 最大轉(zhuǎn)彎角示意圖
3)最大航程約束:考慮最大航程約束,實際總航程L應(yīng)小于最大航程Lmax,即
設(shè)最大轉(zhuǎn)彎角為θ,則新節(jié)點qnew必須滿足
基于改進(jìn)RRT算法的無人機(jī)航跡規(guī)劃的具體步驟如下:
圖9 改進(jìn)RRT算法流程圖
實驗環(huán)境:Windows 10,Intel(R)Core(TM)i7-6700HQ CPU@2.60GHz,內(nèi)存8G。
編譯工具:Matlab 2015b。
參數(shù)設(shè)置:無人機(jī)任務(wù)區(qū)域為500×500,X軸Y軸范圍均為[0,500],起點位置的坐標(biāo)為(10,490),終點位置的坐標(biāo)為(490,10)。動態(tài)步長 ρmin=20,ρmax=40,引力系數(shù),最大轉(zhuǎn)彎角為60°,最大航程 Lmax=1000。迭代次數(shù)為10000,實驗次數(shù)為100。
分別在任務(wù)區(qū)域隨機(jī)生成5個,10個,20個大小不一的威脅圓,并且分別運用原始RRT算法和改進(jìn)RRT算法在相同環(huán)境下進(jìn)行航跡規(guī)劃,記錄擴(kuò)展節(jié)點數(shù)、航跡長度、運行時間,取100次實驗結(jié)果的平均值,并計算航跡長度縮短率,同時分別記錄兩種算法進(jìn)行實驗的失敗次數(shù)。仿真效果如圖10所示,三種威脅數(shù)量情況下,航跡均明顯縮短,且接近最優(yōu)航跡。
圖10 不同威脅數(shù)量下,仿真效果對比圖
表1 仿真數(shù)據(jù)對比
從表1中可以看出,擴(kuò)展節(jié)點數(shù)方面,三種威脅數(shù)量情況下,節(jié)點數(shù)均減少約50%;航跡長度方面,三種威脅數(shù)量情況下,改進(jìn)RRT算法相比原始RRT算法的平均航跡長度縮短率分別為5.92%、8.98%、1.92%,并且由于航跡長度基數(shù)較大,實際航跡長度明顯縮短;運行時間方面,改進(jìn)RRT算法相比原始RRT算法,除5個威脅情況下時間縮短之外,其余時間均略有增加,但仍處于毫秒級別,完全可以滿足無人機(jī)實時航跡規(guī)劃的需求。
實驗結(jié)果表明,基于改進(jìn)RRT算法的無人機(jī)航跡規(guī)劃算法,在簡單、復(fù)雜的環(huán)境下,均可以快速、有效地構(gòu)造出避開威脅區(qū)域,同時滿足無人機(jī)性能約束的較優(yōu)航跡。相比原始RRT算法,在航跡長度方面有較大提高,且消耗時間較短,滿足無人機(jī)實時航跡規(guī)劃的需求。
本文針對原始RRT算法在無人機(jī)航跡規(guī)劃方面,存在的節(jié)點生長隨機(jī)性過強(qiáng),航跡無法保證最優(yōu);搜索步長固定,在靠近威脅區(qū)域生長樹具有局限性;沒有考慮無人機(jī)動力學(xué)約束等問題,提出了改進(jìn)RRT算法,并進(jìn)行了仿真實驗。改進(jìn)算法通過增加動態(tài)步長,解決了靠近威脅區(qū)域生長樹局限的問題;通過在節(jié)點生長時增加自適應(yīng)目標(biāo)引力,將節(jié)點生長向目標(biāo)方向引導(dǎo),解決了RRT算法隨機(jī)性過強(qiáng)的問題;最后在節(jié)點選取時考慮無人機(jī)動力學(xué)約束,使規(guī)劃出的航跡滿足無人機(jī)的實際性能要求。實驗結(jié)果表明,改進(jìn)RRT算法可以快速、有效地規(guī)劃出較優(yōu)航跡,滿足無人機(jī)實時航跡規(guī)劃的需求。同時,本文只在二維平面內(nèi)進(jìn)行航跡規(guī)劃,與實際的三維環(huán)境航跡規(guī)劃還有一定差距,在未來的工作中,將進(jìn)一步研究三維環(huán)境下的航跡規(guī)劃,同時深入研究不確定環(huán)境下的實時航跡規(guī)劃。