基于無人機航路規(guī)劃問題的RRT 算法綜合改進

郭瑩婷，林川

（中國電子科技集團公司第十五研究所，北京 100083）

近年來，隨著信息技術(shù)和人工智能在軍事領(lǐng)域的廣泛應(yīng)用[1-2]，戰(zhàn)爭形態(tài)向著無人化和智能化的方向演變[3-4]，多種無人機航路規(guī)劃算法受到國內(nèi)外學(xué)者的廣泛關(guān)注[5]。

在航路規(guī)劃算法中[6-7]，人工勢場算法等雖然收斂速度較快，但易陷入局部最優(yōu)，模擬退火算法搜索效率較低[8-9]，蟻群算法[10]和遺傳算法等存在航路規(guī)劃實時性差等問題，且這些算法都需要在任務(wù)空間中對障礙物進行建模，不適用于解決無人機在復(fù)雜戰(zhàn)場環(huán)境中的航路規(guī)劃問題?？焖贁U展隨機樹算法（Rapidly-Exploring Random Trees,RRT）[11-12]無需對地圖進行建模，通過對狀態(tài)空間中的點隨機采樣并進行碰撞檢測，有效解決了復(fù)雜環(huán)境和高維空間中的路徑規(guī)劃問題。

該文基于無人機在復(fù)雜戰(zhàn)場環(huán)境下生成飛行航路的軍事背景，針對傳統(tǒng)RRT 算法存在的不足[13]，提出了目標(biāo)引導(dǎo)隨機點擴展方法以及動態(tài)步長選擇策略，提高了算法的效率并減少了航路節(jié)點數(shù)量。文中首先說明了研究問題以及傳統(tǒng)RRT 算法的應(yīng)用；其次從隨機點選擇和生長步長調(diào)整兩方面改進算法步驟；最后通過仿真實驗對比，驗證了改進后算法的有效性。

1 問題描述

無人機執(zhí)行作戰(zhàn)任務(wù)的戰(zhàn)場環(huán)境一般較為復(fù)雜，常常包含各種不同地形和障礙物，例如高地、河流、雷達探測、火力打擊和電子干擾等。研究中，為了簡化任務(wù)空間的建模過程以及減少加載地圖時間，根據(jù)戰(zhàn)場環(huán)境的基本要素，采用簡化后的二維地圖表示無人機作戰(zhàn)區(qū)域，其中實心圓點（圖左上方）表示無人機起點，五角星（圖右下方）表示無人機任務(wù)目標(biāo)打擊點，矩形表示電磁干擾區(qū)域、敵空中攔截蜂群等障礙物，黑色圓形表示敵方雷達探測范圍，三角表示敵高炮火力殺傷范圍。文中實驗環(huán)境二維地圖如圖1 所示，任務(wù)空間大小為10×10 km2的二維圖。

圖1 實驗任務(wù)空間

該文將無人機執(zhí)行任務(wù)空間中的所有障礙物內(nèi)部、雷達探測范圍和火力殺傷范圍都視為需要避開的飛行禁區(qū)，空白處為可跨越的安全區(qū)域，則航路規(guī)劃問題可以抽象為從起點到目標(biāo)點的可飛行航路的最優(yōu)求解問題，無人機按照該飛行航路執(zhí)行作戰(zhàn)任務(wù)時，保證不會進入飛行禁區(qū)。

2 RRT算法改進過程

2.1 傳統(tǒng)RRT算法

傳統(tǒng)RRT 算法將任務(wù)空間中的起點Ns作為根節(jié)點，通過從任務(wù)空間中的安全區(qū)域（空白區(qū)域）中使用隨機采樣的方式生成一個隨機點Nr，通過遍歷隨機樹上的現(xiàn)存節(jié)點，找到距離隨機點Nn最近的節(jié)點Nc，然后以隨機點Nr作為擴展方向，以固定生長步長為距離，以最近的節(jié)點Nc為父節(jié)點，生成一個新節(jié)點Nn。然后檢查新節(jié)點Nn與其父節(jié)點Nc之間有無障礙物，若無障礙物，則將該新節(jié)點Nn加入到擴展隨機樹上作為子節(jié)點，若新節(jié)點Nn與其父節(jié)點Nc之間有障礙物，則重新生成隨機點Nr，重復(fù)以上步驟。其節(jié)點擴展方法如圖2 所示。當(dāng)擴展隨機樹中子節(jié)點接近目標(biāo)點Ng,并且兩點之間的距離在設(shè)定領(lǐng)域范圍之內(nèi)，則停止擴展并從目標(biāo)節(jié)點Ng進行回溯，直到起點Ns。然后返回一條從起點Ns到目標(biāo)點Ng的無障礙飛行航路，算法結(jié)束。

