魏武 韓進 李艷杰 高天嘯

(華南理工大學(xué) 自動化科學(xué)與工程學(xué)院，廣東 廣州 510640)

近幾十年來，路徑規(guī)劃問題的研究在機器人領(lǐng)域興起并成為研究的熱點[1- 2]。路徑規(guī)劃問題就是在環(huán)境完全已知或部分已知的前提下尋找一條與障礙物無碰撞的路徑。環(huán)境完全已知的路徑規(guī)劃問題為全局路徑規(guī)劃，環(huán)境部分已知的路徑規(guī)劃問題為局部路徑規(guī)劃。路徑規(guī)劃算法主要包括基于采樣的方法[3- 4]、人工勢場法[5]、可視圖法[6]、生物智能算法[7- 9]和基于深度學(xué)習(xí)的方法[10]，其中基于采樣的方法以概率路圖法(PRM)[3]和快速擴展隨機樹(RRT)[4]為代表，生物智能算法的典型代表為遺傳算法[7]和蟻群算法[8- 9]。

基于采樣的路徑規(guī)劃方法具有強大的搜索能力，將規(guī)劃算法從幾何學(xué)模型中分離[2]，在探索高維空間方面具有突出的表現(xiàn)。因此，眾多學(xué)者展開了相關(guān)研究，其中RRT算法[4]引起了高度關(guān)注并得到了較為廣泛的應(yīng)用[10- 12]。RRT算法雖然具有概率完備性[13]和計算量小的優(yōu)點，但RRT算法本身不考慮所獲得的路徑代價，故無法保證路徑的質(zhì)量[14]，即不具備漸進最優(yōu)性的特性，而這一特性對于實際應(yīng)用尤為重要。

為了加快RRT算法尋找路徑的速度，文獻[13]提出了雙向RRT算法，與基本的RRT算法最大的不同在于，雙向RRT算法同時存儲從起點、終點處生成的兩棵隨機樹而不是僅有一棵樹。在計算資源充足的前提下，雙向RRT算法的多樹思想為加快收斂速度提供了一個新的思路。針對RRT算法不具備漸進最優(yōu)性的缺點，大量學(xué)者基于RRT算法提出了相應(yīng)的改進算法。文獻[15]提出了具備漸進最優(yōu)性的RRT*算法，其核心在于增添了選擇最優(yōu)父節(jié)點和剪枝兩個優(yōu)化過程，但此算法存在收斂速度慢的缺點。文獻[16]從限定隨機點采樣范圍的角度提出了Informed-RRT*算法，雖然其在一定程度上加快了RRT*算法的收斂速度，但其采樣過程在很大程度上依賴于當(dāng)前最優(yōu)解，且當(dāng)超橢球超出規(guī)劃問題本身時，算法無法進行下去。文獻[17]受多棵擴展樹可加快收斂速度的啟發(fā)而提出的雙向RRT*算法，在保證漸進最優(yōu)性的前提下很大程度上提高了收斂速度。文獻[18]通過擴大RRT*算法兩個優(yōu)化過程的追溯范圍，提出了Quick-RRT*算法，該算法為RRT*算法提供了一種新的樹擴展框架。

基于多樹結(jié)構(gòu)的雙向RRT*和Quick-RRT*算法的優(yōu)勢，本文提出了雙樹Quick-RRT*算法，并通過仿真實驗分析了該算法的性能。

1 問題描述及背景知識

1.1 問題描述

路徑規(guī)劃空間常采用位形空間進行描述。依據(jù)是否與障礙物發(fā)生碰撞，整個位形空間C劃分為自由位形Cfree和碰撞位形Cobs，其中Cobs=CCfree。

路徑規(guī)劃問題是給定起點vstart和終點vgoal(vstart,vgoal∈Cfree)的前提下尋找一條從vstart到vgoal的無碰撞路徑。

1.2 RRT*算法

RRT*是一種單查詢樹狀結(jié)構(gòu)的搜索算法，從起點vstart初始化變量Tree后進入迭代過程，具體迭代過程如下：

