一種改進(jìn)的雙向快速搜索隨機(jī)樹算法

徐秉超， 嚴(yán) 華

(四川大學(xué)電子信息學(xué)院，成都 610065)

隨著機(jī)器人控制及智能化領(lǐng)域研究的不斷發(fā)展，作為基礎(chǔ)的路徑規(guī)劃研究也日趨深化。路徑規(guī)劃指在障礙物環(huán)境下，尋找從初始點(diǎn)至目的點(diǎn)的運(yùn)行路徑，同時(shí)保證路徑可跟蹤，安全無碰撞[1]。常用的路徑規(guī)劃算法分為不同類型，如Dijkstra、A*、D*等組成的前向圖搜索算法，以柵格法和可視圖法為代表的幾何構(gòu)造規(guī)劃算法以及在智能控制理論下出現(xiàn)的基于遺傳算法[2]和神經(jīng)網(wǎng)絡(luò)[3]的規(guī)劃算法。

快速搜索隨機(jī)樹(RRT)算法由Lavall在1998年提出。相比全局搜索和啟發(fā)式算法，RRT算法[4]能在保證概率完備性的同時(shí)極大降低時(shí)間成本。在高維空間里的優(yōu)秀表現(xiàn)也令它倍受關(guān)注。RRT優(yōu)化性能強(qiáng)大，將機(jī)械對(duì)象的實(shí)際動(dòng)力學(xué)約束加入規(guī)劃，可以使生成路徑可跟蹤[5]。該算法無需對(duì)地圖進(jìn)行幾何劃分，可以在動(dòng)態(tài)規(guī)劃中對(duì)局部地圖進(jìn)行再規(guī)劃以生成局部路徑[6],也廣泛應(yīng)用于多目標(biāo)點(diǎn)規(guī)劃任務(wù)[7]。然而，由于使用全局隨機(jī)采樣，RRT算法存在隨機(jī)性大、效率低的問題。為此，目標(biāo)導(dǎo)向RRT[8]和雙向RRT[9]等改進(jìn)方向被接連提出，基于概率的回歸機(jī)制[10]被用于緩解重復(fù)生長(zhǎng)問題。文獻(xiàn)[11]的變步長(zhǎng)雙向RRT算法通過目標(biāo)引力和雙向搜索方式，跟蹤目標(biāo)點(diǎn)并擴(kuò)大連接面積，但同時(shí)降低了全局避障能力，額外耗費(fèi)大量時(shí)間。

在變步長(zhǎng)雙向RRT算法基礎(chǔ)上引入預(yù)生長(zhǎng)、重要障礙斥力和隨機(jī)點(diǎn)篩選機(jī)制策略以縮小問題規(guī)模并提高避障過程中的隨機(jī)性及有效性，優(yōu)化路徑規(guī)劃的效率。

1 相關(guān)工作

1.1 快速搜索隨機(jī)樹

傳統(tǒng) RRT算法主要包括兩個(gè)階段：構(gòu)造隨機(jī)生長(zhǎng)樹和反向搜索可行的軌跡[12]。首先以起始點(diǎn)作為樹的根節(jié)點(diǎn)，在全狀態(tài)空間中隨機(jī)采樣以生長(zhǎng)無碰撞的葉節(jié)點(diǎn)，直到有節(jié)點(diǎn)進(jìn)入目標(biāo)區(qū)域或達(dá)到目標(biāo)點(diǎn)。隨后以距離目標(biāo)點(diǎn)最近的點(diǎn)開始反向搜索父節(jié)點(diǎn)，生成一條從起始位置到目標(biāo)位置的路徑[13]。

RRT算法中的單次生長(zhǎng)如圖1所示。首先以起始點(diǎn)Xinit為根節(jié)點(diǎn)構(gòu)造一棵隨機(jī)搜索樹。然后在狀態(tài)空間中隨機(jī)取一自由點(diǎn)Xrand，找出隨機(jī)搜索樹中距離Xrand最近的樹節(jié)點(diǎn)Xnear。接著，從Xnear向Xrand擴(kuò)展出基礎(chǔ)步長(zhǎng)為ρ的新節(jié)點(diǎn)Xnew。若Xnew與Xnear間的路徑?jīng)]有發(fā)生碰撞，則將Xnew加入搜索樹。重復(fù)上述過程直到節(jié)點(diǎn)進(jìn)入目標(biāo)區(qū)域或到達(dá)設(shè)置的最大迭代次數(shù)。一次完整的規(guī)劃結(jié)果如圖2所示。

