RRT算法路徑優(yōu)化及仿真驗(yàn)證

劉文光，劉浩偉，羅 通，王志民

(江蘇大學(xué) 汽車與交通工程學(xué)院， 江蘇 鎮(zhèn)江 212013)

0 引言

無(wú)人駕駛技術(shù)中，路徑規(guī)劃是重點(diǎn)和難點(diǎn)。為保證無(wú)人車的安全行駛，必須解決無(wú)人車在行駛過(guò)程中的避障問(wèn)題。無(wú)人車避障路徑的優(yōu)劣將直接影響車輛的安全性和實(shí)效性。常見(jiàn)的路徑規(guī)劃算法有遺傳算法、A*算法、RRT算法、人工勢(shì)場(chǎng)法、蟻群算法等[1-5]。其中，RRT算法因?yàn)椴恍枰獙?duì)環(huán)境進(jìn)行精準(zhǔn)的建模，所以在算法布置和計(jì)算效率上具有優(yōu)勢(shì)。

RRT算法的核心是基于采樣擴(kuò)展隨機(jī)生長(zhǎng)樹(shù)節(jié)點(diǎn)，生成一條無(wú)碰的路徑。其主要存在的問(wèn)題是搜索路徑質(zhì)量不高且非最優(yōu)、搜索代價(jià)過(guò)大、收斂慢等。針對(duì)搜索代價(jià)大、效率不高的問(wèn)題，李金良等[6]提出安全場(chǎng)的概念，并結(jié)合實(shí)車數(shù)據(jù)基于安全距離建立安全場(chǎng)，對(duì)RRT算法進(jìn)行改進(jìn)，提高了搜索的效率和成功率，但其算法運(yùn)行過(guò)于依賴數(shù)據(jù)，魯棒性不夠。王海芳等[7]提出同時(shí)創(chuàng)建2顆搜索樹(shù)，交替進(jìn)行相向搜索，同時(shí)以一定的概率對(duì)隨機(jī)點(diǎn)進(jìn)行偏置，以提高收斂效率，并且對(duì)父節(jié)點(diǎn)進(jìn)行重選，增強(qiáng)算法對(duì)環(huán)境的敏感程度，不過(guò)同時(shí)也增加了計(jì)算量。針對(duì)路徑非最優(yōu)及搜索質(zhì)量不高的問(wèn)題，葉鴻達(dá)[8]提出了一種基于A*引導(dǎo)的RRT算法，其可以更好地引導(dǎo)隨機(jī)樹(shù)探索路徑。Karaman團(tuán)隊(duì)[9]提出了漸進(jìn)最優(yōu)RRT算法，基于最優(yōu)標(biāo)準(zhǔn)來(lái)調(diào)整生成的隨機(jī)節(jié)點(diǎn)，以提高采樣效率，但會(huì)花費(fèi)更多的代價(jià)在碰撞檢測(cè)和重新布線上。Zhou等[10]采用最近鄰搜索策略，提高了收斂速度。Zhou[11]結(jié)合APF算法對(duì)RRT進(jìn)行優(yōu)化，加入引力和斥力的作用。而Zhen團(tuán)隊(duì)[12]則對(duì)RRT算法的采樣步長(zhǎng)進(jìn)行基于目標(biāo)吸引力的調(diào)整，提高搜索效率。

基于RRT算法當(dāng)前存在的問(wèn)題和研究現(xiàn)狀，提出了一種優(yōu)化的RRT算法，對(duì)父節(jié)點(diǎn)進(jìn)行約束范圍內(nèi)的重選，并對(duì)隨機(jī)樹(shù)進(jìn)行剪枝，提高搜索效率。同時(shí)對(duì)生成的路徑進(jìn)行平滑處理，使其滿足動(dòng)力學(xué)約束。

1 RRT算法簡(jiǎn)介

RRT算法是一種基于隨機(jī)采樣且能在多維空間中有效規(guī)劃路徑的算法。該算法以一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，再通過(guò)隨機(jī)采樣的方式增加樹(shù)枝節(jié)點(diǎn)，進(jìn)而生成一個(gè)隨機(jī)樹(shù)，當(dāng)隨機(jī)樹(shù)中的樹(shù)枝節(jié)點(diǎn)生長(zhǎng)到了一定數(shù)目且包括了目標(biāo)點(diǎn)，便可以在隨機(jī)樹(shù)樹(shù)枝中找到一條由從初始點(diǎn)到目標(biāo)點(diǎn)的路徑[1]。

