基于改進(jìn)RRT算法的無人車路徑規(guī)劃

李偉東, 李 樂

(大連理工大學(xué) 汽車工程學(xué)院,遼寧 大連 116024)

0 引言

隨著自動(dòng)駕駛技術(shù)的快速發(fā)展，路徑規(guī)劃作為無人車領(lǐng)域的核心技術(shù)之一，也逐漸成為研究熱點(diǎn)。路徑規(guī)劃可以分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃要求地圖信息已知并且不考慮動(dòng)態(tài)障礙物，其能夠規(guī)劃出一條從起點(diǎn)到終點(diǎn)的可行路徑。局部路徑規(guī)劃算法是指無人車在沿著全局路徑行駛時(shí)，如果全局路徑上出現(xiàn)障礙物導(dǎo)致車輛無法正常通過，需要規(guī)劃出用于局部避障的路徑[1]。與高精度地圖相結(jié)合，全局路徑規(guī)劃算法可以在整個(gè)位姿空間內(nèi)找到一條從初始位姿到目標(biāo)位姿的無碰撞路徑，同時(shí)該路徑還滿足環(huán)境約束、搜索時(shí)間約束和車輛運(yùn)動(dòng)學(xué)約束等約束條件。全局路徑規(guī)劃算法可以大致分為以下幾類：(1)基于仿生學(xué)的路徑規(guī)劃算法，如遺傳算法[2]、蟻群算法等[3]；(2)基于搜索的路徑規(guī)劃算法[3-4]；(3)基于采樣的路徑規(guī)劃算法[5]。應(yīng)用最廣泛的基于采樣的算法便是由Lavalle提出的RRT[6]算法。 它最顯著的優(yōu)點(diǎn)是不需要對(duì)空間進(jìn)行預(yù)處理并且具備概率完備性。但是，基本的RRT算法在規(guī)劃路徑時(shí)存在幾個(gè)缺點(diǎn)[7]：(1)采用隨機(jī)采樣算法，搜索過程具有盲目性和隨機(jī)性；(2)路徑曲折，不能應(yīng)用于無人車全局路徑規(guī)劃領(lǐng)域；(3)收斂速度慢，搜索效率低。

很多學(xué)者都致力于改進(jìn)RRT算法的缺陷。LaValle和Kuffner提出了RRT-Connect[7]算法，該算法會(huì)從起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)并行生成隨機(jī)樹來提高搜索速度。Karaman 和Frazzoli提出的RRT*[9]算法引入了路徑代價(jià)信息和重布線操作，雖然得到的路徑質(zhì)量最優(yōu)，但算法的收斂時(shí)間也隨之增加。此外還有很多RRT系列算法在不斷地涌現(xiàn)，例如Dynamic-RRT[10]算法、Informed RRT*[11]算法和批處理信息樹[12]( BIT*, batch informed trees)算法等。與此同時(shí)RRT系列算法也開始與其他算法結(jié)合。馮來春[13]提出將A*算法和RRT算法結(jié)合的方法，利用A*算法在格柵圖中生成的最短路徑來引導(dǎo)RRT算法的擴(kuò)展。Huang Shunyu[14]將人工勢(shì)場(chǎng)法引入到RRT算法中，通過建立障礙物斥力場(chǎng)來限制障礙物附近的搜索區(qū)域，并在隨機(jī)樹的擴(kuò)展中添加引力分量加快算法的收斂速度，但是可能會(huì)出現(xiàn)局部最小值問題。Ivojevic等人[15]針對(duì)類汽車機(jī)器人的路勁規(guī)劃問題，將RRT算法的節(jié)點(diǎn)間的直線連接替換為Dubins曲線，雖然能規(guī)劃出滿足汽車運(yùn)動(dòng)學(xué)約束的路徑，但是路徑過于冗余且轉(zhuǎn)彎太多。朱冰[16]提出基于安全場(chǎng)的改進(jìn)RRT*智能汽車路徑規(guī)劃算法，通過建立安全場(chǎng)模型引導(dǎo)車輛避障行駛，但是路徑的搜索具有較大盲目性。

