基于改進(jìn)RRT算法的UAV航跡規(guī)劃與優(yōu)化研究

高曉燕 韓慶田

1(山東商務(wù)職業(yè)學(xué)院 山東 煙臺 264670)2(海軍航空大學(xué) 山東 煙臺 264001)

0 引 言

在進(jìn)行航跡規(guī)劃的過程中，一般采用較多的算法主要有A*算法、遺傳算法、進(jìn)化計算、蟻群算法等[1-5]。A*算法主要是采用擴展節(jié)點的方法，由于節(jié)點擴展方法其本身存在一定的不足，使得其在環(huán)境復(fù)雜的情況下，存在著組合爆炸的問題，即計算量會幾何增長，最后導(dǎo)致航跡規(guī)劃所需時間會急劇膨脹，效率低下。快速擴展隨機樹(Rapidly-Exploring Random Trees，RRT)算法不需對任務(wù)環(huán)境的情況進(jìn)行特殊構(gòu)造，只需要通過樹節(jié)點進(jìn)行方向探索，通過方向點隨機選取可以得到一條規(guī)劃路徑。由于探索方向點選取具有隨機性，這使得搜索路徑具有全局的最優(yōu)性，且在多維的環(huán)境下易于建模，便于算法實現(xiàn)，利于工程應(yīng)用，因此，比較適合于進(jìn)行多UAV的航跡規(guī)劃。

但是，由于探索方向點選取的隨機性，在經(jīng)過一定探索次數(shù)的搜索后，樹探索可以跳出局部極小的區(qū)域，但是，卻增加了探索過程中失敗的概率，從而降低了規(guī)劃的效率。為了彌補基本RRT算法規(guī)劃時的相應(yīng)缺點，相關(guān)文獻(xiàn)對RRT算法進(jìn)行了一定的改進(jìn)研究。如雙向RRT、RRT Connect等算法[6]，主要是研究在航跡規(guī)劃過程中，從初始點和目標(biāo)點開始雙向進(jìn)行樹的擴展，直到兩棵樹出現(xiàn)交叉時為止，然后進(jìn)行連接，就可以尋找到一條，從初始點到目標(biāo)點的最優(yōu)可行路徑。文獻(xiàn)[7]研究了小型無人機無序多點航跡規(guī)劃問題，結(jié)合用于解決TSP問題的貪心策略，提出了一種貪心MB-RRT*算法。王瀟翔等[8]在常規(guī)RRT方法的基礎(chǔ)上，引入面向目標(biāo)的啟發(fā)式隨機點采樣啟發(fā)和新節(jié)點擴展啟發(fā)算法，算法較為復(fù)雜。文獻(xiàn)[9]提出了MB-RRT*算法，通過懶惰采樣的方法，加快了算法收斂速度。RRT算法的改進(jìn)，不但提高了路徑進(jìn)行航跡規(guī)劃效率，同時減少了規(guī)劃所需的時間，方便應(yīng)用于復(fù)雜環(huán)境下航跡規(guī)劃，但不同的算法規(guī)劃的路徑往往和最優(yōu)的航跡存在一定的差距，路徑質(zhì)量也不盡相同?，F(xiàn)有的算法研究大多是為規(guī)劃路徑更偏重于最優(yōu)路徑，提高航跡規(guī)劃穩(wěn)定性，但在復(fù)雜環(huán)境中執(zhí)行任務(wù)，搜索的失敗點會不斷增加，從而導(dǎo)致算法效率有所降低，規(guī)劃耗時也會增長。

為了在航跡規(guī)劃過程中，在規(guī)劃耗時和路徑質(zhì)量這兩方面達(dá)到一定的平衡，本文分析了RRT算法中步長對于航跡規(guī)劃的影響，進(jìn)而提出了基于啟發(fā)式步長調(diào)整策略，并根據(jù)初步生成路徑進(jìn)行修剪處理，縮短路徑長度，提高UAV航跡規(guī)劃的質(zhì)量。

1 RRT算法

1.1 算法流程

