復(fù)雜環(huán)境下基于采樣空間自調(diào)整的航跡規(guī)劃算法

張 康，陳建平

（南京航空航天大學(xué)航空學(xué)院，南京 210016）

0 引言

航跡規(guī)劃是指在任務(wù)空間中找到無(wú)人機(jī)（Unmanned Aerial Vehicle，UAV）從初始狀態(tài)到指定目標(biāo)狀態(tài)的可行航跡，其應(yīng)用背景十分廣泛，如無(wú)人駕駛汽車(chē)、計(jì)算機(jī)輔助外科手術(shù)、移動(dòng)機(jī)器人等［1-3］。

現(xiàn)有的航跡規(guī)劃算法主要分為確定性方法和隨機(jī)性方法。確定性方法有人工勢(shì)場(chǎng)法［4］、A*算法［5］、智能算法等。人工勢(shì)場(chǎng)法需要根據(jù)環(huán)境信息預(yù)先構(gòu)建勢(shì)場(chǎng)，容易陷入局部最優(yōu)，且在狹窄通道里會(huì)出現(xiàn)擺動(dòng)現(xiàn)象，不適合復(fù)雜環(huán)境的應(yīng)用；A*算法在高維空間中會(huì)出現(xiàn)組合爆炸的問(wèn)題；以粒子群［6］為代表的智能算法通常會(huì)用來(lái)求解滿足性能指標(biāo)的最優(yōu)航跡問(wèn)題，但一般需要大量迭代才能收斂，計(jì)算開(kāi)銷(xiāo)大，運(yùn)算時(shí)間長(zhǎng)。

概率路線圖（Probabilistic Road Map，PRM）法［7］和快速擴(kuò)展隨機(jī)樹(shù)（Rapidly-exploring Random Tree，RRT）法［8］是當(dāng)前使用最廣泛的隨機(jī)采樣方法，相較于確定性方法的優(yōu)勢(shì)在于使用碰撞檢測(cè)技術(shù)避免了對(duì)障礙物的顯示建模，直接在當(dāng)前任務(wù)空間中生成可行航跡，具有實(shí)時(shí)處理復(fù)雜環(huán)境下航跡規(guī)劃問(wèn)題的能力。PRM 算法的性能對(duì)障礙物形狀嚴(yán)重依賴(lài)，難以在不確定環(huán)境中發(fā)揮作用。相比之下，RRT 算法對(duì)環(huán)境波動(dòng)不敏感，且支持非完整約束。這些優(yōu)點(diǎn)使得RRT 在近年來(lái)得到了廣泛的應(yīng)用和發(fā)展。最具代表性的是文獻(xiàn)［9］中提出的具有漸進(jìn)最優(yōu)性的快速擴(kuò)展隨機(jī)樹(shù)（Rapidly-exploring Random Tree star，RRT*）算法，它針對(duì)RRT算法非最優(yōu)性的缺點(diǎn)，在每次迭代中通過(guò)對(duì)新擴(kuò)展節(jié)點(diǎn)的近鄰優(yōu)化來(lái)保證算法的漸進(jìn)最優(yōu)性。但該算法仍然存在一些不足：1）尋路沒(méi)有方向性，規(guī)劃過(guò)程中會(huì)出現(xiàn)很多重復(fù)探索；2）沒(méi)有利用已有樹(shù)的信息，對(duì)整個(gè)空間采樣導(dǎo)致添加了大量不在航跡附近的無(wú)用節(jié)點(diǎn)，收斂速度緩慢。

為了增強(qiáng)RRT 算法的魯棒性，文獻(xiàn)［10］中提出了基于動(dòng)態(tài)域的快速擴(kuò)展隨機(jī)樹(shù)（Dynamic Domain Rapidly-exploring Random Tree，DDRRT）算法。文獻(xiàn)［11］中則通過(guò)引入地圖的代價(jià)模型，改善了DDRRT可能造成的對(duì)采樣空間過(guò)度約減的問(wèn)題。

