基于改進RRT算法的無人駕駛汽車軌跡規劃

賀伊琳,高 奇,趙 丹,劉 偉

(長安大學 汽車學院, 陜西 西安 710064)

智能交通系統(ITS)是將先進的信息技術、傳感技術、控制技術和計算機技術等有效地集成運用于整個交通運輸管理體系,從而建立起一種全方位、實時、準確及高效的綜合運輸系統[1-2]。智能車輛是智能交通系統領域最重要的組成部分[3],無人駕駛是實現智能汽車與智能交通的關鍵技術,也是未來汽車的發展方向與必然趨勢[4]。

軌跡規劃作為無人駕駛汽車技術領域的研究重點之一,其任務是在位姿空間中找到一條從初始位姿點到目標位姿點的連續無碰撞路徑,同時滿足環境約束、時間約束以及動力學約束等條件[5],與車輛的可靠性、安全性與乘坐舒適性密切相關。

常用的軌跡規劃算法有圖搜索算法、仿生智能算法、數值規劃算法與基于采樣算法等。目前,已有不少文獻對此展開研究。文獻[6]與文獻[7]分別針對A*圖搜索算法與D*圖搜索算法進行研究,但是其計算時間和轉角不滿足汽車行駛要求。文獻[8]采用卷積人工神經網絡,輸入為障礙物信息,輸出為參考路徑,但人工神經網絡訓練較為困難。文獻[9]與文獻[10]分別采用了遺傳算法與啟發式蟻群優化算法,然而存在算法收斂速度不穩定的缺點。文獻[11]采用數值規劃法,最小化具有不同限制變量的目標函數,實時性差,計算時間長。快速搜索隨機樹法(RRT)是典型的基于采樣的算法[12]。該算法通過在搜索空間均勻采樣來擴展搜索區域,逐步增加搜索路徑,最終到達目標區域[13],能有效解決機器人在復雜環境下的軌跡規劃問題。

然而RRT算法依賴于隨機點選擇,運算較慢。文獻[14-15]采用雙向RRT算法,對起始點和目標點同時發展生成樹,提高了軌跡擴展速度；文獻[16]以一定的概率使目標點作為隨機點,增大樹節點發展的偏向性,提高運算速度,然而上述兩種算法在提高生成軌跡質量方面效果一般；文獻[17-18]提出啟發式代價RRT算法,將環境代價作為算法的約束條件,得到代價最小的路徑，但啟發式代價函數可用度量距離與真實距離不一致,同時代價計算量巨大,運算速度較慢；文獻[19]結合圖形搜索和隨機樹算法,取得良好的控制效果;文獻[20-21]基于目標偏向與人工勢場,改進RRT算法在機器人上的應用;文獻[22]研究了地圖中的軌跡生成算法,用蒙特卡洛求碰撞概率,同時采用B樣條曲線對軌跡進行優化。以上研究對原始RRT算法提出較大的改進,但研究對象為移動式機器人,難以直接運用于車輛軌跡規劃中。

因此,本文根據無人駕駛汽車特性,針對無人駕駛汽車軌跡規劃問題,考慮車輛運動學約束,同時保證車輛穩定性與乘坐舒適性,提出一種改進的RRT算法。首先，基于車輛模型與約束條件,在擴展區域隨機生成可達節點,實現對隨機點采樣的區域控制;其次，根據目標偏向原理,以一定的概率把目標點作為隨機點進行隨機樹擴張,提高路徑的生成速度,并采用三次B樣條曲線進行軌跡平滑處理；最后，基于Matlab/CarSim聯合仿真平臺,仿真分析了本文提出的軌跡規劃算法的優劣。

1 車輛轉向約束模型

在無人駕駛車輛軌跡規劃過程中需考慮車輛運動學約束,以得到滿足車輛轉向機構運動特性的行駛路徑。

由于轉向系統對車輛的垂向運動影響甚微,因此,將無人駕駛汽車的運動視為平行于水平面的平動,將前后輪分別用一個等效前輪和一個等效后輪來代替。同時，考慮到模型的控制變量是車輛的轉向角,主要涉及車輛的側向動力學,假定車輛速度恒定,忽略車輛的縱向動力學,建立車輛轉向模型,如圖1所示。

圖1 車輛轉向模型Fig.1 Vehicle steering model

由此可得

(1)

其中,δ為前輪轉角;L為軸距;R為路徑半徑。

考慮車輛轉向系機械結構約束,跟蹤所規劃路徑時前輪轉角需小于車輛的最大轉角δmax，即