RRT算法基本的思路是，在一個二維工作空間中，UAV的位置坐標(biāo)和方向角度決定了位姿狀態(tài)。RRT的構(gòu)建過程如下：Tk是一個具有k個節(jié)點的擴展樹，x為擴展樹的節(jié)點，那么可知Tk∈Cfree，設(shè)Cfree與威脅或障礙無重疊的一個自由狀態(tài)空間。xstart為初始狀態(tài)，也是UAV的起點，xfinal為目標(biāo)狀態(tài)，即UAV的目標(biāo)點或終點，是一個點或一個區(qū)域，xfinal?Cfree。設(shè)xrand是空間一個隨機選取的位姿狀態(tài)，xrand∈Cfree。在擴展樹中找出的距離xrand最近節(jié)點xnear。取空間中兩個點p，q∈Cfree，兩個位姿狀態(tài)之間幾何距離用Ddis(p,q)表示。Ddis(xnear,xrand)≤Ddis(x,xrand)，在xnear與xrand的連線上求新節(jié)點xnew，xnew必須要滿足條件xnew∈Cfree，并且Ddis(xnear,xnew)=λ，λ>0為隨機擴展樹最小單位長度，稱為步長。如果存在xnew，則為擴展樹增加一個新的節(jié)點。擴展樹變?yōu)門k+1，Tk+1=Tk⊕xnew，⊕表示新增的葉子節(jié)點，進(jìn)行樹擴展。如果不滿足這個條件，則重新進(jìn)行選擇，如此重復(fù)以上過程，直到滿足最終條件為止[7]?；綬RT算法如圖1所示。

圖1 基本RRT算法

RRT算法流程如下[10]：

Step1初始化規(guī)劃初始點和目標(biāo)點，建立隨機樹節(jié)點位置的范圍。

Step2在空間中進(jìn)行隨機地選取某點xrand，然后尋找隨機樹中距離xrand最近的某葉子節(jié)點xnear。

Step3在xrand與xnear連線上，選取與xnear距離為步長λ的新增葉子節(jié)點xnew。

Step4需要判斷xnew以及xnear到xnew的路徑是否滿足路徑條件，如果滿足，生成新隨機樹，否則，轉(zhuǎn)入Step 2。

Step5判斷新增節(jié)點xnew是否到達(dá)目標(biāo)點或者目標(biāo)點的范圍。如果已經(jīng)到達(dá)目標(biāo)點，則流程結(jié)束；如果未到達(dá)目標(biāo)點或目標(biāo)點范圍，轉(zhuǎn)入Step 2。

1.2 參數(shù)影響分析

RRT航跡規(guī)劃算法需要設(shè)置的參數(shù)比較少，簡單結(jié)構(gòu)有利于RRT算法應(yīng)用推廣，但目前關(guān)于算法參數(shù)的選取缺少較為嚴(yán)格的理論依據(jù)，在實際應(yīng)用中則一般根據(jù)經(jīng)驗試湊獲取，不能保證算法參數(shù)的最優(yōu)性。

與旅行商問題、車間調(diào)度問題等組合優(yōu)化問題不同的是，航跡規(guī)劃問題沒有標(biāo)準(zhǔn)的測試樣例。任務(wù)環(huán)境本身不是算法參數(shù)，而是算法應(yīng)用對象，本文通過仿真實驗來分析研究RRT算法的不同參數(shù)設(shè)置，以及不同任務(wù)環(huán)境對算法規(guī)劃性能的影響。

1.2.1搜索步長影響分析

設(shè)置任務(wù)環(huán)境為二維區(qū)域，尺寸為100×100無量綱單位，確定實驗分析的研究對象，起始位置為(50，50)，目標(biāo)位置為(100，100)。

設(shè)定搜索步長為5，最大搜索段數(shù)100，不同步長下的仿真數(shù)據(jù)如表1所示。

表1 不同步長下的最遠(yuǎn)搜索距離

進(jìn)行50次實驗，搜索50次，不同步長條件下的搜索最遠(yuǎn)距離分布情況如圖2所示。

圖2 不同步長下的最遠(yuǎn)搜索距離分布

可以看出，次數(shù)分布近似成均勻或正態(tài)分布。

1.2.2最大搜索次數(shù)影響

步長從小步長到大步長的過程中，搜索次數(shù)的不同對于搜索最遠(yuǎn)距離的影響，如圖3、圖4所示。

