基于改進RRT算法的四旋翼無人機航跡規劃

成浩浩，楊 森，齊曉慧

(陸軍工程大學石家莊校區 無人機工程系，河北 石家莊 050003)

0 引 言

由于低空飛行環境復雜，四旋翼無人機飛行時常受到障礙物的影響，當飛行航線上突然出現障礙物時，其要具備對飛行路線進行重規劃的能力[1]。在航跡規劃方面很多學者提出了不同的算法，如人工勢場法[2,3]、A*算法[4,5]、概率路線圖法(probabilistic roadmap method，PRM)[6]、蟻群算法[7]、遺傳算法[8-10]等。RRT算法是S.M.LaValle[11]提出來的基于采樣的增量式搜索[12]算法。該算法不需要建立空間信息模型，以隨機采樣的方式在任務空間中均勻采樣，因此具有概率完備性，相較其它算法更適合高維空間中的四旋翼無人機航跡規劃研究。

然而，RRT算法存在全局隨機搜索，運算量大，實時性較差，航跡由隨機點組成，通常不是最優航跡[13]，規劃航跡不平滑的缺點。為此，不少學者進行了改進：針對RRT算法隨機搜索、實時性較差的缺點提出了引導策略，針對路徑不平滑的特點提出動態步長策略或者航平滑優化策略。這些改進方法大都是單一的，效果受到了一定程度的影響。本文從3個方面綜合考慮，提出了3種優化策略：啟發式搜索策略、動態步長策略和雙層平滑度優化策略，并將其應用于四旋翼無人機兩種類型的動態航跡規劃中，在二維和三維環境下進行仿真分析，檢驗這種綜合改進型算法實時性和航跡平滑度等性能。

1 傳統RRT算法簡介

RRT算法包括隨機樹的生長和航跡反向搜索兩個階段。在隨機樹生長階段，以出發點Pstart為樹的根節點，在任務空間隨機生成一個隨機點Prand，找出樹中與隨機點Prand最近的葉節點Pnear，然后從Pnear出發，在與Prand的連線上選擇一個步長ε找到點Pnew，檢查Pnew是否在障礙物區域內，Pnear與Pnew的連線上是否有障礙，如果有，則重新選擇隨機點Prand，否則將Pnew接入樹中形成新的葉節點，隨機樹不斷生長，直到葉節點到達目標點或者到目標點的距離小于一個設定的范圍，隨機樹生長過程結束；在航跡反向搜索階段，形成從目標點到出發點的貫通航跡，規劃結束。隨機樹的生長過程如圖1所示。

圖1 隨機樹生長過程

傳統RRT算法遍歷搜索所有空間，不會存在規劃失敗的情況，但是這種隨機的遍歷搜索會使隨機樹節點產生大量的冗余，算法實時性降低，規劃航跡曲折。

鑒于RRT算法實現分為隨機樹生長的引導和規劃航跡的平滑優化兩個階段，本文從引導策略、步長選擇、航跡優化方法3個方面進行綜合考慮。

2 綜合改進的RRT算法

傳統RRT算法在隨機樹生長過程中會出現很多的冗余，浪費了規劃時間，降低算法的實時性，并且生成的航跡較曲折，導致航跡的跟隨性降低。如果引進基于概率引導的啟發式搜索策略，指引隨機樹朝目標點的方向生長，將會在一定程度地減少規劃時間，提高算法實時性；動態步長策略，側重于解決算法實時性和平滑度之間的矛盾，即提高算法實時性又使規劃的航跡盡量平滑；而雙層平滑度優化策略的引入，則對航跡的平滑度進行優化，規劃出從起始點到目標點的較優路徑。

綜合改進RRT算法流程如圖2所示：首先，利用啟發式搜索策略進行空間搜索，找到下一個待擴展的葉節點；然后利用動態步長策略實現隨機樹擴展生長；最后找到可行航跡后通過雙層平滑優化策略進行航跡平滑優化，規劃出適合四旋翼無人機飛行的可行航跡。

圖2 綜合改進RRT算法流程

2.1 基于概率引導的啟發式搜索策略

