梁永豪，陳秋蓮，王成棟

(廣西大學(xué) 計(jì)算機(jī)與電子信息學(xué)院，廣西 南寧 530004)

0 引 言

機(jī)器人路徑規(guī)劃，是通過算法在有障礙物的環(huán)境中找到一條從起點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑。路徑規(guī)劃算法作為移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一，受到國內(nèi)外學(xué)者的廣泛關(guān)注。

目前，路徑規(guī)劃算法主要包括遺傳算法[1]、粒子群算法[2]、人工勢場法[3]、基于網(wǎng)格的算法[4]和基于采樣的算法[5，6]。基于采樣的算法通用性比較強(qiáng)，受環(huán)境維度和復(fù)雜度的約束較小[7]，是當(dāng)前最流行的路徑規(guī)劃算法之一。快速擴(kuò)展隨機(jī)樹(RRT)[8]是基于采樣算法的典型代表，依靠隨機(jī)采樣和碰撞檢測模塊，在給定的空間內(nèi)生成一組離散的點(diǎn)。雖然RRT可以快速找到路徑，但它不能確保找到的路徑是最優(yōu)的。RRT*[9]是RRT算法的變體，在添加新節(jié)點(diǎn)時(shí)引入兩個(gè)優(yōu)化模塊：重選父節(jié)點(diǎn)和重布線。優(yōu)化模塊確保在采樣點(diǎn)趨于無窮時(shí)，RRT*算法找到最優(yōu)解。

RRT*是RRT算法發(fā)展的一個(gè)里程碑，吸引了很多學(xué)者，主要從兩個(gè)方面對算法的性能進(jìn)行改進(jìn)：采樣策略和樹擴(kuò)展策略。采樣策略主要是通過優(yōu)化采樣點(diǎn)，加速算法收斂。如知情采樣Informed-RRT*[10]限制采樣點(diǎn)在橢圓形采樣區(qū)域內(nèi)；目標(biāo)偏置[11]和人工勢場法[12]利用目標(biāo)點(diǎn)的信息來指導(dǎo)新節(jié)點(diǎn)生成方向；Bi-RRT*[13]則采用雙樹拓展的同時(shí)引入啟發(fā)式思想，加快收斂速度。樹擴(kuò)展策略則是對擴(kuò)展樹本身的結(jié)構(gòu)進(jìn)行優(yōu)化，使路徑長度盡可能短。如Quick-RRT*[14]使用三角不等式來優(yōu)化重選父節(jié)點(diǎn)和重布線模塊，縮短路徑長度；F-RRT*[15]在Quick-RRT*的基礎(chǔ)上以創(chuàng)造父節(jié)點(diǎn)替代重選父節(jié)點(diǎn)；KB-RRT*[16]通過添加運(yùn)動(dòng)學(xué)約束，減少不必要的節(jié)點(diǎn)生長，獲取滿足運(yùn)動(dòng)學(xué)約束，路徑更短的擴(kuò)展樹。但這些改進(jìn)依舊存在不足，如F-RRT*路徑規(guī)劃的效率還有不少的提升空間，同時(shí)在復(fù)雜障礙物環(huán)境下會(huì)產(chǎn)生較多的冗余節(jié)點(diǎn)；目標(biāo)偏置在局部最優(yōu)環(huán)境或者迷宮下，性能顯著下降；人工勢場法在多障礙物環(huán)境下計(jì)算量大，效率比較低。

針對RRT*中存在的盲目搜索、路徑較長和冗余節(jié)點(diǎn)多而導(dǎo)致路徑規(guī)劃效率低，本文提出一種融合樹擴(kuò)展策略和采樣策略的改進(jìn)RRT*算法(AF-RRT*)。采樣策略方面，采用動(dòng)態(tài)步長，減少冗余節(jié)點(diǎn)；利用自適應(yīng)探索，減少采樣的盲目性，加快路徑規(guī)劃速度，同時(shí)避免陷入局部最優(yōu)；樹擴(kuò)展策略方面，引入創(chuàng)造父節(jié)點(diǎn)，并與采樣策略有效融合，減少路徑長度。

1 相關(guān)工作

1.1 RRT算法

