成浩浩，楊 森，齊曉慧

(陸軍工程大學(xué)石家莊校區(qū) 無(wú)人機(jī)工程系，河北 石家莊 050003)

0 引 言

由于低空飛行環(huán)境復(fù)雜，四旋翼無(wú)人機(jī)飛行時(shí)常受到障礙物的影響，當(dāng)飛行航線上突然出現(xiàn)障礙物時(shí)，其要具備對(duì)飛行路線進(jìn)行重規(guī)劃的能力[1]。在航跡規(guī)劃方面很多學(xué)者提出了不同的算法，如人工勢(shì)場(chǎng)法[2,3]、A*算法[4,5]、概率路線圖法(probabilistic roadmap method，PRM)[6]、蟻群算法[7]、遺傳算法[8-10]等。RRT算法是S.M.LaValle[11]提出來(lái)的基于采樣的增量式搜索[12]算法。該算法不需要建立空間信息模型，以隨機(jī)采樣的方式在任務(wù)空間中均勻采樣，因此具有概率完備性，相較其它算法更適合高維空間中的四旋翼無(wú)人機(jī)航跡規(guī)劃研究。

然而，RRT算法存在全局隨機(jī)搜索，運(yùn)算量大，實(shí)時(shí)性較差，航跡由隨機(jī)點(diǎn)組成，通常不是最優(yōu)航跡[13]，規(guī)劃航跡不平滑的缺點(diǎn)。為此，不少學(xué)者進(jìn)行了改進(jìn)：針對(duì)RRT算法隨機(jī)搜索、實(shí)時(shí)性較差的缺點(diǎn)提出了引導(dǎo)策略，針對(duì)路徑不平滑的特點(diǎn)提出動(dòng)態(tài)步長(zhǎng)策略或者航平滑優(yōu)化策略。這些改進(jìn)方法大都是單一的，效果受到了一定程度的影響。本文從3個(gè)方面綜合考慮，提出了3種優(yōu)化策略：?jiǎn)l(fā)式搜索策略、動(dòng)態(tài)步長(zhǎng)策略和雙層平滑度優(yōu)化策略，并將其應(yīng)用于四旋翼無(wú)人機(jī)兩種類(lèi)型的動(dòng)態(tài)航跡規(guī)劃中，在二維和三維環(huán)境下進(jìn)行仿真分析，檢驗(yàn)這種綜合改進(jìn)型算法實(shí)時(shí)性和航跡平滑度等性能。

1 傳統(tǒng)RRT算法簡(jiǎn)介

RRT算法包括隨機(jī)樹(shù)的生長(zhǎng)和航跡反向搜索兩個(gè)階段。在隨機(jī)樹(shù)生長(zhǎng)階段，以出發(fā)點(diǎn)Pstart為樹(shù)的根節(jié)點(diǎn)，在任務(wù)空間隨機(jī)生成一個(gè)隨機(jī)點(diǎn)Prand，找出樹(shù)中與隨機(jī)點(diǎn)Prand最近的葉節(jié)點(diǎn)Pnear，然后從Pnear出發(fā)，在與Prand的連線上選擇一個(gè)步長(zhǎng)ε找到點(diǎn)Pnew，檢查Pnew是否在障礙物區(qū)域內(nèi)，Pnear與Pnew的連線上是否有障礙，如果有，則重新選擇隨機(jī)點(diǎn)Prand，否則將Pnew接入樹(shù)中形成新的葉節(jié)點(diǎn)，隨機(jī)樹(shù)不斷生長(zhǎng)，直到葉節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)或者到目標(biāo)點(diǎn)的距離小于一個(gè)設(shè)定的范圍，隨機(jī)樹(shù)生長(zhǎng)過(guò)程結(jié)束；在航跡反向搜索階段，形成從目標(biāo)點(diǎn)到出發(fā)點(diǎn)的貫通航跡，規(guī)劃結(jié)束。隨機(jī)樹(shù)的生長(zhǎng)過(guò)程如圖1所示。

圖1 隨機(jī)樹(shù)生長(zhǎng)過(guò)程

傳統(tǒng)RRT算法遍歷搜索所有空間，不會(huì)存在規(guī)劃失敗的情況，但是這種隨機(jī)的遍歷搜索會(huì)使隨機(jī)樹(shù)節(jié)點(diǎn)產(chǎn)生大量的冗余，算法實(shí)時(shí)性降低，規(guī)劃航跡曲折。