綜合已有研究成果，針對(duì)上述提到的RRT算法的缺點(diǎn)，本文提出一種應(yīng)用于無人車全局路徑規(guī)劃領(lǐng)域的改進(jìn)RRT算法。本文算法首先使用人工勢(shì)場(chǎng)法和目標(biāo)動(dòng)態(tài)概率采樣策略來減少算法在搜索過程中的盲目性以及在連接終點(diǎn)位姿時(shí)使用Reeds-Shepp曲線，減少目標(biāo)點(diǎn)處額外的位姿調(diào)整，從而減少算法消耗的時(shí)間；然后算法考慮了車輛的非完整性約束，并對(duì)規(guī)劃的路徑進(jìn)行轉(zhuǎn)角約束檢測(cè)和碰撞檢測(cè)，保證汽車跟蹤行駛的安全性；最后對(duì)路徑進(jìn)行后處理優(yōu)化，最終得到的全局路徑更適合無人車跟蹤行駛。

1 汽車運(yùn)動(dòng)學(xué)模型

標(biāo)準(zhǔn)RRT算法僅可以尋找一條能夠連接起點(diǎn)和終點(diǎn)路徑，但是規(guī)劃的路徑曲折且無目的性，不能滿足無人車的運(yùn)動(dòng)學(xué)約束，從而車輛無法跟隨規(guī)劃的全局路徑行駛。因此本文算法在進(jìn)行路徑規(guī)劃時(shí)考慮車輛的運(yùn)動(dòng)學(xué)約束，車輛運(yùn)動(dòng)學(xué)模型如圖1。

圖1 車輛運(yùn)動(dòng)學(xué)模型

以車輛的后軸中心為車輛的參考點(diǎn)，b為車的軸距，φ為前輪轉(zhuǎn)角，θ為航向角，r為轉(zhuǎn)彎半徑，k為路徑曲率。假設(shè)車輛的速度為v，那么根據(jù)圖1就能得到車輛的運(yùn)動(dòng)學(xué)方程，如式(1)：

(1)

車輛受到的運(yùn)動(dòng)學(xué)約束需要滿足下式：

(2)

式中,vmax為最大車速，φmax為最大等效前輪轉(zhuǎn)向角，kmax為最大路徑曲率，Rmin為最小轉(zhuǎn)彎半徑。

2 改進(jìn)的RRT算法

2.1 地圖及障礙物柵格化

為了能夠精確表示地圖信息、障礙物信息以及定位信息，需要將無人車所處環(huán)境的高精度地圖作為算法的基礎(chǔ)。全局路徑規(guī)劃可以使用柵格地圖，二維柵格地圖使用占用柵格的形式存儲(chǔ)環(huán)境中的靜態(tài)障礙物信息，為全局路徑規(guī)劃提供信息[17]。本文使用已知的二維柵格地圖作為地圖信息輸入，在全局路徑規(guī)劃算法開始時(shí)初始化地圖信息。

2.2 基本RRT算法

當(dāng)?shù)玫綀?chǎng)景的柵格地圖后，便可以根據(jù)目標(biāo)位姿和起始位姿構(gòu)造搜索樹了。RRT算法示意圖如圖2所示，以起始點(diǎn)作為隨機(jī)樹的根節(jié)點(diǎn)，通過隨機(jī)采樣，將隨機(jī)樹的擴(kuò)展引導(dǎo)到可行區(qū)域直到搜索到目標(biāo)點(diǎn)，從而生成一條從起始點(diǎn)到目標(biāo)點(diǎn)的全局規(guī)劃路徑。

圖2 RRT算法示意圖

基本步驟如下：

1)以狀態(tài)空間的起點(diǎn)Xinit作為根節(jié)點(diǎn)構(gòu)造隨機(jī)樹；

2)在搜索空間內(nèi)產(chǎn)生隨機(jī)采樣點(diǎn)Xrand，用于引導(dǎo)隨機(jī)樹的擴(kuò)展；

