基于RRT算法的某型無(wú)人機(jī)航路在線規(guī)劃設(shè)計(jì)

路 引，郭昱津，王道波

(1.南京航空航天大學(xué) 中小型無(wú)人機(jī)先進(jìn)技術(shù)工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室， 南京 210016；2.中電集團(tuán)第二十八研究所，南京 210007；3.南京航空航天大學(xué) 自動(dòng)化學(xué)院，南京 210016)

【裝備理論與裝備技術(shù)】

基于RRT算法的某型無(wú)人機(jī)航路在線規(guī)劃設(shè)計(jì)

路 引1,3，郭昱津2，王道波3

(1.南京航空航天大學(xué) 中小型無(wú)人機(jī)先進(jìn)技術(shù)工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室， 南京 210016；2.中電集團(tuán)第二十八研究所，南京 210007；3.南京航空航天大學(xué) 自動(dòng)化學(xué)院，南京 210016)

針對(duì)高原地區(qū)復(fù)雜地形地貌，提出了無(wú)人機(jī)需具備在線航路規(guī)劃能力；分析了快速隨機(jī)搜索樹(shù)(RRT)算法的基本原理，并對(duì)RRT算法的收斂性和參數(shù)選擇進(jìn)行了探討，設(shè)計(jì)了基于RRT算法的在線航路規(guī)劃方法；對(duì)所設(shè)計(jì)的方法進(jìn)行仿真驗(yàn)證。仿真結(jié)果表明：該航路在線規(guī)劃方法達(dá)到了預(yù)期目的，能夠很好的規(guī)避突發(fā)障礙。

無(wú)人機(jī)；航路規(guī)劃；快速隨機(jī)搜索樹(shù)

高原地區(qū)地形地貌復(fù)雜，群山連綿，無(wú)人機(jī)按照預(yù)設(shè)的航線飛行，可能會(huì)遇到山等障礙物，對(duì)無(wú)人機(jī)飛行造成很大的威脅。無(wú)人機(jī)在復(fù)雜的應(yīng)用環(huán)境中執(zhí)行飛行任務(wù)，外界環(huán)境突發(fā)變化，無(wú)人機(jī)很難預(yù)先獲得飛行過(guò)程中的全部環(huán)境信息，突發(fā)威脅僅當(dāng)無(wú)人機(jī)抵達(dá)其附近一定區(qū)域后才能發(fā)現(xiàn)，在無(wú)人機(jī)的飛行過(guò)程中，改變無(wú)人機(jī)的飛行任務(wù)，這都需要無(wú)人機(jī)具備在線的航路規(guī)劃能力，在滿足飛行條件限制的前提下避開(kāi)當(dāng)前的危險(xiǎn)?？紤]實(shí)時(shí)性、可行性等需求，所研制的某型無(wú)人機(jī)采用快速隨機(jī)搜索樹(shù)(RRT)算法作為無(wú)人機(jī)的復(fù)雜航路在線規(guī)劃方法[1-4]。

1 RRT算法

快速隨機(jī)搜索樹(shù)(RRT)算法是一種隨機(jī)性、實(shí)時(shí)性好、速度快的算法，該算法能夠在威脅、障礙物復(fù)雜的情況下快速找到一條路徑。RTT搜索能力強(qiáng)，其獨(dú)特優(yōu)點(diǎn)是經(jīng)過(guò)擴(kuò)展后，可以直接應(yīng)用到動(dòng)力學(xué)規(guī)劃。此算法采用一種能逐步迅速縮短隨機(jī)狀態(tài)與期望狀態(tài)點(diǎn)之間距離的特殊增量方式進(jìn)行構(gòu)造[5]，考慮高原地區(qū)復(fù)雜的地形地貌條件，采用RRT算法進(jìn)行航路規(guī)劃能快速找到一條規(guī)避突發(fā)威脅的航路。