鑒于RRT算法實(shí)現(xiàn)分為隨機(jī)樹(shù)生長(zhǎng)的引導(dǎo)和規(guī)劃航跡的平滑優(yōu)化兩個(gè)階段，本文從引導(dǎo)策略、步長(zhǎng)選擇、航跡優(yōu)化方法3個(gè)方面進(jìn)行綜合考慮。

2 綜合改進(jìn)的RRT算法

傳統(tǒng)RRT算法在隨機(jī)樹(shù)生長(zhǎng)過(guò)程中會(huì)出現(xiàn)很多的冗余，浪費(fèi)了規(guī)劃時(shí)間，降低算法的實(shí)時(shí)性，并且生成的航跡較曲折，導(dǎo)致航跡的跟隨性降低。如果引進(jìn)基于概率引導(dǎo)的啟發(fā)式搜索策略，指引隨機(jī)樹(shù)朝目標(biāo)點(diǎn)的方向生長(zhǎng)，將會(huì)在一定程度地減少規(guī)劃時(shí)間，提高算法實(shí)時(shí)性；動(dòng)態(tài)步長(zhǎng)策略，側(cè)重于解決算法實(shí)時(shí)性和平滑度之間的矛盾，即提高算法實(shí)時(shí)性又使規(guī)劃的航跡盡量平滑；而雙層平滑度優(yōu)化策略的引入，則對(duì)航跡的平滑度進(jìn)行優(yōu)化，規(guī)劃出從起始點(diǎn)到目標(biāo)點(diǎn)的較優(yōu)路徑。

綜合改進(jìn)RRT算法流程如圖2所示：首先，利用啟發(fā)式搜索策略進(jìn)行空間搜索，找到下一個(gè)待擴(kuò)展的葉節(jié)點(diǎn)；然后利用動(dòng)態(tài)步長(zhǎng)策略實(shí)現(xiàn)隨機(jī)樹(shù)擴(kuò)展生長(zhǎng)；最后找到可行航跡后通過(guò)雙層平滑優(yōu)化策略進(jìn)行航跡平滑優(yōu)化，規(guī)劃出適合四旋翼無(wú)人機(jī)飛行的可行航跡。

圖2 綜合改進(jìn)RRT算法流程

2.1 基于概率引導(dǎo)的啟發(fā)式搜索策略

傳統(tǒng)RRT算法在任務(wù)空間隨機(jī)產(chǎn)生Prand，這雖然有利于算法探索未知區(qū)域，但是搜索的點(diǎn)會(huì)產(chǎn)生很多冗余，增加算法運(yùn)行時(shí)間，降低算法實(shí)時(shí)性。文獻(xiàn)[12]提出了雙層啟發(fā)式優(yōu)化策略，即面向目標(biāo)的啟發(fā)式搜索策略和面向可跟蹤性的航跡平滑優(yōu)化策略。在面向目標(biāo)的啟發(fā)式搜索策略中，提出了啟發(fā)式采樣策略和新節(jié)點(diǎn)啟發(fā)式擴(kuò)展策略。面向目標(biāo)的啟發(fā)式采樣策略是在隨機(jī)點(diǎn)的選取上，讓其以一定的概率θ(0<θ<1)選目標(biāo)點(diǎn)為隨機(jī)點(diǎn)；新節(jié)點(diǎn)啟發(fā)式擴(kuò)展策略是在新節(jié)點(diǎn)的選取上，讓新節(jié)點(diǎn)生成的方向以一定的概率τ(0<τ<1)設(shè)定為目標(biāo)點(diǎn)方向。這種優(yōu)化策略在障礙物較少時(shí)可以增強(qiáng)隨機(jī)樹(shù)向目標(biāo)點(diǎn)生長(zhǎng)的趨勢(shì)，使搜索更具有指向性，減少大量的冗余節(jié)點(diǎn)，增強(qiáng)算法實(shí)時(shí)性。但是在障礙物較多時(shí)，這種優(yōu)化策略由于喪失了部分選擇性，反而導(dǎo)致搜索效率較低；同時(shí)兩步的優(yōu)化會(huì)耗費(fèi)更多的時(shí)間。為此本文借鑒了面向目標(biāo)的啟發(fā)式采樣策略，提出了基于概率引導(dǎo)的啟發(fā)式搜索策略，不對(duì)新節(jié)點(diǎn)的選取進(jìn)行引導(dǎo)，通過(guò)調(diào)整θ來(lái)達(dá)到指向目標(biāo)的目的。這種改進(jìn)在不失算法指向性的前提下，可以提高算法的收斂速度，為后面的航跡平滑度優(yōu)化節(jié)省出一定的時(shí)間。

