改進(jìn)RRT算法在無人機(jī)航跡規(guī)劃中的應(yīng)用?

江澤強(qiáng) 倪霄漢 王 鑫 姜元杰

（1.國防大學(xué)訓(xùn)練部公共平臺(tái)中心 北京 100091）（2.95894部隊(duì) 北京 102211）

1 引言

無人機(jī)航跡規(guī)劃是指在給定起始點(diǎn)和目標(biāo)點(diǎn)位置，并綜合考慮無人機(jī)的飛行約束條件下，尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或可行航跡，以保證無人機(jī)能夠完成飛行任務(wù)［1］。目前，常見的航跡規(guī)劃算法主要有A*算法、遺傳算法、蟻群算法、粒子群算法等［2～5］。但是隨著現(xiàn)代戰(zhàn)爭進(jìn)程的發(fā)展及戰(zhàn)場(chǎng)情報(bào)體系的日益完善，航跡規(guī)劃需要考慮的約束條件越來越多，對(duì)無人機(jī)的航跡規(guī)劃算法的求解效率也提出了越來越高的要求［6］。快速擴(kuò)展隨機(jī)樹（Rapidly-Exploring Random Tree，RRT）算法是一種基于采樣的單查詢隨機(jī)搜索算法，1998年由S.M.LaValle首次提出［7］。該算法相對(duì)于其他算法，計(jì)算簡單且不需要對(duì)空間建模，可直接根據(jù)當(dāng)前環(huán)境狀況快速有效地搜索規(guī)劃空間，在高維空間中速度優(yōu)勢(shì)尤為明顯，且該算法在可行路徑的搜索概率意義上是完全的，已成功應(yīng)用于許多非完整系統(tǒng)規(guī)劃問題中，包括移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃和飛行器路徑規(guī)劃等問題［8］。

雖然RRT算法具有諸多優(yōu)良特性，但其缺點(diǎn)也較為明顯，因而限制了進(jìn)一步應(yīng)用:1）在搜索過程中并未對(duì)航跡綜合代價(jià)進(jìn)行考慮，而是在規(guī)劃空間內(nèi)進(jìn)行均勻一致搜索，導(dǎo)致無謂損耗代價(jià)較大；2）節(jié)點(diǎn)選擇的隨機(jī)性使生成的航跡隨機(jī)性很大，同一條件下的規(guī)劃過程缺乏可重復(fù)性；3）基本RRT算法隨機(jī)性太強(qiáng)，得出的往往是可行航跡而非最優(yōu)或次優(yōu)航跡。針對(duì)這些不足，研究者提出了很多改進(jìn)方法［9～15］，在保證可行性的同時(shí)提高了算法的搜索效率。但是目前多數(shù)研究針對(duì)于機(jī)器人，其任務(wù)環(huán)境比較簡單，而無人機(jī)的任務(wù)環(huán)境往往更復(fù)雜，對(duì)算法性能有著更高的要求。因此，本文對(duì)RRT算法的節(jié)點(diǎn)的采樣方式和節(jié)點(diǎn)擴(kuò)展方式進(jìn)行了改進(jìn)，來降低隨機(jī)性對(duì)算法的影響，從而達(dá)到提高航跡規(guī)劃效率、改善航跡規(guī)劃質(zhì)量的目的。

2 RRT算法基本原理

基本RRT的節(jié)點(diǎn)擴(kuò)展過程如圖1所示［8］，將起始點(diǎn)xinit作為樹的根節(jié)點(diǎn)，當(dāng)擴(kuò)展新節(jié)點(diǎn)時(shí)，首先從規(guī)劃空間內(nèi)隨機(jī)選擇一點(diǎn)xrand，，然后在當(dāng)前RRT樹中找到距離xrand最近的節(jié)點(diǎn)xnear，并以一定的擴(kuò)展步長L來計(jì)算新節(jié)點(diǎn)xnew，如果在向新節(jié)點(diǎn)xnew行進(jìn)的過程中遇到障礙或威脅，則需要重新選擇xrand迭代計(jì)算；如果新點(diǎn)xnew是可行的，則將新點(diǎn)xnew添加到隨機(jī)樹中，并建立節(jié)點(diǎn)之間的連接關(guān)系，即xnew是xnear的子節(jié)點(diǎn)。

