張 涌,高 峰,趙奉奎

(南京林業(yè)大學 汽車與交通工程學院,江蘇 南京 210037)

0 引 言

路徑規(guī)劃問題一直是自動駕駛車輛整體規(guī)劃與控制系統(tǒng)的重要組成部分。路徑規(guī)劃是指在已知環(huán)境中車輛的起始位置、結(jié)束位置以及障礙物的分布情況下,規(guī)劃出一條不與障礙物發(fā)生碰撞的路徑,提 供給汽車控制器,以精確控制車輛沿規(guī)劃路徑的行駛[1]。

近年來,研究人員對路徑規(guī)劃算法進行了大量的探索,路徑規(guī)劃算法不斷發(fā)展。其中基于采樣搜索的路徑規(guī)劃算法包括概率圖算法(PRM)和快速探索隨機樹(RRT)算法[2-3]。RRT算法因其適合于求解動態(tài)多障礙條件下的路徑規(guī)劃問題而得到了廣泛的應(yīng)用,但RRT算法存在收斂速度慢、搜索效率低、規(guī)劃的路徑難以最優(yōu)的缺點[4],同時RRT算法在路徑規(guī)劃過程中未考慮車輛運動動學約束,致使生成的路徑曲折不連續(xù),可行度較低[5],為此,國內(nèi)外研究人員提出不同的方法,以用于改進RRT的局限性。為了加快路徑收斂,提高計算速度,J.G.KANG等[6]提出了“后三角重新布線”方法,使規(guī)劃時間的犧牲最小化,并克服了快速探索隨機樹RRT算法的最優(yōu)性限制;Z.WU等[7]提出了一種Fast-RRT算法,該算法由改進RRT和快速優(yōu)化兩個模塊組成,前者的目標是快速穩(wěn)定地找到一條初始路徑,后者的目標是通過合并多條初始路徑得到一條最優(yōu)路徑,以達到更快地找到最優(yōu)路徑;A.H.QURESHI等[8]提出了基于勢函數(shù)的RRT*,在RRT*中加入了人工勢場算法,該算法大大減少迭代次數(shù),從而導致更有效的內(nèi)存利用和加速收斂速度;Y.LI等[9]提出了一種新的算法PQ-RRT*,它結(jié)合了P-RRT*(基于RRT*的潛在函數(shù))和Quick-RRT*的優(yōu)點,保證了快速收斂到最優(yōu)解,并產(chǎn)生更好的初始解;Y.GAN等[10]提出了一種1-0 Bg-RRT算法,使用1-0的Bg-RRT構(gòu)造樹的速度更快,且能夠及時跳出局部最小值大大減少了計算時間和復雜度。這些方法雖然都加快了迭代速度,但未考慮車輛運動學約束問題,規(guī)劃出的路徑不適用于車輛的實際行駛。

考慮到車輛運動學約束的問題,朱冰等[11]提出了具備安全場引導和角度約束等策略的改進RRT*算法,滿足了車輛軌跡曲率約束,同時具有較快的搜索速度和更高的成功率;羅輝等[12]提出一種二階段RRT算法TSRRT(Two-Stage RRT)采用融合最大轉(zhuǎn)向角度的3次Bezier曲線進行上邊界曲率優(yōu)化,使規(guī)劃路徑能夠滿足車輛運動的轉(zhuǎn)向角度,讓車輛在行駛過程中能夠以不停車的方式進行連續(xù)平穩(wěn)轉(zhuǎn)向;同時為了加快算法的收斂速度。針對路徑搜索效率及車輛運動學約束問題,筆者基于城市道路場景,提出了一種基于勢力場優(yōu)化采樣區(qū)域的改進RRT算法,該方法基于勢力場對采樣區(qū)域進行動態(tài)優(yōu)化,采取基于安全距離和270°碰撞檢測的動態(tài)步長選擇策略,并在此基礎(chǔ)上結(jié)合貪心思想及曲率約束對路徑進行后處理;通過仿真實驗,驗證了算法的有效性和適應(yīng)性。

1 車輛運動學模型

