基于改進(jìn)RRT＊算法的自動(dòng)駕駛車輛路徑規(guī)劃研究

宋若旸，闕海霞，馬宗鈺，蘭海潮

（長(zhǎng)安大學(xué) 汽車學(xué)院，陜西 西安 710064）

前言

在科學(xué)技術(shù)不斷發(fā)展的今天，傳統(tǒng)汽車在不斷地走向智能化，并且逐步具備復(fù)雜環(huán)境感知、智能決策、協(xié)同控制和執(zhí)行的功能，而最終的目標(biāo)就是實(shí)現(xiàn)自動(dòng)駕駛[1]。國(guó)際自動(dòng)機(jī)工程師學(xué)會(huì)（SAE）將自動(dòng)駕駛的過(guò)程依據(jù)人工參與駕駛的情況分為五級(jí)來(lái)逐步完成[2]。自動(dòng)駕駛汽車在感知外部環(huán)境后，進(jìn)行路徑規(guī)劃與自身決策控制。

路徑規(guī)劃是在有障礙物的環(huán)境中生成無(wú)碰撞路徑，并根據(jù)一定的準(zhǔn)則對(duì)其進(jìn)行優(yōu)化[3]，是智能駕駛汽車完成自動(dòng)駕駛行為的必要保證[4]。常見(jiàn)的路徑規(guī)劃算法大致可以分為以A*算法為代表的基于搜索的規(guī)劃算法、以 RRT為代表的基于采樣的規(guī)劃算法和以遺傳算法為代表的基于啟發(fā)式的規(guī)劃算法。遺傳算法（Genetic Algorithms簡(jiǎn)稱GA）由John Holland與20世紀(jì)60年代末創(chuàng)建，它源于達(dá)爾文的進(jìn)化論和孟德?tīng)枴⒛Ω倪z傳學(xué)理論，通過(guò)模擬生物進(jìn)化的機(jī)制來(lái)構(gòu)造人工系統(tǒng)，常用于解決傳統(tǒng)算法不能快速求解的復(fù)雜問(wèn)題[5]A*算法利用將出發(fā)點(diǎn)到此處的狀態(tài)函數(shù)和由此處到目標(biāo)點(diǎn)的預(yù)期函數(shù)組成估值評(píng)價(jià)函數(shù)來(lái)進(jìn)行搜索，該算法保證了最優(yōu)路徑的生成[6]。文獻(xiàn)[7]描述了在 A*算法上改進(jìn)的 Hybrid A*算法，它在包括車輛行駛方向 θ的分散的四維環(huán)境中描述車輛狀態(tài)，生成可行的平滑軌跡。快速搜索隨機(jī)樹(shù)算法（RRT）由Lavalle于1998年提出，通過(guò)采樣生成拓展樹(shù)的路徑規(guī)劃算法。該方法搜索速度較快且便于進(jìn)行約束，但是該方法生成的不是最優(yōu)路徑[8]。文獻(xiàn)[9]描述了Sertac Karaman和Matthew R.Walter等人提出的一種改進(jìn)的RRT算法，稱之為RRT*算法。

1 原始RRT＊算法分析

現(xiàn)將標(biāo)準(zhǔn)RRT*算法進(jìn)行分析[9]：X為全局配置空間，令障礙物Xobs分布在障礙物區(qū)域，Xfree=X/Xobs為無(wú)障礙物區(qū)域。xstart和xgoal都在全局配置空間內(nèi)。運(yùn)動(dòng)規(guī)劃所要解決的問(wèn)題即是產(chǎn)生一條可行的路徑 x(t)? Xfree，從初始點(diǎn)x(0)=xstar到達(dá)目標(biāo)點(diǎn)區(qū)域 x (t)∈ xgoal。

圖1 RRT＊算法采樣圖

（1）如圖1所示，首先無(wú)障礙物區(qū)域隨機(jī)采樣即Xrand∈Xfree。