在整個(gè)隨機(jī)樹的擴(kuò)展過程中，隨機(jī)點(diǎn)xrand的選取原則如下:在采樣時(shí)以一定偏向概率PG選取目標(biāo)點(diǎn)xgoal為隨機(jī)點(diǎn)進(jìn)行擴(kuò)展，以概率1-PG在未探索區(qū)域內(nèi)隨機(jī)選取一個(gè)點(diǎn)作為隨機(jī)點(diǎn)進(jìn)行擴(kuò)展。當(dāng)隨機(jī)樹的葉節(jié)點(diǎn)中包含了終點(diǎn)xgoal或者與終點(diǎn)xgoal距離小于閾值ε時(shí)，則認(rèn)為已到達(dá)目標(biāo)點(diǎn)，隨機(jī)樹擴(kuò)展停止。此時(shí)，便可以在隨機(jī)樹中找到一條以樹節(jié)點(diǎn)組成的從初始點(diǎn)xinit到終點(diǎn)xgoal的規(guī)劃路徑。

圖1 RRT的節(jié)點(diǎn)擴(kuò)展示意圖

3 RRT算法改進(jìn)策略

3.1 改進(jìn)節(jié)點(diǎn)的采樣方式

由基本RRT算法可知，在航跡規(guī)劃時(shí)擴(kuò)展樹中的節(jié)點(diǎn)均由xrand擴(kuò)展而來，xrand節(jié)點(diǎn)選取是否合適將直接影響最終生成航跡的質(zhì)量。因此，為了得到質(zhì)量更高的xrand節(jié)點(diǎn)，本文引入了多重隨機(jī)采樣策略，即從規(guī)劃空間中產(chǎn)生一組隨機(jī)采樣｛1，2，…，n｝），然后對(duì)這一組點(diǎn)中選擇最優(yōu)的xrand進(jìn)行擴(kuò)展。

為了得到最優(yōu)xrand，則需要有相應(yīng)的評(píng)價(jià)函數(shù)來對(duì)該組節(jié)點(diǎn)進(jìn)行評(píng)價(jià)。如圖2所示，由基礎(chǔ)數(shù)學(xué)知識(shí)可得最短航跡一定是起始點(diǎn)xinit和終點(diǎn)xgoal的連線，因此對(duì)x1rad和x2rad比較可知選擇離最短航跡近的xrand可縮短剩余航跡長度。同時(shí)，為了加速RRT朝目標(biāo)方向擴(kuò)展，比較和可得，離終d點(diǎn)xgoal越近剩余航跡距離越短。

因此，本文在對(duì)采樣點(diǎn)進(jìn)行評(píng)價(jià)時(shí)，引入了距離和方向兩個(gè)要素，即:

如圖2所示，其中d表示采樣點(diǎn)到終點(diǎn)的歐氏距離，α表示起始點(diǎn)到目標(biāo)點(diǎn)與采樣點(diǎn)到目標(biāo)點(diǎn)的夾角，ω1、ω2分別是距離和角度的權(quán)重系數(shù)。

圖2 節(jié)點(diǎn)評(píng)價(jià)函數(shù)構(gòu)造示意圖

由于距離和方向的量綱不統(tǒng)一，因此還需對(duì)該評(píng)價(jià)函數(shù)中的d和α進(jìn)行歸一化處理，即計(jì)算k個(gè)采樣點(diǎn)的距離和角度值，然后求出其平均值d和α，即:

求出均值后，再對(duì)每一個(gè)采樣點(diǎn)對(duì)應(yīng)的距離和角度進(jìn)行歸一化處理，因此評(píng)價(jià)函數(shù)變?yōu)?/p>

其中:

進(jìn)行處理后，即可根據(jù)采樣點(diǎn)距離和角度的貢獻(xiàn)，選擇評(píng)價(jià)函數(shù)值最小的點(diǎn)作為最終采樣節(jié)點(diǎn)。采用該方法進(jìn)行采樣看似增加了計(jì)算量，但由于采樣節(jié)點(diǎn)質(zhì)量提升，使得擴(kuò)展過程中的無效探索次數(shù)顯著減少，從而提高了算法的整體搜索效率。

3.2 改進(jìn)節(jié)點(diǎn)的擴(kuò)展方式

3.2.1 自適應(yīng)轉(zhuǎn)彎角調(diào)整

在進(jìn)行節(jié)點(diǎn)擴(kuò)展時(shí)，需要滿足無人機(jī)的飛行約束條件，因此還需要根據(jù)無人機(jī)的最大轉(zhuǎn)彎角φmax和擴(kuò)展步長L等限制條件來對(duì)節(jié)點(diǎn)進(jìn)行擴(kuò)展。如圖3所示，當(dāng)確定的采樣點(diǎn)xrand在可行域之外時(shí)，由于不滿足無人機(jī)的轉(zhuǎn)彎角約束而被舍棄。而RRT算法是一種隨機(jī)算法，在實(shí)際擴(kuò)展過程中會(huì)有大量節(jié)點(diǎn)因?yàn)槲茨軡M足約束要求而被舍棄，造成了大量計(jì)算資源被浪費(fèi)。因此，本文通過引入隨機(jī)變量rk對(duì)擴(kuò)展節(jié)點(diǎn)的轉(zhuǎn)彎角進(jìn)行動(dòng)態(tài)調(diào)整，使擴(kuò)展出的節(jié)點(diǎn)能夠包含在無人機(jī)的可行域范圍內(nèi)。