運動學是從幾何學的角度研究物體的運動規(guī)律,在車輛路徑規(guī)劃過程中應(yīng)用運動學模型,可以使規(guī)劃出的路徑更切合實際,滿足行駛過程中的運動學幾何約束。

使用前輪轉(zhuǎn)向的車輛運動學模型,以車道行駛方向為Y軸正方向,建立坐標系,如圖1,車輛的狀態(tài)可以用Xn=(x,y,θ)表示。(x,y)表示車輛在當前在坐標系中的坐標位置,θ為車輛縱軸軸線與坐標軸之間的夾角,φ為車輛前輪轉(zhuǎn)角,d為軸距,v為車輛行駛速度。

圖1 車輛運動學模型Fig. 1 Vehicle kinematics model

當車輛轉(zhuǎn)彎的時候,將方向盤轉(zhuǎn)到極限位置,并使車輛保持比較穩(wěn)定的速度,轉(zhuǎn)向過程中,轉(zhuǎn)向中心點到外側(cè)車輪所行駛的軌跡之間的最小距離,被稱為最小轉(zhuǎn)彎半徑。不同類型車輛的轉(zhuǎn)彎半徑各不相同,最小轉(zhuǎn)彎半徑Rmin以及最大曲率ρmax可以表示為:

(1)

(2)

在車輛以車速v運動過程中車輛運動學模型可以表示為:

(3)

根據(jù)運動學模型及最大小轉(zhuǎn)彎半徑的分析可知,所規(guī)劃的路徑的曲率應(yīng)該滿足最小轉(zhuǎn)彎半徑的限制以及車輛的運動學約束,所以可以根據(jù)轉(zhuǎn)彎曲率和安全距離來對車輛的路徑規(guī)劃進行約束以達到自動駕駛車輛實際的行駛路徑要求。

2 改進的RRT路徑規(guī)劃算法

2.1 RRT及Bg-RRT算法

RRT算法以車輛的起點Xinit作為隨機樹T的初始根節(jié)點,利用隨機抽樣的原則在地圖內(nèi)隨機選取一個節(jié)點Xrand,根據(jù)設(shè)定步長step,沿著可取的隨機節(jié)點的方向選擇新的節(jié)點Xnew,并進行碰撞檢測,若碰撞檢測通過則將新節(jié)點加入隨機樹,依次重復上述過程,直到找到要搜索的目標點。其偽代碼如圖2。

RRT算法在地圖中隨機選取節(jié)點,擴大了搜索空間,然而RRT的隨機采樣也會導致路徑規(guī)劃的盲目性,不僅會造成節(jié)點的冗余,還會增加搜索時間,特別是在結(jié)構(gòu)化道路的場景下,隨機性會使路徑搜索點均勻分布在整個區(qū)域中,解的收斂時間增長。為了提高RRT算法的效率,研究人員提出了Bg-RRT算法,將圖2的RRT算法流程中RANDOM_STATE()函數(shù)改寫為如圖3中形式,即給定一個概率因子,使在一定概率下采樣點為目標點,以提高采樣的效率,加快迭代速度。

圖3 Bg-RRT算法Fig. 3 Bg-RRT algorithm

2.2 改進的RRT算法流程

通過對RRT算法以及基于目標的Bg-RRT算法的分析,提出了一種在城市工況下基于勢力場優(yōu)化采樣區(qū)域的改進RRT算法,首先基于環(huán)境的對采樣區(qū)域進行動態(tài)優(yōu)化,再基于勢力場對采樣區(qū)域調(diào)整,縮小樣本空間加快了迭代速度,增加搜索精度;同時基于270°碰撞檢測的動態(tài)步長選擇策略,減少采樣節(jié)點數(shù),增加了路徑的安全系數(shù),最后進行后處理并生成最終路徑,保證路徑的平滑度及滿足車輛運動學約束。

2.3 基于勢力場的采樣區(qū)域動態(tài)優(yōu)化

2.3.1 基于環(huán)境的采樣區(qū)域動態(tài)優(yōu)化