如圖1所示，無(wú)人機(jī)按照預(yù)設(shè)航路從A點(diǎn)飛向G點(diǎn)，在飛行過(guò)程中，遇到突發(fā)威脅，需要在線規(guī)劃航路，改變飛行路徑，保證飛行安全。

圖1 無(wú)人機(jī)飛行中遇威脅示意圖

2 RRT算法的基本原理

假設(shè)無(wú)人機(jī)飛行區(qū)域?yàn)镽，Rfree為無(wú)障礙區(qū)域，Robs為障礙區(qū)域，Rfree是Robs的補(bǔ)集，兩個(gè)同為R的子集，且Rfree∪Robs=R。Rinit為起點(diǎn)，Rgoal為終點(diǎn)，且滿足Rinit和Rgoal都為Rfree的子集。在飛行區(qū)域R中尋找一條從Rinit到Rgoal的可行路徑。

RRT 算法是通過(guò)逐步迭代的增量方式構(gòu)建隨機(jī)樹(shù)的。圖2為RRT算法的節(jié)點(diǎn)擴(kuò)展過(guò)程。首先RRT算法中樹(shù)的根節(jié)點(diǎn)是在任務(wù)區(qū)域內(nèi)選定的起始節(jié)點(diǎn)Rinit，所生成的隨機(jī)樹(shù)是通過(guò)從根節(jié)點(diǎn)連續(xù)擴(kuò)展出葉節(jié)點(diǎn)方式構(gòu)建的。以概率rg選擇目標(biāo)位置Rgoal為隨機(jī)目標(biāo)Rran，臨近節(jié)點(diǎn)Rnear是在隨機(jī)樹(shù)的葉節(jié)點(diǎn)里離Rran最近的節(jié)點(diǎn)。從Rnear向Rran的方向延伸一個(gè)步長(zhǎng)的距離ε，得到一個(gè)新的節(jié)點(diǎn)Rnew。在延伸過(guò)程中，判斷是否與威脅區(qū)域有沖突，若Rnew與威脅區(qū)域有沖突，則舍棄該擴(kuò)展的新節(jié)點(diǎn)，并重新選取隨機(jī)目標(biāo)點(diǎn)Rran，若Rnew與威脅區(qū)域無(wú)沖突，則接受該擴(kuò)展的新節(jié)點(diǎn)Rnew，并將其新節(jié)點(diǎn)Rnew添加為隨機(jī)樹(shù)的節(jié)點(diǎn)。依此通過(guò)這樣連續(xù)的擴(kuò)展，直至隨機(jī)樹(shù)中的葉節(jié)點(diǎn)與目標(biāo)點(diǎn)Rgoal足夠近的時(shí)候，認(rèn)為隨機(jī)樹(shù)的構(gòu)建工作完成，從構(gòu)建的隨機(jī)樹(shù)中，尋找從起始點(diǎn)Rinit到最終目標(biāo)點(diǎn)Rgoal的路徑，可以獲得一條從起始位置到目標(biāo)位置的可行路徑[6]。

圖2 RRT算法的節(jié)點(diǎn)擴(kuò)展

3 RRT 算法收斂性分析

RRT 算法具有很強(qiáng)的路徑規(guī)劃能力，下面從理論上對(duì)RRT能夠從起始位置擴(kuò)展到目標(biāo)位置的概率完備性進(jìn)行分析[7]。令Dk(R*)是一個(gè)隨機(jī)變量，表示位置R*與RRT隨機(jī)樹(shù)T上的最近節(jié)點(diǎn)之間的距離，dk表示隨機(jī)變量的取值，k表示RRT的節(jié)點(diǎn)數(shù)，ε是RRT算法的擴(kuò)展步長(zhǎng)。

定理1Rfree為n維非凸有界開(kāi)集，起始位置R0和目標(biāo)位置R*位于Rfree的同一個(gè)連通區(qū)內(nèi)，隨著RRT節(jié)點(diǎn)足夠增大，則RRT獲得從R0到R*路徑的概率為1，即