2.2 動(dòng)態(tài)步長(zhǎng)策略

傳統(tǒng)的RRT算法步長(zhǎng)是固定的，當(dāng)選用大步長(zhǎng)時(shí)，算法能夠很快地收斂到目標(biāo)點(diǎn)，但是航跡平滑度很差；當(dāng)選用小步長(zhǎng)時(shí)，航跡的平滑度得到改善但是卻是以犧牲算法實(shí)時(shí)性為代價(jià)的。為了解決算法實(shí)時(shí)性與平滑度之間的矛盾，文獻(xiàn)[14]提出了動(dòng)態(tài)步長(zhǎng)策略，加入了目標(biāo)引力思想，新節(jié)點(diǎn)的計(jì)算公式如下

(1)

其中，ρ1是隨機(jī)點(diǎn)方向步長(zhǎng)，ρ2是目標(biāo)點(diǎn)方向步長(zhǎng)。通過(guò)調(diào)整ρ1和ρ2的權(quán)值達(dá)到改變步長(zhǎng)的目的。這種動(dòng)態(tài)步長(zhǎng)的優(yōu)化達(dá)到了在障礙物密集區(qū)域改變步長(zhǎng)的目的，并且可以使新節(jié)點(diǎn)朝著目標(biāo)點(diǎn)方向生長(zhǎng)。但是到達(dá)閾值后ρ1和ρ2的權(quán)值固定，不能隨著與障礙物距離的遠(yuǎn)近而變化。

受文獻(xiàn)[14]的啟示，本文提出了新的動(dòng)態(tài)步長(zhǎng)策略，步長(zhǎng)選取函數(shù)為

εnew=(keαd-0.8)·ε

(2)

其中，ε是原始步長(zhǎng)，εnew是障礙物附近的步長(zhǎng)，d是算法葉節(jié)點(diǎn)當(dāng)前位置與障礙物之間的距離，α是衰減系數(shù)，k是調(diào)節(jié)因子，α和k的取值與閾值的選取有關(guān)，要確保到達(dá)閾值時(shí)εnew≤ε。

新動(dòng)態(tài)步長(zhǎng)策略的思想是：在隨機(jī)樹(shù)離障礙物較遠(yuǎn)、沒(méi)有到達(dá)閾值時(shí)采用原先步長(zhǎng)ε，加快算法收斂速度；隨機(jī)樹(shù)生長(zhǎng)到障礙物附近、達(dá)到閾值時(shí)采用小步長(zhǎng)εnew，確保航跡平滑度。這種改進(jìn)雖然不能使新節(jié)點(diǎn)朝著目標(biāo)點(diǎn)選取，但是可以使步長(zhǎng)隨著障礙物之間的距離而變化，更好地解決實(shí)時(shí)性與平滑度之間的矛盾。算法的指向性可以通過(guò)調(diào)整第一種改進(jìn)策略中θ的值來(lái)彌補(bǔ)。

2.3 雙層平滑度優(yōu)化策略

傳統(tǒng)RRT規(guī)劃完航跡之后存在的最大的問(wèn)題就是航跡平滑度的問(wèn)題，為了改善航跡平滑度，文獻(xiàn)[15]提出了一種平滑度優(yōu)化策略：先對(duì)圖3中轉(zhuǎn)折角大于120°的相鄰路徑進(jìn)行刪減平滑，然后采用三次貝塞爾曲線進(jìn)行優(yōu)化。這種優(yōu)化方法優(yōu)化后的航跡都是曲線，非常平滑，但是這種三次貝塞爾優(yōu)化增加了路徑中節(jié)點(diǎn)的數(shù)量，考慮到無(wú)人機(jī)的航跡通常是通過(guò)坐標(biāo)點(diǎn)控制，可能會(huì)使實(shí)際的無(wú)人機(jī)飛行路線不一定是貝塞爾優(yōu)化后的航跡。為此，本文提出了雙層優(yōu)化策略，其主要思想是對(duì)規(guī)劃的航跡進(jìn)行兩次優(yōu)化，以期達(dá)到更理想的效果。