(1)使用隨機采樣函數(shù)Sample()產(chǎn)生隨機采樣點vrand。

(2)產(chǎn)生新節(jié)點vnew。先使用Nearest(vrand,Tree)函數(shù)遍歷Tree得到最近節(jié)點(vnearest)，然后Steer(vnearest,vrand,δstep)函數(shù)以vnearest為起點，朝著vrand前進δstep，從而產(chǎn)生vnew。

(3)在vnearest和vnew無碰撞的情況下，優(yōu)化vstart到vnew的路徑。Near(Tree,vnew,rnear)函數(shù)以vnew為圓心、rnear為半徑找出鄰域Vnear，ChooseParent(Vnear,vnew)函數(shù)判斷vnew父節(jié)點替換成Vnear中的點后是否會縮短vstart到vnew的距離，如果縮短則進行替換操作，否則不操作。

(4)優(yōu)化vstart到Vnear中點的路徑。Rewire(Vnear,vnew)函數(shù)判斷Vnear中每個節(jié)點是否可以將其父節(jié)點更新為vnew，如果路徑距離減小則進行父節(jié)點更新操作，否則不操作。優(yōu)化過程使得RRT*算法具有漸進最優(yōu)性[15]。RRT*算法的具體實現(xiàn)過程如下：

{Tree.init(vstart);

for i =1 to K do

vrand←Sample(i);

vnearest←Nearest(vrand,Tree);

vnew←Steer(vnearest,vrand,δstep);

if CollisionFree(vnearest,vnew,map)

Vnear←Near(Tree,vnew,rnear);

vparent←ChooseParent(Vnear,vnew);

Tree←Link(vparent,vnew);

Tree←Rewire(Vnear,vnew);

end if

end for

returnTree;}

1.3 Quick-RRT*算法

Quick RRT*算法利用三角不等式定理對隨機樹結(jié)構(gòu)進行優(yōu)化。與RRT*算法相比，Quick-RRT*算法擴大了ChooseParent(Vnear,vnew)和Rewire(Vnear,vnew)函數(shù)中Vnear的范圍，在保持時間復(fù)雜度和空間復(fù)雜度相同的情況下，獲得了更優(yōu)的初始路徑和更快的收斂速度[18]。Quick-RRT*算法的具體實現(xiàn)過程如下：

{Tree.init(vstart);

for i=1 to K do

vrand←Sample(i);

vnearest←Nearest(vrand,Tree);

vnew←Steer(vnearest,vrand,δstep);

if CollisionFree(vnearest,vnew,map)

Vnear←Near(Tree,vnew,rnear);

VprtNear←Ancestry(Tree,Vnear,dnear);

vparent←ChooseParent(Vnear∪VprtNear,vnew);

Tree←Link(vparent,vnew);

VprtNew←Ancestry(Tree,vnew,dnew);

Tree←Rewire(Vnear,vnew∪VprtNew);

end if

end for

return Tree;}

Quick RRT*算法相較于RRT*算法,增加了Ancestry(Tree,V,d)函數(shù)，該函數(shù)的作用是追溯鄰域V的父節(jié)點集合。d為父節(jié)點層數(shù)，d值越大，得到的隨機樹結(jié)構(gòu)越優(yōu)，但計算資源的需求會越強[18]。

2 雙樹Quick-RRT*算法

基于多樹結(jié)構(gòu)的雙向RRT*算法在提高收斂速度上和Quick-RRT*可與多種啟發(fā)方法結(jié)合的優(yōu)勢，本文提出了雙樹Quick-RRT算法，其具體實現(xiàn)過程如下：

{Tree1.init(vstart);

Tree2.init(vgoal);

for i = 1 to K do

vrand←Sample(i);

vnearest←Nearest(vrand,Tree1);

vnew←Steer(vnearest,vrand,δstep);

if CollisionFree(vnearest,vnew,map)

Vnear←Near(Tree1,vnew,rnear);

VprtNear←Ancestry(Tree1,Vnear,dnear);

vparent←ChooseParent(Vnear∪VprtNear,vnew);

Tree1←Link(vparent,vnew);

VprtNew←Ancestry(Tree1,vnew,dnew);

Tree1←Rewire(Vnear,vnew∪VprtNew);

Tree2←Connect(Tree2,vnew);

if isConnected(Tree1,Tree2)

Path=FillPath(Tree1,Tree2);

end if

Swap(Tree1,Tree2);

end if

end for

return path;}