為了提高RRT*的收斂速率，文獻(xiàn)［12］中通過(guò)對(duì)剪枝處理后的可行路徑節(jié)點(diǎn)附近集中采樣，從而減少了大量的無(wú)用采樣。文獻(xiàn)［13］中借鑒文獻(xiàn)［12］的思路提出了可調(diào)邊界的漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)（RRT*-Adjustable Bounds，RRT*-AB）算法，在迭代中一旦找到可行路徑，便在起始點(diǎn)和目標(biāo)點(diǎn)之間建立一個(gè)可調(diào)整邊界的連通域作為采樣區(qū)域，同樣達(dá)到了集中采樣的效果。文獻(xiàn)［14］中提出的帶啟發(fā)式采樣的漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)（Informed-RRT*）算法將采樣空間限制在一個(gè)超橢球里來(lái)達(dá)到優(yōu)化采樣空間的效果。文獻(xiàn)［15］中提出的固定節(jié)點(diǎn)數(shù)的漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)（RRT*-Fixed Nodes，RRT*-FN）算法通過(guò)限制節(jié)點(diǎn)的最大數(shù)量來(lái)減少算法的占用內(nèi)存。文獻(xiàn)［16］中將Informed-RRT*和RRT*-FN 算法結(jié)合，通過(guò)啟發(fā)式采樣和刪除低權(quán)重的葉子節(jié)點(diǎn)的方法來(lái)加快收斂速度。文獻(xiàn)［17］中通過(guò)機(jī)器學(xué)習(xí)的方法得到新的非均勻抽樣分布，該分布只在非障礙區(qū)域采樣，極大減少了碰撞檢測(cè)次數(shù)。

針對(duì)RRT*算法在復(fù)雜環(huán)境下的尋路效率不足、收斂速度緩慢的問(wèn)題，本文提出了基于采樣空間自調(diào)整的漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)（Adjust Sampling space-RRT*，AS-RRT*）算法。該算法在迭代過(guò)程中的采樣空間隨著樹(shù)的不斷生長(zhǎng)而自動(dòng)調(diào)整，算法流程分為搜索和優(yōu)化兩個(gè)階段：搜索階段不采取近鄰優(yōu)化，在迭代中根據(jù)當(dāng)前樹(shù)生長(zhǎng)情況選擇合適的采樣策略，目的是更快速地找到初始航跡；優(yōu)化階段根據(jù)算法的近鄰優(yōu)化次數(shù)，周期性地更新高質(zhì)量節(jié)點(diǎn)，并通過(guò)學(xué)習(xí)高質(zhì)量節(jié)點(diǎn)產(chǎn)生新的抽樣分布、刪除低質(zhì)量節(jié)點(diǎn)來(lái)保證樹(shù)在最優(yōu)路徑附近高效生長(zhǎng)，加快了收斂速度，降低了算法的內(nèi)存占用。在不同類(lèi)型的環(huán)境下與傳統(tǒng)的RRT*算法進(jìn)行了對(duì)比仿真實(shí)驗(yàn)，結(jié)果驗(yàn)證了本文算法的有效性。

1 相關(guān)工作

1.1 RRT*

RRT*算法［9］是RRT 算法中具有漸進(jìn)最優(yōu)性質(zhì)的優(yōu)化版本，在每次成功擴(kuò)展節(jié)點(diǎn)后通過(guò)和近鄰節(jié)點(diǎn)互相重選父節(jié)點(diǎn)來(lái)降低樹(shù)的總代價(jià)，當(dāng)采樣量足夠大時(shí)，算法會(huì)最終收斂到最優(yōu)航跡。該算法大致流程如算法1 所示。首先，初始狀態(tài)xinit被添加到樹(shù)的根節(jié)點(diǎn)中（第1）行）；主循環(huán)第2）～12）行表示在N次迭代后終止。只有通過(guò)碰撞檢查的邊才能添加到樹(shù)中。近鄰優(yōu)化確保新節(jié)點(diǎn)連接到搜索樹(shù)中的最佳頂點(diǎn)。碰撞檢測(cè)技術(shù)不需要約束的顯式表達(dá)，具有實(shí)時(shí)處理復(fù)雜環(huán)境下航跡優(yōu)化問(wèn)題的能力。

Sample：隨機(jī)地在狀態(tài)空間均勻采樣，生成一個(gè)狀態(tài)點(diǎn)。

Nearest：通過(guò)一個(gè)度量函數(shù)Distance 來(lái)衡量節(jié)點(diǎn)與采樣點(diǎn)的距離，函數(shù)Nearest 返回搜索樹(shù)T中與采樣點(diǎn)距離最近的節(jié)點(diǎn)。