（2）搜索隨機(jī)樹(shù)T上距離隨機(jī)點(diǎn)Xrand距離最近的節(jié)點(diǎn) Xnearest，并拓展一個(gè)步長(zhǎng)生成新的節(jié)點(diǎn) Xnew，檢測(cè)節(jié)點(diǎn)Xnearest與節(jié)點(diǎn)Xnew的連線間是否有障礙物，如沒(méi)有障礙物則將節(jié)點(diǎn)Xnew添加到搜索樹(shù)T上，并計(jì)算其價(jià)值函數(shù)。

（3）在生成的新節(jié)點(diǎn)規(guī)定的鄰域 R范圍內(nèi)搜索節(jié)點(diǎn)集合Xnear，計(jì)算將集合內(nèi)點(diǎn)Xnear-i作為Xnew父節(jié)點(diǎn)Xparent的價(jià)值函數(shù)，并與之前以Xnearest作為父節(jié)點(diǎn)Xparent的價(jià)值函數(shù)相比較。如果以Xnear-i作為Xnew父節(jié)點(diǎn)Xparent的價(jià)值函數(shù)小于原價(jià)值函數(shù) cost，并且檢測(cè)連線后無(wú)碰撞，則將 Xnearest與節(jié)點(diǎn) Xnew的連線斷開(kāi)，連接以 Xnear-i與Xnew，并更新價(jià)值函數(shù)。

（4）遍歷集合Xnear所有內(nèi)點(diǎn)Xnear-i，更新價(jià)值函數(shù)cost，并將此時(shí)價(jià)值函數(shù)cost所對(duì)應(yīng)的節(jié)點(diǎn)Xnear-i作為父節(jié)點(diǎn)Xparent添加至隨機(jī)樹(shù)T。

（5）重復(fù)以上過(guò)程，直至搜索樹(shù) T存在一點(diǎn)與目標(biāo)點(diǎn)Xgoal之間的距離小于所規(guī)定的閾值，停止搜索。

（6）倒敘找路，生成完整的RRT*路徑。

2 交叉路口環(huán)境下改進(jìn)的RRT＊

2.1 環(huán)境分析

選取十字形道路交叉口圖 2，作為典型車輛道路交通環(huán)境來(lái)改進(jìn)RRT*算法的無(wú)人駕駛車輛軌跡研究。其中，s0、s1、s2、s3、s4為車輛行駛道路；p1、p2、p3、p4分別為四條行駛道路外部的中點(diǎn)；p0為s0的中點(diǎn)；其余為障礙物；整個(gè)地圖尺寸為100×100m，車輛行駛道路的寬度為10m。

圖2 交叉路口環(huán)境圖

2.2 改進(jìn)的目標(biāo)偏向策略

帶偏向的RRT通過(guò)以一定的概率P將目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)來(lái)進(jìn)行RRT的拓展，是提高搜索效果的一種有效的方式。如果環(huán)境中障礙物密集，則應(yīng)該降低概率 P；相反，在障礙物稀疏的地圖中應(yīng)提高概率P的值。不管障礙物密集與稀疏，通常認(rèn)為取P為10%較為合理。

目標(biāo)偏向策略可以有效地將隨機(jī)樹(shù)向目標(biāo)點(diǎn)引導(dǎo)，當(dāng)隨機(jī)點(diǎn)與父節(jié)點(diǎn)之間有障礙物且生成新節(jié)點(diǎn)不碰撞的情況下，拓展的新節(jié)點(diǎn)將會(huì)靠近下障礙物。此情況經(jīng)常出現(xiàn)在車輛需要進(jìn)行轉(zhuǎn)向時(shí)，此時(shí)路徑曲率增大，對(duì)拓展樹(shù)造成負(fù)擔(dān)，同時(shí)過(guò)于靠近障礙物會(huì)影響車輛的安全行駛。

2.3 概率采樣策略

如圖2車輛行駛的起點(diǎn)為p1，當(dāng)車輛直行時(shí)取目標(biāo)點(diǎn)為p3。按照原始的 RRT*定義，此環(huán)境中采樣區(qū)域應(yīng)為S=s0+s1+s2+s3+s4，但是當(dāng)車輛進(jìn)行直行行駛時(shí)，區(qū)域s2和s4的期望采樣概率為0，即對(duì)區(qū)域s2和s4的采樣是無(wú)效的采樣，不僅增加了搜索時(shí)間，而且任何一個(gè)不必要方向的引導(dǎo)，都會(huì)對(duì)曲線的質(zhì)量造成影響。概率采樣策略則解決了這一問(wèn)題，以車輛進(jìn)行左轉(zhuǎn)彎為例來(lái)說(shuō)明概率采樣策略：