設(shè)圖3中P1、P2、P3是航跡中連續(xù)的3個(gè)點(diǎn)，航跡連接順序?yàn)镻1→P2→P3，β為航跡中相鄰節(jié)點(diǎn)的轉(zhuǎn)折角，根據(jù)角度的大小，對(duì)航跡進(jìn)行優(yōu)化。

圖3 航跡優(yōu)化圖解

處理方法為：

第一次優(yōu)化：當(dāng)β>90°時(shí)，認(rèn)為航跡比較平滑，不需要進(jìn)行優(yōu)化處理，航跡節(jié)點(diǎn)還是原來(lái)的節(jié)點(diǎn)；當(dāng)β≤90°時(shí)，認(rèn)為轉(zhuǎn)折角度較大，需要進(jìn)行優(yōu)化處理。具體優(yōu)化步驟如下：

(1)首先檢查航跡P1→P3上是否有障礙，如果沒(méi)有障礙，則刪掉節(jié)點(diǎn)P2，直接連接P1和P3兩點(diǎn)作為新的航跡；如果P1→P3上是有障礙，則取P2、P3的中點(diǎn)P；

(2)檢查航跡P1→P上是否有障礙，如果沒(méi)有障礙，則略過(guò)節(jié)點(diǎn)P2，連接P1、P兩點(diǎn)作為新的航跡；若存在障礙，則不能改變?cè)瓉?lái)的航跡，只能以P2作為節(jié)點(diǎn)，不進(jìn)行優(yōu)化處理；

(3)從航跡的起點(diǎn)開(kāi)始依次檢查每個(gè)節(jié)點(diǎn)與后兩個(gè)節(jié)點(diǎn)的曲折情況，確定是否可以進(jìn)行優(yōu)化，并做出相應(yīng)的處理，直到航跡終點(diǎn)，第一次優(yōu)化完成。

以第一次優(yōu)化完的航跡為基礎(chǔ)，進(jìn)行第二次優(yōu)化，優(yōu)化方法為：判斷兩條航跡之間的轉(zhuǎn)折角β，當(dāng)β>150°時(shí)，不進(jìn)行優(yōu)化；當(dāng)β≤150°時(shí)，認(rèn)為航跡較曲折，需要進(jìn)行優(yōu)化，具體優(yōu)化步驟是只進(jìn)行第一次優(yōu)化步驟(2)、步驟(3)，直到新航跡的終點(diǎn)，優(yōu)化完成。

第一次優(yōu)化過(guò)程的主要作用是對(duì)航跡中的節(jié)點(diǎn)進(jìn)行刪減，使節(jié)點(diǎn)數(shù)目更少，加快程序執(zhí)行過(guò)程，并且對(duì)轉(zhuǎn)折角小于90°的航跡進(jìn)行優(yōu)化，使航跡平滑，但是第一次優(yōu)化由于改變節(jié)點(diǎn)，有可能使個(gè)別地方航跡的平滑度降低，所以進(jìn)行第二次優(yōu)化；第二次優(yōu)化沒(méi)有進(jìn)行節(jié)點(diǎn)的刪減，只對(duì)第一步優(yōu)化的航跡中，轉(zhuǎn)折角大于150°的航跡進(jìn)行優(yōu)化，增加航跡的平滑度。

3 突發(fā)威脅航跡規(guī)劃、性能約束及任務(wù)空間模型

3.1 突發(fā)威脅航跡規(guī)劃

根據(jù)任務(wù)需要，一般四旋翼無(wú)人機(jī)動(dòng)態(tài)航跡規(guī)劃可分為兩種情況：一是執(zhí)行運(yùn)輸、投送、打擊任務(wù)時(shí)，四旋翼無(wú)人機(jī)只要能到達(dá)目標(biāo)點(diǎn)即可，這種情況下可以放棄原先的航跡，以四旋翼無(wú)人機(jī)當(dāng)前位置為起點(diǎn)，以目標(biāo)點(diǎn)為終點(diǎn)重新規(guī)劃一條航跡；二是執(zhí)行偵察任務(wù)時(shí)，四旋翼無(wú)人機(jī)避過(guò)障礙后還要回到原來(lái)的航線上繼續(xù)執(zhí)行任務(wù)，這時(shí)只需要規(guī)劃出一段可以繞過(guò)障礙物的航跡。