Steer：給定兩點(diǎn)xnearest和xrand，Steer 函數(shù)返回?cái)U(kuò)展的新節(jié)點(diǎn)xnew，在滿足最大生長(zhǎng)步長(zhǎng)r的情況下，使新節(jié)點(diǎn)xnew盡可能地靠近xrand。

CollisionCheck：檢查連接到新節(jié)點(diǎn)的航跡是否通過(guò)碰撞檢測(cè)，滿足則返回ture，否則返回false。

Near：從搜索樹(shù)T中篩選出與新節(jié)點(diǎn)的距離小于γ的節(jié)點(diǎn)，組成一個(gè)節(jié)點(diǎn)集并返回。

其中：γ=k(logn/n)1/d表示近鄰優(yōu)化半徑；k是與規(guī)劃空間尺寸有關(guān)的常數(shù)；n為節(jié)點(diǎn)數(shù)量；d為規(guī)劃空間的維數(shù)。

Chooseparent：遍歷節(jié)點(diǎn)集Vnear為xnew重新分配父節(jié)點(diǎn)指針，返回使xnew代價(jià)最低的父節(jié)點(diǎn)。

Rewire：如果重連接后代價(jià)更低，則Vnear中的節(jié)點(diǎn)重新分配父節(jié)點(diǎn)指針到xnew。

RRT*算法雖然保證了RRT 算法的漸進(jìn)最優(yōu)性，但由于沒(méi)有在探索和優(yōu)化做一個(gè)合理的權(quán)衡，一成不變地對(duì)整個(gè)空間進(jìn)行均勻采樣，導(dǎo)致了很多優(yōu)化操作不能集中在最優(yōu)航跡附近，也就是說(shuō)空間中那些沒(méi)有降低最終航跡代價(jià)的采樣是無(wú)用的，浪費(fèi)了大量計(jì)算資源。

1.2 改進(jìn)RRT*-FN

Informed-RRT*算法［14］是為了減少無(wú)用采樣、提高收斂速度的一個(gè)RRT*改進(jìn)版本。該算法通過(guò)當(dāng)前可行航跡的最小代價(jià)來(lái)生成一個(gè)超橢球采樣空間，減少了在無(wú)用區(qū)域的采樣。RRT*-FN 算法［15］通過(guò)預(yù)設(shè)值最大節(jié)點(diǎn)數(shù)量來(lái)減少算法所占用的內(nèi)存。改進(jìn)RRT*-FN［16］結(jié)合了前兩者的優(yōu)點(diǎn)，在搜索到初始航跡后通過(guò)啟發(fā)式采樣把采樣空間限制在橢圓子集和航跡點(diǎn)的鄰近區(qū)域，在達(dá)到預(yù)設(shè)最大節(jié)點(diǎn)數(shù)量后刪去不在啟發(fā)采樣區(qū)域的葉子節(jié)點(diǎn)，進(jìn)一步加快了收斂速度。改進(jìn)RRT*-FN算法的大致流程如算法2。

改進(jìn)RRT*-FN 在RRT*基礎(chǔ)上增添了啟發(fā)式采樣和節(jié)點(diǎn)刪除的步驟，雖然有效提升了收斂速度，減少了計(jì)算占用內(nèi)存，但是仍有一些不足之處：1）在找到初始航跡前使用RRT*算法均勻搜索整個(gè)空間，造成了許多對(duì)無(wú)用區(qū)域的近鄰優(yōu)化操作；2）橢圓子集采樣更適合最終航跡長(zhǎng)度和起始點(diǎn)到目標(biāo)點(diǎn)的直線距離相差不大的情況，否則，橢圓區(qū)域可能會(huì)大到覆蓋整個(gè)采樣空間。

2 問(wèn)題定義

令規(guī)劃任務(wù)的狀態(tài)空間通過(guò)集合X?Rn來(lái)表示，n∈N為狀態(tài)空間維數(shù)。Xobs?X用來(lái)表示空間中的障礙區(qū)域，Xfree=Xobs/X表示空間中的自由區(qū)域；xstart∈Xfree為起始點(diǎn)，xgoal∈Xfree為目標(biāo)點(diǎn)；對(duì)于自由空間中任意兩狀態(tài)點(diǎn)x1∈Xfree，x2∈Xfree，定義一個(gè)連續(xù)函數(shù)π：[0，s]來(lái)表示連接兩點(diǎn)的一段可行航跡（π(0)=x1，π(s)=x2），s表示航跡代價(jià)；令空間中所有的可行航跡πf∈Xfree（πf(0)=xinit，πf(s)=xgoal）由一個(gè)集合Σf表示。算法將通過(guò)構(gòu)造搜索樹(shù)T來(lái)找尋航跡，主要考慮了以下兩個(gè)問(wèn)題。