RRT算法的執(zhí)行過(guò)程如下：

1) 首先根據(jù)起始點(diǎn)，產(chǎn)生一個(gè)隨機(jī)點(diǎn)Xrand。

2) 產(chǎn)生隨機(jī)點(diǎn)后，計(jì)算隨機(jī)樹(shù)中的每一個(gè)節(jié)點(diǎn)與新生成的隨機(jī)點(diǎn)之間的距離，找出距離此隨機(jī)點(diǎn)最近的節(jié)點(diǎn)，記為Xnear。

3) 定義一個(gè)路徑長(zhǎng)度d,在Xnear與Xrand連線上以Xnear為原點(diǎn)、長(zhǎng)度為d處確定新的節(jié)點(diǎn)Xnew。

4) 判斷Xnew是否滿足設(shè)定需求，如果不滿足，舍棄，重新產(chǎn)生新的隨機(jī)點(diǎn)Xnew。

5) 循環(huán)產(chǎn)生路徑，直到Xnew與目標(biāo)點(diǎn)的距離小于d，也就是新生成的節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)附近時(shí)，則退出循環(huán)，生成路徑。

由于RRT算法采樣具有隨機(jī)性的特點(diǎn)，所以最終生成的路徑往往不是最優(yōu)路徑，故需要對(duì)RRT算法進(jìn)行優(yōu)化使其能更好地用于實(shí)際路徑搜尋規(guī)劃中[2]。

2 避障策略

為了簡(jiǎn)化問(wèn)題，路徑規(guī)劃的障礙物選取較為規(guī)則的幾何形，如圓、多邊形等。對(duì)于圓形障礙物來(lái)說(shuō)，其邊界的判斷屬于非線性問(wèn)題，通常將圓形進(jìn)行線性化處理(轉(zhuǎn)化為多邊形)，只需要判斷Xnew的橫縱坐標(biāo)是否在圓內(nèi)。這里規(guī)定，當(dāng)Xnew位于圓形障礙物的外接正方形內(nèi)，就視為碰撞。如果圓形障礙物的圓心坐標(biāo)為(X0，Y0)，半徑為r，則需要考慮移動(dòng)機(jī)器人的尺寸，對(duì)障礙物進(jìn)行膨化處理，膨脹尺寸為inf，所以碰撞條件為:

X0-r-inf

Y0-r-inf

(1)

因?yàn)閳A形障礙物的避障策略相對(duì)簡(jiǎn)單，在仿真中并未設(shè)置圓形障礙物。在仿真環(huán)境中搭建的隨機(jī)環(huán)境，主要選取長(zhǎng)方形障礙物。長(zhǎng)方形的碰撞機(jī)制研究相對(duì)復(fù)雜一些，具體碰撞機(jī)制如下︰在擴(kuò)展隨機(jī)樹(shù)的過(guò)程中，由Xnear到Xnew連接的邊不能與長(zhǎng)方形障礙物的任何一邊相交，即將長(zhǎng)方形障礙物碰撞檢測(cè)問(wèn)題轉(zhuǎn)化為直線與矩形相交問(wèn)題。直線與矩形相交的判斷主要分為兩步:

第一步，判斷Xnew與Xnear是否在矩形某一條邊的一側(cè)。如果Xnew與Xnear在矩形任意一條邊的同側(cè)，則不用進(jìn)行后續(xù)判斷，Xnew與Xnear連線必定不與矩形相交。這里不存在兩點(diǎn)位于矩形內(nèi)部的情況，因?yàn)閄new由Xnear產(chǎn)生，而Xnear必位于矩形外側(cè)，如果Xnew與Xnear位于某—條邊的兩側(cè)，則進(jìn)行第二步判斷[3]。

第二步，當(dāng)Xnew與Xnear在矩形任意一邊的不同側(cè)時(shí)，分為2種情況:第一種情況是Xnew位于矩形內(nèi)部，則Xnew與Xnear連線必定與矩形相交。第二種情況是兩點(diǎn)均在矩形外部。在這種情況下并不能保證兩點(diǎn)連線不與矩形相交，兩點(diǎn)位于矩形外側(cè)且位于矩形上邊的兩側(cè)，但兩點(diǎn)連線與矩形相交。在這種情況下，利用直線與矩形的性質(zhì)進(jìn)行避碰計(jì)算。