圖1 RRT算法節(jié)點(diǎn)生長(zhǎng)示意圖Fig.1 Schematic diagram of node growth RRT algorithm

圖2 RRT尋徑示意圖Fig.2 Diagram of RRT path searching

由圖2可知，傳統(tǒng)RRT算法雖然避免了局部最優(yōu)的陷阱，但因小部分的局部最優(yōu)陷阱而失去全部導(dǎo)向性，致使在大部分環(huán)境中，性能得不償失。為了解決此問題，需在傳統(tǒng)算法中加入適當(dāng)?shù)膶?dǎo)向功能。路徑規(guī)劃有兩個(gè)要求，一是到達(dá)目標(biāo)點(diǎn)，另一個(gè)是避開障礙，需要在目標(biāo)跟蹤過程中加入啟發(fā)式降低隨機(jī)性，在避障過程中則需要完全隨機(jī)從而保證避障的效果。傳統(tǒng)RRT沒有對(duì)生長(zhǎng)區(qū)域的有效性進(jìn)行評(píng)估，例如在本文研究環(huán)境中，一方面為空間形成的區(qū)域區(qū)分度，即非主路徑區(qū)域擴(kuò)展的可能性應(yīng)稍低，在圖2環(huán)境中為右上和左下的部分區(qū)域；另一方面為時(shí)間形成的區(qū)域區(qū)分度，即已生長(zhǎng)區(qū)域應(yīng)有更低的可能性進(jìn)行擴(kuò)展，在圖2環(huán)境中為部分左上區(qū)域。

1.2 變步長(zhǎng)雙向RRT

考慮到RRT算法的諸多不足，研究人員提出了雙向生長(zhǎng)樹和目標(biāo)引力機(jī)制。

雙向生長(zhǎng)樹更多地在實(shí)際物理系統(tǒng)限制上進(jìn)行優(yōu)化，它從起點(diǎn)和目標(biāo)點(diǎn)分別擴(kuò)展生長(zhǎng)樹，緩和了目標(biāo)點(diǎn)所在位置對(duì)規(guī)劃的影響，不再需要對(duì)鄰近邊界的目標(biāo)點(diǎn)進(jìn)行特殊處理，并且將目標(biāo)點(diǎn)擴(kuò)展為點(diǎn)集，提高了搜索樹和目標(biāo)連接的效率，從而降低了形成路徑的時(shí)間和復(fù)雜度。

在人工勢(shì)場(chǎng)法[14]中，路徑前進(jìn)的方向取決于目標(biāo)點(diǎn)和所有障礙，給規(guī)劃提供有效導(dǎo)向，因而實(shí)時(shí)性較好。在借鑒人工勢(shì)場(chǎng)法而加入目標(biāo)引力的RRT算法中，新節(jié)點(diǎn)Xnew將使用式(1)得出：

(1)

式(1)中：ρ為基礎(chǔ)步長(zhǎng)；k為引力系數(shù)，即在每一次生長(zhǎng)過程中，Xnew相對(duì)于Xnear向隨機(jī)點(diǎn)前進(jìn)ρ的同時(shí)，向目標(biāo)點(diǎn)前進(jìn)kρ長(zhǎng)度。

目標(biāo)引力在一定程度上解決了空間形成的區(qū)域區(qū)分度和到達(dá)目標(biāo)過程中的導(dǎo)向，提高了規(guī)劃效率，但因?yàn)橹蝗∮昧四繕?biāo)引力，受其影響的避障過程無法實(shí)現(xiàn)完全隨機(jī)，從而降低全局避障能力，對(duì)效率造成極大損失。

2 算法的改進(jìn)