3)遍歷整個(gè)隨機(jī)樹上已產(chǎn)生的節(jié)點(diǎn)，選出距離隨機(jī)點(diǎn)Xrand最近的樹節(jié)點(diǎn)Xnear；

4)從Xnear節(jié)點(diǎn)沿著Xrand節(jié)點(diǎn)的方向以擴(kuò)展步長(zhǎng)生成新的節(jié)點(diǎn)Xnew作為新的樹節(jié)點(diǎn)。如果擴(kuò)展過程中遇到障礙物則取消本次擴(kuò)展。重復(fù)上述迭代過程直到目標(biāo)節(jié)點(diǎn)變?yōu)槿~子節(jié)點(diǎn)或者超過規(guī)定的迭代次數(shù)時(shí)搜索結(jié)束。從目標(biāo)點(diǎn)回溯到起點(diǎn)便可得到規(guī)劃的路徑。

2.3 目標(biāo)動(dòng)態(tài)概率采樣

為了減少RRT算法搜索過程的盲目性，解決盲目搜索的問題，本文算法在隨機(jī)采樣時(shí)引入了目標(biāo)動(dòng)態(tài)概率采樣。動(dòng)態(tài)概率采樣策略首先會(huì)設(shè)置一個(gè)目標(biāo)采樣閾值pbias(0

(3)

當(dāng)隨機(jī)樹擴(kuò)展過程中遇到的障礙物較少時(shí)，直接使用目標(biāo)點(diǎn)作為采樣點(diǎn)往往會(huì)加快搜索速度。但是當(dāng)環(huán)境中障礙物較多時(shí)，在目標(biāo)點(diǎn)方向上進(jìn)行簡(jiǎn)單的概率擴(kuò)展，可能會(huì)陷入局部困境問題。因此為了應(yīng)對(duì)多種不同的場(chǎng)景，目標(biāo)點(diǎn)采樣概率閾值pbias應(yīng)該被設(shè)置為動(dòng)態(tài)變化的值，Niter為當(dāng)前迭代次數(shù)，Ncurr為當(dāng)前所生成的有效節(jié)點(diǎn)個(gè)數(shù)，目標(biāo)點(diǎn)采樣概率閾值pbias公式如式(4)：

(4)

式中,k代表目標(biāo)概率采樣閾值的最大值。Ncurr和Niter的比值代表柵格地圖中障礙物的稠密程度。比值越大代表當(dāng)前有效節(jié)點(diǎn)占比越多，障礙物越少，隨機(jī)樹向目標(biāo)節(jié)點(diǎn)擴(kuò)展的可能性越大。相反，意味著無效節(jié)點(diǎn)數(shù)目較多，障礙物越稠密，隨機(jī)樹向隨機(jī)點(diǎn)擴(kuò)展的可能性更大。

2.4 基于人工勢(shì)場(chǎng)的節(jié)點(diǎn)擴(kuò)展優(yōu)化

RRT算法由于其搜索的隨機(jī)性強(qiáng)，會(huì)在空間進(jìn)行搜索時(shí)產(chǎn)生許多無用節(jié)點(diǎn)，收斂速度低，搜索時(shí)間過長(zhǎng)。同時(shí)，在人工勢(shì)場(chǎng)法路徑規(guī)劃中，無人車總是朝向虛擬勢(shì)場(chǎng)的負(fù)梯度方向運(yùn)動(dòng)，在復(fù)雜和特殊的環(huán)境可能會(huì)出現(xiàn)勢(shì)能最小處在某個(gè)受到的引力和斥力相互抵消的非目標(biāo)點(diǎn)位置而不是目標(biāo)點(diǎn)位置的情況，出現(xiàn)局部最小值的情況，導(dǎo)致目標(biāo)節(jié)點(diǎn)不可達(dá)[18]。

針對(duì)上述問題，本文將人工勢(shì)場(chǎng)法的目標(biāo)引力作用和障礙物斥力作用融入到RRT算法的擴(kuò)展節(jié)點(diǎn)過程中，根據(jù)先驗(yàn)格柵地圖構(gòu)造地圖的人工勢(shì)場(chǎng)，并使隨機(jī)樹的擴(kuò)展能夠在勢(shì)場(chǎng)的引導(dǎo)下朝著目標(biāo)點(diǎn)進(jìn)行擴(kuò)展，加快算法的收斂速度。同時(shí)依靠目標(biāo)動(dòng)態(tài)概率采樣和RRT算法的擴(kuò)展隨機(jī)性，可以有效避免傳統(tǒng)的人工勢(shì)場(chǎng)法在進(jìn)行路徑規(guī)劃時(shí)會(huì)陷入局部最小值的問題。人工勢(shì)場(chǎng)的引力場(chǎng)Ua(p)、斥力場(chǎng)Ur(p)以及合力勢(shì)場(chǎng)Utotal的函數(shù)如下所示：

(5)

(6)

Utotal(p)=Ua(p)+Ur(p)

(7)

其中:ka是目標(biāo)引力場(chǎng)增益系數(shù)，kr是斥力場(chǎng)增益系數(shù)。ρ0是一個(gè)距離閾值，當(dāng)節(jié)點(diǎn)到最近的障礙物距離大于該閾值則不產(chǎn)生斥力。ρ(p,pgoal)和ρ(p,pobs)分別代表當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)和最近障礙物的距離。如圖3所示，圖(a)的環(huán)境從起點(diǎn)到終點(diǎn)產(chǎn)生的總勢(shì)場(chǎng)圖如圖(b)所示。