問(wèn)題1 在限定搜索時(shí)間里找到一條可行航跡πf∈Σf（πf(0)=xinit，πf(s)=xgoal）。

問(wèn)題2 在有限時(shí)間里不斷優(yōu)化可行航跡πf∈Σf，使航跡代價(jià)s最小。

3 AS-RRT*

為了減少尋路時(shí)間、降低航跡代價(jià)，本文提出的AS-RRT*算法將流程分為搜索和優(yōu)化兩個(gè)階段，分別采用不同的采樣策略和擴(kuò)展策略，目的是同時(shí)保證快速性和優(yōu)化性。算法大致流程如算法3所示。

算法在初始化參數(shù)后進(jìn)入搜索階段（第2）～5）行），基于樹(shù)的生長(zhǎng)情況的自適應(yīng)選擇向光區(qū)采樣和背光區(qū)采樣（第3）行），引入一個(gè)基于碰撞增量的代價(jià)模型來(lái)降低障礙物附近節(jié)點(diǎn)被擴(kuò)展的概率（第4）行），此階段不考慮近鄰節(jié)點(diǎn)的優(yōu)化，目的是快速找到一條可行航跡（第6）行）。在找到初始航跡Πinit后，篩選高質(zhì)量節(jié)點(diǎn)放入集合Velite中（第7）行），使用一種多變量概率模型來(lái)描述抽樣分布，參數(shù)α表示概率模型的均值和協(xié)方差矩陣（第8）行）。接著進(jìn)入迭代優(yōu)化階段（第9）～17）行），基于近鄰優(yōu)化次數(shù)NOT（Optimized Times）來(lái)更新精英集和抽樣分布（第12）～16）行），通過(guò)Matlab 的mvnrnd 函數(shù)生成采樣點(diǎn)（第10）行），同RRT*一樣對(duì)新擴(kuò)展節(jié)點(diǎn)采取近鄰優(yōu)化，當(dāng)節(jié)點(diǎn)數(shù)量超過(guò)限定值時(shí)，則刪去樹(shù)中低質(zhì)量葉子節(jié)點(diǎn)（第11）行）。

3.1 搜索階段

向光采樣和背光采樣依據(jù)當(dāng)前節(jié)點(diǎn)總的擴(kuò)展失敗率οEFR(Expansion Failure Rate)來(lái)選擇，它是擴(kuò)展失敗次數(shù)和搜索次數(shù)的比值，反映了當(dāng)前樹(shù)的生長(zhǎng)情況。如果擴(kuò)展失敗率低，說(shuō)明障礙物的影響較小，算法會(huì)趨向于在目標(biāo)點(diǎn)附近的向光區(qū)域采樣；反之，則說(shuō)明障礙物影響較大，算法會(huì)在遠(yuǎn)離樹(shù)中心的背光區(qū)域外采樣，加強(qiáng)探索力度引導(dǎo)樹(shù)逃離障礙區(qū)域。搜索階段的采樣和擴(kuò)展策略如算法4和算法5所示。

3.1.1 向光區(qū)域均勻采樣

向光區(qū)域是一個(gè)以目標(biāo)點(diǎn)為中心的超球錐，對(duì)n維空間下的超球錐子集的均勻采樣可以通過(guò)約束一個(gè)超球的極坐標(biāo)來(lái)方便地實(shí)現(xiàn)。

圖1 是在一個(gè)中心有障礙的簡(jiǎn)單二維環(huán)境下樹(shù)的生長(zhǎng)情況，方塊為起始點(diǎn)，圓圈為目標(biāo)點(diǎn)，黑色表示障礙物。一開(kāi)始，向光區(qū)域?yàn)槠鹗键c(diǎn)和目標(biāo)點(diǎn)間的連線，保證在自由空間里以最高效率向目標(biāo)點(diǎn)生長(zhǎng)；在與障礙物發(fā)生碰撞后，向光區(qū)域擴(kuò)張為一個(gè)扇形，由于擴(kuò)展失敗率很低，向光區(qū)域可以繼續(xù)引導(dǎo)樹(shù)向目標(biāo)點(diǎn)生長(zhǎng)。