傳統RRT算法在任務空間隨機產生Prand，這雖然有利于算法探索未知區域，但是搜索的點會產生很多冗余，增加算法運行時間，降低算法實時性。文獻[12]提出了雙層啟發式優化策略，即面向目標的啟發式搜索策略和面向可跟蹤性的航跡平滑優化策略。在面向目標的啟發式搜索策略中，提出了啟發式采樣策略和新節點啟發式擴展策略。面向目標的啟發式采樣策略是在隨機點的選取上，讓其以一定的概率θ(0<θ<1)選目標點為隨機點；新節點啟發式擴展策略是在新節點的選取上，讓新節點生成的方向以一定的概率τ(0<τ<1)設定為目標點方向。這種優化策略在障礙物較少時可以增強隨機樹向目標點生長的趨勢，使搜索更具有指向性，減少大量的冗余節點，增強算法實時性。但是在障礙物較多時，這種優化策略由于喪失了部分選擇性，反而導致搜索效率較低；同時兩步的優化會耗費更多的時間。為此本文借鑒了面向目標的啟發式采樣策略，提出了基于概率引導的啟發式搜索策略，不對新節點的選取進行引導，通過調整θ來達到指向目標的目的。這種改進在不失算法指向性的前提下，可以提高算法的收斂速度，為后面的航跡平滑度優化節省出一定的時間。

2.2 動態步長策略

傳統的RRT算法步長是固定的，當選用大步長時，算法能夠很快地收斂到目標點，但是航跡平滑度很差；當選用小步長時，航跡的平滑度得到改善但是卻是以犧牲算法實時性為代價的。為了解決算法實時性與平滑度之間的矛盾，文獻[14]提出了動態步長策略，加入了目標引力思想，新節點的計算公式如下

(1)

其中，ρ1是隨機點方向步長，ρ2是目標點方向步長。通過調整ρ1和ρ2的權值達到改變步長的目的。這種動態步長的優化達到了在障礙物密集區域改變步長的目的，并且可以使新節點朝著目標點方向生長。但是到達閾值后ρ1和ρ2的權值固定，不能隨著與障礙物距離的遠近而變化。

受文獻[14]的啟示，本文提出了新的動態步長策略，步長選取函數為

εnew=(keαd-0.8)·ε

(2)

其中，ε是原始步長，εnew是障礙物附近的步長，d是算法葉節點當前位置與障礙物之間的距離，α是衰減系數，k是調節因子，α和k的取值與閾值的選取有關，要確保到達閾值時εnew≤ε。

新動態步長策略的思想是：在隨機樹離障礙物較遠、沒有到達閾值時采用原先步長ε，加快算法收斂速度；隨機樹生長到障礙物附近、達到閾值時采用小步長εnew，確保航跡平滑度。這種改進雖然不能使新節點朝著目標點選取，但是可以使步長隨著障礙物之間的距離而變化，更好地解決實時性與平滑度之間的矛盾。算法的指向性可以通過調整第一種改進策略中θ的值來彌補。

2.3 雙層平滑度優化策略

傳統RRT規劃完航跡之后存在的最大的問題就是航跡平滑度的問題，為了改善航跡平滑度，文獻[15]提出了一種平滑度優化策略：先對圖3中轉折角大于120°的相鄰路徑進行刪減平滑，然后采用三次貝塞爾曲線進行優化。這種優化方法優化后的航跡都是曲線，非常平滑，但是這種三次貝塞爾優化增加了路徑中節點的數量，考慮到無人機的航跡通常是通過坐標點控制，可能會使實際的無人機飛行路線不一定是貝塞爾優化后的航跡。為此，本文提出了雙層優化策略，其主要思想是對規劃的航跡進行兩次優化，以期達到更理想的效果。

設圖3中P1、P2、P3是航跡中連續的3個點，航跡連接順序為P1→P2→P3，β為航跡中相鄰節點的轉折角，根據角度的大小，對航跡進行優化。

圖3 航跡優化圖解

處理方法為：

第一次優化：當β>90°時，認為航跡比較平滑，不需要進行優化處理，航跡節點還是原來的節點；當β≤90°時，認為轉折角度較大，需要進行優化處理。具體優化步驟如下：

(1)首先檢查航跡P1→P3上是否有障礙，如果沒有障礙，則刪掉節點P2，直接連接P1和P3兩點作為新的航跡；如果P1→P3上是有障礙，則取P2、P3的中點P；