3 RRT算法優(yōu)化

盡管RRT算法相對(duì)于其他算法比較高效，可以較好地解決障礙物復(fù)雜場(chǎng)景路徑的規(guī)劃問(wèn)題，具有很多優(yōu)勢(shì)，但是由于RRT算法取點(diǎn)的隨機(jī)性，并不能保證所得出的路徑最佳，所以須對(duì)RRT算法進(jìn)行優(yōu)化以便更好地解決路徑優(yōu)化的問(wèn)題[4]?，F(xiàn)根據(jù)RRT算法在路徑規(guī)劃過(guò)程中，路徑非最優(yōu)，提出這樣2個(gè)優(yōu)化策略：約束范圍內(nèi)重選父節(jié)點(diǎn)，基于重選的父節(jié)點(diǎn)修剪隨機(jī)樹(shù)樹(shù)枝。

3.1 重新選擇父節(jié)點(diǎn)

在最新生成的節(jié)點(diǎn)Xnew附近以一定半徑范圍作為約束搜尋相鄰的節(jié)點(diǎn)，使之作為替代Xnew父節(jié)點(diǎn)的候選節(jié)點(diǎn)。

計(jì)算約束范圍內(nèi)的附近節(jié)點(diǎn)到起點(diǎn)的路徑距離和Xnew到每個(gè)附近節(jié)點(diǎn)的總路徑距離，具體過(guò)程見(jiàn)圖1。圖1(a)中表達(dá)的是擴(kuò)展隨機(jī)樹(shù)過(guò)程中的某一時(shí)刻，節(jié)點(diǎn)的數(shù)字表示本節(jié)點(diǎn)產(chǎn)生的順序，0是起點(diǎn)，10是最新產(chǎn)生的節(jié)點(diǎn)Xnew，節(jié)點(diǎn)6是節(jié)點(diǎn)10這一時(shí)刻的父節(jié)點(diǎn)Xnear，節(jié)點(diǎn)間連線上的數(shù)字表示兩節(jié)點(diǎn)間的距離。

圖1 重選父節(jié)點(diǎn)過(guò)程示意圖

在為節(jié)點(diǎn)10重新選擇父節(jié)點(diǎn)過(guò)程中，以節(jié)點(diǎn)10為中心，以某一半徑作為約束，對(duì)范圍內(nèi)的節(jié)點(diǎn)10的相鄰節(jié)點(diǎn)作為父節(jié)點(diǎn)的候選點(diǎn)，也就是點(diǎn)5、7、9。計(jì)算其路徑代價(jià)如表1所示。

由表1可知，候選點(diǎn)5作為節(jié)點(diǎn)10新的父節(jié)點(diǎn)的總路徑距離是最小的，所以把節(jié)點(diǎn)10的父節(jié)點(diǎn)改成6，重新生成的路徑隨機(jī)樹(shù)如圖1(b)所示。

表1 候選點(diǎn)路徑代價(jià)

3.2 修剪隨機(jī)樹(shù)

在重新選擇父節(jié)點(diǎn)的基礎(chǔ)上，通過(guò)修剪隨機(jī)樹(shù)來(lái)調(diào)整隨機(jī)樹(shù)樹(shù)杈的數(shù)目，減少不必要的樹(shù)枝生成，進(jìn)而縮短路徑距離。

修剪邏輯為：如果新生成的Xnew可以作為相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)，并且縮短路徑距離，則進(jìn)行修改，過(guò)程示意圖如圖2所示。

圖2 修剪隨機(jī)樹(shù)過(guò)程示意圖

如圖2(a)所示，節(jié)點(diǎn)10為新生成的節(jié)點(diǎn)Xnew，相鄰的節(jié)點(diǎn)為6、7、9，如果用新生成的節(jié)點(diǎn)10代替他們各自的父節(jié)點(diǎn)0、5、6，則路徑代價(jià)如表2所示。

表2 修剪后的路徑代價(jià)

如果節(jié)點(diǎn)10作為原節(jié)點(diǎn)6的父節(jié)點(diǎn)，原路徑由0-6變成0-1-5-10-6，代價(jià)大于原路徑，舍棄本方案；同理候選點(diǎn)9結(jié)果也是如此。