證明：R0與R*位于Rfree內(nèi)的同一連通區(qū)內(nèi)，那么在Rfree內(nèi)存在一個(gè)節(jié)點(diǎn)序列R0,R1,…Rm，以及相應(yīng)的超球體B(R1),B(R2),…B(Rm)，使得R0∈B1(R1)和R*∈Bm(Rm)，并且同時(shí)滿足：

Bi(Ri)∩Bi+1(Ri+1)≠?，i=1,2,…,m-1

令Ci=Bi∩Bi+1，對(duì)B(Ri)的構(gòu)造，可以使得Ci為開(kāi)集，測(cè)度μ(Ci)>0。

4 RRT參數(shù)研究

與其他的路徑規(guī)劃方法相比，RRT算法的一個(gè)顯著優(yōu)點(diǎn)是該算法需要設(shè)置的參數(shù)較少，主要包括隨機(jī)點(diǎn)Rran和擴(kuò)展步長(zhǎng)ε等。簡(jiǎn)單的結(jié)構(gòu)有利于RRT算法的應(yīng)用推廣，但目前關(guān)于算法參數(shù)的選取缺少嚴(yán)格的理論依據(jù)，在實(shí)際應(yīng)用中一般根據(jù)經(jīng)驗(yàn)試湊獲得，不能保證算法參數(shù)的最優(yōu)性。此處主要探討參數(shù)擴(kuò)展步長(zhǎng)ε對(duì)RRT算法的影響。

在Rran與隨機(jī)樹(shù)所有節(jié)點(diǎn)計(jì)算距離之后，得到與之最近的節(jié)點(diǎn)Rnear，然后沿著Rnear與Rran的方向，從Rnear開(kāi)始延伸一個(gè)步長(zhǎng)的距離ε。較大的步長(zhǎng)有利于減少路徑所需要的節(jié)點(diǎn)數(shù)，但也增加了節(jié)點(diǎn)擴(kuò)展過(guò)程中遇到威脅區(qū)域的概率，被迫重新進(jìn)行隨機(jī)節(jié)點(diǎn)的選擇，反而降低了優(yōu)化效率；較小的步長(zhǎng)能夠增大從Rnear到Rnew擴(kuò)展的成功率，但需要很多的步數(shù)才能到達(dá)目標(biāo)位置。在簡(jiǎn)單威脅環(huán)境下，可使用較大的步長(zhǎng)ε，以加快規(guī)劃的速度，且所得路徑較短；在復(fù)雜威脅環(huán)境下，過(guò)大的步長(zhǎng)會(huì)限制RRT算法的擴(kuò)展能力，導(dǎo)致規(guī)劃成功率降低，且所得路徑的長(zhǎng)度較大，不符合最優(yōu)性要求，此時(shí)可以采用較小的步長(zhǎng)。綜合來(lái)看，RRT 算法中的參數(shù)ε選取至關(guān)重要，直接影響著規(guī)劃問(wèn)題的求解優(yōu)劣和成功率，并且對(duì)任務(wù)環(huán)境有一定的依賴性，可以根據(jù)實(shí)際環(huán)境情況，進(jìn)行此參數(shù)的選擇。

5 RRT算法的在線實(shí)時(shí)規(guī)劃

無(wú)人機(jī)按照預(yù)設(shè)的航線飛行，一邊飛行，一邊探測(cè)附近區(qū)域是否有突發(fā)威脅信息，一旦探測(cè)到新威脅，立即更新威脅信息表。如果突發(fā)威脅與預(yù)設(shè)航路有交點(diǎn)，則啟動(dòng)航路在線規(guī)劃，將航路前面一定距離的節(jié)點(diǎn)作為新起始點(diǎn)，利用RRT算法在線重新規(guī)劃新航路[8]。無(wú)人機(jī)按照重新規(guī)劃的新航路飛行到指定航點(diǎn)，為了減少計(jì)算量，需確定合適的規(guī)劃空間。無(wú)人機(jī)在正常飛行過(guò)程中，遇到突發(fā)威脅時(shí)，航路在線規(guī)劃流程如圖3所示。