圖2 傳統(tǒng)RRT算法節(jié)點擴展方法

RRT 算法的基本思想是像樹一樣快速擴張以探索安全區(qū)域，從而找到可行路徑，搜索節(jié)點的隨機性保證了算法的探索能力和搜索速度，使該算法不受任務(wù)復(fù)雜度與空間復(fù)雜度的影響。此外，由于其實現(xiàn)原理相對簡單，且該算法能夠滿足許多通用情況下的路徑規(guī)劃需求，因此被大量應(yīng)用于機器人路徑規(guī)劃、無人駕駛尋找路徑等領(lǐng)域中。但是，正是由于其節(jié)點選擇的隨機性大，傳統(tǒng)RRT 算法在構(gòu)造隨機樹的過程中會產(chǎn)生大量冗余節(jié)點，當(dāng)空間從二維擴展到三維時，求解過程所需要的存儲空間呈線性上升，還會由于擴展過多無用節(jié)點造成搜索時間的增加。且由于其擴展過程中生長步長的固定，生成的路徑靈活性較低，路徑一般較為曲折，這可能會導(dǎo)致航路的延長。

因此，為了進一步優(yōu)化RRT 算法，文獻[14]提出了RRT*算法，通過最優(yōu)準(zhǔn)則重選父節(jié)點，提高了算法全局搜索的效率；文獻[15]提出了雙向RRT 算法，通過從初始點和目標(biāo)點同時擴展隨機樹來加快搜索速度；文獻[16]提出了DRT-RRT*算法，利用反向生長最優(yōu)快速搜索隨機樹的實時采樣重規(guī)劃算法。

在該文算法應(yīng)用環(huán)境中，無人機執(zhí)行作戰(zhàn)任務(wù)的戰(zhàn)場環(huán)境已知，因此，文中擬綜合改進傳統(tǒng)RRT 算法，在保證其搜索隨機性的同時，提高算法收斂速度，并盡可能提高其生成航路的平滑性。

2.2 引力場引導(dǎo)隨機樹生長

為了減少隨機樹生長過程中產(chǎn)生的冗余節(jié)點，優(yōu)化求解過程存儲空間，降低傳統(tǒng)RRT 算法在搜索過程中的隨機性，文中根據(jù)無人機作戰(zhàn)任務(wù)環(huán)境涉及到的作戰(zhàn)區(qū)域以及對抗要素，參考人工勢場算法的目標(biāo)導(dǎo)向作用，建立了引力場用來引導(dǎo)隨機樹節(jié)點向目標(biāo)的生長，以此來減少RRT 算法航路搜索時間，加快算法收斂速度。

在傳統(tǒng)RRT 算法的基礎(chǔ)上，建立以目標(biāo)為導(dǎo)向的引力場引導(dǎo)隨機樹的擴展，在隨機樹節(jié)點擴展時，對生長節(jié)點Nn(xn,yn)構(gòu)造引力函數(shù)Fg，其公式如下：

其中，Nn(xn,yn)為生長節(jié)點在任務(wù)空間中的位置，xn和yn分別為其橫坐標(biāo)與縱坐標(biāo)，Ng(xg,yg)為目標(biāo)節(jié)點在任務(wù)空間中的位置，kg為引力常數(shù)，r(·)表示兩個節(jié)點在任務(wù)空間中的歐氏距離。每一次節(jié)點的擴展生長除了由隨機采樣點Nr的方向與固定生長步長step_length 決定外，還受到目標(biāo)點對該生長節(jié)點引力的作用，使得擴展節(jié)點在選擇方向時，向著目標(biāo)點的方向快速生長。需要注意的是，為了保持傳統(tǒng)RRT 算法的隨機性，當(dāng)擴展節(jié)點越靠近目標(biāo)，所受目標(biāo)節(jié)點的引力就越小，這同時也是為了減少隨機節(jié)點在朝向目標(biāo)生長時陷入局部最優(yōu)的可能性。

2.3 動態(tài)生長步長策略