RRT的搜索過程類似于樹不斷地向四周擴(kuò)散生長。起點(diǎn)xstart為擴(kuò)展樹G的根節(jié)點(diǎn)，樹中節(jié)點(diǎn)用集合V表示，節(jié)點(diǎn)間的連接用集合E存儲(chǔ)。在自由空間隨機(jī)采樣節(jié)點(diǎn)xrand引導(dǎo)樹的生長方向。找到離隨機(jī)點(diǎn)最近的節(jié)點(diǎn)xnearest，在xnearest和xrand連線上以步長eta生成新節(jié)點(diǎn)xnew。若最近鄰節(jié)點(diǎn)與新節(jié)點(diǎn)之間的路徑與障礙物無碰撞，則將新節(jié)點(diǎn)加入樹中，否則重新采樣。重復(fù)上述過程，直到新節(jié)點(diǎn)擴(kuò)散到目標(biāo)點(diǎn)，完成整個(gè)搜索，返回該節(jié)點(diǎn)到根節(jié)點(diǎn)的連線作為路徑。偽代碼如算法1所示：

Algorithm1:RRT()

Input:xstart,xgoal,Map,K,eta,

Output:G=(V,E)

(1) V ← {xstart},E ← ?;

(2)fori=1 to Kdo

(3) xrand← SampleFree(i);

(4) xnearest← Nearest(V,xrand);

(5) xnew← NewState(xnearest,xrand,eta);

(6)ifCollisionFree(xnew,xnearest)then

(7) V ← V ∪ {xnew};

(8) E ← E ∪ {(xnearest,xnew)};

(9)ifDistance(xgoal,xnew) <λthen

(10)returnG=(V,E);

(11)endif

(12)endif

(13)endfor

其中函數(shù)SampleFree()在自由空間內(nèi)生成隨機(jī)節(jié)點(diǎn)；Nearest()利用歐式距離選擇樹節(jié)點(diǎn)中離隨機(jī)節(jié)點(diǎn)最近的節(jié)點(diǎn)；NewState()從兩點(diǎn)連線方向上擴(kuò)展固定步長，得到新節(jié)點(diǎn)；CollisionFree()判斷給定兩點(diǎn)間的連線是否在自由空間。

1.2 RRT*算法

RRT的隨機(jī)擴(kuò)展性使算法穩(wěn)定性差，無法收斂至全局最優(yōu)。RRT*在樹擴(kuò)展過程中，確定隨機(jī)點(diǎn)位置后，對新加入搜索樹的新節(jié)點(diǎn)，增加重選父節(jié)點(diǎn)和重布線操作。

RRT*的偽代碼如算法2所示，第(7)行和第(10)行分別為重選父節(jié)點(diǎn)ChooseParent()和重布線Rewire()。通過隨機(jī)點(diǎn)的增加實(shí)現(xiàn)路徑的漸進(jìn)優(yōu)化，當(dāng)隨機(jī)點(diǎn)數(shù)量趨于無窮時(shí)能夠獲得最優(yōu)解。

Algorithm2:RRT*()

Input:xstart,xgoal,Map,K,eta,Rnear

Output:G=(V,E)

(1) V ← {xstart},E ← ?;

(2)fori=1 to Kdo

(3) xrand← SampleFree(i);

(4) xnearest← Nearest(V,xrand);

(5) xnew← NewState(xnearest,xrand,eta);

(6)ifCollisionFree (xnew,xnearest)then

(7) xmin← ChooseParent(xnew,V,Rnear);

(8) V ← V ∪ {xnew};

(9) E ← E ∪ {(xmin,xnew)};

(10) G ← Rewire(V,E,xnew,Rnear);

(11)ifDistance(xgoal,xnew) <λthen

(12)returnG=(V,E);

(13)endif

(14)endif

(15)endfor

(1)重選父節(jié)點(diǎn)ChooseParent()函數(shù)：以新節(jié)點(diǎn)xnew為圓心，產(chǎn)生以Rnear為半徑的鄰域Xnear，以鄰域內(nèi)的節(jié)點(diǎn)為備選父節(jié)點(diǎn)。首先計(jì)算xnew與鄰域內(nèi)所有節(jié)點(diǎn)連接后的權(quán)值大小，若鄰域內(nèi)中存在節(jié)點(diǎn)xmin，使以xmin為父節(jié)點(diǎn)的xnew權(quán)值最小，則刪除xnew與原父節(jié)點(diǎn)xnearest的邊，在樹中新增xnew與xmin的邊集，如圖1(a)所示。

圖1 RRT*的樹擴(kuò)展