圖3 總勢(shì)場(chǎng)示意圖

因此算法擴(kuò)展過程中，受到的目標(biāo)引力函數(shù)與障礙物斥力函數(shù)分別如式(8)所示：

Fa(p)=kaρ(p,pgoal)

(8)

(9)

Ftotal(p)=Fa(p)+Fr(p)

(10)

在人工勢(shì)場(chǎng)中，目標(biāo)產(chǎn)生的引力和障礙物產(chǎn)生的斥力的合力Ftotal的方向就是總勢(shì)場(chǎng)的負(fù)梯度方向。在新節(jié)點(diǎn)Xnew擴(kuò)展過程中融合人工勢(shì)場(chǎng)的作用，使節(jié)點(diǎn)擴(kuò)展的方向不僅沿著Xrand，還會(huì)考慮勢(shì)場(chǎng)合力Ftotal的引導(dǎo)作用。擴(kuò)展節(jié)點(diǎn)由兩個(gè)向量疊加而成，擴(kuò)展節(jié)點(diǎn)的計(jì)算公式如式(11)所示：

Xnew=Xnear+d(wrnrand+(1-wr)nf)

(11)

式中,d為擴(kuò)展步長(zhǎng)，wr為隨機(jī)點(diǎn)的擴(kuò)展偏向權(quán)重，nrand為Xrand方向的單位向量，nf為合力Ftotal方向的單位向量。

新節(jié)點(diǎn)擴(kuò)展過程示意圖如圖4所示。

圖4 節(jié)點(diǎn)擴(kuò)展示意圖

2.5 路徑約束條件

由前文分析可知，由于汽車的動(dòng)力學(xué)約束，生成路徑中的轉(zhuǎn)角應(yīng)該小于最大前輪轉(zhuǎn)角φmax，因此在生成新節(jié)點(diǎn)和生成路徑時(shí)需要考慮角度約束。轉(zhuǎn)角的計(jì)算如圖5所示，路徑中3個(gè)連續(xù)節(jié)點(diǎn)X1，X2，X3及其夾角角度φ，計(jì)算公式如式(12)所示：

(12)

在擴(kuò)展過程中會(huì)根據(jù)公式判斷擴(kuò)展的新節(jié)點(diǎn)Xnew和Xnear的連線與Xnear及其父節(jié)點(diǎn)Xparent的連線之間的夾角是否大于前輪最大轉(zhuǎn)角，如果大于約束，則放棄本次擴(kuò)展。