(2)檢查航跡P1→P上是否有障礙，如果沒有障礙，則略過節點P2，連接P1、P兩點作為新的航跡；若存在障礙，則不能改變原來的航跡，只能以P2作為節點，不進行優化處理；

(3)從航跡的起點開始依次檢查每個節點與后兩個節點的曲折情況，確定是否可以進行優化，并做出相應的處理，直到航跡終點，第一次優化完成。

以第一次優化完的航跡為基礎，進行第二次優化，優化方法為：判斷兩條航跡之間的轉折角β，當β>150°時，不進行優化；當β≤150°時，認為航跡較曲折，需要進行優化，具體優化步驟是只進行第一次優化步驟(2)、步驟(3)，直到新航跡的終點，優化完成。

第一次優化過程的主要作用是對航跡中的節點進行刪減，使節點數目更少，加快程序執行過程，并且對轉折角小于90°的航跡進行優化，使航跡平滑，但是第一次優化由于改變節點，有可能使個別地方航跡的平滑度降低，所以進行第二次優化；第二次優化沒有進行節點的刪減，只對第一步優化的航跡中，轉折角大于150°的航跡進行優化，增加航跡的平滑度。

3 突發威脅航跡規劃、性能約束及任務空間模型

3.1 突發威脅航跡規劃

根據任務需要，一般四旋翼無人機動態航跡規劃可分為兩種情況：一是執行運輸、投送、打擊任務時，四旋翼無人機只要能到達目標點即可，這種情況下可以放棄原先的航跡，以四旋翼無人機當前位置為起點，以目標點為終點重新規劃一條航跡；二是執行偵察任務時，四旋翼無人機避過障礙后還要回到原來的航線上繼續執行任務，這時只需要規劃出一段可以繞過障礙物的航跡。

第一種航跡規劃方式只要把當前位置設為起始點，終點為目標點即可，設置簡單，但是當四旋翼無人機距目標點較遠時，重規劃航跡花費的時間較長，實時性降低，但是對于目標點改變的情況只能使用這種方式；第二種規劃方法需要找出分離點和接入點，并且分別設為起始點和目標點，規劃一條航跡，當四旋翼無人機到達分離點時按照新規劃的航跡飛行，到達接入點時回到原來的航線上，通常這種方式規劃航跡短，實時性高，但是對障礙物在目標點附近時沒有必要采用此方式。

3.2 動態性能約束

四旋翼無人機能夠正常飛行，需要滿足其動態性能要求，主要性能約束如下：

最大飛行距離Lmax：設四旋翼無人機的航跡是由航跡段{lii=1,2,3,…,n}坐標組成，則約束可表示為

(3)

最小直飛距離lmin：設四旋翼無人機的航跡是由航跡段{lii=1,2,3,…,n}坐標組成，則約束可表示為

lmin≤li

(4)

最大轉彎角度αmax：由于四旋翼無人機可以懸停，所以可以忽略此約束[16]。

最大俯仰角φmax：設三維航跡方程為z=f(x,y)，ai為第i段航跡的水平投影，則ai=(xi-xi-1,yi-yi-1)，該約束條件表示為

(5)

同時滿足以上約束條件的航跡才是可行航跡。二維條件下不需要考慮四旋翼無人機最大俯仰角的限制，只需滿足式(3)、式(4)即可。

3.3 任務空間模型

3.3.1 二維任務空間模型

二維模型是基于四旋翼無人機定高飛行時的情景，此時四旋翼無人機的任務空間R=[(X,Y)0≤X≤100,0≤Y≤100]，(m)。其中X是任務空間的橫坐標，Y是任務空間的縱坐標。在二維空間中，可以將障礙物建模為圓。二維任務空間和障礙物的建模如圖4所示，Pstart、Pgoal分別是起始點和目標點，圓形區域是障礙物。

圖4 二維任務空間模型

3.3.2 三維任務空間模型

實際中，四旋翼無人機是在三維環境中飛行，仿真需要給出三維環境下四旋翼無人機的任務空間描述，設其為R=[(X,Y,Z)0≤X≤100, 0≤Y≤100, 0≤Z≤100]，(m)。其中X、Y、Z分別為任務空間的橫坐標、縱坐標和豎坐標。障礙威脅可以用以下方程來描述