(2)重布線Rewire()函數(shù)：重布線則是在RRT采樣后加入了剪枝過程，優(yōu)化新節(jié)點(diǎn)附近的舊節(jié)點(diǎn)。若鄰域內(nèi)存在節(jié)點(diǎn)的權(quán)值大于以xnew為父節(jié)點(diǎn)到該節(jié)點(diǎn)的權(quán)值，則剪掉這些節(jié)點(diǎn)與原父節(jié)點(diǎn)的連線，以xnew作為新的父節(jié)點(diǎn)，更新鄰域內(nèi)其它節(jié)點(diǎn)的路徑長度，如圖1(b)所示。

2 AF-RRT*算法

RRT*算法能漸進(jìn)達(dá)到最優(yōu)，但存在隨機(jī)性過大、路徑代價(jià)大和冗余節(jié)點(diǎn)等不足，計(jì)算量隨節(jié)點(diǎn)數(shù)量的增加而急劇增加，需要較長的時(shí)間才能逼近最優(yōu)解。本文提出一種融合樹擴(kuò)展策略和采樣策略的AF-RRT*算法，提高路徑規(guī)劃效率。

2.1 AF-RRT*算法描述

AF-RRT*在RRT*基礎(chǔ)上，進(jìn)行3方面的改進(jìn)：①動(dòng)態(tài)步長，減少由于震蕩現(xiàn)象導(dǎo)致的冗余節(jié)點(diǎn)；②自適應(yīng)探索，減少采樣的隨機(jī)性；③創(chuàng)造父節(jié)點(diǎn)代替RRT*的重選父節(jié)點(diǎn)，使擴(kuò)展樹貼近障礙物生長，減短路徑長度。偽代碼如算法3所示：

Algorithm3:AF-RRT*()

Input:xstart,xgoal,Map,K,Ddich,eta,Rnear,Cnow,Ccol,Pgoal,Prand

Output:G=(V,E)

(1) V ← {xstart},E ← ?;

(2)Fori=1 to Kdo

(3) xrand← SampleFree(i);

(4) xnearest← Nearest(V,xrand);

(5) Eta ← DSS(xnearest,xgoal,eta);

(6) xnew← AES(xrand,xgoal,xnearest,Eta,&Cnow,Ccol,Pgoal,Prand);

(7)ifxnew≠ xrandthen

(8) xreachest← FindReachest(G,xnearest,xnew,xstart);

(9) xcreate←CreateNode(G,xreachest,xnew,Ddich,xstart);

(10)ifxcreate≠ ?then

(11) V ← V ∪ {xcreate,xnew};

(12) E←E∪{(Parent(xreachest),xcreate),(xcreate,xnew)};

(13)else

(14) V ← V ∪ {xnew};

(15) E ← E ∪ {(xreachest,xnew)};

(16)endif

(17) G ← Rewire(V,E,xnew,Rnear);

(18)ifDistance(xgoal,xnew) <λthen

(19)returnG = (V,E);

(20)endif

(21)endif

(22)endfor

其中函數(shù)DSS()根據(jù)當(dāng)前最鄰近節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離動(dòng)態(tài)調(diào)整步長；AES()根據(jù)擴(kuò)展樹各節(jié)點(diǎn)位置，進(jìn)行自適應(yīng)探索，生成新節(jié)點(diǎn)；FindReachest()搜索與新節(jié)點(diǎn)連線不經(jīng)過障礙物的最遠(yuǎn)祖先節(jié)點(diǎn)；CreateNode()生成創(chuàng)造節(jié)點(diǎn)，在障礙物邊緣連接最遠(yuǎn)祖先節(jié)點(diǎn)和新節(jié)點(diǎn)。

算法在第(1)、第(2)行初始化擴(kuò)展樹，沒達(dá)到最大迭代次數(shù)時(shí)，進(jìn)入循環(huán)。

第(3)～第(7)行產(chǎn)生新節(jié)點(diǎn)。確定隨機(jī)點(diǎn)xrand與最近節(jié)點(diǎn)xnearest后，DSS()根據(jù)xnearest與目標(biāo)點(diǎn)的距離計(jì)算動(dòng)態(tài)步長，AES()使用當(dāng)前步長生成新節(jié)點(diǎn)xnew。