圖5 轉(zhuǎn)角約束示意圖

除了路徑轉(zhuǎn)角約束以外，為了保證所規(guī)劃出的路徑對(duì)于無人車來說是安全無碰撞的，還需要對(duì)路徑進(jìn)行碰撞檢測(cè)。本文采用方向包圍盒(OBB, oriented bounding box)的碰撞檢測(cè)方法，一個(gè)對(duì)象的OOB指的是包含該對(duì)象且相對(duì)于坐標(biāo)軸方向任意的最小長(zhǎng)方體[19]。如果兩個(gè)凸多邊形不發(fā)生碰撞，則一定存在一條分離軸，使得兩物體在該分離軸上的投影不相交。在格柵地圖中，OBB的檢測(cè)原理如圖6所示，矩形A和B分別代表車輛和障礙物的OOB。Ax、Ay為以矩形A中心點(diǎn)OA為原點(diǎn)坐標(biāo)的局部坐標(biāo)系下的單位向量，Bx、By為以矩形B中心點(diǎn)為OB原點(diǎn)坐標(biāo)的局部坐標(biāo)系下的單位向量。α為全局坐標(biāo)系下車輛的航向角，φ為兩矩形中心連線與格柵地圖的全局坐標(biāo)系x軸正方向的夾角。

圖6 OBB碰撞檢測(cè)示意圖

在實(shí)際應(yīng)用中只需要判斷兩矩形分別在Ax、Ay、Bx、By4個(gè)方向的投影是否有重疊即可。如果矩形A和矩形B沒有碰撞的話，應(yīng)該滿足下式4個(gè)條件：

(13)

式中，L為矩形中心點(diǎn)的距離LA、LB分別代表矩形A、B的長(zhǎng)度；WA、WB分別代表矩形A、B的寬度。

2.6 目標(biāo)點(diǎn)貪心連接

Reeds-Shepp[20]曲線是由多段半徑為最小轉(zhuǎn)彎半徑的圓弧曲線或直線拼接而成，能夠在一個(gè)位姿到另一個(gè)位姿之間生成滿足運(yùn)動(dòng)學(xué)約束的前進(jìn)或倒車軌跡。在原RRT算法中，當(dāng)隨機(jī)樹新擴(kuò)展的節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離小于擴(kuò)展步長(zhǎng)時(shí)，就宣告搜索結(jié)束找到路徑，但如果有終點(diǎn)位姿要求，RRT算法就需要反復(fù)搜索以保證滿足終點(diǎn)的位姿要求。本文采用Reeds-Shepp曲線來代替原RRT算法的目標(biāo)點(diǎn)連接方式，當(dāng)新擴(kuò)展節(jié)點(diǎn)進(jìn)入到目標(biāo)點(diǎn)的可直接連接范圍后，每次搜索都嘗試使用Reeds-Shepp曲線對(duì)目標(biāo)點(diǎn)連接，從而避免反復(fù)搜索以滿足終點(diǎn)位姿要求。通常目標(biāo)點(diǎn)的連接范圍可以設(shè)置為10倍的擴(kuò)展步長(zhǎng)，這樣以來就能減少大量搜索時(shí)間和擴(kuò)展的節(jié)點(diǎn)，避免多余的位姿調(diào)整。

2.7 路徑后處理優(yōu)化

假設(shè)使用該算法規(guī)劃得到的初始全局路徑點(diǎn)的集合為P{P1,P2,P3,…,Pn}，如圖7所示，假設(shè)節(jié)點(diǎn)P1與節(jié)點(diǎn)P4之間能夠通過直線連接并且連線又同時(shí)滿足轉(zhuǎn)角約束和無碰撞約束，則P1和P4之間的P2和P3節(jié)點(diǎn)均為冗余節(jié)點(diǎn)，可以將其從路徑節(jié)點(diǎn)集合中刪除。在得到初始路徑后進(jìn)行路徑剪枝，從起點(diǎn)開始，遍歷整個(gè)節(jié)點(diǎn)集合，找到并去除所有冗余節(jié)點(diǎn)，能夠減少路徑長(zhǎng)度并使車輛駕駛的平順性提高。