第一種航跡規(guī)劃方式只要把當(dāng)前位置設(shè)為起始點(diǎn)，終點(diǎn)為目標(biāo)點(diǎn)即可，設(shè)置簡(jiǎn)單，但是當(dāng)四旋翼無(wú)人機(jī)距目標(biāo)點(diǎn)較遠(yuǎn)時(shí)，重規(guī)劃航跡花費(fèi)的時(shí)間較長(zhǎng)，實(shí)時(shí)性降低，但是對(duì)于目標(biāo)點(diǎn)改變的情況只能使用這種方式；第二種規(guī)劃方法需要找出分離點(diǎn)和接入點(diǎn)，并且分別設(shè)為起始點(diǎn)和目標(biāo)點(diǎn)，規(guī)劃一條航跡，當(dāng)四旋翼無(wú)人機(jī)到達(dá)分離點(diǎn)時(shí)按照新規(guī)劃的航跡飛行，到達(dá)接入點(diǎn)時(shí)回到原來(lái)的航線上，通常這種方式規(guī)劃航跡短，實(shí)時(shí)性高，但是對(duì)障礙物在目標(biāo)點(diǎn)附近時(shí)沒(méi)有必要采用此方式。

3.2 動(dòng)態(tài)性能約束

四旋翼無(wú)人機(jī)能夠正常飛行，需要滿足其動(dòng)態(tài)性能要求，主要性能約束如下：

最大飛行距離Lmax：設(shè)四旋翼無(wú)人機(jī)的航跡是由航跡段{lii=1,2,3,…,n}坐標(biāo)組成，則約束可表示為

(3)

最小直飛距離lmin：設(shè)四旋翼無(wú)人機(jī)的航跡是由航跡段{lii=1,2,3,…,n}坐標(biāo)組成，則約束可表示為

lmin≤li

(4)

最大轉(zhuǎn)彎角度αmax：由于四旋翼無(wú)人機(jī)可以懸停，所以可以忽略此約束[16]。

最大俯仰角φmax：設(shè)三維航跡方程為z=f(x,y)，ai為第i段航跡的水平投影，則ai=(xi-xi-1,yi-yi-1)，該約束條件表示為

(5)

同時(shí)滿足以上約束條件的航跡才是可行航跡。二維條件下不需要考慮四旋翼無(wú)人機(jī)最大俯仰角的限制，只需滿足式(3)、式(4)即可。

3.3 任務(wù)空間模型

3.3.1 二維任務(wù)空間模型

二維模型是基于四旋翼無(wú)人機(jī)定高飛行時(shí)的情景，此時(shí)四旋翼無(wú)人機(jī)的任務(wù)空間R=[(X,Y)0≤X≤100,0≤Y≤100]，(m)。其中X是任務(wù)空間的橫坐標(biāo)，Y是任務(wù)空間的縱坐標(biāo)。在二維空間中，可以將障礙物建模為圓。二維任務(wù)空間和障礙物的建模如圖4所示，Pstart、Pgoal分別是起始點(diǎn)和目標(biāo)點(diǎn)，圓形區(qū)域是障礙物。

圖4 二維任務(wù)空間模型

3.3.2 三維任務(wù)空間模型

實(shí)際中，四旋翼無(wú)人機(jī)是在三維環(huán)境中飛行，仿真需要給出三維環(huán)境下四旋翼無(wú)人機(jī)的任務(wù)空間描述，設(shè)其為R=[(X,Y,Z)0≤X≤100, 0≤Y≤100, 0≤Z≤100]，(m)。其中X、Y、Z分別為任務(wù)空間的橫坐標(biāo)、縱坐標(biāo)和豎坐標(biāo)。障礙威脅可以用以下方程來(lái)描述

(6)

其中，Zi為第i個(gè)山峰的高度，(Xi,Yi)是第i個(gè)山峰的峰頂坐標(biāo)，(Xλi,Yλi)為第i個(gè)山峰X(qián)方向和Y方向上的坡度。三維環(huán)境下的任務(wù)空間和障礙物模型如圖5所示。

圖5 三維任務(wù)空間模型

4 仿真驗(yàn)證與分析