圖1 向光區(qū)域采樣Fig.1 Sampling in light area

3.1.2 背光區(qū)域

背光區(qū)域的形狀是一個(gè)超球，超球的中心是當(dāng)前樹(shù)的中心xcenter，也就是所有節(jié)點(diǎn)坐標(biāo)的均值，半徑r為xcenter到擴(kuò)展失敗次數(shù)最多的節(jié)點(diǎn)x″的距離，在背光區(qū)域外的采樣點(diǎn)xrand滿足以下約束：

為了減少不必要的碰撞檢測(cè)，引入一個(gè)基于碰撞增量的代價(jià)模型C，在計(jì)算最近的節(jié)點(diǎn)時(shí)，那些擴(kuò)展失敗的節(jié)點(diǎn)將被考慮額外的代價(jià)。

其中：n為單個(gè)節(jié)點(diǎn)的擴(kuò)展失敗次數(shù)；k的取值范圍為0到1，目的是保證不會(huì)高估節(jié)點(diǎn)的額外代價(jià)。

圖2表示的是一個(gè)規(guī)劃難度較高的Bug trap 難題［10］，起始點(diǎn)在凹障礙物內(nèi)，且與目標(biāo)點(diǎn)無(wú)法直接相連。算法在向光區(qū)域多次嘗試無(wú)果后轉(zhuǎn)而在背光區(qū)域外采樣，因此障礙物包圍區(qū)域里的節(jié)點(diǎn)內(nèi)疏外密，而且由于考慮了額外代價(jià)，在障礙物附近的節(jié)點(diǎn)出現(xiàn)了多分枝現(xiàn)象，相較于傳統(tǒng)RRT 算法用了更少的擴(kuò)展次數(shù)和碰撞檢測(cè)次數(shù)。

圖2 背光區(qū)域采樣Fig.2 Sampling in dark area

3.2 優(yōu)化階段

3.2.1 節(jié)點(diǎn)篩選

基于降低航跡代價(jià)的原則篩選節(jié)點(diǎn)，定義節(jié)點(diǎn)x質(zhì)量高低的指標(biāo)函數(shù)為J，它與當(dāng)前可行航跡Πcurrent有關(guān)，Πcurrent表示起始點(diǎn)到目標(biāo)點(diǎn)的節(jié)點(diǎn)序列。

其中：J1代表了節(jié)點(diǎn)到當(dāng)前航跡的最小距離，J2代表了節(jié)點(diǎn)到連接起始點(diǎn)和目標(biāo)點(diǎn)間直線的距離；k1和k2表示權(quán)重系數(shù)。指標(biāo)函數(shù)J越低則節(jié)點(diǎn)的質(zhì)量越高。算法6為篩選節(jié)點(diǎn)流程。

3.2.2 機(jī)器學(xué)習(xí)模型

合適的機(jī)器學(xué)習(xí)模型可以更好地描述數(shù)據(jù)。高斯混合模型由多個(gè)單高斯分布組成，當(dāng)分模型個(gè)數(shù)K選取合適時(shí)，可以用來(lái)逼近任意的抽樣分布。

高斯混合模型被定義為：

現(xiàn)已有很多成熟的算法求解高斯混合模型的參數(shù)，本文采用最大期望化算法求解高斯混合模型參數(shù)，利用Matlab 的mvnrnd函數(shù)生成采樣點(diǎn)。

3.2.3 分模型個(gè)數(shù)K

合適的K值選取是求解高斯混合模型的關(guān)鍵步驟，它需要合理地反映出當(dāng)前航跡被障礙物分成了幾個(gè)部分，也就是說(shuō)K應(yīng)該和航跡的有效節(jié)點(diǎn)數(shù)量是等同的。有效節(jié)點(diǎn)被定義為航跡節(jié)點(diǎn)序列Π經(jīng)過(guò)裁剪后的新節(jié)點(diǎn)序列Πprune。