圖3 不同步長下的最遠(yuǎn)搜索距離(搜索5次)

圖4 不同步長下的最遠(yuǎn)搜索距離(搜索50次)

在搜索次數(shù)較少的情況下，搜索最遠(yuǎn)距離與步長基本成線性關(guān)系；當(dāng)不斷加大搜索次數(shù)情況下，搜索最遠(yuǎn)距離與步長之間成非線性的關(guān)系，出現(xiàn)搜索逐漸放慢的趨勢。

1.2.3不同威脅環(huán)境下航跡生成

設(shè)置任務(wù)環(huán)境為二維區(qū)域，尺寸為100×100無量綱單位，確定實驗分析的研究對象，起始位置為(1，1)，目標(biāo)位置為(100，100)。

(1) 復(fù)雜威脅環(huán)境仿真。復(fù)雜多威脅環(huán)境下RRT算法，步長分別為1、5和10時的搜索仿真結(jié)果如表2所示。

表2 復(fù)雜威脅環(huán)境RRT結(jié)果

(2) 簡單威脅環(huán)境仿真。簡單威脅環(huán)境下RRT算法，步長分別為1、5和10時的搜索仿真結(jié)果如表3所示。大于十分之一總長度的步長時，搜索50次，基本搜索點數(shù)均勻分布，因此在進(jìn)行步長選擇時，不可太低。

表3 復(fù)雜威脅環(huán)境RRT結(jié)果

2 基于啟發(fā)信息的RRT算法改進(jìn)

雖然RRT算法具有搜索速度快、不會出現(xiàn)停滯和概率完備性的優(yōu)點。但是算法的規(guī)劃方式有一定的局限性，需要通過改進(jìn)提高算法性能[11-12]。主要缺陷體現(xiàn)在三個方面：

(1) 擴展方式比較平均，通過隨機采樣的方式，按隨機概率進(jìn)行新節(jié)點的采樣和生成。

(2) 算法的不可重復(fù)性，每次規(guī)劃的路徑是有所不同。

(3) 實時性較為欠缺。

(4) 規(guī)劃路徑質(zhì)量比較一般。

RRT算法的有些特點是算法的屬性決定的，有些缺點可以通過改進(jìn)加以修正。對于擴展方式比較平均的問題，本文采用啟發(fā)式搜索策略，使得搜索區(qū)域有所改進(jìn)，具有一定的趨勢偏向于優(yōu)良結(jié)果的方向進(jìn)行搜索，從而提高搜索效率。同時，通過搜索策略的改進(jìn)，提高了航跡路徑的質(zhì)量。

2.1 基于啟發(fā)信息的RRT航跡改進(jìn)算法

現(xiàn)有文獻(xiàn)根據(jù)傳統(tǒng)標(biāo)準(zhǔn)RRT算法的缺點進(jìn)行了一些改進(jìn)，而且結(jié)果證明改進(jìn)的RRT算法在規(guī)劃耗時和路徑質(zhì)量上各有一定的提高，但是，對于復(fù)雜的多威脅源環(huán)境中，仍然存在規(guī)劃時間長，以及質(zhì)量有待提高問題[12]。

RRT算法中，擴展樹的生長是以起始點作為樹的根節(jié)點的，在自由空間中隨機采樣，然后根據(jù)條件確定新的葉子節(jié)點，直到樹的葉子節(jié)點到達(dá)目標(biāo)范圍。隨機點的選取具有任意性和隨機性，使得擴展樹的生長具有隨機性，從而導(dǎo)致樹的根節(jié)點到葉子節(jié)點的規(guī)劃路徑有時遠(yuǎn)離最短的路徑，有時會接近最短路徑，存在較大隨機性。而且，對于同一任務(wù)的規(guī)劃缺乏可重復(fù)出現(xiàn)的性質(zhì)。

為了減少這種航跡規(guī)劃的隨機性，使得快速擴展樹的生長具有向目標(biāo)擴展的特性。本文在基本擴展隨機樹的基礎(chǔ)上，借鑒搜索算法思想，尋找最短的路徑，在構(gòu)建擴展樹時，對算法進(jìn)行如下改進(jìn)：