第(8)～第(16)行是創(chuàng)造父節(jié)點(diǎn)的過程，分為兩步。FindReachest()找到新節(jié)點(diǎn)xnew的最遠(yuǎn)祖先節(jié)點(diǎn)xreachest。CreateNode()根據(jù)xnew和xreachest的位置，在障礙物邊緣生成創(chuàng)造節(jié)點(diǎn)xcreate。若xcreate不為空，則將xnew和xcreate加入擴(kuò)展樹；否則將xnew加入擴(kuò)展樹。

第(17)～第(22)行是重布線和算法終止條件。

2.2 動(dòng)態(tài)步長

RRT*算法生成新節(jié)點(diǎn)xnew后，判斷其是否在目標(biāo)點(diǎn)xgoal的λ鄰域，若是，則找到一條可行路徑。為了保證算法的效率，通常取步長為eta>λ。但是，若xnearest與xgoal的距離在(λ，eta)之間，使用固定步長進(jìn)行擴(kuò)展會(huì)造成目標(biāo)點(diǎn)附近的震蕩。大地圖中采用大擴(kuò)展步長時(shí)震蕩現(xiàn)象尤為明顯，會(huì)增加路徑規(guī)劃時(shí)間和冗余節(jié)點(diǎn)。DSS()函數(shù)通過動(dòng)態(tài)步長來解決震蕩問題。每次擴(kuò)展，先計(jì)算xnearest和xgoal的距離Dis，若Dis>eta時(shí)，則擴(kuò)展步長不變；否則，將擴(kuò)展步長調(diào)整為Dis。

2.3 自適應(yīng)探索

擴(kuò)展樹新節(jié)點(diǎn)的探索可以分為強(qiáng)探索和弱探索。強(qiáng)探索狀態(tài)下擴(kuò)展樹往隨機(jī)節(jié)點(diǎn)生長，增加多樣性；弱探索狀態(tài)下擴(kuò)展樹往目標(biāo)點(diǎn)生長，縮短搜索時(shí)間。AES()函數(shù)可根據(jù)擴(kuò)展樹的探索狀態(tài)生成新節(jié)點(diǎn)。

Function:AES()

Input:xrand,xgoal,xnearest,Eta,Cnow,Ccol,Ccont,Pgoal,Prand

Output:xnew

(1) xnew← xrand;

(2) P ← Estatus(Cnow,Ccol,Pgoal,Prand);

(3) xpnew← xnearest+P*(xgoal-xnearest)*Eta+(1-P)*(xrand-xnearest)*Eta

(4)ifCollisionFree (xnew,xnearest)then

(5) xnew← xpnew;

(6)else

(7) xpnew← xnearest+(1-P)*(xgoal-xnearest)*Eta+P*(xrand-xnearest)*Eta

(8)ifCollisionFree (xnew,xnearest)then

(9) xnew← xpnew;

(10)else

(11) Cnow← Cnow+1

(12)endif

(13)endif

(14)returnxnew

其中Estatus()為擴(kuò)展樹的探索狀態(tài)函數(shù)。初始狀態(tài)為弱探索，Cnow為擴(kuò)展樹新節(jié)點(diǎn)生成失敗次數(shù)。Cnow除以Ccol取整，若為奇數(shù)，則返回值為Prand，擴(kuò)展樹進(jìn)入強(qiáng)探索狀態(tài)；否則，返回值為Pgoal，擴(kuò)展樹進(jìn)入弱探索狀態(tài)。Prand+Pgoal=1。調(diào)整參數(shù)Ccol，可控制擴(kuò)展樹探索狀態(tài)切換的速度。

在確定P值后，新節(jié)點(diǎn)由目標(biāo)點(diǎn)和隨機(jī)點(diǎn)共同確定。目標(biāo)點(diǎn)和隨機(jī)點(diǎn)兩個(gè)方向的權(quán)值分別是P和(1-P)，進(jìn)行矢量合成確定新節(jié)點(diǎn)的擴(kuò)展方向和擴(kuò)展距離。

新節(jié)點(diǎn)坐標(biāo)由式(1)確定，其中Eta為擴(kuò)展步長

(1)

弱探索狀態(tài)時(shí)，擴(kuò)展樹偏向目標(biāo)點(diǎn)生長，如圖2黑色平行四邊形所示。若新節(jié)點(diǎn)xnew和最近點(diǎn)xnearest的連線不經(jīng)過障礙物，則擴(kuò)展成功；否則，交換隨機(jī)點(diǎn)和目標(biāo)點(diǎn)的權(quán)值，使樹偏向隨機(jī)點(diǎn)擴(kuò)展，如圖2灰色平行四邊形所示。再次進(jìn)行碰撞檢測，若通過則擴(kuò)展成功；否則失敗次數(shù)加1，重新采樣。通過交換目標(biāo)點(diǎn)和隨機(jī)點(diǎn)兩個(gè)方向的權(quán)值，快速繞開障礙物。

