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

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

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

0 引言

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

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

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

1 RRT算法簡介

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

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

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

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

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

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

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

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

2 避障策略

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

X0-r-inf

Y0-r-inf

(1)

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

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

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

3 RRT算法優(yōu)化

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

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

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

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

圖1 重選父節(jié)點過程示意圖

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

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

表1 候選點路徑代價

3.2 修剪隨機樹

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

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

圖2 修剪隨機樹過程示意圖

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

表2 修剪后的路徑代價

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

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

修剪隨機樹的意義在于通過新生成的節(jié)點來替換老節(jié)點的父節(jié)點，修改隨機樹的樹枝，去掉多余的樹枝路徑，縮短路徑距離。

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

4 路徑處理

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

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

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

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

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

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

圖4 三角形原則修剪過程示意圖

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

5 Matlab仿真實驗

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

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

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

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

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

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

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

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

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

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

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

表3 Map1仿真結(jié)果

表4 Map2仿真結(jié)果

表5 Map3仿真結(jié)果

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

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

6 結(jié)論

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