(1) 引入啟發(fā)因子。在這種航跡規(guī)劃過程中引入啟發(fā)信息的方法，可以提高搜索的效率，有效減低擴展隨機樹生長中的隨機特性，使得規(guī)劃的路徑能夠比較接近最短路徑。

通過選擇合適的估計函數(shù)，從而尋找到最優(yōu)路徑。基本思想是在路徑中的任何一個節(jié)點k，都有一個估計函數(shù)f(k)=p(k)+q(k)，f(k)為擴展樹中的節(jié)點k從初始節(jié)點到目標(biāo)節(jié)點的估計函數(shù)，p(k)為狀態(tài)空間中從初始節(jié)點到中間節(jié)點k的實際代價值，q(k)為狀態(tài)空間中從中間節(jié)點k到目標(biāo)節(jié)點的最佳路徑的代價估計值。A*算法的搜索方向是沿著到目標(biāo)點估計值最小的方向進(jìn)行擴展的。估計函數(shù)中q(k)的選取，對路徑求解過程的效率和優(yōu)良性有一定的影響。在這里，我們考慮二維空間環(huán)境，估計函數(shù)取兩個節(jié)點之間的歐氏距離，即直線距離。

f(k)=D(Ps,Pk)+D(Pk,Pf)

(1)

在RRT算法中擴展樹新增加葉子節(jié)點xnew時，引入啟發(fā)性函數(shù)，即通過計算每個節(jié)點到目標(biāo)節(jié)點的估計值來選擇新增的葉子節(jié)點。也就是說，在擴展樹的生長過程中，選取多個隨機選擇采樣點作為候選采樣點，并分別計算候選采樣節(jié)點xtemp到目標(biāo)節(jié)點的代價估計值。這里取歐式距離：

q(xtemp)=D(xtemp,xf)

(2)

f(xtemp)=p(xparent)+D(xtemp,xf)

(3)

由于已生成的擴展樹的任意點xparent的估計值是確定的值p(xparent)，因此比較不同的候選采樣點的估計函數(shù)f(xtemp)，即為比較D(xtemp,xf)。以啟發(fā)因子D(xtemp,xf)作為擴展樹進(jìn)一步生長的啟發(fā)因子，可以減少葉子節(jié)點選取的隨機性，有利于擴展樹朝著目標(biāo)節(jié)點生長，這個過程可以形象地稱為陽光引導(dǎo)。

(2) 優(yōu)化采樣點選取方法。由于選取點的隨機性，容易產(chǎn)生局部最小值的問題，即在相同的區(qū)域循環(huán)往復(fù)進(jìn)行采樣。為了避免這類問題，本文在隨機點選取過程中，對于小于步長距離的隨機點直接舍棄，只有大于步長距離的點才可以作為候選采樣點，避免了局部搜索，使得擴展樹葉子節(jié)點向著更縱深的空間進(jìn)行探索和生長，避免產(chǎn)生局部最小值問題。

改進(jìn)的RRT的算法流程為：

Step1初始化出發(fā)點和目標(biāo)點，建立隨機樹節(jié)點位置范圍。

Step2在空間中進(jìn)行隨機選取z個采樣候選點xtemp,i，i=1,2,…,z，分別計算D(xtemp,i,xf)，尋找值最小的點作為xrand。

Step3尋找與xrand距離最近的葉子節(jié)點xnear。

Step4在xrand與xnear連線上，選取與xnear距離為步長λ的新增葉子節(jié)點xnew。

Step5判斷xnew以及xnear到xnew的路徑是否滿足路徑條件，如果滿足路徑條件，生成新的隨機樹，如果不滿足條件，轉(zhuǎn)入Step 2。

Step6判斷新增節(jié)點xnew是否到達(dá)目標(biāo)點或者目標(biāo)點范圍。如果已經(jīng)到達(dá)目標(biāo)點，流程結(jié)束；如果未到達(dá)目標(biāo)點或目標(biāo)點范圍，轉(zhuǎn)入Step 2。

(3) 選擇合適啟發(fā)概率。選擇概率為大于0.5，基本搜索點數(shù)成均勻分布，增大了搜索空間，提高了全局搜索能力。如圖5所示。

圖5 不同啟發(fā)概率下RRT搜索點數(shù)