傳統(tǒng)RRT 算法生長步長固定，當(dāng)選擇步長較小時，生成無人機航路的時間較長，且由于生長節(jié)點過多且曲折，導(dǎo)致無人機在實際飛行過程中需要頻繁調(diào)整航向，由于其本身的性能限制，無人機在實際飛行中可能無法達到完美契合規(guī)劃路徑的狀態(tài)。因而從理論上來講，固定生長步長越大，生成無人機航路的速度就越快。但是，當(dāng)固定生長步長過大時，無人機遇到障礙物后需要花費更多的時間來避開，反而可能造成搜索速度的下降、航路的延長，且由于步長過大，生長節(jié)點在到達目標(biāo)點附近時，可能會反復(fù)震蕩，無法接近目標(biāo)，這不符合無人機在實際環(huán)境中執(zhí)行作戰(zhàn)任務(wù)的通常規(guī)則[17-18]。

因此，該文提出了動態(tài)生長步長策略，該策略的基本思想是通過判斷隨機樹上新節(jié)點與障礙物之間的距離動態(tài)確定生長步長，當(dāng)節(jié)點生長過程中遇到障礙物時，根據(jù)無人機實際飛行中能夠達到的最小轉(zhuǎn)向距離確定新的步長；無人機繞過障礙物后，可以使步長適當(dāng)增加，加快在安全區(qū)域中生成航路的速度，減少搜索時間。對于節(jié)點生長步長ρnode的調(diào)整如下：

其中，ρfixed為固定生長步長，ρmin是根據(jù)無人機性能設(shè)定的最小轉(zhuǎn)向距離，Dnode是生長節(jié)點與最近障礙物之間的距離。當(dāng)新的節(jié)點Nn與其父節(jié)點Nc之間有障礙物時，需要較為謹慎的避開障礙物，因此，根據(jù)ρmin小范圍調(diào)整前進距離與方向使其靈活避開障礙物。繞過障礙物后，在安全區(qū)域按照固定生長步長大幅度調(diào)整前進距離。該動態(tài)生長步長選擇策略不僅可以使無人機靈活避開障礙物，增加了算法生成航路的平滑性，也在一定程度上提高了航路生成速度。

2.4 算法流程設(shè)計

在綜合考慮引力場引導(dǎo)隨機樹生長和動態(tài)生長步長策略后，算法的具體流程如下：

步驟1：初始化任務(wù)空間地圖，生成起點Ns、目標(biāo)點Ng，根據(jù)無人機性能確定ρmin，根據(jù)任務(wù)空間地圖區(qū)域大小確定ρfixed，同時將生長步長ρnode設(shè)定為ρfixed，將起點Ns存入隨機樹節(jié)點列表node_list中。

步驟2：計算node_list 中新加入的節(jié)點Nn與目標(biāo)節(jié)點Ng的距離r，當(dāng)該距離大于生長步長ρnode時，進入步驟3，否則，確定為成功找到路徑，從目標(biāo)點Ng回溯節(jié)點直到起點Ns生成路徑，結(jié)束程序并繪圖。

步驟3：在任務(wù)空間中隨機選取節(jié)點Nr，在節(jié)點列表node_list 中找到距離Nr最近的節(jié)點Nc，以Nc與隨機節(jié)點Nr的連線作為擴展方向，以生長步長ρnode為距離，以最近的節(jié)點Nc為父節(jié)點，生成一個新節(jié)點Nn。

步驟4：判斷新節(jié)點Nn與其父節(jié)點Nc之間有無障礙物，若有，確定生長步長ρnode為ρmin，然后返回步驟3，否則確定生長步長ρnode為ρfixed，執(zhí)行步驟5。

步驟5：判斷Nn的引力值，若Nn的引力值大于其父節(jié)點Nc的引力值，則返回步驟3，否則，將該新節(jié)點加入到node_list 中作為子節(jié)點，進入步驟2。

算法具體流程如圖3 所示。

圖3 綜合改進算法流程圖

3 實驗結(jié)果對比分析

為了驗證綜合改進后的RRT 算法在上述無人機航路規(guī)劃場景中的效果，該算法在Matlab R2016a 中進行仿真驗證，實驗計算機為聯(lián)想筆記本電腦，處理器為Core(TM) i7-1065G7，頻率為1.30 GHz，內(nèi)存為16 GB。在仿真驗證中，根據(jù)無人機飛行性能設(shè)定ρmin為0.2 km，根據(jù)任務(wù)空間地圖區(qū)域大小以及障礙物確定ρfixed為0.5 km，起點Ns(0,0) km、目標(biāo)點Ng(9.5,9.5)km。同時，考慮到RRT 算法的隨機性，單次或者少數(shù)實驗不能保證實驗結(jié)果的準(zhǔn)確性，因此，在相同場景下進行100 次規(guī)劃航路實驗，取運算結(jié)果平均值作為實驗結(jié)果。

