基于RRT的動態規避航跡規劃算法

陳良劍，趙文龍，婁嘉駿

（1.南昌航空大學信息工程學院，南昌330063；2.寧波水表（集團）股份有限公司，寧波315032）

0 引言

航跡規劃是無人機研究領域的重要分支。求解航跡規劃問題時，如果環境是靜態的、完全可知的，可以在規劃航跡之前，先獲取當前空間區域的環境信息，并建立相應的空間模型，然后通過一次全局規劃獲得一條最優航跡，但實際情況下，規劃環境是動態的，因此通常情況下，無法在短時間內建立起任務區域的空間模型。無人機研究者針對未知環境下的求解航跡規劃問題提出了很多方法和策略，其中由Lavalle SM［1］提出的RRT（Rapidly-exploring Random Tree）算法是一種非常有效的未知環境下的求解規劃算法［2－3］。

RRT算法是基于采樣的單一查詢路徑規劃算法，其不需要對任務區域進行預處理，且搜索樹能快速地朝著未知的區域搜索［3］。但RRT算法由于隨機性太強，導致該算法的規劃性能極其不穩定。針對這個問題，文獻［4］提出一種多采樣策略，每次隨機采樣一組隨機點，然后選擇其中最優的隨機點作為采樣點。文獻［5］、［6］對RRT算法引入啟發因子，使RRT算法以犧牲部分探索性能作為代價，增加算法對目標點的規劃指向性。這些改進的基本思想都是提高路徑的指向性，降低算低隨機性，以此提高算法的穩定性，但在較為復雜的任務環境下，這些改進會造成算法的采樣無效點大幅增加，進而導致規劃時間增加。而且這些改進應用于無人機的航跡規劃時，并未考慮無人機自身的性能約束，因此算法獲得的路徑大多不適合無人機飛行。

結合上述討論，提出一種基于RRT的動態規避航跡規劃算法，在引入啟發因子的基礎上，結合無人機的性能約束，通過動態調整的方法增加RRT的穩定性和規避性能。最終使算法能夠擁有適當的目標指向性，而且避免算法在障礙物密集的情況下產生大量的采樣無效點。

1 RRT算法基本原理

假設C代表需要規劃航跡的任務區域，Cobs∈C代表任務區域內的障礙區域，Cfree∈C代表任務區域內的自由區域，任務區域滿足Cobs∪Cfree＝C且Cobs∩Cfree＝Φ，Qstart∈Cfree是起始點，Qgoal∈Cfree是目標點。下面結合圖1，對原始RRT算法的實現原理加以說明。

圖1 RRT算法示意圖

Step2再次隨機采樣，獲取新采樣點Qrand∈Cfree，然后搜索距離Qrand最近的節點Qnear∈T，在直線QnearQrand上截取一個點Qnew。若線段QnearQnew沒有經過障礙區域，則將Qnew作為正式節點添加到隨機樹中。否則，舍棄Qnew重新進行采樣。循環該步驟進行若干次采樣。

Step3當隨機樹的節點延伸至目標節點區域時，停止循環采樣。從起始點Qstart開始，依次搜索父節點，直至到達目標節點Qgoal最后生成一條無碰撞的路徑。

2 動態規避RRT算法

2.1 啟發式約束采樣策略

傳統的RRT算法在任務空間內隨機采樣，這有利于規劃算法對未知區域的探索，但最終生成的擴展隨機樹會產生大量的冗余節點，大幅增加算法的運行時間［9］。而且隨機采樣會導致最終的規劃路線出現大量的拐點，因無人機自身的性能約束，擁有大量拐點的路線不適合無人機飛行［10－11］，因此將RRT算法應用于無人機領域，需要結合無人機的性能約束條件進行討論。

無人機在執行任務時由于受到自身機動性能的限制，無人機的實際飛行航跡需要受到以下條件約束：

（1）最小飛行距離：無人機由于自身的機動性能限制，進行飛行狀態變更時，需要至少飛行一定的距離才能改變當前飛行狀態，實現預定的飛行動作。設最小飛行距離是lmin。RRT算法的采樣步長需滿足ρ≥lmin。

（2）最大轉向角：無人機因受限于自身的性能，無法在飛行時實現大角度轉向［12］，因此規劃算法在采樣時需考慮到采樣路徑的轉向角，并盡可能減少轉向，保證航跡的平滑。設最大轉向角是Φmax，整條路線的轉向角需滿足Φ≤Φmax。

針對上述約束條件，以及RRT算法的隨機性過度問題，提出啟發式約束采樣策略，將RRT算法的采樣分成兩個步驟。

