宋若旸,闕海霞,馬宗鈺,蘭海潮
(長安大學(xué) 汽車學(xué)院,陜西 西安 710064)
在科學(xué)技術(shù)不斷發(fā)展的今天,傳統(tǒng)汽車在不斷地走向智能化,并且逐步具備復(fù)雜環(huán)境感知、智能決策、協(xié)同控制和執(zhí)行的功能,而最終的目標(biāo)就是實(shí)現(xiàn)自動駕駛[1]。國際自動機(jī)工程師學(xué)會(SAE)將自動駕駛的過程依據(jù)人工參與駕駛的情況分為五級來逐步完成[2]。自動駕駛汽車在感知外部環(huán)境后,進(jìn)行路徑規(guī)劃與自身決策控制。
路徑規(guī)劃是在有障礙物的環(huán)境中生成無碰撞路徑,并根據(jù)一定的準(zhǔn)則對其進(jìn)行優(yōu)化[3],是智能駕駛汽車完成自動駕駛行為的必要保證[4]。常見的路徑規(guī)劃算法大致可以分為以A*算法為代表的基于搜索的規(guī)劃算法、以 RRT為代表的基于采樣的規(guī)劃算法和以遺傳算法為代表的基于啟發(fā)式的規(guī)劃算法。遺傳算法(Genetic Algorithms簡稱GA)由John Holland與20世紀(jì)60年代末創(chuàng)建,它源于達(dá)爾文的進(jìn)化論和孟德爾、摩根的遺傳學(xué)理論,通過模擬生物進(jìn)化的機(jī)制來構(gòu)造人工系統(tǒng),常用于解決傳統(tǒng)算法不能快速求解的復(fù)雜問題[5]A*算法利用將出發(fā)點(diǎn)到此處的狀態(tài)函數(shù)和由此處到目標(biāo)點(diǎn)的預(yù)期函數(shù)組成估值評價(jià)函數(shù)來進(jìn)行搜索,該算法保證了最優(yōu)路徑的生成[6]。文獻(xiàn)[7]描述了在 A*算法上改進(jìn)的 Hybrid A*算法,它在包括車輛行駛方向 θ的分散的四維環(huán)境中描述車輛狀態(tài),生成可行的平滑軌跡??焖偎阉麟S機(jī)樹算法(RRT)由Lavalle于1998年提出,通過采樣生成拓展樹的路徑規(guī)劃算法。該方法搜索速度較快且便于進(jìn)行約束,但是該方法生成的不是最優(yōu)路徑[8]。文獻(xiàn)[9]描述了Sertac Karaman和Matthew R.Walter等人提出的一種改進(jìn)的RRT算法,稱之為RRT*算法。
現(xiàn)將標(biāo)準(zhǔn)RRT*算法進(jìn)行分析[9]:X為全局配置空間,令障礙物Xobs分布在障礙物區(qū)域,Xfree=X/Xobs為無障礙物區(qū)域。xstart和xgoal都在全局配置空間內(nèi)。運(yùn)動規(guī)劃所要解決的問題即是產(chǎn)生一條可行的路徑 x(t)? Xfree,從初始點(diǎn)x(0)=xstar到達(dá)目標(biāo)點(diǎn)區(qū)域 x (t)∈ xgoal。
圖1 RRT*算法采樣圖
(1)如圖1所示,首先無障礙物區(qū)域隨機(jī)采樣即Xrand∈Xfree。
(2)搜索隨機(jī)樹T上距離隨機(jī)點(diǎn)Xrand距離最近的節(jié)點(diǎn) Xnearest,并拓展一個(gè)步長生成新的節(jié)點(diǎn) Xnew,檢測節(jié)點(diǎn)Xnearest與節(jié)點(diǎn)Xnew的連線間是否有障礙物,如沒有障礙物則將節(jié)點(diǎn)Xnew添加到搜索樹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,并且檢測連線后無碰撞,則將 Xnearest與節(jié)點(diǎn) Xnew的連線斷開,連接以 Xnear-i與Xnew,并更新價(jià)值函數(shù)。
(4)遍歷集合Xnear所有內(nèi)點(diǎn)Xnear-i,更新價(jià)值函數(shù)cost,并將此時(shí)價(jià)值函數(shù)cost所對應(yīng)的節(jié)點(diǎn)Xnear-i作為父節(jié)點(diǎn)Xparent添加至隨機(jī)樹T。
(5)重復(fù)以上過程,直至搜索樹 T存在一點(diǎn)與目標(biāo)點(diǎn)Xgoal之間的距離小于所規(guī)定的閾值,停止搜索。
(6)倒敘找路,生成完整的RRT*路徑。
選取十字形道路交叉口圖 2,作為典型車輛道路交通環(huán)境來改進(jìn)RRT*算法的無人駕駛車輛軌跡研究。其中,s0、s1、s2、s3、s4為車輛行駛道路;p1、p2、p3、p4分別為四條行駛道路外部的中點(diǎn);p0為s0的中點(diǎn);其余為障礙物;整個(gè)地圖尺寸為100×100m,車輛行駛道路的寬度為10m。
圖2 交叉路口環(huán)境圖
帶偏向的RRT通過以一定的概率P將目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)來進(jìn)行RRT的拓展,是提高搜索效果的一種有效的方式。如果環(huán)境中障礙物密集,則應(yīng)該降低概率 P;相反,在障礙物稀疏的地圖中應(yīng)提高概率P的值。不管障礙物密集與稀疏,通常認(rèn)為取P為10%較為合理。
目標(biāo)偏向策略可以有效地將隨機(jī)樹向目標(biāo)點(diǎn)引導(dǎo),當(dāng)隨機(jī)點(diǎn)與父節(jié)點(diǎn)之間有障礙物且生成新節(jié)點(diǎn)不碰撞的情況下,拓展的新節(jié)點(diǎn)將會靠近下障礙物。此情況經(jīng)常出現(xiàn)在車輛需要進(jìn)行轉(zhuǎn)向時(shí),此時(shí)路徑曲率增大,對拓展樹造成負(fù)擔(dān),同時(shí)過于靠近障礙物會影響車輛的安全行駛。
如圖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,即對區(qū)域s2和s4的采樣是無效的采樣,不僅增加了搜索時(shí)間,而且任何一個(gè)不必要方向的引導(dǎo),都會對曲線的質(zhì)量造成影響。概率采樣策略則解決了這一問題,以車輛進(jìn)行左轉(zhuǎn)彎為例來說明概率采樣策略:
首先,車輛在直線行駛時(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)偏向策略后,對公式(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)的概率。
本文基于Matlab/Simulink軟件版本為2019b,搭建十字形交叉路口環(huán)境,對車輛直行駕駛進(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ū)域的采樣會使路徑向道路兩旁偏移,從而導(dǎo)致生成的路徑變長且不是預(yù)期的直線行駛。在改進(jìn)的RRT*算法中,一方面目標(biāo)偏向策略將路徑向直行的方向引導(dǎo),另一方面采用概率采樣策略使得路徑不向兩側(cè)偏移,最終導(dǎo)致路徑生成長度變短,并且減少搜索時(shí)間,生成的路徑可以更好地使車輛以直線的方式向目標(biāo)點(diǎn)行駛。其中使用原始 RRT*算法進(jìn)行路徑規(guī)劃時(shí),規(guī)劃路徑長度為104.35m,仿真時(shí)長為5.96s;使用改進(jìn) RRT*算法進(jìn)行路徑規(guī)劃時(shí),規(guī)劃路徑長度為100.35m,仿真時(shí)長為5.71s。
圖3 車輛直行行駛結(jié)果圖
本文針對無人駕駛車輛在交叉路口環(huán)境下駕駛行為,在原始 RRT*算法的基礎(chǔ)上改進(jìn)了目標(biāo)偏向策略并提出了一種新的概率采樣策略,改進(jìn)的 RRT*算法保障了車輛的安全行駛,并且可以快速地生成合理的路徑。本文提出的在交叉路口環(huán)境下改進(jìn)的 RRT*算法未考慮在折點(diǎn)處的平滑策略,因此未來將進(jìn)一步平滑曲線,進(jìn)一步地保障車輛行駛的操縱穩(wěn)定性與平順性。