車輛通過自身多種傳感器獲取道路信息及障礙信息并生成地圖,基于現(xiàn)實的車道環(huán)境所生成的全局信息已知的地圖場景如圖5,根據(jù)實際車輛行駛情況做出如下假設(shè):①僅考慮車輛按車道方向行駛;②僅考慮車輛在路面平面內(nèi)進行運動。

考慮實際車輛運行情況對采樣區(qū)域進行優(yōu)化,建立如式(4)的采樣區(qū)域優(yōu)化函數(shù)opt,對采樣區(qū)域進行優(yōu)化,得到優(yōu)化后的采樣區(qū)域Copt如圖5中陰影區(qū)域,優(yōu)化分兩步進行,首先根據(jù)車道邊界及車輛當前位置及目標位置進行初步優(yōu)化得到如式(5)的區(qū)域Copt1,考慮到全局規(guī)劃因素,這里的目標位置由人為設(shè)定;再根據(jù)車輛行駛的規(guī)律及意圖結(jié)合車道中心線及安全距離約束,將采樣區(qū)域進一步優(yōu)化,得到式(6)的區(qū)域Copt2(x),最終優(yōu)化后的采樣區(qū)域為式(7)中的Copt。

Copt=fopt(l1,l2,ld,Xmax,Ymax,s,x,y,xg,yg,Cobs)

(4)

(5)

(6)

(7)

式中:l1為左側(cè)車道中心線在坐標系中的橫坐標;l2為右側(cè)車道中心線在坐標系中的橫坐標;ld為左側(cè)車道中心線在坐標系中的橫坐標;x、y分別為車輛在坐標系中的當前位置坐標;xg、yg分別為車輛在坐標系中的目標位置坐標;s為車輛安全距離;Xmax和Ymax分別為車道地圖橫縱坐標最大值;Cobs為障礙車區(qū)域。

圖5 采樣區(qū)域動態(tài)優(yōu)化Fig. 5 Dynamic optimization of the sampling area

2.3.2 基于勢力場的采樣區(qū)域調(diào)整

通過上述的基于環(huán)境的采樣區(qū)域動態(tài)優(yōu)化將RRT的采樣空間進行了初步優(yōu)化,為了實現(xiàn)更高精度和更快速度的路徑搜索,建立了基于道路場景的勢力場函數(shù):

(8)

式中:Uatt為目標點對車輛施加的引力勢場函數(shù);Ureq為障礙車對車輛施加的斥力勢場函數(shù);Fatt為車輛受到的引力;Freq為車輛受到的斥力;U為環(huán)境對車輛的施加的總勢場;F為車輛受到的合力。

