張 康，陳建平

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

0 引言

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

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

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

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

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

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

1 相關(guān)工作

1.1 RRT*

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

Sample：隨機地在狀態(tài)空間均勻采樣，生成一個狀態(tài)點。

Nearest：通過一個度量函數(shù)Distance 來衡量節(jié)點與采樣點的距離，函數(shù)Nearest 返回搜索樹T中與采樣點距離最近的節(jié)點。

Steer：給定兩點xnearest和xrand，Steer 函數(shù)返回擴展的新節(jié)點xnew，在滿足最大生長步長r的情況下，使新節(jié)點xnew盡可能地靠近xrand。

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

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

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

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

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

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

1.2 改進RRT*-FN

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

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

2 問題定義

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

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

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

3 AS-RRT*

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

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

3.1 搜索階段

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

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

向光區(qū)域是一個以目標點為中心的超球錐，對n維空間下的超球錐子集的均勻采樣可以通過約束一個超球的極坐標來方便地實現(xiàn)。

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

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

3.1.2 背光區(qū)域

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

為了減少不必要的碰撞檢測，引入一個基于碰撞增量的代價模型C，在計算最近的節(jié)點時，那些擴展失敗的節(jié)點將被考慮額外的代價。

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

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

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

3.2 優(yōu)化階段

3.2.1 節(jié)點篩選

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

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

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

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

高斯混合模型被定義為：

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

3.2.3 分模型個數(shù)K

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

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

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

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

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

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

3.2.5 更新周期

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

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

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

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

4 算法性能分析

4.1 概率完備性

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

4.2 計算復(fù)雜度

5 仿真實驗與結(jié)果分析

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

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

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

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

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

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

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

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

6 結(jié)語

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

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