為了驗(yàn)證與分析本文所提出的綜合改進(jìn)RRT算法的性能，在二維任務(wù)空間中進(jìn)行仿真實(shí)驗(yàn)，并與兩種具有代表性的RRT改進(jìn)算法進(jìn)行性能比較；同時(shí)分析綜合改進(jìn)RRT算法中的重要參數(shù)——引導(dǎo)概率θ對(duì)算法的影響。在三維空間中對(duì)兩種類(lèi)型的動(dòng)態(tài)航跡規(guī)劃進(jìn)行仿真，驗(yàn)證綜合改進(jìn)RRT算法的可行性。

4.1 不同優(yōu)化方法之間的比較

對(duì)傳統(tǒng)RRT算法的改進(jìn)，最典型的優(yōu)化方法是加入引力使隨機(jī)樹(shù)盡可能朝著目標(biāo)點(diǎn)方向生長(zhǎng)來(lái)加快收斂速度和動(dòng)態(tài)步長(zhǎng)策略來(lái)改善算法實(shí)時(shí)性與航跡平滑度的矛盾。本部分將綜合改進(jìn)RRT算法與啟發(fā)式RRT算法和動(dòng)態(tài)步長(zhǎng)RRT算法進(jìn)行比較，分析各種改進(jìn)型算法的特點(diǎn)。進(jìn)行30次仿真比較，結(jié)果如圖6、圖7所示，具體性能指標(biāo)由表1給出。

圖6 綜合改進(jìn)RRT與啟發(fā)式RRT的比較

圖7 綜合改進(jìn)RRT與動(dòng)態(tài)步長(zhǎng)RRT的比較

比較類(lèi)別綜合改進(jìn)RRT啟發(fā)式RRT動(dòng)態(tài)步長(zhǎng)RRT隨機(jī)樹(shù)葉節(jié)點(diǎn)數(shù)/個(gè)Max544374Min353235Mean42.6437.1858規(guī)劃時(shí)間/msMax105.39.7210.69Min8.094.765.38Mean21.76.887.64航跡長(zhǎng)度/mMax134.2144.08135.97Min127.85130.84129.70Mean130.58136.01132.74

圖6中細(xì)實(shí)線是啟發(fā)式RRT算法規(guī)劃的航跡，粗實(shí)線是改進(jìn)RRT算法規(guī)劃的航跡，“*”是啟發(fā)式RRT算法的葉節(jié)點(diǎn)，“+”是綜合改進(jìn)RRT算法的葉節(jié)點(diǎn)。圖7中細(xì)實(shí)線是動(dòng)態(tài)步長(zhǎng)RRT算法規(guī)劃的航跡，粗實(shí)線是改進(jìn)RRT算法規(guī)劃的航跡，“*”是動(dòng)態(tài)步長(zhǎng)RRT算法的葉節(jié)點(diǎn)，“+”是綜合改進(jìn)RRT算法的葉節(jié)點(diǎn)。從圖6、圖7可以看出，啟發(fā)式RRT算法在沒(méi)有障礙物時(shí)規(guī)劃出來(lái)的航跡幾乎為直線，沒(méi)有轉(zhuǎn)彎角度，航跡最優(yōu)，但是有障礙物時(shí)規(guī)劃航跡轉(zhuǎn)折角度較大，動(dòng)態(tài)步長(zhǎng)RRT算法完全由引力函數(shù)提供隨機(jī)樹(shù)生長(zhǎng)方向和步長(zhǎng)，航跡對(duì)引力函數(shù)依賴(lài)性很大，不利于適應(yīng)復(fù)雜環(huán)境，綜合改進(jìn)RRT算法不僅對(duì)隨機(jī)樹(shù)的生長(zhǎng)提供指引，而且對(duì)規(guī)劃的航跡進(jìn)行優(yōu)化，航跡的平滑度優(yōu)于啟發(fā)式RRT算法和動(dòng)態(tài)步長(zhǎng)RRT算法。