首先，采用啟發式算法思想決定每次采樣的方式。即引入一個目標指向概率pg，每次采樣時隨機獲取一個概率值pr，若pr＜pg，則采用定點采樣的方式，以目標點作為采樣點。否則，采用傳統的隨機采樣方式獲取采樣點。

然后，結合上述約束條件圈定采樣區域，每次采樣前先根據臨近節點、最大轉向角Φmax，計算出可行采樣區域Cs。最后將隨機采樣的范圍約束到可行采樣區域內，可行采樣區域如圖2所示。

圖2 可行采樣區域示意圖

圖2中航跡QrandQnear和QnearQ0在二維平面的投影如公式（2）所示。

引入啟發式約束采樣策略，可以增加隨機樹向目標點生長的趨勢，并且使規劃的路線更適合無人機飛行。但由于上述采樣策略增加了大量約束條件，大幅降低了RRT算法對未知區域的探索能力。因此本文在啟發式約束采樣策略的基礎上加入動態步長規避策略，增強算法的探索能力和避障性能。

2.2 動態步長規避策略

動態步長規避策略的核心思想是通過碰撞檢測獲取碰撞點，然后根據碰撞點調整采樣步長避讓障礙物。算法的主要流程是，首先將與障礙物有碰撞的擴展樹樹枝分割成若干份，然后從采樣點開始，向著臨近節點的方向對分割點進行碰撞檢測，當檢測到不處于障礙物區域的分割點時，將該點記錄，作為擴展樹枝上的自由點。然后將該自由點旋轉一定角度后，判斷不處于障礙區域后添加到擴展隨機樹中。這種處理方法可以在障礙物密集的環境中避免多次采樣，減少算法的規劃時間，并通過偏轉路徑的方法盡可能繞過障礙物，還為后續節點留下了足夠的延伸空間。動態步長規避策略的詳細流程結合圖3進行說明。

圖3 動態步長規避策略示意圖

首先將固定步長ρ分割為若干份長度為l（l≥lmin）的線段，然后從Qsample開始沿固定步長每隔長度l進行一次判斷，當找到第一個不處于障礙區域的判斷點Qfree時，將此時Qfree和Qsample的距離記為d1，Qfree和Qnear的距離記為d2。

將r（r＝d1＋αl，α∈［0，1，…，n］）作為半徑，以Qsample為中心點，將Qfree朝更接近目標點的位置偏轉90°獲得偏轉點Qbias（xbias，ybias）：

然后在Qnear和Qbias的連線上，將Qnear作為起始點，以d2作為步長重新獲取新節點Qnew。判斷Qnew不處于障礙區域，且該節點轉向角滿足最大轉向角約束時，將Qnew添加至擴展隨機樹中，否則，重新進行采樣。

2.3 航跡優化

引入啟發式約束采樣策略和動態步長規避策略的RRT算法，可以通過調整目標偏置概率、固定步長以及分割長度等參數，適應不同的規劃環境。但由于RRT算法本身的隨機特性，最終航跡仍然會包含大量的冗余節點［11］。這些冗余節點的存在會大幅增加航跡的路程和拐點數量，這樣的路線十分不利于無人機飛行［12］。因此，最終獲得的航跡需要通過航跡優化方法進行調整。航跡優化方法具體流程如下。

Step1從目標開始依次在擴展隨機樹上搜尋父節點，最終獲得一條未經過障礙區域的待優化路徑P。

Step2從起點開始依次向路徑P上的后續節點連接，檢測連線是否經過障礙區域，然后選擇連線未經過障礙區域，且最接近目標的節點作為新的父節點。再從新的父節點開始，按照上述規則依次向后搜尋，直至完成整條航跡的優化。

3 仿真結果與分析

本文采用MATLAB平臺對改良后的RRT算法進行仿真實驗，對其正確性和可行性加以驗證。然后在相同的任務區域下，分別對啟發式RRT算法、多采樣RRT算法和動態規避RRT算法進行多組測試，然后對測試得到的數據進行對比分析。

3.1 目標指向概率效果測試

改進后的算法，其啟發式約束采樣概率的引導因子對于算法的影響較大，因此在對算法規劃效果進行測試之前，需先對引導因子對算法的影響進行測試，確定引導因子對算法的影響效果，然后獲取相對最優的引導因子區間。首先在二維任務區域內，將采樣步長設置為ρ＝50，無人機最大轉向角Φ＝60°，然后修改引導因子進行規劃測試實驗，實驗數據繪制成折線圖之后如圖4所示。從測試結果可以看出，引導因子的加入對算法運算時間有十分顯著的優化，而且引導因子最優區間處于［0.5，0.7］，因此本文后續測試啟發因子取0.6。

圖4 引導因子影響效果

3.2 仿真驗證

