基于改進RRT算法的UAV航跡規劃與優化研究

高曉燕 韓慶田

1(山東商務職業學院 山東 煙臺 264670)2(海軍航空大學 山東 煙臺 264001)

0 引 言

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

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

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

1 RRT算法

1.1 算法流程

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

圖1 基本RRT算法

RRT算法流程如下[10]：

Step1初始化規劃初始點和目標點，建立隨機樹節點位置的范圍。

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

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

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

Step5判斷新增節點xnew是否到達目標點或者目標點的范圍。如果已經到達目標點，則流程結束；如果未到達目標點或目標點范圍，轉入Step 2。

1.2 參數影響分析

RRT航跡規劃算法需要設置的參數比較少，簡單結構有利于RRT算法應用推廣，但目前關于算法參數的選取缺少較為嚴格的理論依據，在實際應用中則一般根據經驗試湊獲取，不能保證算法參數的最優性。

與旅行商問題、車間調度問題等組合優化問題不同的是，航跡規劃問題沒有標準的測試樣例。任務環境本身不是算法參數，而是算法應用對象，本文通過仿真實驗來分析研究RRT算法的不同參數設置，以及不同任務環境對算法規劃性能的影響。

1.2.1搜索步長影響分析

設置任務環境為二維區域，尺寸為100×100無量綱單位，確定實驗分析的研究對象，起始位置為(50，50)，目標位置為(100，100)。

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

表1 不同步長下的最遠搜索距離

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

圖2 不同步長下的最遠搜索距離分布

可以看出，次數分布近似成均勻或正態分布。

1.2.2最大搜索次數影響

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

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

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

在搜索次數較少的情況下，搜索最遠距離與步長基本成線性關系；當不斷加大搜索次數情況下，搜索最遠距離與步長之間成非線性的關系，出現搜索逐漸放慢的趨勢。

1.2.3不同威脅環境下航跡生成

設置任務環境為二維區域，尺寸為100×100無量綱單位，確定實驗分析的研究對象，起始位置為(1，1)，目標位置為(100，100)。

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

表2 復雜威脅環境RRT結果

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

表3 復雜威脅環境RRT結果

2 基于啟發信息的RRT算法改進

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

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

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

(3) 實時性較為欠缺。

(4) 規劃路徑質量比較一般。

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

2.1 基于啟發信息的RRT航跡改進算法

現有文獻根據傳統標準RRT算法的缺點進行了一些改進，而且結果證明改進的RRT算法在規劃耗時和路徑質量上各有一定的提高，但是，對于復雜的多威脅源環境中，仍然存在規劃時間長，以及質量有待提高問題[12]。

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

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

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

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

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

(1)

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

q(xtemp)=D(xtemp,xf)

(2)

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

(3)

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

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

改進的RRT的算法流程為：

Step1初始化出發點和目標點，建立隨機樹節點位置范圍。

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

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

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

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

Step6判斷新增節點xnew是否到達目標點或者目標點范圍。如果已經到達目標點，流程結束；如果未到達目標點或目標點范圍，轉入Step 2。

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

圖5 不同啟發概率下RRT搜索點數

首先設置任務環境為二維區域，尺寸為100×100無量綱單位，確定實驗分析的研究對象，起始位置為(50，50)，目標位置為(100，100)。

2.2 RRT航跡優化方法

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

圖6 RRT航跡

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

3 仿真實驗與分析

3.1 啟發式RRT航跡規劃仿真

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

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

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

表4 簡單威脅環境下啟發式RRT算法運行結果

(2) 復雜威脅或障礙環境。設定圓形區域為威脅或障礙，復雜威脅或障礙環境下的仿真結果如圖8所示，仿真運行結果見表5。

(a) 基本RRT (b) 啟發式RRT圖8 復雜威脅環境下啟發式RRT算法

表5 復雜威脅環境下啟發式RRT算法運行結果

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

基于啟發式RRT算法首先生成一條初始航跡，根據各葉節點的聯通情況和威脅或故障的規避，生成修正后的航跡，如圖9所示。

圖9 帶修正的啟發式RRT算法

4 結 語

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