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

張 涌,高 峰,趙奉奎

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

0 引 言

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

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

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

1 車輛運動學模型

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

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

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

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

(1)

(2)

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

(3)

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

2 改進的RRT路徑規劃算法

2.1 RRT及Bg-RRT算法

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

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

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

2.2 改進的RRT算法流程

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

2.3 基于勢力場的采樣區域動態優化

2.3.1 基于環境的采樣區域動態優化

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

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

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

(4)

(5)

(6)

(7)

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

圖5 采樣區域動態優化Fig. 5 Dynamic optimization of the sampling area

2.3.2 基于勢力場的采樣區域調整

通過上述的基于環境的采樣區域動態優化將RRT的采樣空間進行了初步優化,為了實現更高精度和更快速度的路徑搜索,建立了基于道路場景的勢力場函數:

(8)

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

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

(9)

(10)

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

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

(11)

(12)

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

(13)

(14)

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

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

(15)

2.4 基于安全距離和270°碰撞檢測的動態步長

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

(16)

(17)

(18)

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

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

2.5 路徑后處理

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

2.5.1 基于車輛安全距離的逆向尋優

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

圖8 基于安全距離的逆向尋優示意Fig. 8 Schematic diagram of reverse optimization based on safe distance

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

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

(19)

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

(20)

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

(21)

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

則曲線曲率為:

(22)

則要滿足車輛轉向要求則需滿足:

(23)

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

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

3 仿真驗證

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

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

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

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

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

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

圖11 后處理仿真結果Fig. 11 Aftertreatment simulation results

針對兩種地圖環境各進行30次仿真,結果如表2,逆向尋優對路徑長度的減少是比較明顯的,3次B樣條線在平滑路徑上有非常明顯的作用,同時也相應減少了路徑長度。為了評價路徑平滑度這里采用符合曲率連續的路徑段數來作為平滑度的評價參數,路徑分段數越少則平滑度越高。

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

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

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

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

4 結 語

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

后續將進行實車驗證并嘗試解決非結構化道路場景下的路徑規劃問題,為更好的滿足各種環境下的自動駕駛車輛及其他車輛的路徑規劃的實際工程需求。