圖3 RRT算法在線規(guī)劃流程

利用RRT算法進(jìn)行無(wú)人機(jī)在線航路規(guī)劃算法如下：

1) 無(wú)人機(jī)按照預(yù)設(shè)航線飛行；

2) 若無(wú)人機(jī)到達(dá)目標(biāo)點(diǎn)，則算法終止，飛行結(jié)束；否則，進(jìn)入第3步；

3) 無(wú)人機(jī)沿預(yù)設(shè)航路飛行，同時(shí)探測(cè)附近是否有威脅信息；

4) 若無(wú)人機(jī)探測(cè)到有突發(fā)威脅信息，進(jìn)入第5步；否則，轉(zhuǎn)第2步；

5) 利用RRT算法在線重新規(guī)劃航路，更新飛行航路；

6) 轉(zhuǎn)第2步。

6 RRT算法的仿真驗(yàn)證

選取無(wú)人機(jī)任務(wù)區(qū)域?yàn)?00 km×100 km的矩形地形區(qū)域，隨機(jī)生成100個(gè)障礙威脅，起始位置的坐標(biāo)為(0,0)，目標(biāo)位置坐標(biāo)為(100,100)。設(shè)置RRT算法的參數(shù)為rg=0.5，步長(zhǎng)ε=5。首先進(jìn)行離線情況下的航路預(yù)規(guī)劃，所得RRT隨機(jī)樹(shù)和飛行路徑如圖4所示。離線航路長(zhǎng)度為182.06 km，程序運(yùn)行時(shí)間為4.186 1 s。

當(dāng)無(wú)人機(jī)飛行至圖5中的A點(diǎn)位置，任務(wù)環(huán)境中出現(xiàn)突發(fā)威脅區(qū)域，如圖5所示，由該圖可知，新出現(xiàn)的突發(fā)威脅與無(wú)人機(jī)的待飛航路由重合，因此無(wú)人機(jī)此時(shí)不能再按照原有路徑飛行，必須進(jìn)行航路重規(guī)劃。此時(shí)進(jìn)行數(shù)字地圖更新，并以無(wú)人機(jī)當(dāng)前位置為起點(diǎn)，重新調(diào)用RRT算法進(jìn)行航路重規(guī)劃，所得結(jié)果如圖6所示。

航路重規(guī)劃的運(yùn)行時(shí)間為1.781 5 s，耗時(shí)較少，能夠滿足無(wú)人機(jī)航路重規(guī)劃的實(shí)時(shí)性要求。新的航路長(zhǎng)度為118.38 km。

圖4 無(wú)人機(jī)離線航路預(yù)規(guī)劃路徑

圖5 無(wú)人機(jī)飛行中突發(fā)威脅

圖6 無(wú)人機(jī)航路重規(guī)劃路徑

7 結(jié)束語(yǔ)

無(wú)人機(jī)的航路規(guī)劃是無(wú)人機(jī)飛行控制的重要組成部分，根據(jù)某型無(wú)人機(jī)的飛行控制研制需求，為了適應(yīng)復(fù)雜地形地貌高原地區(qū)的飛行要求，分析了快速隨機(jī)搜索樹(shù)(RRT)算法的基本原理，并對(duì)RRT算法的收斂性和參數(shù)選擇進(jìn)行了探討，設(shè)計(jì)了基于快速隨機(jī)搜索樹(shù)(RRT)算法作為無(wú)人機(jī)的航路在線規(guī)劃方法，仿真結(jié)果表明該航路在線規(guī)劃方法達(dá)到了預(yù)期目的，能夠很好的規(guī)避突發(fā)障礙。

[1] 楊力.無(wú)人機(jī)航路規(guī)劃技術(shù)研究[D].南京航空航天大學(xué)碩士論文,2009.