起點樹Tree1和終點樹Tree2進行初始化后,進入迭代過程，具體迭代過程如下：

(1)產(chǎn)生新節(jié)點，使用Sample()、Nearest(vrand,Tree1)函數(shù)產(chǎn)生vnearest，使用Steer(vnearest,vrand,δstep)函數(shù)產(chǎn)生vnew。

(2)在vnearest和vnew無碰撞的情況下，優(yōu)化vstart到vnew的路徑。Near(Tree,vnew,rnear)和Ancestry(Tree,Vnear,dnear)兩個函數(shù)找出vnew的潛在父節(jié)點，ChooseParent(Vnear∪VprtNear,vnew)函數(shù)在潛在父節(jié)點中選擇父節(jié)點，使得從vstart到vnew的路徑距離最小，然后使用Link(vparent,vnew)函數(shù)連接vnew和vparent這兩個節(jié)點。

(3)優(yōu)化vstart到Vnear中點的路徑。使用Ancestry(Tree,vnew,dnew)函數(shù)找出vnew的父節(jié)點vprtNear，將vnew和vprtNear作為VprtNear中點的潛在父節(jié)點，使用Rewire(Vnear,vnew∪VprtNew)函數(shù)在潛在父節(jié)點中找出父節(jié)點，使得路徑距離最小。

(5)使用FillPath(Tree1,Tree2)函數(shù)拼接路徑。在Tree2與vnew相連接的情況下，Tree1從vnew追溯父節(jié)點直到vstart，這是Tree1部分的路徑，Tree2采用相同的方法，從與vnew相連接的節(jié)點開始追溯父節(jié)點直到vgoal，這是Tree2部分路徑，將這兩段路徑進行拼接，得到可行路徑。

(6)使用Swap(Tree1,Tree2)函數(shù)交換兩棵樹中的內(nèi)容，因為上述迭代過程Tree1只增加了一個節(jié)點，Tree2中增加了多個節(jié)點，所以需要交換兩棵樹內(nèi)容，使得兩棵樹經(jīng)過數(shù)次迭代后節(jié)點數(shù)量保持平衡。

Quick-RRT*和雙樹的融合關(guān)鍵在于Connect(Tree2,vnew)和Swap(Tree1,Tree2)函數(shù)。當(dāng)Tree1完成vnew的拓展后，Connect(Tree2,vnew)將Tree2與vnew進行直線連接，連接過程會向Tree2中增加多個點，且直線連接不需要優(yōu)化，拓展只是增加一個點且需要優(yōu)化路徑，所以Connect()函數(shù)提高了算法的效率。Swap(Tree1,Tree2)將兩棵樹中的內(nèi)容交換，該函數(shù)可以保證兩棵樹中點的數(shù)量大致平衡。

3 算法性能分析

3.1 概率完備性

對于雙向RRT算法，隨機樹中的點v在Cfree中服從均勻分布，且該算法具有概率完備性[13]。雙樹Quick-RRT*算法較于雙向RRT算法，產(chǎn)生點的方法相同，區(qū)別在于優(yōu)化了點之間的連接方式，故本算法中的點v在Cfree中同樣服從均勻分布。同樣地，雙樹Quick-RRT*算法具有概率完備性。

3.2 漸進最優(yōu)性

(1)

文獻[19]中指出，若算法滿足概率完備性，且滿足該文獻中的假設(shè)11(代價函數(shù)的疊加性)、12(代價函數(shù)的連續(xù)性)和13(以Cfree中的點x為圓心，ε(ε∈R+)為半徑的圓(或超球體)Bε,x?Cfree)，則算法滿足漸進最優(yōu)性。