考慮到傳統(tǒng)斥力場在道路場景中的局限性及計算代價過大的問題,針對傳統(tǒng)斥力場函數(shù)進行改進。首先根據(jù)式(7)中優(yōu)化后的采樣區(qū)域給斥力場加入采樣邊界約束;再將傳統(tǒng)障礙物斥力場作用域限制變?yōu)樽詣玉{駛車輛最大探索邊界限制;最后將傳統(tǒng)的斥力影響原則改為前側(cè)、左側(cè)、右側(cè)、左前側(cè)、右前側(cè)5個方向的斥力影響,經(jīng)過探索得出5個方向道的障礙物距離如圖6,再將自動駕駛車輛5個方向到障礙物的距離分別用d1～d5來表示。通過這些距離建立優(yōu)化后的斥力場函數(shù)Ureq(X),對所建立的斥力場函數(shù)進行負梯度求導得到斥力函數(shù)Freq(X):

(9)

(10)

圖6 勢力場模型Fig. 6 Force field model

同時基于傳統(tǒng)人工勢場法建立引力場函數(shù)Uatt(X)并對所建立的引力場函數(shù)進行負梯度求導得到引力函數(shù)Fatt(X),通過計算得出所求出的勢力場對自動駕駛車輛的合力F(X)并利用反三角函數(shù)求出合力方向與X軸夾角α,如圖7。

(11)

(12)

F(X)=Fatt(X)+Freq(X)

(13)

(14)

式中:dg為靜態(tài)與目標點之間的歐式距離;Fy(X)為y方向的分力;Fx(X)為x方向的分力。

若僅根據(jù)勢力場作用的合力方向作為采樣方向會受到勢力場的局部最優(yōu)影響,因此將這個合力方向變成一個方向范圍區(qū)域,以增加采樣的魯棒性,這里加入車輛轉(zhuǎn)向角作為約束指標,前輪最大轉(zhuǎn)向角由內(nèi)輪最大轉(zhuǎn)角和外輪最大轉(zhuǎn)角兩個數(shù)據(jù)組成,一般內(nèi)輪最大轉(zhuǎn)角39.6°,外輪最大轉(zhuǎn)角33.5°;車的轉(zhuǎn)向角度與車的實際大小,運載底盤有關(guān)系,根據(jù)式(1)[13]及市面常規(guī)車輛最小轉(zhuǎn)彎半徑和軸距計算得出,一般汽車的轉(zhuǎn)向角在30°～40°之間,筆者為了便于計算人為選取最大轉(zhuǎn)角為35°,則根據(jù)轉(zhuǎn)角進一步約束得到最終采樣區(qū)域C。

(15)

2.4 基于安全距離和270°碰撞檢測的動態(tài)步長

根據(jù)上述的采樣區(qū)域優(yōu)化后,為使RRT能夠更快速的規(guī)劃出自動駕駛車輛的可行路徑,避免節(jié)點冗余,相對理想的狀態(tài)是在無障礙且滿足自動駕駛車輛安全距離比例較大的自由空間里,路徑搜索的步長應(yīng)該選擇較大值,加快路徑的搜索;然而,在接近障礙物或安全距離滿足比例較小的情況時,為保證車輛行駛安全,應(yīng)當相對細致的規(guī)劃,步長應(yīng)選擇較小值,確保自動駕駛車輛可以成功規(guī)避障礙。筆者的改進RRT算法的采樣區(qū)域雖然基于人工勢場進行了約束,但每個采樣點所受合力方向是單獨計算的,故無需考慮勢力場對采樣步長的影響,而選擇較為高效的碰撞檢測方法進行步長選擇。由于傳統(tǒng)的碰撞檢測方法是以采樣點為圓心,遍歷360°圓周式范圍內(nèi)是否有障礙物;根據(jù)前文假設(shè),車輛向前行駛,則無需考慮車后冗余的90°范圍,即從減少計算量的角度,建立式(16)的基于安全距離的270°碰撞檢測函數(shù)用以選取步長,動態(tài)步長可以較快的得到相對較優(yōu)的車輛行駛路徑,并有效減少路徑所需的節(jié)點數(shù)。

(16)

(17)

(18)

式中:l為采樣步長;λ為安全距離比例系數(shù);ρ(λ)為基于比例系數(shù)λ的270°碰撞檢測函數(shù);lmax為最大步長限制值;δ為以車輛前進方向為一二象限下的270°方向角;Cobs為障礙車區(qū)域A=[cosδsinδ;δ∈-π/4,5π/4]。

通過270°的碰撞檢測函數(shù)得到安全距離比例系數(shù)λ,用于控制步長的選擇。當自動駕駛車輛靠近障礙區(qū)域行駛時,采樣步長根據(jù)自動駕駛車輛當前位置與障礙的距離的變化而調(diào)整,當自動駕駛車輛與障礙的距離大于λ倍的安全距離時,步長設(shè)置為0.95λs,但不高于最大步長限制,既保證路徑快速規(guī)劃又保證采樣的魯棒性。

2.5 路徑后處理

在采樣區(qū)域優(yōu)化及步長選擇策略基礎(chǔ)上,改進的RRT算法搜索出的路徑已經(jīng)大大優(yōu)化,但由于RRT算法本身的隨機采樣性[14],仍會存在路徑距離代價大,安全系數(shù)低且曲率不連續(xù)的問題,這對后續(xù)的軌跡跟蹤造成嚴重影響,故需對生成的初始路徑進行逆向?qū)?yōu)和平滑后處理。

2.5.1 基于車輛安全距離的逆向?qū)?yōu)