圖7 路徑剪枝示意圖

但是此時(shí)得到路徑仍由若干線段連接而成，不滿足車輛對(duì)平順性和安全性的要求，則需對(duì)路徑進(jìn)行平滑處理。由于三階貝塞爾曲線能夠保證始末位姿要求和曲率連續(xù)要求，則對(duì)3個(gè)節(jié)點(diǎn)形成的兩段直線采用兩段三階貝塞爾曲線進(jìn)行平滑處理。n階貝塞爾曲線表達(dá)式如式(14)所示：

(14)

式中,Pi代表貝塞爾曲線的控制點(diǎn)，u代表貝塞爾曲線的參數(shù)且0≤u≤1。Bi,n為n次Bernstein基函數(shù)，且滿足：

(15)

則原來由3個(gè)隨機(jī)樹節(jié)點(diǎn)確定的兩段直線路徑經(jīng)過兩段三階貝塞爾曲線平滑后的結(jié)果示意圖如圖8所示。

圖8 貝塞爾曲線應(yīng)用示意圖

圖中P1、P2、P3分別為路徑相連的兩直線的節(jié)點(diǎn)。A1、A2、A3、P為第一段三階貝塞爾曲線的控制點(diǎn)。B1、B2、B3、P為第二段三階貝塞爾曲線的控制點(diǎn)。其中控制點(diǎn)P為A3和B3連線的中點(diǎn)。

本文的改進(jìn)RRT算法的流程圖如圖9所示。

圖9 改進(jìn)RRT算法流程圖

3 仿真實(shí)驗(yàn)對(duì)比分析

本文使用Matlab對(duì)本文所提出的改進(jìn)RRT算法進(jìn)行仿真實(shí)驗(yàn)，測(cè)試主機(jī)的處理器為Intel(R) Core(TM) i7-9700，主頻為3.0 GHz，內(nèi)存大小為16 GB。

為了驗(yàn)證本文的改進(jìn)RRT算法具有更好的性能，將RRT算法、RRT*算法和改進(jìn)RRT算法在簡(jiǎn)單障礙物環(huán)境、復(fù)雜障礙物環(huán)境和單一通道環(huán)境3種二維仿真柵格地圖環(huán)境下進(jìn)行全局路徑規(guī)劃對(duì)比試驗(yàn)，3種仿真地圖大小均為500×500個(gè)像素的柵格地圖表示，起點(diǎn)均為(50,50)，終點(diǎn)均為(450,450)。在3種環(huán)境下每種算法均進(jìn)行100次重復(fù)實(shí)驗(yàn)，并統(tǒng)計(jì)算法各個(gè)指標(biāo)的平均值。3種環(huán)境下各算法的規(guī)劃結(jié)果如圖10～12所示，其中(a)圖為基本RRT算法規(guī)劃效果，(b)圖為基本RRT*算法規(guī)劃效果，(c)圖為改進(jìn)RRT算法得到的路徑優(yōu)化前的效果，(d)圖為改進(jìn)RRT算法對(duì)初始路徑優(yōu)化后的效果。

圖10為簡(jiǎn)單障礙物環(huán)境的算法規(guī)劃對(duì)比圖，地圖中障礙物較大，環(huán)境較為簡(jiǎn)單。圖(a)為基本RRT算法規(guī)劃的一條可行路徑，但由于RRT算法的盲目搜索特性，生長(zhǎng)樹會(huì)隨機(jī)分布在無障礙區(qū)域，算法效率較低并且得到路徑質(zhì)量很差。圖(b)為RRT*算法規(guī)劃的一條可行路徑，由于RRT*算法具有漸進(jìn)最優(yōu)的特性，所以RRT*能夠得到較優(yōu)的路徑長(zhǎng)度，但是需要大量反復(fù)迭代，節(jié)點(diǎn)利用率和搜索效率較差，并且不滿足路徑安全約束。圖(c)和圖(d)是本文改進(jìn)RRT算法在進(jìn)行路徑優(yōu)化前后的規(guī)劃路徑，由圖可見路徑剪枝優(yōu)化效果顯著。