基于變步長(zhǎng)雙向RRT算法，改進(jìn)算法首先進(jìn)行預(yù)生長(zhǎng)，即先在直線方向進(jìn)行生長(zhǎng)的嘗試，以接近人類思維的方式，降低前期生長(zhǎng)的代價(jià)；接著，加入隨機(jī)點(diǎn)篩選機(jī)制，對(duì)部分時(shí)間形成的區(qū)域區(qū)分度進(jìn)行處理；最后，引入重要障礙物斥力，消除引力在避障過程中的影響，降低不必要的效率損失。

2.1 預(yù)生長(zhǎng)

人類思維下，在無障礙的環(huán)境中，使用直線連接起點(diǎn)和目標(biāo)點(diǎn)具有最高的效率。如圖3所示，預(yù)生長(zhǎng)分別從兩棵樹的根節(jié)點(diǎn)開始，沿著起始點(diǎn)到目標(biāo)點(diǎn)的直線方向，以2倍基礎(chǔ)步長(zhǎng)2ρ檢測(cè)碰撞，以基礎(chǔ)步長(zhǎng)進(jìn)行生長(zhǎng)直到發(fā)生碰撞，并刪除最后一個(gè)生長(zhǎng)的節(jié)點(diǎn)。刪除操作用于保證搜索樹節(jié)點(diǎn)與障礙物存在適當(dāng)距離。與障礙物距離過近的節(jié)點(diǎn)在后續(xù)生長(zhǎng)中將有更大可能性發(fā)生碰撞，因而生長(zhǎng)失敗的概率更大，效率降低。

圖3 預(yù)生長(zhǎng)示意圖Fig.3 Diagram of pre-extended

預(yù)生長(zhǎng)過程模擬了人類的直觀思維。在該過程中，除了進(jìn)行少量碰撞檢測(cè)外無需其他計(jì)算，生長(zhǎng)效率大于正常生長(zhǎng)流程，在低消耗下快速通過初期的自由空間，減少在前期無障礙區(qū)域的時(shí)間花費(fèi)，縮小了需進(jìn)行規(guī)劃計(jì)算的區(qū)域，一定程度上縮小了問題規(guī)模。

2.2 隨機(jī)點(diǎn)篩選機(jī)制

目標(biāo)引力解決了一部分區(qū)域區(qū)分度問題，卻依舊未能對(duì)時(shí)間形成的區(qū)域區(qū)分度進(jìn)行處理。為此，一個(gè)基于metropolis接受準(zhǔn)則的隨機(jī)點(diǎn)篩選機(jī)制將被用來嘗試緩解該問題。metropolis接受準(zhǔn)則被用于模擬退火過程，它在一個(gè)已有解的基礎(chǔ)上，接受優(yōu)化解的同時(shí)以一定概率接受惡化解。隨著逐步降低接受惡化解的概率，最終取得近似最優(yōu)解。該準(zhǔn)則可以跳出局部最優(yōu)，同時(shí)保證概率完備性。

具體的，每一次隨機(jī)生長(zhǎng)過程中，分別計(jì)算最近鄰點(diǎn)Xnear和生長(zhǎng)樹中的最新節(jié)點(diǎn)Xend到目標(biāo)點(diǎn)的距離。按照式(2)判斷隨機(jī)點(diǎn)是否被接受。與原準(zhǔn)則不同，為了提高效率，篩選機(jī)制不再使用概率P間接判斷解的接受與否，而將趨近近似最優(yōu)解的過程直接量化在對(duì)距離的判斷中，原概率P可由式(2)表示。

(2)

(3)

式中：d為計(jì)算兩個(gè)節(jié)點(diǎn)間歐式距離的函數(shù)。通過比例a，完成metropolis接受準(zhǔn)則中概率的功能，實(shí)現(xiàn)在生長(zhǎng)節(jié)點(diǎn)離目標(biāo)區(qū)域較遠(yuǎn)的時(shí)候，改進(jìn)算法能接受更壞的情況，即距離目標(biāo)區(qū)域比最新生成節(jié)點(diǎn)遠(yuǎn)得多的情況。隨著生長(zhǎng)節(jié)點(diǎn)逐漸接近目標(biāo)區(qū)域，接受壞點(diǎn)的范圍逐漸縮小，最終趨近最優(yōu)解。