從表1中可以看出，隨機(jī)樹(shù)節(jié)點(diǎn)數(shù)量上，啟發(fā)式RRT平均需要37.18個(gè)節(jié)點(diǎn)就可以規(guī)劃處從起始點(diǎn)到目標(biāo)點(diǎn)的航跡，數(shù)量最少，效率最高，動(dòng)態(tài)步長(zhǎng)RRT算法僅依賴(lài)引力函數(shù)，平均需要58個(gè)節(jié)點(diǎn)才能規(guī)劃出航跡，效率最低，綜合改進(jìn)RRT算法介于兩者之間；從規(guī)劃時(shí)間上看，啟發(fā)式RRT算法和動(dòng)態(tài)步長(zhǎng)RRT算法相差不多，而綜合改進(jìn)RRT算法需要的時(shí)間大約是另外兩種算法的3倍，這是因?yàn)閱l(fā)式RRT算法和動(dòng)態(tài)步長(zhǎng)RRT算法規(guī)劃完航跡后沒(méi)有進(jìn)行航跡的平滑度優(yōu)化，而綜合改進(jìn)RRT算法的平滑度優(yōu)化策略耗費(fèi)了大量的時(shí)間；航跡長(zhǎng)度上，綜合改進(jìn)RRT算法最短，這得益于對(duì)航跡的優(yōu)化，啟發(fā)式RRT算法最長(zhǎng)，動(dòng)態(tài)步長(zhǎng)介于兩者之間。

總體來(lái)說(shuō)，啟發(fā)式RRT和動(dòng)態(tài)步長(zhǎng)RRT算法實(shí)時(shí)性好，適合應(yīng)用于障礙物較少，實(shí)時(shí)性要求高的場(chǎng)合；綜合改進(jìn)RRT算法的航跡最短，平滑度最好，但是規(guī)劃時(shí)間最長(zhǎng)，實(shí)時(shí)性最差，綜合改進(jìn)RRT算法是犧牲算法的實(shí)時(shí)性來(lái)提高航跡的平滑度的，在實(shí)時(shí)性要求不是很高的情況下可以選擇綜合改進(jìn)RRT算法進(jìn)行航跡規(guī)劃。

4.2 算法參數(shù)影響

綜合改進(jìn)RRT算法中的參數(shù)有原始步長(zhǎng)ε和引導(dǎo)概率θ。原始步長(zhǎng)是根據(jù)任務(wù)環(huán)境、四旋翼無(wú)人機(jī)的動(dòng)態(tài)性能約束和任務(wù)要求來(lái)確定的，引導(dǎo)概率θ則直接決定了算法性能的優(yōu)劣。在圖8所示隨機(jī)任務(wù)空間中設(shè)置50個(gè)隨機(jī)障礙，算法中步長(zhǎng)設(shè)為5，在θ∈[0,1)的范圍內(nèi)，每隔0.1對(duì)θ進(jìn)行取值，進(jìn)行50次仿真驗(yàn)證，對(duì)規(guī)劃距離、規(guī)劃時(shí)間兩項(xiàng)最重要的指標(biāo)取平均進(jìn)行統(tǒng)計(jì)，其結(jié)果見(jiàn)表2，θ取不同值時(shí)的性能比較如圖9所示。

圖8 隨機(jī)任務(wù)空間

θ規(guī)劃時(shí)間/s航跡長(zhǎng)度/mMaxMinMeanMaxMinMean05.450.371.04147.4123.6133.70.11.030.120.37167.0117.6132.40.20.590.090.21143.7116.5123.90.30.860.080.23138.1114.9123.20.41.130.070.32128.3114.6121.90.51.380.070.35141114.8121.40.62.440.040.42128.5114.5120.70.72.880.090.56141.7115.4120.30.83.470.150.82125.6114.2118.20.97.920.161.60125.8113.6118.0

圖9 θ取不同值時(shí)的仿真性能

由圖9(a)可以看出，規(guī)劃時(shí)間在θ=0.2時(shí)取得最小值，隨著θ的減小或者增大，規(guī)劃時(shí)間不斷增加。這是因?yàn)棣热≈岛苄r(shí)，算法的方向啟發(fā)性不強(qiáng)，隨機(jī)點(diǎn)在任務(wù)空間中生成的隨機(jī)性很大，需要耗費(fèi)較長(zhǎng)時(shí)間隨機(jī)樹(shù)才會(huì)到達(dá)目標(biāo)點(diǎn)；當(dāng)θ取值很大甚至接近于1時(shí)，算法的方向啟發(fā)性很強(qiáng)，搜索未知區(qū)域的能力明顯減弱，隨機(jī)樹(shù)擴(kuò)展的過(guò)程中遇到障礙時(shí)，需要重復(fù)搜索才能找到合適的葉節(jié)點(diǎn)，繞過(guò)障礙物，到達(dá)目標(biāo)點(diǎn)，重復(fù)搜索的過(guò)程會(huì)耗費(fèi)大量的時(shí)間，導(dǎo)致算法實(shí)時(shí)性降低。由圖9(b)可以看出，航跡距離隨著θ的增大而減小，特別是θ∈[0,0.2]時(shí)航跡減小比較明顯。這是因?yàn)棣仍酱?，算法的指向性越?qiáng)，規(guī)劃出的航跡越接近從起始點(diǎn)到目標(biāo)點(diǎn)的直線。