圖10 簡(jiǎn)單環(huán)境下算法效果對(duì)比

從表1的實(shí)驗(yàn)數(shù)據(jù)中可以得出，由于地圖環(huán)境比較簡(jiǎn)單，目標(biāo)概率采樣和人工勢(shì)場(chǎng)的引導(dǎo)作用可以被充分利用，減少了大量無用節(jié)點(diǎn)的擴(kuò)展，明顯加快了搜索速度，相比于RRT算法和RRT*算法，擴(kuò)展節(jié)點(diǎn)數(shù)目分別減少了43.50%和83.37%，搜索時(shí)間分別減少了62.12%和77.46%。并且在經(jīng)過節(jié)點(diǎn)剪枝和路徑優(yōu)化后，改進(jìn)RRT算法的路徑長(zhǎng)度較優(yōu)，其路徑長(zhǎng)度相比于RRT算法的路徑長(zhǎng)度縮短13.86%，但是由于考慮了運(yùn)動(dòng)學(xué)約束和碰撞約束，相比于路徑長(zhǎng)度最優(yōu)的RRT*的算法增加了3.39%。

表1 簡(jiǎn)單障礙物環(huán)境實(shí)驗(yàn)結(jié)果對(duì)比

圖11為復(fù)雜環(huán)境地圖的規(guī)劃效果，環(huán)境中存在大量大小不一的障礙物，從起點(diǎn)到終點(diǎn)可以有多條路徑可以選擇。圖(a)中RRT算法的規(guī)劃效果圖，可以看出隨機(jī)樹的擴(kuò)展雜亂無章，路徑曲折。圖(b)中RRT*算法能夠利用自身的重布線特性在復(fù)雜的環(huán)境中規(guī)劃出一條較優(yōu)路線，但是擴(kuò)展樹所需節(jié)點(diǎn)較多，搜索效率較差。圖(c)和圖(d)中的規(guī)劃效果明顯優(yōu)于RRT算法和RRT*算法，并總能找到較優(yōu)路徑，并且在需要多次轉(zhuǎn)折的場(chǎng)景下，路徑優(yōu)化的效果仍顯著。

圖11 復(fù)雜環(huán)境下算法效果對(duì)比

表2的實(shí)驗(yàn)結(jié)果得知，本文改進(jìn)的RRT*算法的節(jié)點(diǎn)擴(kuò)展數(shù)目相比于RRT算法和RRT*算法分別減少了67.36%和85.07%，規(guī)劃時(shí)間分別減少了63.14%和77.46%。規(guī)劃得到的路徑長(zhǎng)度相比RRT算法縮短了23.12%，并且和路徑長(zhǎng)度最優(yōu)的RRT*算法差距不大。因此本文的改進(jìn)RRT算法該環(huán)境下能夠保證路徑長(zhǎng)度達(dá)到相對(duì)較小的同時(shí)提高了算法的規(guī)劃效率，路徑也變得更加平滑。

表2 復(fù)雜障礙物環(huán)境實(shí)驗(yàn)結(jié)果對(duì)比

圖12為單一通道障礙物環(huán)境，特點(diǎn)是從起點(diǎn)到終點(diǎn)只有一條狹窄的通道可以經(jīng)過。3種算法的規(guī)劃效果對(duì)比來看，RRT和RRT*在尋找通道入口前需要經(jīng)過大量的隨機(jī)擴(kuò)展，節(jié)點(diǎn)利用率和搜索效率非常低。而本文改進(jìn)的RRT算法能夠根據(jù)人工勢(shì)場(chǎng)的引導(dǎo)作用，快速找到通道的入口，搜索時(shí)間相比于RRT算法和RRT*算法大幅降低，并且能夠依靠障礙物斥力作用和碰撞檢測(cè)機(jī)制，能夠在狹窄通道內(nèi)較快得到一條安全無碰撞的可行的全局路徑。