如果節(jié)點(diǎn)10作為節(jié)點(diǎn)7的父節(jié)點(diǎn)，原路徑由0-6-7變成0-1-5-10-7，代價(jià)相比較于原路徑較少，則使用本方案。

修剪隨機(jī)樹(shù)的意義在于通過(guò)新生成的節(jié)點(diǎn)來(lái)替換老節(jié)點(diǎn)的父節(jié)點(diǎn)，修改隨機(jī)樹(shù)的樹(shù)枝，去掉多余的樹(shù)枝路徑，縮短路徑距離。

如圖2(b)所示，重新選擇父節(jié)點(diǎn)使新生成節(jié)點(diǎn)連成通路后的距離縮短，修剪隨機(jī)樹(shù)則使新生成節(jié)點(diǎn)后的樹(shù)枝精簡(jiǎn)，減少路徑長(zhǎng)度代價(jià)，但新路徑的建立必須要保證新節(jié)點(diǎn)和父節(jié)點(diǎn)相連通且不觸及障礙物的條件下,重新選擇父節(jié)點(diǎn)和修剪隨機(jī)樹(shù)2個(gè)方法各司其職，相互配合，能夠進(jìn)一步縮短路徑距離和搜索代價(jià)。

4 路徑處理

由于RRT算法是基于隨機(jī)采樣的，導(dǎo)致算法搜索到的路徑比較曲折、不平滑，路徑還包含了許多非必要的節(jié)點(diǎn)，特別是在復(fù)雜的多障礙物環(huán)境下，曲折點(diǎn)更多，由于汽車轉(zhuǎn)彎半徑有限，不利于汽車的行駛，故現(xiàn)提出2個(gè)路徑處理方法。

4.1 基于最小轉(zhuǎn)彎半徑約束的路徑修改

如圖3(a)所示，在生成的路徑中，節(jié)點(diǎn)3處的夾角小于汽車的最小轉(zhuǎn)彎半徑所要求的最小路徑夾角，因此，在路徑規(guī)劃結(jié)束后再對(duì)路徑進(jìn)行檢查修剪，在路徑夾角不符合要求時(shí)進(jìn)行修剪。如圖3(b)所示，由于原節(jié)點(diǎn)3處的夾角過(guò)小，不符合要求，所以在節(jié)點(diǎn)3的附近插入一個(gè)新的節(jié)點(diǎn)4，使其滿足轉(zhuǎn)彎條件。

圖3 最小轉(zhuǎn)彎半徑約束示意圖

4.2 基于三角形原則的路徑修改

在算法規(guī)避障礙物生成路徑的過(guò)程中，可能會(huì)出現(xiàn)如圖4(a)所示的情況，由于RRT算法的隨機(jī)性，生成的路徑雖然避開(kāi)了障礙物，但是比較曲折，存在多余的路徑。現(xiàn)根據(jù)三角形兩邊和大于第三邊的原則[13]，對(duì)路徑進(jìn)行修改。

圖4 三角形原則修剪過(guò)程示意圖

在圖4(b)中，點(diǎn)1、2、3構(gòu)成三角形，現(xiàn)把路徑1-2、2-3消除，生成1-3路徑，進(jìn)而縮短路徑距離，但是也必須保證新生成的1-3路徑與障礙物存在一定距離。點(diǎn)3、4、5構(gòu)成的三角形也進(jìn)行相同的處理，生成3-5新路徑。路徑最終由1-2-3-4-5變?yōu)?-3-5，在縮短了路徑的同時(shí)，還一定程度上使生成的路徑更加平滑。

5 Matlab仿真實(shí)驗(yàn)

為驗(yàn)證改進(jìn)RRT算法相比于經(jīng)典RRT算法的改進(jìn)效果，現(xiàn)將其布置到同一張地圖上進(jìn)行仿真實(shí)驗(yàn)，觀察其優(yōu)化效果。同時(shí)，為了與其他優(yōu)化算法做對(duì)比，這里選取A*引導(dǎo)RRT算法[2]參與仿真實(shí)驗(yàn)，進(jìn)行橫向?qū)Ρ取?/p>

現(xiàn)將改進(jìn)的RRT和A*引導(dǎo)RRT算法及經(jīng)典RRT算法分別在Matlab生成的隨機(jī)地圖上進(jìn)行重復(fù)仿真實(shí)驗(yàn)50次，分析其生成途徑的總長(zhǎng)平均值，用來(lái)評(píng)判改進(jìn)后的RRT算法和A*引導(dǎo)RRT算法各自相對(duì)經(jīng)典RRT算法的優(yōu)化程度如何，同時(shí)記錄平均迭代時(shí)間，評(píng)判算法迭代效率如何。