|δ|<δmax。

(2)

2 基于RRT算法的軌跡規劃研究

RRT算法是一種基于概率采樣的增量式搜索算法,搜索所需邊界點少,搜索范圍趨向于未知區域,可達性好,具有概率完整性,不需要對空間建模,能快速搜索復雜高維空間,適用于非完整與高自由度狀態下的軌跡規劃,在智能尋跡領域應用廣泛。

圖2 隨機樹擴展方式圖Fig.2 Expansion of random tree

在搜索過程中,選擇Xinit作為隨機樹起始點Tr.int,設定算法迭代步數為k。通過RANDOM-STATE在整體狀態空間X中生成單個狀態位置隨機點Xrand,若與障礙物沖突,則重新生成隨機點。利用NEAREST-NEIGHBOR(Xrand,T)在隨機樹上尋找與隨機點最近的樹節點Xnear。依據兩點間位置關系,利用NEW-STATE(ε,Δt)沿該樹節點到隨機點Xrand擴展一個步長ε，并產生新的樹節點Xnew。算法持續迭代,通過不斷增加子樹節點Tree.add,延伸擴展產生隨機樹NewTree,直到目標節點Xgoal成為樹節點或距離樹節點不足一個步長,完成搜索,實現隨機樹的構造。此時從目標點起,回溯全樹,則可獲得一條從起始位置到目標位置的可行路徑。

3 基于改進型RRT算法的軌跡規劃研究

3.1 基于RRT算法的軌跡規劃需求分析

由于原始RRT算法的可行區域為整體環境區域,以可達性為首要要求,采樣隨機性高,適合于擴展未知區域,方向變化較大,運動約束性差,生成的路徑適用于可隨時進行全方位轉向的移動式機器人的運動。而車輛運動約束較高,行進方式一般為區域性向前行駛,不會產生較大幅度的轉向、頻繁且長時間后退等行駛行為,原始RRT算法與汽車的運動符合度低，同時,原始RRT算法隨機點產生范圍為整個可行區域,算法偏向性小,產生較多無效節點,路徑生成較慢。

因此,基于建立的車輛運動約束模型,本文對軌跡規劃提出的具體要求如下:

1)前輪轉角滿足機械轉向結構的約束;

2)在隨機點生成階段剔除無效節點,提高搜索效率,滿足軌跡規劃的實時性要求;

3)車輛行駛過程中,將轉角保持在較低值,且減小轉角變化率,保證車輛行駛的穩定性和乘坐的舒適性;

4)盡可能使路徑平直化,使前輪轉角在可控范圍內,減少轉向行為;

5)對生成軌跡做平滑處理,降低跟蹤軌跡時的側向加速度。

3.2 改進RRT算法設計

為滿足上述車輛軌跡規劃的要求,提出改進的RRT算法。在RRT算法的采樣過程中進行采樣區域限制,根據車輪可達轉角,將隨機點的采樣限制在前進方向的可行域內,可行域以車輛縱軸線為中間線,分別向左右轉動到車輪極限轉角位置,從而產生一個向前的采樣范圍。同時，該區域能避免無效區隨機點的產生,提高規劃速度和軌跡生成質量。

基于改進的RRT算法確定采樣區域、起始點與目標點,在可行域內產生隨機點,并對隨機點進行碰撞檢測,將不發生碰撞的隨機點加入可達集,連接曲率變化最小的點,生成初始軌跡,最后對軌跡進行平滑處理與優化。具體算法流程圖如圖3所示。

圖3 改進RRT算法流程圖Fig.3 Flow chart of improved RRT algorithm

3.3 隨機點擴展算法研究

3.3.1 目標偏向策略 原始RRT算法的隨機點為可行區域內任意一點,隨機點選擇范圍大,隨機樹的發展缺乏一定的導向性,導致算法在尋找目標點時將產生不必要的工作,耗時較長。因此,基于目標偏向原理,在進行隨機點選擇時,以概率P將目標點作為隨機點進行隨機樹擴張,以增加隨機樹向目標點的擴展概率,從而有效降低采樣點的數量,加快隨機樹的生成[18],提高軌跡規劃速度，同時，軌跡也更為平滑,有利于提高行駛穩定性。RRT算法與目標偏向RRT算法曲線生成對比如圖4所示。由圖4可知,基于目標偏向的RRT算法采樣點顯著減少,可大大縮短軌跡規劃所用的時間,生成路徑也較為平整光滑,滿足車輛軌跡的一般要求。