圖2 新節(jié)點(diǎn)擴(kuò)展的自適應(yīng)探索

2.4 創(chuàng)造父節(jié)點(diǎn)

RRT*在現(xiàn)有的樹節(jié)點(diǎn)中重選父節(jié)點(diǎn)，如果鄰域內(nèi)路徑代價(jià)普遍比較大，父節(jié)點(diǎn)重選作用不大。增大鄰域半徑可以擴(kuò)大父節(jié)點(diǎn)選擇范圍，但鄰域內(nèi)節(jié)點(diǎn)數(shù)量增加會(huì)顯著增加計(jì)算量。本文以創(chuàng)造父節(jié)點(diǎn)代替重選父節(jié)點(diǎn)，在障礙物邊緣創(chuàng)造一個(gè)節(jié)點(diǎn)作為新節(jié)點(diǎn)的父節(jié)點(diǎn)，使擴(kuò)展樹貼近障礙物生長，降低路徑代價(jià)。創(chuàng)造父節(jié)點(diǎn)由兩個(gè)步驟來完成：查找最遠(yuǎn)祖先節(jié)點(diǎn)和創(chuàng)造新節(jié)點(diǎn)。

(1)查找最遠(yuǎn)祖先節(jié)點(diǎn)：偽代碼見函數(shù)FindReachest()。把最鄰近節(jié)點(diǎn)賦給xreachest。判斷xreachest的父節(jié)點(diǎn)與xnew的連線是否在自由空間，若是則把xreachest的父節(jié)點(diǎn)賦給xreachest，繼續(xù)搜索其父節(jié)點(diǎn)。否則返回當(dāng)前xreachest。

Function:FindReachest()

Input:G,xnew,xnearest,xstart

Output:xreachest

(1) xreachest← xnearest;

(2)Whilexreachest≠ xstartdo

(3)ifCollisionFree(xnew,Parent(xreachest))then

(4) xreachest← Parent(xreachest);

(5)else

(6)returnxreachest

(7)endif

(8)endwhile

(9)returnxreachest

由于只搜索xnew的祖先節(jié)點(diǎn)，不搜索xnew周圍的節(jié)點(diǎn)，搜索節(jié)點(diǎn)數(shù)量大幅度減少，降低了搜索時(shí)間。

(2)創(chuàng)造新節(jié)點(diǎn)：在xreachest及其父節(jié)點(diǎn)之間的連線上找到節(jié)點(diǎn)xallow，使得xallow與xnew的連線L位于障礙物邊緣。在連線L上找到一點(diǎn)xcreate，使xcreate可同時(shí)連接xnew和xreachest的父節(jié)點(diǎn)，如圖3粗實(shí)線路徑所示。從圖3中可以明顯看出粗實(shí)線路徑比虛線路徑短。與重選父節(jié)點(diǎn)的路徑(圖1(a))相比，路徑長度更是短得多。

圖3 創(chuàng)造父節(jié)點(diǎn)過程

創(chuàng)造新節(jié)點(diǎn)過程的偽代碼見函數(shù)CreateNode()，第(4)～第(11)行找到節(jié)點(diǎn)xallow確定連線L的位置，第(13)～第(20)行找到節(jié)點(diǎn)xcreate。其中參數(shù)Ddich用來控制新節(jié)點(diǎn)與障礙物的逼近程度。Ddich越小，路徑長度越短，計(jì)算時(shí)間越多。調(diào)整Ddich的大小，可以平衡路徑長度和時(shí)間代價(jià)。

Function:CreateNode()

Input:G,xreachest,xnew,Ddich,xstart

Output:xcreate

(1) xallow← xreachest;

(2)ifxreachest≠ xstartthen

(3) xforbid← Parent(xreachest);

(4)WhileDistance(xallow,xforbid) >Ddichdo

(5) xmid← (xallow+xforbid)/2;

(6)ifCollisionFree(xnew,xmid)then

(7) xallow← xmid;

(8)else

(9) xforbid← xmid;

(10)endif

(11)endwhile

(12) xforbid← xnew;