首先設(shè)置任務(wù)環(huán)境為二維區(qū)域，尺寸為100×100無量綱單位，確定實驗分析的研究對象，起始位置為(50，50)，目標(biāo)位置為(100，100)。

2.2 RRT航跡優(yōu)化方法

在已生成的初步擴展樹中，Pi-1,Pi,Pi+1,…,Pj-1,Pj為擴展樹上的一系列葉節(jié)點，即航跡規(guī)劃點，雖然這些點形成的航跡線滿足了避開威脅或障礙條件以及航向角等約束，但是形成的航跡路徑長度較長。為了進(jìn)一步優(yōu)化路徑，本文提出一種逐步迭代的RRT的航跡規(guī)劃優(yōu)化方法，如圖6所示。

圖6 RRT航跡

從Pj建立與上級一直到根節(jié)點的所有節(jié)點，如果Pj-2Pj線段滿足避障和航向角等約束，則更新航跡擴展樹，Pj-2稱為Pj的上一級父節(jié)點。圖6中PiPj符合航跡線要求，而Pi-1Pj不符合航跡線要求，因此將Pj的父節(jié)點更新為Pi。這樣，由終節(jié)點逐步進(jìn)行優(yōu)化，更新各節(jié)點的父節(jié)點，直到最后的父節(jié)點為起始節(jié)點。然后對新形成的各節(jié)點進(jìn)行進(jìn)一步迭代優(yōu)化，直至各節(jié)點的父節(jié)點無法迭代為止。這樣路徑既滿足航跡要求，又縮短了航跡路徑。

3 仿真實驗與分析

3.1 啟發(fā)式RRT航跡規(guī)劃仿真

仿真采用MATLAB平臺來實現(xiàn)。硬件環(huán)境為Windows XP SP2系統(tǒng)，英特爾2.5 GHz，8 GB內(nèi)存普通計算機上。任務(wù)區(qū)域大小為100×100無量綱區(qū)域，UAV初始位置為(0.5，0.5)，目標(biāo)位置為(100，100)，圓心的位置表示威脅源的位置，半徑的不同表示威脅范圍的不同。

(1) 簡單威脅或障礙環(huán)境。設(shè)定圓形區(qū)域為威脅或障礙，簡單威脅環(huán)境下的仿真結(jié)果如圖7所示，仿真運行結(jié)果見表4。

(a) 基本RRT (b) 啟發(fā)式RRT圖7 簡單威脅環(huán)境下啟發(fā)式RRT算法

表4 簡單威脅環(huán)境下啟發(fā)式RRT算法運行結(jié)果

(2) 復(fù)雜威脅或障礙環(huán)境。設(shè)定圓形區(qū)域為威脅或障礙，復(fù)雜威脅或障礙環(huán)境下的仿真結(jié)果如圖8所示，仿真運行結(jié)果見表5。

(a) 基本RRT (b) 啟發(fā)式RRT圖8 復(fù)雜威脅環(huán)境下啟發(fā)式RRT算法

表5 復(fù)雜威脅環(huán)境下啟發(fā)式RRT算法運行結(jié)果

3.2 帶路徑修正RRT的航跡仿真

基于啟發(fā)式RRT算法首先生成一條初始航跡，根據(jù)各葉節(jié)點的聯(lián)通情況和威脅或故障的規(guī)避，生成修正后的航跡，如圖9所示。

圖9 帶修正的啟發(fā)式RRT算法

4 結(jié) 語

本文通過改變步長、搜索次數(shù)來分析其UAV航跡生成影響，提出一種結(jié)合目標(biāo)信息的啟發(fā)式方法，解決了擴展樹生長過程中隨機性較大的問題，提高了全局搜索能力；通過分析不同概率對于搜索結(jié)果的影響，選擇合適的概率，既增強全局搜索能力，又提高搜索速度，同時考慮局部搜索精度；提出一種航跡迭代優(yōu)化方法，減小了冗余節(jié)點，使得航跡更短。仿真結(jié)果表明,基于啟發(fā)信息的改進(jìn)RRT算法具有較快的收斂速度和更短的搜索時間，迭代優(yōu)化方法，減少了冗余規(guī)劃點，縮短了航跡，提高了航跡規(guī)劃效率。