為了解決搜索到的路徑存在的路徑距離代價較大的問題,采用基于安全距離的逆向?qū)?yōu)。如圖8,X1～X4分別為4個路徑方向的節(jié)點,從X1→X4需經(jīng)過路徑a1→a2→a3,若僅根據(jù)貪心思想,由于a1+a2+a3>b且該路徑處于自由區(qū)域內(nèi),則可將X1→X4的路徑替換為路徑b,此時的路徑代價最小,但這樣的方式無法保證車輛的安全行駛,因此,為考慮車輛行駛的安全問題,這里采用基于車輛安全距離的逆向?qū)?yōu)方法,首先根據(jù)貪心思想可知a1+a2+a3>c+a3,再通過基于車輛安全距離的270°的碰撞檢測方法判斷路徑可行,則將X1→X4的原始路徑替換為路徑c→a3,在實現(xiàn)路徑代價減小的同時滿足車輛安全行駛的要求。

圖8 基于安全距離的逆向?qū)?yōu)示意Fig. 8 Schematic diagram of reverse optimization based on safe distance

2.5.2 基于三次B樣條曲線的路徑生成

為了滿足車輛最大轉(zhuǎn)角的限制,生成的路徑應(yīng)該滿足最大曲率的約束,B樣條曲線具有局部性和連續(xù)性等特點,可以在對路徑進行平滑處理時只在局部進行調(diào)整而不改變整個路徑形狀[15]。為達到平滑路徑的目的,這里采用參數(shù)化的三次B樣條曲線來規(guī)劃路徑,如圖9中的4個控制點Pi(i=0,1,2,3)為4個節(jié)點,Di(i=1,2,3)為三段連接線段,則3次b樣條曲線可以定義為:

(19)

其中Ni,3(t)為三次B樣條線的基函數(shù),根據(jù)deBoor-Cox遞推可以得到:

(20)

這里將參數(shù)節(jié)點的向量區(qū)間設(shè)置為[0,1],則通過式(21)表達三次B樣條生成的路徑點:

(21)

式中:ωi為兩條線段的夾角。

則曲線曲率為:

(22)

則要滿足車輛轉(zhuǎn)向要求則需滿足:

(23)

通過三次B樣條擬合平滑后的路徑,更符合車輛的實際行駛狀態(tài)。

圖9 三次B樣條曲線的控制段Fig. 9 Control segment plot of a cubic B-spline curve

3 仿真驗證

分別建立少靜態(tài)障礙物下超車、多靜態(tài)障礙物下超車、多靜態(tài)障礙物下右轉(zhuǎn)3種工況下所對應(yīng)的地圖1、地圖2、地圖3;在此基礎(chǔ)上建立多動態(tài)障礙物及部分靜態(tài)障礙物下的超車工況所對應(yīng)的地圖4;這4種工況具有比較典型的路況代表性,可以有效驗證算法的有效性和適應(yīng)性。

在搭載Intel Corei5-8300H CPU和16G內(nèi)存的電腦上利用Matlab2019(a)分別對RRT,Bg-RRT,以及改進的RRT算法進行仿真。路徑搜索過程的仿真結(jié)果如圖10,通過3種地圖下的對比可知,相較于前2種算法,筆者提出的改進算法采樣方向更為精準,有效解決采樣點冗余的情況;同時在初始路徑平滑度上明顯得到提升,并且擁有較高的安全系數(shù)。

圖10 路徑搜索過程仿真對比Fig. 10 Path search process simulation comparison

為了更好的對結(jié)果進行量化對比,對3種地圖下3種算法分別進行30次仿真,記錄平均節(jié)點數(shù)、平均路徑搜索時間、和平均路徑長度如表1,可以明顯看出3種地圖下通過改進的RRT算法在采樣節(jié)點數(shù)、路徑搜索時間及路徑長度方面都有著明顯的優(yōu)化效果,經(jīng)過加權(quán)計算得出,改進RRT算法相較Bg-RRT算法在節(jié)點數(shù)、路徑搜索時間、路徑長度分別減少了72.16%、83.57%、6.22%。有效的減少了路徑搜索所需的節(jié)點數(shù),大大提高了采樣效率,降低了生成的路徑長度。