圖3 改進(jìn)節(jié)點(diǎn)擴(kuò)展示意圖

因此，改進(jìn)后的xnew計(jì)算方式為

其中，rk的取值決定了節(jié)點(diǎn)的偏轉(zhuǎn)方向和偏轉(zhuǎn)角度，為了加速向目標(biāo)方向擴(kuò)展，減少無效探索次數(shù)，通過判斷xnear與xgoal的相對(duì)位置來確定rk的取值范圍，即:

1）當(dāng)xgoal在可行域上方時(shí)，rk∈［0，1］

2）當(dāng)xgoal在可行域下方時(shí)，rk∈［-1，0）

3.2.2 自適應(yīng)步長調(diào)整

傳統(tǒng)的RRT算法在進(jìn)行擴(kuò)展時(shí)，通常以固定步長L進(jìn)行擴(kuò)展，如圖4所示當(dāng)擴(kuò)展節(jié)點(diǎn)在威脅區(qū)內(nèi)或經(jīng)過威脅區(qū)時(shí)，即代表此次擴(kuò)展失敗。這就導(dǎo)致在威脅較多時(shí)或部分威脅密集區(qū)域容易使探索過程陷入局部極小區(qū)域，無效探索次數(shù)上升，浪費(fèi)了計(jì)算資源。為了避免上述缺陷，本文結(jié)合最小直飛航程約束l對(duì)擴(kuò)展步長進(jìn)行了動(dòng)態(tài)調(diào)整，在有效規(guī)避威脅的情況下使算法加速朝著目標(biāo)方向擴(kuò)展，及時(shí)跳出局部極小區(qū)域。

圖4 固定步長擴(kuò)展示意圖

具體方法如圖5所示。

1）根據(jù)無人機(jī)的最小直飛航程約束l將固定擴(kuò)展步長L分為 n份記為l1，l2，…，ln-1，ln，等分點(diǎn)分別記為p1，p2，…，pn-1；

2）從點(diǎn) p1開始逐步判斷 l1，l2，…，ln-1，ln與是否穿越障礙；

3）假設(shè)ln-1為首次與障礙物相交，則可知ln-2到l1均在威脅外，pn-2為最后一個(gè)不與威脅相交的等分點(diǎn)，即xnew=pn-2；

4）將xnew加入擴(kuò)展樹T，完成此次擴(kuò)展。

圖5 自適應(yīng)步長擴(kuò)展示意圖

3.3 改進(jìn)后航跡規(guī)劃算法流程

Step1:設(shè)xinit為無人機(jī)的初始位置，初始化搜索樹T，初始化無人機(jī)的航跡規(guī)劃空間。

Step2:按照以下步驟擴(kuò)展搜索樹T:

Step2.1:設(shè)偏向概率為PG，p為［0，1］的隨機(jī)數(shù)，如果p＜PG，則直接將xgoal作為隨機(jī)點(diǎn)xrand；否則，根據(jù)3.1節(jié)中改進(jìn)的節(jié)點(diǎn)采樣方式在規(guī)劃空間內(nèi)產(chǎn)生一組隨機(jī)點(diǎn)（k∈｛1，2，…，n｝），然后根據(jù)式（4）對(duì)這一組采樣點(diǎn)進(jìn)行評(píng)價(jià)擇優(yōu)，選取評(píng)價(jià)函數(shù)值最小的點(diǎn)作為xrand。確定好xrand后，進(jìn)入Step2.2。

Step2.2:在搜索樹T中找出距離xrand距離最近的節(jié)點(diǎn)xnear，并結(jié)合無人機(jī)的飛行約束，根據(jù)3.2中改進(jìn)的節(jié)點(diǎn)擴(kuò)展方式計(jì)算xnew節(jié)點(diǎn)，然后進(jìn)入Step2.3。

Step2.3:如果xnew與xnear的連線不在任何威脅范圍內(nèi)，則將xnew加入搜索樹T，否則返回Step2.1。