通過基于metropolis接受準(zhǔn)則的隨機(jī)點(diǎn)篩選機(jī)制，有效對(duì)因時(shí)間形成的不同生長(zhǎng)可能性區(qū)域進(jìn)行劃分，一定程度上減少隨機(jī)搜索樹的重復(fù)生長(zhǎng)，提高生長(zhǎng)效率。

2.3 重要障礙物斥力

目標(biāo)引力在路徑規(guī)劃中的首要任務(wù)即尋找目標(biāo)的過程中加入了導(dǎo)向。但同時(shí)，規(guī)劃過程中占比更高的避障過程也受此影響而無法使用完全隨機(jī)過程進(jìn)行處理，從而削弱了避障性能。修改的障礙物斥力將被引入以緩解該問題。

圖4對(duì)比了僅有目標(biāo)引力和有較完善的勢(shì)場(chǎng)兩種情況下的理論生長(zhǎng)過程。在僅有目標(biāo)引力的情況下，搜索樹生長(zhǎng)到節(jié)點(diǎn)X3才發(fā)現(xiàn)障礙。在經(jīng)歷了數(shù)次失敗或無效的反向生長(zhǎng)后，最終才能到達(dá)一個(gè)較好的節(jié)點(diǎn)X2。然而存在一個(gè)更好更快的路徑完成這個(gè)子任務(wù)，即從X1經(jīng)虛曲線直接到達(dá)X2，這就需要一個(gè)適當(dāng)?shù)恼系K物斥力機(jī)制。

圖4 重要障礙物斥力示意圖Fig.4 Diagram for repulsion of obstacle

傳統(tǒng)人工勢(shì)場(chǎng)的斥力場(chǎng)計(jì)算因考慮所有障礙導(dǎo)致復(fù)雜度激增[15]。針對(duì)該問題，劃分障礙物的權(quán)重對(duì)于引入斥力極為重要。

在單步生長(zhǎng)過程中，最近鄰點(diǎn)Xnear到目標(biāo)點(diǎn)的第一個(gè)障礙物是一個(gè)典型而優(yōu)秀的選擇，借鑒人工勢(shì)場(chǎng)法的斥力部分，改進(jìn)算法斥力體系中的斥力系數(shù)krep使用式(4)計(jì)算：

(4)

式(4)中：C1為障礙物斥力調(diào)整系數(shù)；l為起始點(diǎn)到目標(biāo)點(diǎn)的距離；d為障礙物邊緣到最近鄰點(diǎn)的距離；C0根據(jù)基礎(chǔ)步長(zhǎng)決定，目的是在障礙距離過遠(yuǎn)時(shí)，精簡(jiǎn)計(jì)算步驟；C2為可接受的反向生長(zhǎng)的比例，建議取值在1.0～1.1。設(shè)置斥力最大值可以防止在特殊情況下，擴(kuò)展出過度反向生長(zhǎng)的無效節(jié)點(diǎn)。

在使用時(shí)，將引力系數(shù)與計(jì)算得到的斥力系數(shù)直接相減，可得到合力系數(shù)k*，新節(jié)點(diǎn)生成公式為

k*=k-krep

(5)

引入重要障礙物斥力，可以緩解路徑規(guī)劃中避障過程因目標(biāo)引力而產(chǎn)生的效率損失，甚至在避障過程中實(shí)現(xiàn)完全隨機(jī)。雖然在障礙環(huán)境極度簡(jiǎn)單的情況下將增加部分時(shí)間成本，但在大部分環(huán)境中，尋徑效率有較好的提升效果。

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

3.1 實(shí)驗(yàn)環(huán)境

假設(shè)目標(biāo)機(jī)器人為理想圓點(diǎn)，使用仿真軟件及數(shù)據(jù)證明改進(jìn)算法的可行性與搜索效率。實(shí)驗(yàn)使用Windows 10，Intel Core 2.5 GHz，8 G內(nèi)存，編譯工具為MATLAB R2018a。

隨機(jī)地圖仿真實(shí)驗(yàn)中，系統(tǒng)隨機(jī)生成規(guī)格為[1 000,1 000]的地圖，原點(diǎn)[0,0]為左上角，如圖5所示。黑色圓形為設(shè)置的障礙物，圓心及半徑在設(shè)定的一定范圍內(nèi)隨機(jī)，障礙數(shù)量為100。將[25,25]設(shè)置為起始點(diǎn)，目標(biāo)點(diǎn)為[975,975]。