當(dāng)θ=0時(shí)，改進(jìn)的算法相當(dāng)于對(duì)傳統(tǒng)RRT算法進(jìn)行路徑優(yōu)化；當(dāng)θ=1時(shí)，一直選目標(biāo)點(diǎn)為隨機(jī)點(diǎn)，失去算法的搜索性，容易導(dǎo)致航跡規(guī)劃失敗。仿真實(shí)驗(yàn)結(jié)果表明，θ取值不能過(guò)大也不能過(guò)小，過(guò)大會(huì)導(dǎo)致算法搜索能力降低，造成規(guī)劃失敗或者花費(fèi)更多的時(shí)間搜索導(dǎo)致實(shí)時(shí)性降低；過(guò)小會(huì)導(dǎo)致算法啟發(fā)性不強(qiáng)，路徑曲折，達(dá)不到期望的效果。由仿真結(jié)果可知θ的取值在0.5左右可以取得較好的效果。

4.3 突發(fā)威脅航跡規(guī)劃

當(dāng)四旋翼無(wú)人機(jī)按照規(guī)劃的航跡飛行，航跡上突然出現(xiàn)障礙物時(shí)，四旋翼無(wú)人機(jī)要具有重新規(guī)劃航跡的能力。本部分對(duì)三維空間中綜合改進(jìn)RRT算法的兩種類(lèi)型的動(dòng)態(tài)航跡規(guī)劃進(jìn)行仿真，設(shè)原始步長(zhǎng)為ε=5、引導(dǎo)概率θ=0.5、障礙物半徑為10，驗(yàn)證所提出的綜合改進(jìn)RRT算法的可行性。仿真結(jié)果如圖10、圖11所示。

圖11 繞過(guò)障礙，回到原來(lái)航跡

圖10、圖11中“*”是起始點(diǎn)，“o”是目標(biāo)點(diǎn)，黑色細(xì)線是四旋翼無(wú)人機(jī)原先的航跡，黑色粗線是障礙物出現(xiàn)后重新規(guī)劃的航跡，五角星是障礙物出現(xiàn)時(shí)四旋翼無(wú)人機(jī)的位置。兩種動(dòng)態(tài)航跡規(guī)劃方式都可以重新規(guī)劃一條繞過(guò)障礙物的新航跡，達(dá)到避障的目的，驗(yàn)證了綜合改進(jìn)RRT算法的可行性，實(shí)際應(yīng)用中具體采用哪種方式要結(jié)合具體情況而確定。

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

針對(duì)傳統(tǒng)RRT算法隨機(jī)性強(qiáng)、收斂速度慢、規(guī)劃航跡不平滑的缺點(diǎn)，本文受其它文獻(xiàn)啟發(fā)對(duì)其進(jìn)行了改進(jìn)，提出了綜合改進(jìn)型的RRT算法，并且與兩種典型的RRT改進(jìn)算法進(jìn)行了比較，可以發(fā)現(xiàn)綜合改進(jìn)后的RRT算法航跡平滑度增加，但是規(guī)劃時(shí)間增長(zhǎng)，實(shí)時(shí)性降低。本文進(jìn)一步在二維任務(wù)空間中對(duì)綜合改進(jìn)RRT的參數(shù)θ取不同值時(shí)進(jìn)行仿真，驗(yàn)證了引導(dǎo)概率θ對(duì)算法的影響，并給出選取原則；在三維環(huán)境下將改進(jìn)后的RRT算法用于兩種動(dòng)態(tài)航跡規(guī)劃中進(jìn)行仿真驗(yàn)證，取得了滿意的效果，驗(yàn)證了綜合改進(jìn)RRT算法能夠適應(yīng)動(dòng)態(tài)航跡規(guī)劃的要求。

然而，基于RRT算法隨機(jī)性強(qiáng)的特點(diǎn)，本文沒(méi)有對(duì)最優(yōu)航跡的選取進(jìn)行研究，RRT算法與最優(yōu)航跡選取算法的結(jié)合將是解決此問(wèn)題的思路，也是以后研究的方向。