代價函數(shù)的疊加性和連續(xù)性確保了兩個相似路徑的代價也相似，假設(shè)13確保了最優(yōu)軌跡附近有自由空間以允許收斂。概率完備性確保了當(dāng)時間充足時，地圖中存在n(n→∞)個點的連通圖。當(dāng)算法迭代時，連通圖中的路徑會不斷縮短，代價函數(shù)的連續(xù)性確保了在路徑縮短的同時，代價函數(shù)也會縮小，最終趨近于最優(yōu)路徑，也就是漸進最優(yōu)性。

雙樹Quick-RRT*算法滿足概率完備性，路徑長度lpath滿足假設(shè)11、12，假設(shè)13是技術(shù)性假設(shè)，對地圖的一種要求，與算法本身無關(guān)，故雙樹Quick-RRT*算法滿足漸進最優(yōu)性。

4 仿真實驗

為了分析本文提出的算法的性能，設(shè)計了3種環(huán)境進行仿真分析，考慮到移動機器人的自身半徑，對障礙物進行了膨化處理，并將機器人看做一個質(zhì)點。仿真實驗平臺及配置為：Matlab R2018b，64位Windows 10，處理器Intel(R)Core(TM)i7- 7500U，CPU主頻2.7 GHz，內(nèi)存12 GB。

仿真地圖環(huán)境如圖1所示，所有環(huán)境的地圖規(guī)模均為[1 184,872]，以地圖左上角為坐標原點，最優(yōu)路徑長度為lopt，次優(yōu)路徑長度lsubOpt滿足lopt

圖1 仿真實驗環(huán)境

采用本文提出的雙樹Quick-RRT*算法與RRT*、Quick-RRT*和雙向RRT*算法進行對比仿真實驗，其中RRT*和Quick RRT*是單樹算法，雙向RRT*和雙樹Quick-RRT*是雙樹算法，算法中設(shè)置的變量及參數(shù)如下：步長δstep=30像素，產(chǎn)生Vnear的半徑rnear=80像素，Quick-RRT*和雙樹Quick-RRT*算法中選擇父節(jié)點的深度dnear=dnew=1。

使用以下5個參數(shù)對上述4種算法進行比較：初始路徑的長度(linit)、tfind、t5%，算法發(fā)現(xiàn)路徑的成功率(Rsuc)和路徑長度(lpath)。前3個參數(shù)取各算法運行100次后的均值；Rsuc和時間呈正相關(guān)，4種算法都具備概率完備性，當(dāng)時間充足時，Rsuc都會達到100%。本文采用以下方法計算Rsuc：算法運行100次，記錄每次的tfind，計算tfind從0 s到指定時刻的分布數(shù)量，就是該時刻的成功率。lpath用以顯示算法的路徑收斂效果。由于算法具有隨機性，在相同條件下多次運行同一算法，tfind都會不同，這就導(dǎo)致算法在剛開始運行時沒有發(fā)現(xiàn)可行路徑，或者只有少數(shù)次數(shù)產(chǎn)生了lpath，這些少數(shù)次數(shù)無法代表100次運行的結(jié)果，所以需要Rsuc作為判斷依據(jù)。本文按以下方法統(tǒng)計lpath：算法運行100次，每次運行都會定時記錄lpath的值，故當(dāng)100次運行完畢，每一個時刻都會有100次有效或者無效的數(shù)據(jù)，無效數(shù)據(jù)指該時刻沒有產(chǎn)生lpath(此時lpath=0)，當(dāng)該時刻有60次以上的有效數(shù)據(jù)才計算該算法的lpath均值。采用此種方法計算時，因算法之間的時間差異較大，故不同算法間的時間跨度會較大。

上文說明的數(shù)據(jù)是分為兩批實驗記錄的，因為同時記錄所有數(shù)據(jù)難度較大。lpath是算法運行100次定時記錄的，linit、tfind、t5%和Rsuc是另外運行100次記錄的。linit和lpath的起始值有差異，主要受兩個因素的影響，一是數(shù)據(jù)不在同一批實驗中獲得，二是linit不受Rsuc的影響，而lpath受Rsuc的影響。

4.1 U形障礙物環(huán)境