表1 路徑搜索過程30次仿真平均結(jié)果Table 1 Average results of 30 simulations during path search process

3種地圖下路徑后處理過程如圖11,其中逆向?qū)?yōu)路徑是在初步路徑的基礎(chǔ)上基于貪心思想和車輛安全距離約束對其進行逆向?qū)?yōu),以實現(xiàn)路徑代價減小且同時保證車輛行駛安全,圖11中的樣條平滑路徑是在逆向?qū)?yōu)路徑的基礎(chǔ)上,基于車輛最大轉(zhuǎn)角約束使用3次B樣條線進行平滑的路徑,解決了逆向?qū)?yōu)路徑仍存在個別轉(zhuǎn)角過大的問題,經(jīng)過后處理平滑后的路徑更符合車輛實際行駛路徑。

圖11 后處理仿真結(jié)果Fig. 11 Aftertreatment simulation results

針對兩種地圖環(huán)境各進行30次仿真,結(jié)果如表2,逆向?qū)?yōu)對路徑長度的減少是比較明顯的,3次B樣條線在平滑路徑上有非常明顯的作用,同時也相應(yīng)減少了路徑長度。為了評價路徑平滑度這里采用符合曲率連續(xù)的路徑段數(shù)來作為平滑度的評價參數(shù),路徑分段數(shù)越少則平滑度越高。

表2 后處理30次仿真平均結(jié)果Table 2 Average results of 30 simulations after treatment

綜上所述,提出的改進RRT算法在以城市路口為基礎(chǔ)設(shè)計的3種地圖場景下的路徑規(guī)劃效率有明顯的提升,相較于RRT、Bg-RRT算法,改進RRT算法搜索路徑所需節(jié)點數(shù)、時間都大大減少;經(jīng)過后處理,路徑的平滑度得到提升,生成的路徑符合了實際車輛運行的需求。

結(jié)合前3個地圖場景的仿真,可以明顯看出改進后的算法在路徑搜索時間上的提升明顯,為了進一步驗證算法的適應(yīng)性,在地圖4場景下進行多動態(tài)障礙物及部分靜態(tài)障礙物的超車尋徑仿真,為較好可視化且考慮到RRT算法的全局規(guī)劃性,筆者采取分幀進行展示,其中地圖4是按真實道路等比例繪制,圖中A、B兩車分別以30 km/h和20 km/h的時速按地箭頭方向進行行駛,C車處于故障靜止狀態(tài)。仿真結(jié)果如圖12中,從T=0 s到T=5 s,平均規(guī)劃時間為507.8 ms,有效驗證了改進后的RRT算法可以根據(jù)障礙物位置的變化快速高效規(guī)劃出有效的可行路徑,證明了改進后算法在低速場景下的有效性和適應(yīng)性。

圖12 地圖4下分幀仿真結(jié)果Fig. 12 Frame simulation results under map 4

4 結(jié) 語

針對城市工況提出了一種基于勢力場優(yōu)化采樣區(qū)域的改進RRT算法;該方法首先基于道路環(huán)境及車輛位置對采樣區(qū)域動態(tài)優(yōu)化,有效縮小了采樣區(qū)域減少迭代時間,再基于勢力場和車輛最大轉(zhuǎn)角約束對采樣區(qū)域進行實時調(diào)整,實現(xiàn)更高精度的采樣,最后采取基于安全距離和270°碰撞檢測的動態(tài)步長選擇策略,大大提高搜索效率,并在此基礎(chǔ)上結(jié)合貪心思想及曲率約束對路徑進行后處理,實現(xiàn)更少路徑代價及路徑平滑,通過仿真實驗驗證了其有效性及適應(yīng)性。

后續(xù)將進行實車驗證并嘗試解決非結(jié)構(gòu)化道路場景下的路徑規(guī)劃問題,為更好的滿足各種環(huán)境下的自動駕駛車輛及其他車輛的路徑規(guī)劃的實際工程需求。