(6)

其中，Zi為第i個山峰的高度，(Xi,Yi)是第i個山峰的峰頂坐標，(Xλi,Yλi)為第i個山峰X方向和Y方向上的坡度。三維環境下的任務空間和障礙物模型如圖5所示。

圖5 三維任務空間模型

4 仿真驗證與分析

為了驗證與分析本文所提出的綜合改進RRT算法的性能，在二維任務空間中進行仿真實驗，并與兩種具有代表性的RRT改進算法進行性能比較；同時分析綜合改進RRT算法中的重要參數——引導概率θ對算法的影響。在三維空間中對兩種類型的動態航跡規劃進行仿真，驗證綜合改進RRT算法的可行性。

4.1 不同優化方法之間的比較

對傳統RRT算法的改進，最典型的優化方法是加入引力使隨機樹盡可能朝著目標點方向生長來加快收斂速度和動態步長策略來改善算法實時性與航跡平滑度的矛盾。本部分將綜合改進RRT算法與啟發式RRT算法和動態步長RRT算法進行比較，分析各種改進型算法的特點。進行30次仿真比較，結果如圖6、圖7所示，具體性能指標由表1給出。

圖6 綜合改進RRT與啟發式RRT的比較

圖7 綜合改進RRT與動態步長RRT的比較

比較類別綜合改進RRT啟發式RRT動態步長RRT隨機樹葉節點數/個Max544374Min353235Mean42.6437.1858規劃時間/msMax105.39.7210.69Min8.094.765.38Mean21.76.887.64航跡長度/mMax134.2144.08135.97Min127.85130.84129.70Mean130.58136.01132.74

圖6中細實線是啟發式RRT算法規劃的航跡，粗實線是改進RRT算法規劃的航跡，“*”是啟發式RRT算法的葉節點，“+”是綜合改進RRT算法的葉節點。圖7中細實線是動態步長RRT算法規劃的航跡，粗實線是改進RRT算法規劃的航跡，“*”是動態步長RRT算法的葉節點，“+”是綜合改進RRT算法的葉節點。從圖6、圖7可以看出，啟發式RRT算法在沒有障礙物時規劃出來的航跡幾乎為直線，沒有轉彎角度，航跡最優，但是有障礙物時規劃航跡轉折角度較大，動態步長RRT算法完全由引力函數提供隨機樹生長方向和步長，航跡對引力函數依賴性很大，不利于適應復雜環境，綜合改進RRT算法不僅對隨機樹的生長提供指引，而且對規劃的航跡進行優化，航跡的平滑度優于啟發式RRT算法和動態步長RRT算法。

從表1中可以看出，隨機樹節點數量上，啟發式RRT平均需要37.18個節點就可以規劃處從起始點到目標點的航跡，數量最少，效率最高，動態步長RRT算法僅依賴引力函數，平均需要58個節點才能規劃出航跡，效率最低，綜合改進RRT算法介于兩者之間；從規劃時間上看，啟發式RRT算法和動態步長RRT算法相差不多，而綜合改進RRT算法需要的時間大約是另外兩種算法的3倍，這是因為啟發式RRT算法和動態步長RRT算法規劃完航跡后沒有進行航跡的平滑度優化，而綜合改進RRT算法的平滑度優化策略耗費了大量的時間；航跡長度上，綜合改進RRT算法最短，這得益于對航跡的優化，啟發式RRT算法最長，動態步長介于兩者之間。

總體來說，啟發式RRT和動態步長RRT算法實時性好，適合應用于障礙物較少，實時性要求高的場合；綜合改進RRT算法的航跡最短，平滑度最好，但是規劃時間最長，實時性最差，綜合改進RRT算法是犧牲算法的實時性來提高航跡的平滑度的，在實時性要求不是很高的情況下可以選擇綜合改進RRT算法進行航跡規劃。

4.2 算法參數影響