(13)WhileDistance(xallow,xforbid) >Ddichdo

(14) xmid← (xallow+xforbid)/2;

(15)ifCollisionFree(Parent(xreachest),xmid)then

(16) xallow← xmid;

(17)else

(18) xforbid← xmid;

(19)endif

(20)endwhile

(21)endif

(22)ifxallow≠ xreachestthen

(23) xcreate← xmid;

(24)else

(25) xcreate← ?;

(26)endif

(27)returnxcreate

3 實(shí) 驗(yàn)

仿真實(shí)驗(yàn)通過python實(shí)現(xiàn)，實(shí)驗(yàn)環(huán)境為：Windows10，Intel(R) Core(TM) i7-5500U，主頻2.40 GHz，內(nèi)存8 G。地圖中S點(diǎn)為起點(diǎn)，T點(diǎn)為終點(diǎn)，黑色矩形為障礙物，白色為自由區(qū)域，黑色細(xì)線是算法在路徑規(guī)劃中生成的擴(kuò)展樹。為驗(yàn)證算法的有效性，在4個(gè)不同障礙物的全局環(huán)境，分別與原始算法RRT*、F-RRT*進(jìn)行對比實(shí)驗(yàn)。地圖大小均為640×480，公共參數(shù)設(shè)置：初始步長eta=40，目標(biāo)點(diǎn)鄰域λ=15，搜索半徑Rnear=45，Ddich的取值為2，Ccol的取值為50，Prand取值為0.2，Pgoal取值為0.8。

3.1 仿真實(shí)驗(yàn)

3.1.1 簡單障礙物環(huán)境

簡單障礙物環(huán)境如圖4所示，只有一個(gè)矩形障礙物。3種算法分別進(jìn)行100次路徑規(guī)劃所得的平均數(shù)據(jù)，如表1所示。

表1 簡單障礙物環(huán)境下的對比

圖4 簡單障礙物環(huán)境的路徑規(guī)劃結(jié)果

在擴(kuò)展節(jié)點(diǎn)數(shù)和路徑規(guī)劃時(shí)間這兩個(gè)指標(biāo)上，AF-RRT*明顯優(yōu)于RRT*和F-RRT*。AF-RRT*擴(kuò)展節(jié)點(diǎn)數(shù)比RRT*少75.25%，路徑規(guī)劃時(shí)間少90.19%，路徑長度短11.52%。AF-RRT*與F-RRT*的路徑長度接近，但是擴(kuò)展節(jié)點(diǎn)數(shù)減少77.43%，路徑規(guī)劃時(shí)間減少90.48%。

3.1.2 迷宮障礙物環(huán)境

地圖上中有兩個(gè)平行的矩形組成一個(gè)簡單迷宮，如圖5所示。3種算法100次路徑規(guī)劃所得的平均數(shù)據(jù)見表2。AF-RRT*的路徑長度與F-RRT*算法接近，擴(kuò)展節(jié)點(diǎn)數(shù)量和路徑規(guī)劃時(shí)間遠(yuǎn)少于其它兩種算法。AF-RRT*相比于F-RRT*，擴(kuò)展節(jié)點(diǎn)數(shù)減少41.30%，路徑規(guī)劃時(shí)間減少63.25%。

表2 迷宮障礙物環(huán)境下的對比

圖5 迷宮障礙物環(huán)境的路徑規(guī)劃結(jié)果

3.1.3 凹形障礙物環(huán)境

地圖中有一個(gè)凹形障礙物，如圖6所示。3種算法100次路徑規(guī)劃所得的平均數(shù)據(jù)見表3。

表3 凹形障礙物環(huán)境下的對比

圖6 凹形障礙物環(huán)境的路徑規(guī)劃結(jié)果

AF-RRT*相比于F-RRT*，擴(kuò)展節(jié)點(diǎn)數(shù)減少48.43%，路徑規(guī)劃時(shí)間減少69.21%。

3.1.4 復(fù)雜障礙物環(huán)境仿真

地圖上中有許多各種不同類型的矩形障礙物，如圖7所示。

圖7 復(fù)雜障礙物環(huán)境的路徑規(guī)劃結(jié)果