圖5 隨機(jī)地圖Fig.5 Random map

在本組實(shí)驗(yàn)中，設(shè)置基本步長(zhǎng)ρ=20，引力系數(shù)k=2，C0=2，C1=1/8 000，C2=1.01。

在矩形障礙地圖實(shí)驗(yàn)及特殊陷阱地圖實(shí)驗(yàn)中，使用確定的地圖進(jìn)行實(shí)驗(yàn)，地圖的規(guī)格為[500,500]的矩陣，設(shè)置[25,25]為起點(diǎn)，右下角[475,475]為目標(biāo)點(diǎn)。基礎(chǔ)步長(zhǎng)設(shè)置為5，其余參數(shù)相同。

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

首先對(duì)改進(jìn)算法的可行性進(jìn)行實(shí)驗(yàn)，驗(yàn)證算法是否能在給定環(huán)境中找到一條可行路徑。然后在相同實(shí)驗(yàn)條件下對(duì)比不同算法的性能，得到實(shí)驗(yàn)數(shù)據(jù)并分析。

3.2.1 可行性實(shí)驗(yàn)

圖6為一次實(shí)驗(yàn)的結(jié)果。兩棵搜索樹分別由左上角的起始點(diǎn)和右下角的目標(biāo)點(diǎn)出發(fā)，圖6中展示了尋徑過程中產(chǎn)生的節(jié)點(diǎn)及連線和最終路徑。實(shí)驗(yàn)證明了在設(shè)定條件下，本文改進(jìn)算法可以完成路徑搜索，輸出一條無碰撞的可行路徑。

圖6 算法可行性驗(yàn)證Fig.6 The verification on the feasibility of algorithm

3.2.2 效率驗(yàn)證

實(shí)驗(yàn)將比較改進(jìn)算法、目標(biāo)偏向RRT、可變步長(zhǎng)的雙向RRT算法、人工勢(shì)場(chǎng)法和A*算法。其中基礎(chǔ)的A*算法用于獲得最佳路徑。其余不同算法使用相對(duì)相似的參數(shù)在相同環(huán)境和地圖下進(jìn)行仿真實(shí)驗(yàn)。共進(jìn)行三組實(shí)驗(yàn)，分別使用隨機(jī)生成環(huán)境、矩形障礙環(huán)境和一個(gè)特殊陷阱地圖。設(shè)定單次規(guī)劃時(shí)間超過30 s或連續(xù)擴(kuò)展失敗100次記作失敗。對(duì)于存在隨機(jī)性的RRT系列算法，每組進(jìn)行50次尋徑，統(tǒng)計(jì)成功尋徑的平均時(shí)間、路徑長(zhǎng)度以及生成的節(jié)點(diǎn)數(shù)量。

實(shí)驗(yàn)首先在隨機(jī)生成地圖中進(jìn)行，表1為實(shí)驗(yàn)結(jié)果。人工勢(shì)場(chǎng)法面對(duì)此類環(huán)境極易陷入局部最優(yōu)，無法完成規(guī)劃任務(wù)。RRT系列算法表現(xiàn)較優(yōu)。改進(jìn)算法和變步長(zhǎng)雙向RRT路徑規(guī)劃成功率均為100%，且在各類指標(biāo)上均優(yōu)于目標(biāo)偏向RRT。其中，改進(jìn)算法相對(duì)變步長(zhǎng)雙向RRT在節(jié)點(diǎn)數(shù)量和平均規(guī)劃時(shí)間指標(biāo)上都優(yōu)化了30%左右。

表1 隨機(jī)地圖仿真實(shí)驗(yàn)數(shù)據(jù)

圖7對(duì)比了改進(jìn)算法與變步長(zhǎng)雙向RRT算法的結(jié)果，可變雙向RRT在避障過程中，存在不必要的導(dǎo)向性，進(jìn)行了許多方向明確而重復(fù)的生長(zhǎng)，降低了規(guī)劃效率。而改進(jìn)算法在面對(duì)障礙時(shí)隨機(jī)性更充分，從而并沒有展現(xiàn)出過強(qiáng)的方向性，避障效率更高。

