基于改進(jìn)RRT算法的無人機(jī)實時航跡規(guī)劃?

（海軍工程大學(xué)基礎(chǔ)部 武漢 430033）

1 引言

隨著現(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)航跡。

2 任務(wù)數(shù)學(xué)描述

2.1 無人機(jī)性能約束

無人機(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ī)劃。

2.2 環(huán)境威脅建模

無人機(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ī)損毀。

3 RRT算法的改進(jìn)策略

3.1 增加動態(tài)步長

圖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ū)別示意圖

3.2 增加自適應(yīng)目標(biāo)引力

在進(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)步長，以順利通過威脅檢測。

3.3 考慮無人機(jī)性能約束

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必須滿足

4 改進(jìn)RRT算法流程圖

基于改進(jìn)RRT算法的無人機(jī)航跡規(guī)劃的具體步驟如下：

圖9 改進(jìn)RRT算法流程圖

5 仿真實驗與結(jié)果分析

實驗環(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ī)劃的需求。

6 結(jié)語

本文針對原始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ī)劃。