首先，車輛在直線行駛時(shí)滿足在區(qū)域s2和s4的期望采樣概率為0，即在改進(jìn)的RRT*算法中區(qū)域s2和s4的采樣點(diǎn)為零。進(jìn)而，在期望采樣概率非零的區(qū)域平均采樣即：

Psij，i=0，1，3表示在區(qū)域si，i=0，1，3中任一點(diǎn)j的采樣概率。計(jì)算區(qū)域面積占比，即在任一區(qū)域生成隨機(jī)采樣點(diǎn)的概率：

ti，i=0，1，3表示區(qū)域si，i=0，1，3的區(qū)域面積占比；Ssi，i=0，1，3表示區(qū)域si，i=0，1，3的面積。

隨機(jī)采樣點(diǎn)的區(qū)域分布情況如R所示：

考慮目標(biāo)偏向策略后，對(duì)公式（2）進(jìn)行修正，改進(jìn)的RRT*算法在期望采樣概率非0的區(qū)域平均采樣：

randp為概率修正系數(shù)，randp=1-P；Ti，i=0，1，3表示在區(qū)域si，i=0，1，3的區(qū)域生成采樣點(diǎn)的概率。

3 仿真結(jié)果與分析

本文基于Matlab/Simulink軟件版本為2019b，搭建十字形交叉路口環(huán)境，對(duì)車輛直行駕駛進(jìn)行仿真驗(yàn)證與分析，在相同環(huán)境下將標(biāo)準(zhǔn)RRT*算法和改進(jìn)的RRT*算法進(jìn)行比較。如圖3中（a）和（b）分別采用的原始方法和改進(jìn)方法，當(dāng)車輛使用標(biāo)準(zhǔn)RRT*算法進(jìn)行路徑規(guī)劃時(shí)，雖然標(biāo)準(zhǔn)RRT*算法里存在著價(jià)值函數(shù)使得搜索時(shí)遵循最短路徑原則，但是在非直行區(qū)域的采樣會(huì)使路徑向道路兩旁偏移，從而導(dǎo)致生成的路徑變長(zhǎng)且不是預(yù)期的直線行駛。在改進(jìn)的RRT*算法中，一方面目標(biāo)偏向策略將路徑向直行的方向引導(dǎo)，另一方面采用概率采樣策略使得路徑不向兩側(cè)偏移，最終導(dǎo)致路徑生成長(zhǎng)度變短，并且減少搜索時(shí)間，生成的路徑可以更好地使車輛以直線的方式向目標(biāo)點(diǎn)行駛。其中使用原始 RRT*算法進(jìn)行路徑規(guī)劃時(shí)，規(guī)劃路徑長(zhǎng)度為104.35m，仿真時(shí)長(zhǎng)為5.96s；使用改進(jìn) RRT*算法進(jìn)行路徑規(guī)劃時(shí)，規(guī)劃路徑長(zhǎng)度為100.35m，仿真時(shí)長(zhǎng)為5.71s。

圖3 車輛直行行駛結(jié)果圖

4 結(jié)束語(yǔ)

本文針對(duì)無(wú)人駕駛車輛在交叉路口環(huán)境下駕駛行為，在原始 RRT*算法的基礎(chǔ)上改進(jìn)了目標(biāo)偏向策略并提出了一種新的概率采樣策略，改進(jìn)的 RRT*算法保障了車輛的安全行駛，并且可以快速地生成合理的路徑。本文提出的在交叉路口環(huán)境下改進(jìn)的 RRT*算法未考慮在折點(diǎn)處的平滑策略，因此未來(lái)將進(jìn)一步平滑曲線，進(jìn)一步地保障車輛行駛的操縱穩(wěn)定性與平順性。