圖4 目標偏向對比圖Fig.4 Chart of target bias comparison

3.3.2 隨機點擴展規則設計 考慮到車輛的行駛穩定性與乘坐舒適性,基于上述車輛約束條件,設定車輛最大航向角為40°,左右輪轉角相等,考慮采樣點范圍,設計隨機點可達區,擴展的隨機點需位于規定范圍內,且隨機點與樹節點距離需大于一個步長,繼而確定可優先被擴展的點。

隨機點產生在車輛轉角扇形范圍內,如圖5所示,當車輛位于Xnear節點沿X軸正向行駛時,轉角邊界為粗實線。隨機點與最近樹節點的連線與最近樹節點與父節點的連線夾角小于最大航向角,生成的隨機點有效。

圖5 隨機點產生限制范圍Fig.5 Limited range of random points

隨機點坐標為(x,y),最近樹節點坐標為(x(t1),y(t1)),父節點坐標為(x(t0),y(t0)),最近樹節點與父節點連線及最近樹節點與隨機點連線的斜率可分別表示為

(3)

(4)

其中,k1為最近樹節點與父節點連線的斜率,k2為最近樹節點與隨機點連線的斜率。

兩直線夾角β為

(5)

可擴展區的轉角約束條件為

β<δmax。

(6)

RRT算法的最近點Xnearest的獲取采用歐氏距離進行計量,即以直線距離的長度來尋找與隨機點最近的樹節點,從而產生路徑。雖然路徑在車輛轉向系約束的可達范圍內,但可能導致較大的方向盤轉角。因此,考慮車輛運行的穩定性與乘坐舒適性,轉角小的隨機點將優先被擴展。

3.4 軌跡平滑處理算法設計

上述算法生成的軌跡是由短線段連接而成的不平滑軌跡,為了進一步提高規劃軌跡的平滑度和汽車的跟蹤穩定性,需對原始軌跡進行平滑處理。軌跡平滑處理常用的曲線為拋物樣條曲線和參數樣條曲線,然而這兩種方法生成的曲線通過所有給定的點,某些點的存在可能會對曲線平滑度造成不利影響。貝塞爾曲線具有連續性和局部性,因此,本文選擇基于貝塞爾曲線的B樣條曲線進行軌跡平滑處理。分別采用二次B樣條曲線與三次B樣條曲線對特定控制曲線進行擬合,擬合結果如圖6所示。由圖6可知,經B樣條曲線擬合后,所得曲線能夠沿給定趨勢變化,剔除了控制曲線中的尖點及曲率半徑較小處的點,曲線的平滑度有了一定程度的提高。其中,三次B樣條擬合曲線的曲率半徑更小,尖點處的過渡更為平滑,適用于車輛規劃軌跡,因此,本文選取三次B樣條曲線進行軌跡平滑處理。

圖6 B樣條曲線擬合Fig.6 B spline curve fitting

B樣條曲線方程可表示為

(7)

其中,di(i=0,1，…，n)為控制頂點,即隨機樹節點;Ni,k(i=0,1，…，n)為k次規范B樣條基函數,最高次數為k,基函數由節點u的序列決定,u:u0≤u1≤…≤un+k+1。

三次B樣條曲線的基函數為

b0=1/6*(-u3+3*u2-3*u+1)，

b1=1/6*(3*u3-6*u2+4)，

b2=1/6*(-3*u3+3*u2+3*u+1)，

b3=1/6*u3。

(8)

B樣條擬合曲線為

x=b0*a(1,i)+b1*a(1,i+1)+b2*a(1,i+2)+b3*a(1,i+3)，

y=b0*a(2,i)+b1*a(2,i+1)+b2*a(2,i+2)+b3*a(2,i+3)。

(9)

4 仿真結果與分析

4.1 仿真場景構建

為全面、有效地模擬車輛運行環境,本文基于Matlab/CarSim聯合仿真平臺,對車輛在直線路況和直角路況下進行軌跡規劃的仿真與分析。

直線路段由雙向6車道構成,取單向3車道,車輛由中間車道換道至第3車道。單向車道總寬11.25m,建立直線路況場景。

直角路段由雙向4車道構成,取單向雙車道,車輛右轉90°行駛,單向車道總寬7.5m,建立直角路況場景。

4.2 仿真結果及分析