U形障礙物環(huán)境中的vstart設(shè)置為[592,436]，vgoal設(shè)置為[1 000,436]。4種算法的仿真結(jié)果如圖2所示。

圖2 4種算法在U形障礙物環(huán)境下的仿真結(jié)果

從圖2可知，在U形障礙物環(huán)境下，雙樹Quick-RRT*算法的性能最佳，路徑完全貼合U形障礙物邊緣。4種算法的lfirst、tfind和t5%如表1所示，相同時間的成功率和路徑長度如圖3所示。

從表1可知，雙樹Quick-RRT*算法的性能表現(xiàn)最優(yōu)，相較于Quick-RRT*算法，tfind縮短了89.8%，linit卻更短，t5%縮短了68.0%；相較于RRT*和雙向RRT*算法，tfind分別縮短了79.1%和57.0%，t5%分別縮短了83.0%和77.7%。可以得出，雙樹Quick-RRT*算法大大縮短了tfind和t5%，產(chǎn)生的路徑更優(yōu)。

表1 4種算法在U形障礙物環(huán)境中的性能比較

圖3 U形障礙物環(huán)境下4種算法的路徑長度和發(fā)現(xiàn)路徑的成功率

圖3(a)更加具體地展示了tfind的時間區(qū)間分布，雙樹Quick-RRT*算法全部成功所需時間最少，曲線上升最快，雙向RRT*算法次之，Quick-RRT*所需時間最長?？梢园l(fā)現(xiàn)，雙樹結(jié)構(gòu)算法比單樹結(jié)構(gòu)算法到達Rsuc=100所需的時間更短。

從圖3(b)可知：雙樹Quick-RRT*算法出現(xiàn)數(shù)據(jù)的時間最早，說明在100次實驗中，有60次以上的lpath所需時間最短，該曲線最為平緩，說明算法不需要花費過多的時間優(yōu)化路徑；雙向RRT*和RRT*算法雖然產(chǎn)生數(shù)據(jù)的時間也比較早，但曲線前后差值大，說明算法需要更多的時間優(yōu)化路徑；Quick-RRT*算法有60次以上的lpath所需時間最多，曲線前后差值也比雙樹Quick-RRT*算法大。

綜合上述分析可知：Quick-RRT*算法發(fā)現(xiàn)路徑需要的時間最長；相較于其他4種算法，雙樹Quick-RRT*算法的tfind最短，相同時間發(fā)現(xiàn)路徑的成功率最高，路徑收斂速度最快。

4.2 狹長通道環(huán)境

狹長通道環(huán)境中，vstart設(shè)置為[100,100]，vgoal設(shè)置為[1 100,700]。4種算法在該環(huán)境下的仿真結(jié)果如圖4所示。

圖4 4種算法在狹長通道環(huán)境下的仿真結(jié)果

從圖4可知，Quick-RRT*和雙樹Quick-RRT*算法的性能明顯優(yōu)于RRT*和雙向RRT*算法。4種算法的linit、tfind和t5%參數(shù)如表2所示，相同時間的成功率和路徑長度如圖5所示。

表2 4種算法在狹長通道環(huán)境中的性能比較

因為狹長通道空間有限，限制了路徑優(yōu)化的空間，使得linit與lsubOpt的差距減小，路徑優(yōu)化時間減少，例如圖5(b)中曲線比圖3(b)中曲線更加平緩，表2中tfind和t5%的差值減小，Quick-RRT*需要增加一位小數(shù)以顯示值的不同。雙樹Quick-RRT*算法相較于Quick-RRT*，linit雖略長，但tfind縮短了64.0%，t5%縮短了63.9%，而相較于RRT*和雙向RRT*算法，tfind分別縮短了67.4%和54.4%，t5%分別縮短了81.5%和54.9%。從表2可知：雙樹Quick-RRT*算法在保證路徑質(zhì)量的前提下，大幅度提高了算法的搜索路徑效率。

圖5 狹長通道環(huán)境下4種算法的的路徑長度和發(fā)現(xiàn)路徑的成功率