[2] MARTIN S R,WRIGHT S E,SHEPPARD J W.Offline and Online Evolutionary Bi-Directional RRT Algorithms for Efficient Re-Planning in Dynamic Environments[C]//Proceedings of the 3rd Annual IEEE Conference on Automation Science and Engineering,Scottsdale,2007:1131-1136.

[3] JAMES J,STEVEN M.An Efficient Approach to Single-Query Path Planning[C]//Proceedings of the 2000 IEEE International Conference on Robotics and Automation,2000,995-1002.

[4] 彭輝，王林，沈林成.區(qū)域目標(biāo)搜索中基于改進(jìn) RRT 的 UAV 實(shí)時(shí)航跡規(guī)劃[J].國(guó)防科技大學(xué)學(xué)報(bào),2009,31(5):86-91.

[5] 李猛.基于智能優(yōu)化與RRT算法的無(wú)人機(jī)任務(wù)規(guī)劃方法研究[D].南京：南京航空航天大學(xué),2012.

[6] LAVALLE S M.Planning Algorithms[M].Cambridge：Cambridge University Press,2006:229-243.

[7] LAVALLE S M,KUFFNER J J.DONALD B R,et al.Rapidly-Exploring Random Trees[J].Progress and Prospects.Algorithmic and Computational Robotics:New Directions,Wellesley,2001:293-308.

[8] PENG H,SU F,BU Y L,et al.Cooperative Area Search for Multiple UAVs Based RRT and Decentralized Receding Horizon Optimization[C]//Proceedings of the 7th Asian Control Conference,Hong Kong,2009:298-303.

[9] 李子杰，劉湘?zhèn)?基于進(jìn)化算法的多無(wú)人機(jī)協(xié)同航路規(guī)劃[J].火力與指揮控制，2015(2):85-89.

[10]沈培志，張邦鈺，聶奇剛，等.基于蟻群算法的無(wú)人機(jī)航路規(guī)劃輔助決策研究[J].四川兵工學(xué)報(bào)，2015(8):145-148.

(責(zé)任編輯周江川)

Design on Route Online Planning for a Certain UAV Based on RRT Algorithm

LU Yin1,3, GUO Yu-jin2, WANG Dao-bo3

(1.Key Laboratory of Unmanned Aerial Vehicle Technology, Nanjing University of Aeronautics and Astronautics, Ministry of Industry and Information Technology, Nanjing 210016, China; 2.The 28thResearch Institute of China Electronics Technology Group Corporation, Nanjing 210007, China; 3.College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China)

Route online planning function of the UAV was putted forward in view of the complex plateau topography. Rapidly-exploring Random Tree(RRT) algorithm was analyzed and the convergence of RRT algorithm and parameter selection were discussed. RRT algorithm, as the method of route online planning, was designed. The method was applied to the route online planning simulation. And the simulation results show that the design of the method has reached the expected purpose and can avoid unexpected obstacles very well.

UAV; route planning; rapidly-exploring random tree

2016-07-25；

高空長(zhǎng)航時(shí)無(wú)人直升機(jī)保性能飛行控制與旋翼轉(zhuǎn)速優(yōu)化策略研究(61374188)

路引(1988—)，男，博士，主要從事無(wú)人機(jī)飛行力學(xué)與飛行控制研究。

10.11809/scbgxb2016.12.004

路引，郭昱津，王道波.基于RRT算法的某型無(wú)人機(jī)航路在線規(guī)劃設(shè)計(jì)[J].兵器裝備工程學(xué)報(bào)，2016(12):18-21.

format:LU Yin, GUO Yu-jin, WANG Dao-bo.Design on Route Online Planning for a Certain UAV Based on RRT Algorithm[J].Journal of Ordnance Equipment Engineering,2016(12):18-21.

V279+.2

A

2096-2304(2016)12-0018-04

修回日期：2016-08-25