算法7 為裁剪函數(shù)的流程，它從起始點(diǎn)開(kāi)始逐漸向目標(biāo)點(diǎn)移動(dòng)，遍歷所有子節(jié)點(diǎn)直到找到最少代價(jià)的無(wú)碰撞航跡，裁剪過(guò)程結(jié)束后，航跡中不會(huì)再有可以直接連接的額外節(jié)點(diǎn)，圖3 為一段航跡的裁剪示意圖，實(shí)線為裁剪后的航跡，虛線為失效的航跡。

圖3 航跡裁剪示意圖Fig.3 Schematic diagram of pruning path

3.2.4 最大節(jié)點(diǎn)數(shù)量

優(yōu)化階段的擴(kuò)展策略考慮近鄰節(jié)點(diǎn)的優(yōu)化，當(dāng)節(jié)點(diǎn)數(shù)量達(dá)到最大值時(shí)，便刪去樹(shù)中最低質(zhì)量的葉子節(jié)點(diǎn)，質(zhì)量高低由3.2.1節(jié)定義的函數(shù)J判斷，擴(kuò)展流程見(jiàn)算法8，最大節(jié)點(diǎn)數(shù)量由式（18）定義：

其中：Nmax表示最大節(jié)點(diǎn)數(shù)量（Maximum Number of Nodes）；εi為地圖尺寸；d為地圖維度；λ為生長(zhǎng)步長(zhǎng)為初始航跡的節(jié)點(diǎn)數(shù)量。

3.2.5 更新周期

與其他通過(guò)找到新的可行航跡來(lái)調(diào)整采樣策略的算法不同，本文根據(jù)近鄰優(yōu)化次數(shù)來(lái)周期性地更新抽樣分布，具有穩(wěn)定的更新頻率。在算法更新了m次后的更新周期P被定義為：

其中：m為更新次數(shù)；P0為初始更新周期。

圖4 中展示了抽樣分布的變化過(guò)程，由于節(jié)點(diǎn)會(huì)不斷被篩選更新，抽樣分布也逐漸被學(xué)習(xí)更新到最優(yōu)路徑的附近區(qū)域。

圖4 更新m次的抽樣分布變化Fig.4 Sampling distribution change after m-times updating

4 算法性能分析

4.1 概率完備性

在找尋初始航跡的搜索階段：理想情況下，向光區(qū)域采樣策略可以引導(dǎo)樹(shù)快速達(dá)到目標(biāo)點(diǎn)；極壞情況下，那些擴(kuò)展失敗節(jié)點(diǎn)的額外代價(jià)將達(dá)到最大值，背光區(qū)域作用失效，同RRT一樣對(duì)空間均勻采樣。因此本文算法與RRT 一樣具有概率完備性。

4.2 計(jì)算復(fù)雜度

5 仿真實(shí)驗(yàn)與結(jié)果分析

在Map1～4 等不同類(lèi)型環(huán)境下仿真來(lái)驗(yàn)證本文算法的有效性，將其與RRT*算法、改進(jìn)RRT*-FN 算法進(jìn)行比較，從而來(lái)表現(xiàn)本文算法的優(yōu)越性?？紤]到算法本身具有的隨機(jī)性和不同環(huán)境下的規(guī)劃難度不同，對(duì)相同環(huán)境下的不同算法各進(jìn)行30 次實(shí)驗(yàn)，不同環(huán)境下的仿真時(shí)間、起始點(diǎn)和目標(biāo)點(diǎn)的位置見(jiàn)表1。算法的運(yùn)行環(huán)境：64 位Windows10 操作系統(tǒng)；處理器Intel Core i5-8250U CPU；主頻1.60 GHz；內(nèi)存8 GB；計(jì)算軟件Matlab 2017b。