對傳統(tǒng)RRT 算法、加入引力場引導(dǎo)隨機樹的RRT 算法、動態(tài)生長步長策略的RRT 算法、綜合改進RRT 算法進行比較，四種算法在比較過程中應(yīng)保持初始參數(shù)一致，圖4（a）、（b）、（c）、（d）四幅圖分別展示了四種算法在任務(wù)空間中擴展的隨機樹。從圖4可以看出，加入引力場的算法在隨機樹擴展過程中更加具有目標(biāo)性和方向性，其擴展出隨機樹的樹枝相較于傳統(tǒng)算法有明顯的減少。這表明，引力場引導(dǎo)隨機樹可以有效地減少隨機樹分支、冗余節(jié)點的產(chǎn)生，而加入動態(tài)生長步長策略的算法生成的無人機航路相較于傳統(tǒng)RRT 算法路徑更加平滑，比較符合無人機在實際飛行過程中的狀態(tài)。綜合改進的RRT 算法集成了上述兩種算法的優(yōu)勢，其擴展樹相較于其他三種算法，不僅在搜索節(jié)點上具有目標(biāo)性，擴展樹分支較少，且形成的路徑更加平滑。

圖4 算法搜索路徑對比圖

表1 為傳統(tǒng)RRT 算法、加入引力場引導(dǎo)隨機樹的RRT 算法、動態(tài)生長步長策略的RRT 算法以及綜合改進的RRT 算法四種算法的實驗仿真數(shù)據(jù)。從表中數(shù)據(jù)可以看出，傳統(tǒng)RRT 算法在擴展過程中搜索的節(jié)點較多，航路較長，且產(chǎn)生航路的時間普遍偏長。由引力場引導(dǎo)的RRT 算法相較于傳統(tǒng)RRT 算法在搜索時間上節(jié)省了63.4%，路徑節(jié)點減少了6.7%，這表明，引力場引導(dǎo)隨機樹節(jié)點的生長能夠有效縮短搜索航路的時間。加入動態(tài)生長步長策略的RRT 算法相較于傳統(tǒng)RRT 算法在搜索時間上節(jié)省了80.1%，路徑節(jié)點減少了57.3%，這表明動態(tài)調(diào)整生長步長可以明顯減少路徑節(jié)點的數(shù)量，并且加快算法收斂速率。綜合改進的RRT 算法，較傳統(tǒng)RRT 算法的優(yōu)勢更加明顯，不僅在搜索時間上節(jié)省了86.5%，且路徑節(jié)點減少幅度更大，為58.4%。

表1 算法仿真測試結(jié)果

實驗數(shù)據(jù)充分證明，綜合改進的RRT 算法，相較于傳統(tǒng)RRT 算法，擴展節(jié)點數(shù)量少、收斂速度快、時間效率高、路徑更平滑、避障更加靈活，在整體性能上優(yōu)于傳統(tǒng)RRT 算法，更加符合無人機實際飛行情況，有利于無人機在作戰(zhàn)任務(wù)中更好地發(fā)揮性能。

4 結(jié)束語

該文提出了一種綜合改進的RRT 算法，針對無人機在實際戰(zhàn)場環(huán)境中執(zhí)行作戰(zhàn)任務(wù)的飛行航路生成問題展開研究。針對傳統(tǒng)RRT 算法隨機性強、生長步長固定等問題，將引力場引導(dǎo)隨機樹擴展和動態(tài)生長步長選擇兩種策略結(jié)合起來融入傳統(tǒng)RRT 算法的改進中，既保證了RRT 算法具有一定的隨機性，又使RRT 算法在收斂速度、搜索時間、計算效率與路徑平滑度等方面擁有更優(yōu)的性能。同時，有效利用了戰(zhàn)場環(huán)境、無人機性能等已知條件，實現(xiàn)了無人機在任務(wù)空間中快速生成有效航路和避障的能力。最后，通過仿真實驗結(jié)果驗證了改進的RRT 算法具有更佳的效果。

在下一步的研究中，會將該算法擴展應(yīng)用于戰(zhàn)場環(huán)境更加復(fù)雜的實際地理空間中，同時考慮戰(zhàn)場環(huán)境態(tài)勢的動態(tài)變化，以及更為復(fù)雜的對抗要素，根據(jù)給定的模型初始參數(shù)動態(tài)生成自適應(yīng)的無人機優(yōu)選航路信息，進一步改進優(yōu)化無人機作戰(zhàn)航路規(guī)劃算法。