綜合改進RRT算法中的參數有原始步長ε和引導概率θ。原始步長是根據任務環境、四旋翼無人機的動態性能約束和任務要求來確定的，引導概率θ則直接決定了算法性能的優劣。在圖8所示隨機任務空間中設置50個隨機障礙，算法中步長設為5，在θ∈[0,1)的范圍內，每隔0.1對θ進行取值，進行50次仿真驗證，對規劃距離、規劃時間兩項最重要的指標取平均進行統計，其結果見表2，θ取不同值時的性能比較如圖9所示。

圖8 隨機任務空間

θ規劃時間/s航跡長度/mMaxMinMeanMaxMinMean05.450.371.04147.4123.6133.70.11.030.120.37167.0117.6132.40.20.590.090.21143.7116.5123.90.30.860.080.23138.1114.9123.20.41.130.070.32128.3114.6121.90.51.380.070.35141114.8121.40.62.440.040.42128.5114.5120.70.72.880.090.56141.7115.4120.30.83.470.150.82125.6114.2118.20.97.920.161.60125.8113.6118.0

圖9 θ取不同值時的仿真性能

由圖9(a)可以看出，規劃時間在θ=0.2時取得最小值，隨著θ的減小或者增大，規劃時間不斷增加。這是因為θ取值很小時，算法的方向啟發性不強，隨機點在任務空間中生成的隨機性很大，需要耗費較長時間隨機樹才會到達目標點；當θ取值很大甚至接近于1時，算法的方向啟發性很強，搜索未知區域的能力明顯減弱，隨機樹擴展的過程中遇到障礙時，需要重復搜索才能找到合適的葉節點，繞過障礙物，到達目標點，重復搜索的過程會耗費大量的時間，導致算法實時性降低。由圖9(b)可以看出，航跡距離隨著θ的增大而減小，特別是θ∈[0,0.2]時航跡減小比較明顯。這是因為θ越大，算法的指向性越強，規劃出的航跡越接近從起始點到目標點的直線。

當θ=0時，改進的算法相當于對傳統RRT算法進行路徑優化；當θ=1時，一直選目標點為隨機點，失去算法的搜索性，容易導致航跡規劃失敗。仿真實驗結果表明，θ取值不能過大也不能過小，過大會導致算法搜索能力降低，造成規劃失敗或者花費更多的時間搜索導致實時性降低；過小會導致算法啟發性不強，路徑曲折，達不到期望的效果。由仿真結果可知θ的取值在0.5左右可以取得較好的效果。

4.3 突發威脅航跡規劃

當四旋翼無人機按照規劃的航跡飛行，航跡上突然出現障礙物時，四旋翼無人機要具有重新規劃航跡的能力。本部分對三維空間中綜合改進RRT算法的兩種類型的動態航跡規劃進行仿真，設原始步長為ε=5、引導概率θ=0.5、障礙物半徑為10，驗證所提出的綜合改進RRT算法的可行性。仿真結果如圖10、圖11所示。

圖11 繞過障礙，回到原來航跡

圖10、圖11中“*”是起始點，“o”是目標點，黑色細線是四旋翼無人機原先的航跡，黑色粗線是障礙物出現后重新規劃的航跡，五角星是障礙物出現時四旋翼無人機的位置。兩種動態航跡規劃方式都可以重新規劃一條繞過障礙物的新航跡，達到避障的目的，驗證了綜合改進RRT算法的可行性，實際應用中具體采用哪種方式要結合具體情況而確定。

5 結束語

針對傳統RRT算法隨機性強、收斂速度慢、規劃航跡不平滑的缺點，本文受其它文獻啟發對其進行了改進，提出了綜合改進型的RRT算法，并且與兩種典型的RRT改進算法進行了比較，可以發現綜合改進后的RRT算法航跡平滑度增加，但是規劃時間增長，實時性降低。本文進一步在二維任務空間中對綜合改進RRT的參數θ取不同值時進行仿真，驗證了引導概率θ對算法的影響，并給出選取原則；在三維環境下將改進后的RRT算法用于兩種動態航跡規劃中進行仿真驗證，取得了滿意的效果，驗證了綜合改進RRT算法能夠適應動態航跡規劃的要求。

然而，基于RRT算法隨機性強的特點，本文沒有對最優航跡的選取進行研究，RRT算法與最優航跡選取算法的結合將是解決此問題的思路，也是以后研究的方向。