狹長通道空間的限制，使得算法的有效采樣點范圍大大縮小，加快了算法找到有效路徑的效率，也減小了算法之間成功率的差異。圖5(a)中曲線之間的差異比圖3(a)中小，RRT*和雙向RRT*算法的曲線在后期幾乎重合，說明狹長通道環(huán)境減小了單樹結(jié)構(gòu)算法和雙樹結(jié)構(gòu)算法之間的效率差異。雙樹Quick-RRT*和Quick-RRT*算法的曲線依舊有明顯的差異，說明本文提出的算法在效率上仍有明顯的優(yōu)勢。

圖5(b)中雙樹Quick-RRT*算法在2 s時的路徑長度因為有新產(chǎn)生的lpath加入而有微小的上升，而不是路徑?jīng)]有收斂。雙樹Quick-RRT*與Quick-RRT*算法的曲線幾乎重合，但前者的時間縮短了40%。雙樹Quick-RRT*與雙向RRT*算法的曲線相比，開始時間相同，但后者的路徑長度更長。相較于RRT*算法，雙樹Quick-RRT*算法的搜索路徑和路徑收斂效率高，產(chǎn)生的路徑短。綜合上述分析可知：即使在狹長空間這種有嚴重空間限制的環(huán)境中，雙樹Quick-RRT*算法使用相同的時間產(chǎn)生的路徑長度最短。

4.3 簡易迷宮環(huán)境

簡易迷宮環(huán)境中，vstart設(shè)置為[100,700]，vgoal設(shè)置為[1 000,100]，4種算法在簡易迷宮環(huán)境下的仿真結(jié)果如圖6所示，linit、tfind和t5%如表3所示，相同時間的成功率和路徑長度如圖7所示。

圖6 4種算法在簡易迷宮環(huán)境下的仿真結(jié)果

從圖6中可知，在簡易迷宮環(huán)境下，Quick-RRT*和雙樹Quick-RRT*算法的性能優(yōu)于RRT*和雙向RRT*算法。

表3 4種算法在簡易迷宮環(huán)境下的性能比較

從表3可知，雙樹Quick-RRT*算法的參數(shù)值仍然是4種算法中最小的，相較于RRT*、Quick-RRT*和雙向RRT*算法，tfind分別縮短了85.0%、89.7%和34.7%，t5%分別縮短了81.6%、55.9%和68.6%，說明本文提出的算法明顯縮短了tfind和t5%。

從圖7(a)可知，雙樹Quick-RRT*算法的Rsuc依舊是最快到達100%，雙向RRT*算法次之，Quick-RRT*算法最慢，說明雙樹Quick-RRT*算法在較為復(fù)雜的環(huán)境下仍然大幅度縮短了tfind。從圖7(b)可知：雙樹Quick-RRT*和Quick-RRT*算法的曲線幾乎重合，但前者出現(xiàn)數(shù)據(jù)的時間是后者的1/10，大幅度縮短了時間；與另外兩種算法相比，雙樹Quick-RRT*算法的搜索路徑效率和路徑長度都具有優(yōu)勢。

圖7 簡易迷宮環(huán)境下4種算法的路徑長度和發(fā)現(xiàn)路徑的成功率

綜合上述分析可知：在簡易迷宮環(huán)境中，相較于其他3種算法，雙樹Quick-RRT*算法的tfind仍舊是最小，路徑收斂效果最好。

5 結(jié)語

為進一步提高Quick-RRT*算法的收斂速度，本文提出了一種雙樹Quick-RRT*算法，該算法使用Quick-RRT*算法建立起點樹和終點樹，并分析了該算法的概率完備性和漸進最優(yōu)性。在3種不同環(huán)境下的仿真實驗結(jié)果結(jié)明：相較于其他3種算法，本文算法提高了尋路效率和質(zhì)量，tfind縮短了約69%，t5%縮短了約70%，linit縮短了約5%。

本文的研究對象是二維環(huán)境中移動機器人，未來可以考慮將雙樹Quick-RRT*算法應(yīng)用于多維度關(guān)節(jié)空間的移動機械臂的路徑規(guī)劃問題。