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

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

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

前言

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

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

1 原始RRT＊算法分析

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

圖1 RRT＊算法采樣圖

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

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

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

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

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

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

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

2.1 環(huán)境分析

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

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

2.2 改進的目標偏向策略

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

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

2.3 概率采樣策略

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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