陳良劍，趙文龍，婁嘉駿

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

0 引言

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

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

結(jié)合上述討論，提出一種基于RRT的動(dòng)態(tài)規(guī)避航跡規(guī)劃算法，在引入啟發(fā)因子的基礎(chǔ)上，結(jié)合無(wú)人機(jī)的性能約束，通過(guò)動(dòng)態(tài)調(diào)整的方法增加RRT的穩(wěn)定性和規(guī)避性能。最終使算法能夠擁有適當(dāng)?shù)哪繕?biāo)指向性，而且避免算法在障礙物密集的情況下產(chǎn)生大量的采樣無(wú)效點(diǎn)。

1 RRT算法基本原理

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

圖1 RRT算法示意圖

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

Step3當(dāng)隨機(jī)樹(shù)的節(jié)點(diǎn)延伸至目標(biāo)節(jié)點(diǎn)區(qū)域時(shí)，停止循環(huán)采樣。從起始點(diǎn)Qstart開(kāi)始，依次搜索父節(jié)點(diǎn)，直至到達(dá)目標(biāo)節(jié)點(diǎn)Qgoal最后生成一條無(wú)碰撞的路徑。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖3 動(dòng)態(tài)步長(zhǎng)規(guī)避策略示意圖

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

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

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

2.3 航跡優(yōu)化

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

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

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

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

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

3.1 目標(biāo)指向概率效果測(cè)試

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

圖4 引導(dǎo)因子影響效果

3.2 仿真驗(yàn)證

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

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

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

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

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

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

3.3 算法實(shí)驗(yàn)分析

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

本次對(duì)比實(shí)驗(yàn)的部分?jǐn)?shù)據(jù)如表1、表2所示，本次實(shí)驗(yàn)通過(guò)對(duì)20組對(duì)比實(shí)驗(yàn)的規(guī)劃時(shí)間和路徑長(zhǎng)度計(jì)算方差的方式，確定算法的穩(wěn)定性指標(biāo)。

表1 三種算法時(shí)間分析

表2 三種算法路程分析

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

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

4 結(jié)語(yǔ)

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