為了驗證本文提出基于車輛約束的改進RRT算法的優劣,使用不同轉角約束下的改進RRT算法與原始RRT算法在上述場景中進行仿真對比。將車輛尺寸等效至障礙物邊界設計中,車輛可看做質點,取步長為15,目標偏向概率為0.1。由于RRT算法是基于滿足約束條件的隨機點生成過程,因此進行了多次仿真模擬。在上述場景路況中的規劃軌跡分別如圖7與圖8所示,紅線為所得規劃軌跡。

由圖7(a)與圖8(a)可知,在不同路況下,原始RRT算法產生的樹節點隨機性強,路徑擴展偏向性較低,生成路徑中存在一定程度的折線與尖點,同時，路徑橫向波動大,車輛跟蹤該軌跡時橫向位移也大,在尖點處車輛極有可能出現無法滿足運動學約束而難以跟蹤規劃軌跡的現象。對于直線路況,轉角約束越大,生成軌跡的平滑度越高。然而,由圖8可知,直角路況下在轉角35°約束與25°約束時路徑較為平滑,而轉角15°約束下彎道轉折點處的軌跡反而產生了較大波動。

圖7 直線路況仿真結果Fig.7 Simulation results of straight road

圖8 直角路況仿真結果Fig.8 Simulation results of right angle section

不同約束下所規劃軌跡的曲率均值、曲率極大值與曲率方差如表1所示。

表1 軌跡曲率指標對比Tab.1 Comparison of curvature index for trajectory

由表1可知，兩種路況下,無約束RRT算法的曲率均值、極大值與方差較大,加入轉角約束后曲率均值、極大值與方差呈下降趨勢。相比于無約束控制,15°轉角約束下直線路況規劃軌跡的曲率均值、極大值與方差分別降低了22.9%，51.7%與43.3%,表明隨著約束的增加,路徑更為平滑;直角路況下,在一定的轉角約束范圍內,隨著約束的加強,曲率均值也逐漸降低,由無約束下的0.095 m-1降低到0.005 2m-1，然而，隨著轉角約束由25°加強到15°時,曲率均值開始增大至0.006 7 m-1。

兩種路況下的平均采樣點數、平均樹節點數與仿真平均時長如表2所示。由表2可知,無約束RRT算法的平均采樣點數、平均樹節點數與規劃所用時長明顯高于有轉角約束的RRT算法,且隨著約束的加強,平均采樣點數與規劃所用時長均減小，平均樹節點數比無轉角約束條件下明顯減小。相比于無轉角約束,15°轉角約束下直線路況的平均采樣點數、平均樹節點數與規劃所用時長分別降低了62.9%，38.2%與78.5%,直角路況分別降低了34.0%，41.9%與27.4%,直線路況下,平均采樣點數和算法完成時間下降程度更為顯著。

綜上所述,基于本文提出的改進RRT算法生成軌跡的平均曲率降低,路徑平滑度增加;同時，由于平均采樣點數與平均樹節點數減少,軌跡生成速率有了較大幅度的提高,從而提高軌跡規劃的實時性,且能夠適應較寬的車速范圍,在一定程度上滿足車輛在高速行駛時的軌跡規劃要求。改進RRT算法中約束選擇對生成軌跡有較大影響,直線路況時,隨著約束的增加,軌跡平均曲率降低,路徑更為平滑;然而在直角路況下,過大的約束反而會導致較大的曲率,應合理選擇約束轉角。

表2 軌跡生成實時性指標對比Tab.2 Comparison of real-time index for trajectory

5 結 語

1)本文以無人駕駛汽車為研究對象,提出了一種基于車輛運動學約束與行駛穩定性的改進RRT軌跡規劃算法,并在不同路況下仿真分析了所提出算法的優劣。

2)本文提出的改進RRT算法,基于車輛的轉向特性,控制隨機點的擴展區域,同時，基于目標偏向策略以一定概率將目標點作為隨機點進行隨機樹擴張,最后采用三次B樣條曲線平滑生成的軌跡,在無人駕駛汽車軌跡規劃中具有較強的實用性。

3)本文提出的改進RRT算法采用仿真進行驗證,在直線路況及直角路況中均能規劃出合理軌跡,滿足無人駕駛汽車對軌跡規劃可達性、穩定性與實時性的要求,將機器人運動軌跡研究中較為流行的RRT搜索算法運用于車輛軌跡規劃領域。

4)本文提出的改進RRT算法未考慮車速變化對規劃軌跡的影響,因此,未來對轉角約束與車速的關系進行系統研究,對最大限度地保證穩定性與道路通行能力具有重要意義。