Step2.4:如果||xnew-xgoal||≤ε，即認(rèn)為已經(jīng)到達(dá)目標(biāo)點(diǎn)，進(jìn)入Step3；如果搜索樹的節(jié)點(diǎn)探索次數(shù)超過最大探索次數(shù)Nmax時(shí)，則表示此次擴(kuò)展失敗，強(qiáng)制結(jié)束該算法，輸出失敗信息；否則，返回Step2。

Step3:從目標(biāo)點(diǎn)逆向回溯至擴(kuò)展樹的根節(jié)點(diǎn)，并繪制從xinit到xgoal的規(guī)劃航跡。

4 仿真實(shí)驗(yàn)

為了驗(yàn)證本文改進(jìn)策略的有效性，本文在Windows 7操作系統(tǒng)上基于Matlab編程實(shí)現(xiàn)該算法，在 Inte?CoreTMi7-3770K CPU@3.5GHz，內(nèi)存16G的PC機(jī)上進(jìn)行仿真實(shí)驗(yàn)。假設(shè)規(guī)劃空間大小為200km×200km，空間內(nèi)存在的威脅均以黑色實(shí)心圓表示，起始點(diǎn)坐標(biāo)為（0，0），目標(biāo)點(diǎn)坐標(biāo)為（180，190），步長L為8，最小直飛航程約束l為2，偏向概率PG為0.2，最大轉(zhuǎn)彎角φmax為60°，采樣數(shù)k為5，ω1、ω2均取0.5，最大探索次數(shù)Nmax為5000。

本文共設(shè)置了四組對(duì)比實(shí)驗(yàn)，每組實(shí)驗(yàn)進(jìn)行30次，統(tǒng)計(jì)平均值。其中實(shí)驗(yàn)1為未經(jīng)改進(jìn)的基本RRT算法，實(shí)驗(yàn)2僅對(duì)節(jié)點(diǎn)的采樣方式進(jìn)行了改進(jìn)，實(shí)驗(yàn)3僅對(duì)節(jié)點(diǎn)的擴(kuò)展方式進(jìn)行了改進(jìn)，實(shí)驗(yàn)4同時(shí)對(duì)節(jié)點(diǎn)的采樣方式和擴(kuò)展方式進(jìn)行了改進(jìn)。所得實(shí)驗(yàn)數(shù)據(jù)如表1所示，規(guī)劃出的最優(yōu)路徑如圖6所示。

表1 對(duì)比實(shí)驗(yàn)數(shù)據(jù)

從表1所得結(jié)果可以看出實(shí)驗(yàn)2中對(duì)節(jié)點(diǎn)的采樣方式進(jìn)行改進(jìn)后擴(kuò)展節(jié)點(diǎn)質(zhì)量得到提高，平均節(jié)點(diǎn)數(shù)減少，規(guī)劃出的航跡長度明顯減小，但由于探索次數(shù)增加，規(guī)劃效率并未得到顯著提升；實(shí)驗(yàn)3中對(duì)節(jié)點(diǎn)的擴(kuò)展方式進(jìn)行改進(jìn)后提高了節(jié)點(diǎn)探索的成功率，降低了無效探索次數(shù)，規(guī)劃時(shí)間明顯降低，規(guī)劃出的航跡長度也明顯減少，但由于探索時(shí)節(jié)點(diǎn)質(zhì)量不高，因而產(chǎn)生的節(jié)點(diǎn)數(shù)較多；實(shí)驗(yàn)4中同時(shí)采用了實(shí)驗(yàn)2和實(shí)驗(yàn)3中的改進(jìn)策略后，探索次數(shù)和節(jié)點(diǎn)數(shù)均有所下降，平均規(guī)劃時(shí)間減少，算法效率得到提升，還進(jìn)一步縮短了規(guī)劃出的航跡長度。同時(shí)，從圖6的最優(yōu)路徑結(jié)果示意圖中也可以看出實(shí)驗(yàn)4所得出的最優(yōu)航跡較為平滑，更加接近最優(yōu)解，也更加符合無人機(jī)航跡規(guī)劃的要求。

圖6 最優(yōu)路徑結(jié)果示意圖

5 結(jié)語

針對(duì)RRT算法在無人機(jī)航跡規(guī)劃時(shí)存在的不足，本文在基本RRT算法的基礎(chǔ)上進(jìn)行了優(yōu)化。通過對(duì)節(jié)點(diǎn)采樣方式和擴(kuò)展方式的改進(jìn)，降低了RRT算法的隨機(jī)性，提高采樣節(jié)點(diǎn)質(zhì)量，有效減少了航跡規(guī)劃中的無效探索次數(shù)，降低了規(guī)劃時(shí)間，90ms的平均規(guī)劃時(shí)間足以滿足無人機(jī)快速航跡規(guī)劃要求。