二維仿真的任務區域是一個600800的無量綱區域，黑色區域代表障礙區域，本次仿真實驗在同一個任務區域中，對基本RRT算法、啟發式RRT算法、多采樣RRT算法以及本文提出的動態規避RRT算法進行對比驗證。對于RRT算法，采樣的步長會直接影響規劃路徑的時間和質量。所以采樣步長應該根據實際的任務區域選取，障礙物密集時步長應相對減小，反之，障礙物較少時采樣步長應適當增加。本次二維仿真實驗的障礙物是小型的圓形障礙物，障礙物較為密集，因此本次實驗的采樣步長設置為ρ＝50，啟發式RRT算法的啟發因子以及啟發式約束采樣策略的啟發因子選取為pg＝0.6，多采樣RRT每次采樣3個點，選擇其中不處于障礙區域且更接近目標點的隨機點作為新的采樣點，無人機最大轉向角Φ＝90°。

由圖5的仿真結果對比可以發現，在障礙物密集的任務區域，原始RRT算法規劃路線需要進行大量采樣，采樣點已經遍布整個地圖，這導致原始RRT算法的規劃時間大幅增加。作為對比，啟發式RRT算法和多采樣RRT算法由于指向性更明確，路徑分枝大幅減少。但由于任務區域中障礙物較多，規劃性能仍然較差。本文提出的動態規避RRT算法由于包含了避障的策略，因此只擁有極少的路徑分支，而且在障礙物附近有很明顯的避讓路徑，可以看出動態步長規避策略能夠十分有效地規避障礙物。

圖5 二維仿真結果

本次三維仿真的任務區域是一個80×80×50的三維空間，通過仿真實驗對動態規避算法在三維任務區域的規劃效果進行驗證。三維任務區域以6座環形山峰作為本次仿真的障礙物。本次三維仿真實驗的采樣步長設置為ρ＝5，啟發式約束采樣策略的概率閾值選取為pg＝0.6，三維規劃需考慮無人機的俯仰角約束，即爬升角度不可大于俯仰角，本次實驗俯仰角取60°。

如圖6的仿真結果所示，在三維任務區域，動態規避RRT算法仍然可以在障礙物相對密集的條件完成航線的規劃。

圖6 三維仿真結果

3.3 算法實驗分析

算法的性能測試采用二維的任務區域，采樣步長ρ＝50，啟發式RRT算法的啟發因子以及啟發式約束采樣策略的啟發因子選取為pg＝0.6，無人機最大轉向角Φ＝90°。由于基本RRT算法在密集障礙區域中規劃耗時過長，因此本次測試僅在相同任務區域環境下，分別對啟發式RRT和多采樣RRT以及動態RRT算法進行20組有效對比實驗。對比實驗以算法的規劃時間以及最終路徑的路程長度作為算法性能的評價指標，通過對比實驗的數據，分析動態規避RRT算法是否能夠達到預期效果。

本次對比實驗的部分數據如表1、表2所示，本次實驗通過對20組對比實驗的規劃時間和路徑長度計算方差的方式，確定算法的穩定性指標。

表1 三種算法時間分析

表2 三種算法路程分析

由表中數據可以看出啟發式RRT算法和多采樣RRT算法在密集環境下的表現差不多，啟發式RRT算法的路徑搜索效率要更高一些，多采樣RRT的最終路徑相對穩定一些。而動態規避算法的數據明顯要優于上述兩個算法，平均規劃時間僅需要其余算法的1／3，這是因為實驗任務區域有大量的障礙物，而動態規避算法強化了RRT算法的避障性能，因此動態規避算法在密集障礙區域中可以有效躲避障礙，進而避免產生大量無效采樣點的情況。而且得益于對最終航跡的優化，動態規避算法能夠有效減少最終航跡的長度。

總體來說，啟發式算法和多采樣算法適合應用在障礙物較少的任務區域，而在障礙物密集的任務區域，動態規避算法的規劃性能要明顯優于其余兩種算法。因此在障礙物較為密集的情況下，可以選擇動態規避算法進行航跡規劃。

4 結語

本文以RRT算法應用于無人機航跡規劃作為切入點，針對RRT算法隨機性太強的問題，結合啟發式RRT算法的思想，提出了啟發式約束采樣策略，加強了算法的指向性。但由于算法增強指向性后，在障礙物密集的環境中會產生大量的無效點，因此再引入動態步長規避策略加以改善，然后針對規劃成功的路徑進行迭代優化，改善最終航跡。最后對算法的可行性進行驗證，并且對動態規避算法和啟發式算法以及多采樣算法做對比實驗。通過對比實驗的結果，可以分析得出動態規避RRT算法的規劃穩定性有十分明顯的提升，而且由于動態規避RRT算法加強了避障能力，因此在障礙物密集的環境下有比較優秀的表現。