為了驗(yàn)證改進(jìn)后的RRT算法優(yōu)化效果的魯棒性和普適性，分別在3幅不同的隨機(jī)地圖上進(jìn)行仿真實(shí)驗(yàn)，其效果如圖5—13所示。

圖5 Map1(基于RRT生成的路徑)

圖6 Map1(基于A*引導(dǎo)RRT生成的路徑)

圖7 Map1(基于改進(jìn)RRT生成的路徑)

圖9 Map2(基于A*引導(dǎo)RRT生成的路徑)

圖10 Map2(基于改進(jìn)RRT生成的路徑)

圖11 Map3(基于RRT生成的路徑)

圖12 Map3(基于A*引導(dǎo)RRT生成的路徑)

在3幅地圖，9個(gè)實(shí)驗(yàn)組中，均設(shè)置起點(diǎn)為(0,0)，終點(diǎn)為[(25,-25),(26,-25),(26,-26),(26,-25)]包圍的區(qū)域內(nèi)，每個(gè)實(shí)驗(yàn)組共進(jìn)行50次實(shí)驗(yàn)，得出的數(shù)據(jù)如表3所示。

表3 Map1仿真結(jié)果

表4 Map2仿真結(jié)果

表5 Map3仿真結(jié)果

由3組仿真實(shí)驗(yàn)可以看出，A*引導(dǎo)RRT算法及改進(jìn)的RRT算法(本算法)相較于原RRT算法在路徑長(zhǎng)度上均有不錯(cuò)的優(yōu)化效果，在不同地圖特征下優(yōu)化度略有差異，不過(guò)總體優(yōu)化度接近，但改進(jìn)的RRT算法通過(guò)對(duì)約束下的父節(jié)點(diǎn)重選和對(duì)隨機(jī)樹(shù)剪枝的過(guò)程，大大提高了迭代收斂的效率?？梢钥吹剑?組實(shí)驗(yàn)中，改進(jìn)的RRT算法迭代時(shí)長(zhǎng)均比A*引導(dǎo)RRT算法短，可見(jiàn)，本算法在提高路徑優(yōu)化度的同時(shí)還保持了較高的搜索效率。圖7、圖10、圖13均是改進(jìn)后的RRT算法生成的路徑圖。從圖中可以看出，相較于原RRT算法生成的圖5、圖8、圖11路徑圖來(lái)說(shuō)，改進(jìn)后RRT算法生成的隨機(jī)樹(shù)具有更好的發(fā)散性，而原RRT算法生成的隨機(jī)樹(shù)則比較曲折凌亂。故此，在最后生成的路徑中可以比較得出，優(yōu)化算法生成的路徑較為平緩，更符合車輛動(dòng)力學(xué)規(guī)律，保障汽車能平緩行駛。

圖13 Map3(基于改進(jìn)RRT生成的路徑)

6 結(jié)論

RRT算法在經(jīng)過(guò)約束后對(duì)父節(jié)點(diǎn)進(jìn)行重選，使生成的隨機(jī)樹(shù)樹(shù)枝更加簡(jiǎn)潔，指向性也更加明確，使找到目標(biāo)點(diǎn)后生成的路徑距離縮短，但由于重新選擇父節(jié)點(diǎn)可能會(huì)導(dǎo)致生成的路徑更加曲折，所以還需要對(duì)路徑進(jìn)行進(jìn)一步的優(yōu)化，即對(duì)隨機(jī)樹(shù)進(jìn)行剪枝，通過(guò)修剪隨機(jī)樹(shù)修剪多余的隨機(jī)樹(shù)樹(shù)枝，減少冗余通路，能有效地減少路徑的長(zhǎng)度。這兩步主要的功能是對(duì)規(guī)劃的路徑距離進(jìn)行縮減，而后續(xù)的基于對(duì)最小轉(zhuǎn)彎半徑約束的路徑優(yōu)化和基于三角形原則的路徑優(yōu)化，則是對(duì)生成的路徑進(jìn)行優(yōu)化，使路徑更加平滑，使生成的路徑更貼合實(shí)際汽車的行駛要求并滿足動(dòng)力學(xué)約束。