3種算法在復(fù)雜障礙物環(huán)境下分別進(jìn)行100次路徑規(guī)劃，平均數(shù)據(jù)見表4。在擴(kuò)展節(jié)點(diǎn)數(shù)和路徑規(guī)劃時(shí)間這兩指標(biāo)上，AF-RRT*同樣明顯優(yōu)于RRT*和F-RRT*。與F-RRT*算法相比，AF-RRT*的擴(kuò)展節(jié)點(diǎn)數(shù)減少72.45%，路徑規(guī)劃時(shí)間減少85.29%。

表4 復(fù)雜障礙物環(huán)境下的對比

3.1.5 仿真實(shí)驗(yàn)分析

從結(jié)果對比可以看出，F(xiàn)-RRT*跟RRT*算法存在著無效搜索和在目標(biāo)點(diǎn)震蕩的問題。在多障礙物環(huán)境下，由于擴(kuò)展樹需要頻繁繞過障礙物，F(xiàn)-RRT*探索無效區(qū)域時(shí)在障礙物邊緣大量地創(chuàng)造新節(jié)點(diǎn)，這會(huì)使擴(kuò)展節(jié)點(diǎn)數(shù)和路徑規(guī)劃時(shí)間大幅度上升。本文算法AF-RRT*利用自適應(yīng)探索策略和動(dòng)態(tài)步長針對性減少了在盲目搜索和擴(kuò)展樹震蕩的過程中創(chuàng)造的新節(jié)點(diǎn)，使得擴(kuò)展節(jié)點(diǎn)數(shù)和路徑規(guī)劃時(shí)間大大減少。普通環(huán)境下，如簡單障礙物和復(fù)雜障礙物環(huán)境，AF-RRT*的路徑規(guī)劃時(shí)間比F-RRT*減少80%～90%；在局部最優(yōu)或者迷宮類環(huán)境下，如迷宮障礙物和凹形障礙物，AF-RRT*的路徑規(guī)劃時(shí)間也比F-RRT*減少60%～70%。

3.2 消融實(shí)驗(yàn)

為進(jìn)一步檢驗(yàn)AF-RRT*算法各組成部分的有效性，保持相同參數(shù)不變，在復(fù)雜障礙物環(huán)境對AF-RRT*算法進(jìn)行消融實(shí)驗(yàn)，如圖8所示。

圖8 AF-RRT*消融實(shí)驗(yàn)的路徑規(guī)劃結(jié)果

無創(chuàng)造父節(jié)點(diǎn)、無自適應(yīng)探索策略、無動(dòng)態(tài)步長的AF-RRT*和完整的AF-RRT*在多障礙物環(huán)境下分別進(jìn)行100次路徑規(guī)劃，平均數(shù)據(jù)見表5。無創(chuàng)造父節(jié)點(diǎn)樹擴(kuò)展策略的AF-RRT*算法路徑長度較長；無自適應(yīng)探索策略的AF-RRT*算法，擴(kuò)展節(jié)點(diǎn)數(shù)較多和路徑規(guī)劃時(shí)間較長；無動(dòng)態(tài)步長的AF-RRT*擴(kuò)展節(jié)點(diǎn)數(shù)較多。說明創(chuàng)造父節(jié)點(diǎn)在使擴(kuò)展樹緊貼障礙物生長，自適應(yīng)探索策略在減少盲目搜索，動(dòng)態(tài)步長在減少冗余節(jié)點(diǎn)，是有效的。其中自適應(yīng)探索策略發(fā)揮了較為關(guān)鍵的作用，對提高AF-RRT*算法的性能做出了較大的貢獻(xiàn)。

表5 AF-RRT*消融實(shí)驗(yàn)數(shù)據(jù)

4 結(jié)束語

本文以RRT*算法為基礎(chǔ)，在樹擴(kuò)展策略和采樣策略兩方面作了改進(jìn)。引入創(chuàng)造父節(jié)點(diǎn)的樹擴(kuò)展策略，改變RRT*樹擴(kuò)展的過程，使擴(kuò)展樹緊貼障礙物生長，縮短了路徑長度。通過自適應(yīng)探索策略，縮短了路徑規(guī)劃的時(shí)間，同時(shí)避免陷入局部最優(yōu)。通過動(dòng)態(tài)步長減少了擴(kuò)展樹在目標(biāo)點(diǎn)震蕩。仿真實(shí)驗(yàn)結(jié)果表明，AF-RRT*在路徑規(guī)劃效率和路徑質(zhì)量上相較于RRT*和F-RRT*具有較明顯的優(yōu)越性。消融實(shí)驗(yàn)驗(yàn)證了各改進(jìn)點(diǎn)的有效性。