基于RRT的動態(tài)規(guī)避航跡規(guī)劃算法

陳良劍，趙文龍，婁嘉駿

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

0 引言

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

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

結(jié)合上述討論，提出一種基于RRT的動態(tài)規(guī)避航跡規(guī)劃算法，在引入啟發(fā)因子的基礎(chǔ)上，結(jié)合無人機的性能約束，通過動態(tài)調(diào)整的方法增加RRT的穩(wěn)定性和規(guī)避性能。最終使算法能夠擁有適當?shù)哪繕酥赶蛐裕冶苊馑惴ㄔ谡系K物密集的情況下產(chǎn)生大量的采樣無效點。

1 RRT算法基本原理

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

圖1 RRT算法示意圖

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

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

2 動態(tài)規(guī)避RRT算法

2.1 啟發(fā)式約束采樣策略

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

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

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

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

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

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

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

圖2 可行采樣區(qū)域示意圖

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

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

2.2 動態(tài)步長規(guī)避策略

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

圖3 動態(tài)步長規(guī)避策略示意圖

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

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

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

2.3 航跡優(yōu)化

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

Step1從目標開始依次在擴展隨機樹上搜尋父節(jié)點，最終獲得一條未經(jīng)過障礙區(qū)域的待優(yōu)化路徑P。

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

3 仿真結(jié)果與分析

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

3.1 目標指向概率效果測試

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

圖4 引導因子影響效果

3.2 仿真驗證

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

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

圖5 二維仿真結(jié)果

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

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

圖6 三維仿真結(jié)果

3.3 算法實驗分析

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

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

表1 三種算法時間分析

表2 三種算法路程分析

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

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

4 結(jié)語

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