圖12 單一通環(huán)境下算法效果對(duì)比

從表3的實(shí)驗(yàn)數(shù)據(jù)中可以得出，改進(jìn)RRT算法的平均擴(kuò)展節(jié)點(diǎn)數(shù)為 41.25個(gè)，比 RRT減少 65.97%，比RRT*減少87.21%；平均規(guī)劃時(shí)長(zhǎng)為1.05 s，比RRT縮短58.33%，比RRT*縮短76.87%；平均路徑長(zhǎng)度為616.83 m，比RRT減少14.80%，比RRT*少幅度增加了2.32%。

表3 單一通道環(huán)境實(shí)驗(yàn)結(jié)果對(duì)比

根據(jù)上述的仿真圖和實(shí)驗(yàn)結(jié)果可以明顯的看出，本文的改進(jìn)RRT算法無論是在規(guī)劃時(shí)長(zhǎng)、擴(kuò)展節(jié)點(diǎn)數(shù)還是路徑長(zhǎng)度上都相比標(biāo)準(zhǔn)RRT算法具有明顯優(yōu)勢(shì)。雖然本文的RRT算法得到的路徑略長(zhǎng)于RRT*算法得到的路徑長(zhǎng)度，但是花費(fèi)時(shí)間和無用節(jié)點(diǎn)的數(shù)量明顯減少。除了規(guī)劃時(shí)間、節(jié)點(diǎn)數(shù)量和路徑成本等指標(biāo)以外，RRT和RRT*算法得到的路徑曲折并且不滿足碰撞約束，汽車的行駛安全和平順性不能得到保障。并且由圖10到圖12的(c)圖和(d)圖可以發(fā)現(xiàn)，本文提出的路徑后剪枝和優(yōu)化方法效果顯著。因此，本文所提出的改進(jìn)RRT算法在3種復(fù)雜的障礙物環(huán)境下，具有規(guī)劃效率更高，路徑長(zhǎng)度較優(yōu)，路徑光滑且安全的特點(diǎn)。

4 結(jié)束語

本文針對(duì)RRT算法在無人車全局路徑規(guī)劃領(lǐng)域存在的問題，提出一種綜合改進(jìn)算法。首先引入目標(biāo)動(dòng)態(tài)概率采樣和人工勢(shì)場(chǎng)引導(dǎo)隨機(jī)樹擴(kuò)展的策略，提高了算法的搜索效率，減少冗余節(jié)點(diǎn)的產(chǎn)生，內(nèi)存占用更少；然后定義了路徑的轉(zhuǎn)角約束和碰撞約束，使路徑能夠滿足汽車的運(yùn)動(dòng)學(xué)約束并使無人車安全無碰撞行駛；其次當(dāng)隨機(jī)樹擴(kuò)展到目標(biāo)點(diǎn)可連接范圍后，每次迭代都嘗試使用Reeds-Shepp曲線連接目標(biāo)點(diǎn)，減少多余的位姿調(diào)整，進(jìn)一步提高算法效率；最后在路徑初步規(guī)劃完成后，提出一種路徑修剪的方法，對(duì)路徑進(jìn)行簡(jiǎn)化，并使用兩段三階貝塞爾曲線對(duì)路徑轉(zhuǎn)角進(jìn)行平滑連接，去除冗余節(jié)點(diǎn)的同時(shí)還能使路徑更加光滑。通過與RRT算法和RRT*算法仿真實(shí)驗(yàn)對(duì)比，結(jié)果表明本文提出的改進(jìn)RRT算法在多種環(huán)境下的規(guī)劃得到的路徑在搜索效率、路徑質(zhì)量上都更具優(yōu)越性。該算法對(duì)無人車的全局路徑規(guī)劃具有一定的參考意義。