圖7 隨機(jī)環(huán)境實(shí)驗(yàn)結(jié)果Fig.7 Results of random map experiment

簡(jiǎn)單矩形障礙環(huán)境的實(shí)驗(yàn)結(jié)果在表2中列出，數(shù)據(jù)表明人工勢(shì)場(chǎng)法的路徑因?yàn)檎系K環(huán)境不會(huì)突變而更為平滑，但由于計(jì)算量大，時(shí)間方面并沒有優(yōu)勢(shì)。圖8展示了一次路徑規(guī)劃的結(jié)果，在該組實(shí)驗(yàn)中，預(yù)生長(zhǎng)的作用尤為凸顯。由于預(yù)生長(zhǎng)的作

圖8 矩陣障礙地圖實(shí)驗(yàn)結(jié)果Fig.8 Results of rectangle obstacle map experiment

表2 矩形障礙仿真實(shí)驗(yàn)數(shù)據(jù)

用，實(shí)際進(jìn)行計(jì)算規(guī)劃的區(qū)域不到原計(jì)算規(guī)模的一半，并且降低了節(jié)點(diǎn)數(shù)目，提高了效率，相比可變步長(zhǎng)的雙向RRT的規(guī)劃時(shí)間優(yōu)化了50%左右。

第三組實(shí)驗(yàn)使用如圖9所示地圖，其包含一個(gè)典型的陷阱，相比于全局搜索，許多啟發(fā)式算法面對(duì)該類陷阱的表現(xiàn)都較差。從表3的實(shí)驗(yàn)數(shù)據(jù)可看出，基礎(chǔ)的人工勢(shì)場(chǎng)法無法解決此類路徑規(guī)劃問題，而RRT算法由于其概率完備性，存在一定概率完成任務(wù)。其中改進(jìn)算法由于引入障礙斥力和隨機(jī)點(diǎn)篩選機(jī)制，在陷阱中的表現(xiàn)略優(yōu)于其余比較算法，在成功率和時(shí)間指標(biāo)上都有所提升，但還未達(dá)到解決此類問題的程度。其根本原因是對(duì)生長(zhǎng)可能性的區(qū)域區(qū)分還不夠完善，改進(jìn)算法中的篩選機(jī)制只能劃分遠(yuǎn)離目標(biāo)點(diǎn)的已生長(zhǎng)的區(qū)域，而不能劃分所有已生長(zhǎng)區(qū)域，從而較好地避免重復(fù)生長(zhǎng)，仍有提升空間。

表3 特殊陷阱仿真實(shí)驗(yàn)數(shù)據(jù)

圖9 特殊陷阱地圖Fig.9 Special trap environment

4 結(jié)論

針對(duì)RRT算法在規(guī)劃過程中表現(xiàn)出的部分問題，對(duì)可變步長(zhǎng)雙向RRT算法進(jìn)行改進(jìn)，采用預(yù)生長(zhǎng)、重要障礙斥力和隨機(jī)點(diǎn)篩選機(jī)制三種機(jī)制，得到以下結(jié)論。

(1)預(yù)生長(zhǎng)機(jī)制可以實(shí)現(xiàn)對(duì)路徑規(guī)劃的直觀處理，縮小實(shí)際規(guī)劃問題規(guī)模，提高路徑規(guī)劃速度。

(2)引入的障礙物斥力在不明顯提高計(jì)算成本的同時(shí)，有效解決了目標(biāo)引力對(duì)于避障過程的誤導(dǎo)問題。

(3)隨機(jī)點(diǎn)篩選機(jī)制有效減少了重復(fù)生長(zhǎng)的出現(xiàn)，對(duì)于結(jié)點(diǎn)數(shù)量的指標(biāo)有較大改善，通過提高生長(zhǎng)的成功率改善規(guī)劃效率。

(4)對(duì)于特殊陷阱環(huán)境，相比傳統(tǒng)方法，改進(jìn)算法有一定進(jìn)步，但還無法完全解決重復(fù)生長(zhǎng)問題，有待深入研究。