圖5 是不同環(huán)境下運(yùn)行結(jié)果的比較。如圖5（a）所示，Map1 是一個(gè)障礙物密度較大的環(huán)境場(chǎng)景，可以看出RRT*算法一成不變地對(duì)整個(gè)空間均勻采樣，產(chǎn)生了大量那些遠(yuǎn)離航跡的采樣，而這些節(jié)點(diǎn)對(duì)最終航跡并沒(méi)有起到降低代價(jià)的作用；改進(jìn)RRT*-FN 算法在一定程度上改善了RRT*算法對(duì)空間的盲目探索，在找到初始航跡后，便可以使用啟發(fā)式采樣向樹(shù)中添加有意義的節(jié)點(diǎn)，達(dá)到集中采樣的效果來(lái)優(yōu)化航跡；相比之下，本文所提出的學(xué)習(xí)高質(zhì)量節(jié)點(diǎn)的策略、自適應(yīng)調(diào)整抽樣分布的方法具有更好地識(shí)別航跡特征的能力，優(yōu)化效果更為出色。Map2 是一個(gè)帶有U 形障礙的特殊環(huán)境場(chǎng)景，從起始點(diǎn)到目標(biāo)點(diǎn)需要穿過(guò)兩個(gè)狹窄通道，規(guī)劃難度較大，如圖5（b）所示，由于最終航跡長(zhǎng)度和起始點(diǎn)到目標(biāo)點(diǎn)的直線距離相差很大，橢圓子集采樣效果退化嚴(yán)重，大多節(jié)點(diǎn)都集中在了障礙物內(nèi)，包裹航跡的附近節(jié)點(diǎn)分布不均勻，導(dǎo)致改進(jìn)RRT*-FN算法的優(yōu)化性能大大減弱。Map3 是一個(gè)由20 個(gè)隨機(jī)半徑、隨機(jī)位置威脅球組成的三維環(huán)境場(chǎng)景，如圖5（c）所示，在三維環(huán)境下本文算法依然可以靈活地調(diào)整采樣空間，相較于另外兩種算法也具有更好的優(yōu)化效果。Map4 是一個(gè)帶有兩個(gè)狹窄通道的三維環(huán)境場(chǎng)景，如圖5（d）所示，可以看出隨著環(huán)境空間維度的提升，規(guī)劃難度加大，算法的性能對(duì)比也變得越來(lái)越明顯。

圖5 Map1～4上的算法性能對(duì)比Fig.5 Performance comparison of three algorithms on Map1-4

表2 列出了30 次仿真實(shí)驗(yàn)的各算法性能指標(biāo)的平均值。從表2 可以發(fā)現(xiàn)，從找尋初始航跡的消耗時(shí)間上看，近鄰優(yōu)化步驟使得RRT*算法比RRT算法要慢很多，本文方法根據(jù)樹(shù)的生長(zhǎng)情況去自適應(yīng)調(diào)整采樣策略，因此比RRT 算法用了更少的時(shí)間。從最終生成的航跡代價(jià)來(lái)看，在相同的仿真時(shí)間下，本文方法依據(jù)當(dāng)前近鄰優(yōu)化次數(shù)周期性地學(xué)習(xí)樹(shù)中高質(zhì)量節(jié)點(diǎn)，新的抽樣分布更能反映當(dāng)前最優(yōu)航跡的幾何特征，相較于其他兩種算法具有更高的優(yōu)化效率，最終生成了更低代價(jià)的航跡。

表1 仿真參數(shù)設(shè)置Tab.1 Simulation parameter setting

圖6 是不同環(huán)境下算法的收斂結(jié)果對(duì)比。由圖6 可以看出，本文方法在搜索初始解時(shí)目的性強(qiáng)，優(yōu)化階段集中采樣效果好，因此相較于另外兩種算法具有更好的收斂性，而且在規(guī)劃空間維度變高后差距更為明顯。

表2 不同算法性能指標(biāo)平均值Tab.2 Average performance indices of different algorithms

圖6 時(shí)間與路徑長(zhǎng)度的關(guān)系對(duì)比Fig.6 Comparison of relationship between time and path length

6 結(jié)語(yǔ)

本文提出的AS-RRT*算法主要是在RRT*算法基礎(chǔ)上對(duì)采樣空間的建模進(jìn)行改進(jìn)，分為搜索和優(yōu)化兩階段考慮、在向光和背光區(qū)域的有偏采樣、學(xué)習(xí)高質(zhì)量節(jié)點(diǎn)等策略都是為了降低隨機(jī)采樣帶來(lái)的負(fù)面影響。最后的實(shí)驗(yàn)結(jié)果中也表明了本文方法的有效性。

本文只考慮了在靜態(tài)環(huán)境下以無(wú)人機(jī)質(zhì)點(diǎn)模型為載體的離線路徑規(guī)劃，未來(lái)方向可考慮任務(wù)目標(biāo)的運(yùn)動(dòng)學(xué)約束、在動(dòng)態(tài)環(huán)境下在線的路徑規(guī)劃。