改進RRT在汽車避障局部路徑規劃中的應用

宋曉琳+周南+黃正瑜+曹昊天

摘 要：根據傳統快速搜索隨機樹算法（rapidly random-exploring trees，簡稱RRT）搜索速度快、所需時間短，但隨機性大以及約束不足等特點，建立了直道和彎道的期望路徑模型，采用高斯分布描述隨機采樣點，并引入啟發式搜索機制，改進RRT算法.與原算法仿真對比，結果表明：改進算法所規劃的路徑質量顯著提高，規劃時間縮短一倍.同時，在Prescan軟件中搭建直道和彎道仿真場景，跟隨規劃路徑，結果表明：改進后RRT算法所得路徑具有很好的跟隨效果，且側向加速度在車輛穩定性要求范圍內，說明采用改進后的RRT算法進行汽車局部路徑規劃可行實用.

關鍵詞：快速搜索隨機樹；汽車局部路徑規劃；高斯分布；路徑跟隨

中圖分類號：TP242 文獻標志碼：A

An Improved RRT Algorithm of Local Path Planning for Vehicle Collision Avoidance

SONG Xiaolin，ZHOU Nan，HUANG Zhengyu，CAO Haotian

（Stake Key Laboratory of Advanced Design and Manufacturing for Vehicle Body， Hunan University，Changsha 410082，China）

Abstract：The original Rapidly-exploring Random Trees（RRT） algorithm has a rapid exploring-speed and short required time in path planning though it has large randomness and lacks of constraints. Thus， an improved RRT was proposed where the expected paths were built in both straight and curved roads. The random points were accorded with normal distribution around the expected paths. Heuristic search method that led the random points to the goal with a certain probability was also used for improvement. Compared with the original RRT algorithm， it performs quite well in both time-efficient and path quality in the simulation. Meanwhile， the effectiveness of the improved RRT algorithm was verified in Prescan. The path can be followed well and the secure lateral acceleration was satisfied. In conclusion， the improved RRT is effective in the path planning for vehicle collision avoidance.

Key words： rapidly-exploring random trees； vehicle path planning； Gauss distribution； path following

近年來隨著汽車向智能化的不斷發展，無人駕駛汽車技術引得越來越多人開始關注.路徑規劃作為其中一項關鍵技術，許多學者開展了有益的探索，并取得了一些研究成果.比如A*算法[1]、D*算法[2]等啟發式搜索算法，在復雜環境下有著很好的規劃速度，但是路徑并不理想；人工勢場法[3]是一種虛擬力算法，其優點是規劃的路徑平滑，但是容易陷入局部最優解；人工勢場法與彈性繩模型結合[4-6]在車輛局部路徑規劃時有理想的路徑，但由于模型比較復雜，搜索效率低；此外蟻群算法、遺傳算法、神經網絡算法、水滴算法等智能仿生算法[7-10]，在處理復雜動態環境信息時有較好表現，但由于需迭代計算，時效性不夠好.

快速搜索隨機樹算法（RRT）[11]由Lavalle于1998年提出，是一種基于樹結構的典型算法，在移動機器人路徑規劃中有著較早應用.由于算法在進行路徑規劃時是隨機采樣，不需要預處理，因此有著很快的搜索速度.路徑點之間可以經過運動學、動力學仿真生成可執行曲線，因此該方法可用于解決含有運動學約束的路徑規劃問題.但RRT算法存在一些不足：1）重現性差，對同一環境進行多次路徑規劃結果不同；2）尋找最優或者次優路徑能力較差；3）隨機采樣點在整個可行域內隨機尋點的搜索方式，存在很多不必要的運算，影響算法速度.

隨著RRT算法的應用和改進，一些學者也提出了不同的方法.偏向RRT[12]以及雙向RRT[13]的提出使得算法的搜索效率得到了提高；DRRT[14]與MP-RRT[15]原算法在搜索路徑的穩定性上做出了改進.但上述改進之后的結果并不適用于汽車行駛路徑.針對以上不足，本文將建立道路模型，考慮路徑約束，改進RRT算法，使其規劃出的路徑能夠適用于汽車運動.

1 道路環境建模

在環境已知的情況下，建立道路環境模型.直道環境模型根據道路長度以及車道寬即可得到，彎道環境模型如圖1所示，位于道路上的慣性坐標系的原點選取在道路中心線上，道路寬度為W，規劃起始點即主車位置A，目標點C，障礙車位置為B.高速路直線之間的緩和曲線通常采用回旋線來實現平滑過渡，回旋線的特征是其曲率變化為常數.假定長度為l的回旋線線段起始曲率為C1，終止曲率為Cf，變化常數為C2，則有C（l）=C1+C2×l成立，C2=（Cf-C1）/l.回旋線常采用多項式逼近的形式表示：

式中：R0為道路中心線初始橫向偏移；C0為初始的方向角.根據圖1，結合邊界條件R（0）=0，R′（0）=0可以將R0和C0消除掉.

為了保持車道寬度一致，彎道的左右邊界是通過中心線上點向著其法線方向上下平移單個車道寬度來得到.在道路坐標系下中心線上的點可由式（2）表示.

中心線上一點的切向量和法向量則可以表示成：

因此道路左右邊界則可以由（4）來表示

2 RRT算法原理

基本的RRT算法如圖2所示，RRT算法以給定的起始點為隨機樹根節點，根據當前環境快速有效地搜索可行域空間，通過隨機采樣點，將搜索導向空白區域并增加隨機樹的葉節點直至目標點區域，從而生成從起始點到目標點的路徑.

算法的一般步驟如下：

1）首先通過environment（）函數建立環境數據模型，初始化各項參數；

2） 通過while循環來判斷樹節點是否達到目標點goal范圍內，若沒有，則開始擴展點.先生成隨機采樣點Prand，在樹節點上通過函數Nearest（）選擇距離Prand最近的樹節點作為擴展節點Pnear，通過擴展函數New得到新的樹節點Tnew，并將其添加到隨機樹上，直到循環終止.

3）通過getpath（）函數來得到生成的路徑path.

原算法主體程序如下：

如圖3所示，傳統的RRT算法應用到車輛路徑規劃中存在以下問題：

1）由于隨機采樣點隨機性大，導致搜索空間中有過多的冗余搜索，表現為搜索樹布滿了道路環境空間；

2）搜索出來的路徑曲率變化過大，甚至出現小范圍內直角變化，這樣的路徑并不能滿足汽車行駛的正常狀態；

3）路徑在靠近障礙時才開始避障，對于運動中的汽車會造成失穩或者與障礙物發生碰撞.

道路長度/m

3 RRT算法的改進

3.1 期望路徑模型

針對原RRT算法表現出來的問題，提出了一種隨機采樣點高斯分布的改進RRT算法.根據汽車行駛環境不同，設計了兩種期望路徑模型.

3.1.1 直道模型

駕駛經驗豐富的駕駛員期望的避障路徑模型如圖4（a）所示.期望路徑以函數Epz表示，其中各段均為直線段，start為起始點，goal為目標點.

提前避障在車輛避障路徑規劃中是必要的，故在模型中需要添加提前避障距離S，并根據車速調整大小.設V為當前車速，tc為換道時間，通常完成換道時間tc為1～2 s，ΔS為自定義安全提前量.

對于車速為V的汽車其剎車距離

則提前避障距離

圖4（a）中fz2表示提前避障區域，區間長度為S，fz4區間長度也為S.

期望路徑只是粗略的路徑模型，在此基礎上進行平滑得出的路徑并不能滿足汽車安全穩定行駛的要求.因此采用RRT算法進行路徑尋優搜索.為了使隨機采樣點分布在期望路徑周圍，利用高斯分布函數生成的點集中在其均值周圍的特點，結合期望路徑函數則可以實現這一目的.在道路坐標系下隨機采樣點的高斯分布概率函數為：

令μ=Epz（x），則可以得到如圖4（b）所示的隨機采樣點分布趨勢圖.

道路長度/m（a）期望路徑模型

道路長度/m（b）隨機采樣點高斯分布

σ的大小決定了隨機點在Epz（x）周圍的集中程度，σ越小越靠近Epz（x）.特別地，對于fz2與fz4周圍的隨機采樣點，如圖4（a）以fz2為例，通過相應水平范圍的隨機點高斯分布旋轉處理得到，即對

旋轉角度：

3.1.2 彎道模型

將彎道分為多段，采用直線代替彎道曲線的形式來完成期望路徑模型的建立.如圖5（a）彎道的期望路徑以函數Epw來表示.

隨機采樣點在fw各段函數區間的分布同直道的處理相同，從而可以得到如圖5（b）所示的分布趨勢圖.

3.2 約束條件

要使一條規劃出來的路徑有效地應用于汽車運動，即路徑可跟蹤，則規劃路徑時必須滿足道路環境約束.首先，隨機樹節點的生成要滿足道路環境約束，設Bleft，Bright分別為道路的左右邊界，則樹節點位置坐標要滿足：

考慮到汽車是具有幾何形體的，設其車寬為D，則上述y方向的約束變為：

假定汽車質心沿著規劃的路徑運動，為了保證行駛過程中的穩定性，規劃出的路徑的曲率變化不能過大.若在實際情況下前輪最大轉角為θmax，則路徑中子節點與其父節點的連線和父節點與其父節點的連線之間的夾角β必須滿足β<θmax，通常不同車型的θmax值在30°～40°之間.如圖2中子節點Tb的父節點為Ta，Tc的父節點為Tb，那么夾角約束表現為：

其中：K1為直線TaTb的斜率；K2為TcTb的斜率.βT為夾角限制值.

為了保證所擴展的點不與障礙車有交集，即樹節點不與障礙車碰撞的要求，采用安全橢圓包絡障礙車，并適當放大安全橢圓以保證避障要求.若新節點與其父節點的連線不與安全橢圓相交，則所擴展的新點滿足避障要求.取連線上的五等分點Pi（x，y），則約束方程表現為：

其中（xob，yob）為障礙車的位置，半車長a=2 m；半車寬b=1 m；s為安全橢圓放大系數，當s=2時，安全橢圓正好包絡矩形的障礙車，因此從安全避障考慮，s≥2.

3.3 啟發式搜索

原算法在擴展隨機樹時，由于缺乏導向機制，算法的收斂速度在一定程度上受到了影響，為了提高算法計算速度，引入啟發式機制來對隨機采樣點以及隨機樹節點的選擇進行處理.采樣點Prand在隨機生成過程中會以一定概率ρ0選擇目標點，從而將隨機樹節點向目標點引導，通常ρ0=0.1.

其中，GaussRand（）為隨機采樣點生成函數.

另外，在選擇Pnear時不再單獨以距離Prand最近作為選擇標準，而是以隨機樹節點的擇優系數Ch來決定，Pnear確定為Ch值最小所對應的樹節點.

其中ωi，ωj為權重系數，且ωi+ωj=1；Dpr為樹節點到Prand的距離，Dg為樹節點到目標點goal的距離.當ωj>ωi時，選擇出的Pnear具有向目標點靠近的趨勢.通過以上啟發式的處理，使得算法更快地收斂，減少計算時間.

3.4 平滑處理

RRT算法規劃出來的路徑通常會存在小范圍內的曲折現象，路徑并不連續.為了使得路徑能夠滿足汽車在運動時的穩定性和安全性要求，需要對規劃出來的路徑進行光滑處理.B樣條在處理路徑光滑時能夠不改變整個路徑形狀而進行局部調整[16]，利用B樣條這一特性，來對算法所規劃出來的路徑進行插值擬合，從而達到光滑路徑的目的，通常所采用的為3次B樣條曲線.

當有m+1個控制頂點Pi（i=0，1，…，m）時，3次B樣條曲線表示為：

3.5 算法流程

隨機樹節點的擴展受到隨機采樣點的影響，通過以上方式的處理，隨機采樣點不再是在可行域內隨機分布，而是具有一定的趨向性.這樣使得隨機樹節點的分布也具有趨向性，算法的隨機性得到了改善，所規劃出來的路徑質量得到提高.主體程序如下：

算法的實現過程如下：

1）初始化階段，首先通過environment（）函數建立道路環境模型，根據現有的環境模型，以expect（）函數建立期望路徑模型.

2） 路徑求解階段，進入while循環來判斷樹節點是否達到目標點goal范圍內，若沒有，則開始擴展點.隨機采樣點Prand通過Pick（）函數在goal和GaussRand（）函數所生成的點之間進行概率選擇；根據當前Prand計算樹節點的擇優系數Ch，并由Fitbest（）函數得出Pnear；通過函數New在Pnear的基礎上擴展出新節點Tnew，當新節點滿足約束函數constraint（）時，新節點則添加到整個隨機樹Tree上，否則，返回循環重新尋點直到其終止.

3）路徑處理階段，getPath（）函數從所得的Tree中獲取最短路徑，最后通過函數smoothing（）對所得路徑path進行光滑處理，得到一條平緩的路徑.

4 仿真實驗驗證

改進的RRT在應用于信息多變的不確定環境時會存在建模困難的缺陷，本文主要對確定車道環境下的改進RRT開展研究.由于條件有限，不能在實際車輛中進行試驗驗證.而Prescan軟件能夠對實際場景進行建模，并且有根據實車建立的車輛模型數據庫，能夠模擬實際車輛行駛過程.因此，為驗證改進RRT算法的有效性，在Prescan軟件平臺中搭建直道和彎道場景如圖6所示，仿真實驗硬件平臺Win10+Core-i7@3.6Hz+ARM@8G.仿真參數如表1～表3所示.

4.1 規劃時間的影響參數分析

影響算法計算效率的重要參數主要包括搜索步長ΔL、約束夾角βT.步長越小，為了搜索到目標點所需要的節點數目也就越多，計算時間越長；約束夾角βT越小，規劃路徑也越平緩，但計算時間也越長.改變步長以及約束夾角并進行大量仿真實驗，統計表明：在ΔL=3，βT=15°時，改進RRT算法在規劃時間以及路徑效果上的綜合表現較好.圖7為ΔL=3時，不同角度下計算時間的統計，以20次計算時間平均值做一次統計.

4.2 規劃路徑對比

為說明改進RRT算法的效果，在直道場景中，采用了其余2種規劃算法來進行對比.一種是蟻群算法[7]，另一種是彈性繩算法[5].直道仿真對比結果如圖8所示，改進前后的算法規劃效果在彎道中的對比如圖9所示，路徑效果參數對比如表4所示.由圖和表的結果對比可知：

1）相對于蟻群算法和原RRT算法，改進RRT算法與彈性繩算法規劃的路徑都更為平滑，不存在頻繁的大曲率變化，且路徑較短.

2）從直道的規劃時間上看，傳統的蟻群算法用時最長，而彈性繩算法平均計算時間為1.42 s.改進后的RRT算法平均計算時間0.25 s，相對于原RRT計算時間略有增加，這是由于增加了模型處理過程以及各約束條件所導致的.但與其余2種算法相比，改進RRT算法依然保持其在計算時間上的優勢.

3） 原RRT算法在靠近障礙時才開始避障，改進RRT算法能夠實現提前避障，并且根據車速不同自動調節避障提前量，這對安全行駛很重要.

4.3 路徑跟隨驗證

對一條規劃出來的路徑，需要驗證其有效性，即路徑的跟隨性，本文采用路徑跟隨時主車的側向加速度曲線監測路徑的穩定性，跟隨速度20 m/s.直道和彎道仿真結果分別如圖10、圖11所示.

由圖10（a）（b）可知，直道跟隨路徑基本和目標路徑一致，跟隨誤差在0.2 m以內，誤差較??；車輛保持穩定行駛時的最大側向加速度由地面附著力決定，通常為0.8g.由圖10（c）可知，跟隨時的側向加速度在0.4g以內，說明整個過程中都能夠保證主車按照路徑行駛時的穩定性.彎道仿真結果如圖11所示，跟隨誤差都在0.1 m以內，跟隨效果好；側向加速度都在0.5g范圍內，滿足穩定性要求.

5 結 論

本文在將RRT算法應用到汽車避障局部路徑規劃時，針對原算法在隨機性以及路徑曲率變化上的不足，建立了直道和彎道的期望路徑模型，使隨機采樣點在期望路徑模型周圍近似呈高斯分布，采用啟發式搜索方式使采樣點和隨機樹節點向目標點靠近，從而降低算法的隨機性；并且在路徑規劃時加入約束，使得所規劃出的路徑更為合理.仿真實驗結果表明：改進RRT算法不僅規劃出的路徑質量高、實時好、跟隨性強，而且車輛的穩定性也滿足要求.因此，改進RRT算法可以應用于實時汽車路徑規劃.

參考文獻

[1] YAO J， LIN C， XIE X， et al. Path planning for virtual human motion using improved A* algorithm[C]//Proceedings of the Conference on Information Technology： New Generations （ITNG）.Washington， DC： IEEE Computer Society， 2010： 1154-1158.

[2] DIJKSTRA E W. A note on two problems in connexion with graphs[J]. Numerische Mathematik， 1959， 1（1）： 269-271.

[3] KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research， 1986， 5（1）： 90-98.

[4] SONG X， CAO H， HUANG J. Vehicle path planning in various driving situations based on the elastic band theory for highway collision avoidance[J]. Proceedings of the Institution of Mechanical Engineers， Part D： Journal of Automobile Engineering， 2013， 227（12）： 1706-1722.

[5] SONG X， CAO H， HUANG J. Influencing factors research on vehicle path planning based on elastic bands for collision avoidance[J]. SAE International Journal of Passenger Cars-Electronic and Electrical Systems， 2012， 5（2）：625-637.

[6] 肖浩， 宋曉琳， 曹昊天. 基于危險斥力場的自動駕駛汽車主動避撞局部路徑規劃[J]. 工程設計學報， 2012，19（5）： 379-384.

XIAO Hao， SONG Xiaolin， CAO Haotian.Local path planning for autonomous vehicle collison avoidance based on dangerous repellant fields[J]. Chinese Journal of Engineering Design，2012， 19（5）： 379-384.（In Chinese）

[7] 康冰， 王曦輝， 劉富. 基于改進蟻群算法的搜索機器人路徑規劃[J]. 吉林大學學報：工學版， 2014， 44（4）：1062-1068.

KANG Bing， WANG Xihui， LIU Fu. Path planning of searching robot based on improved ant colony algorithm[J]. Journal of Jilin University：Engineering and Technology Edition， 2014， 44（4）： 1062-1068.（In Chinese）

[8] HU Y， YANG S X. A knowledge based genetic algorithm for path planning of a mobile robot[C]//Proceedings of the Conference on Robotics and Automation. Washington，DC： IEEE Computer Society， 2004： 4350-4355.

[9] 吳乙萬，黃智.基于動態虛擬障礙物的智能車輛局部路徑規劃方法[J].湖南大學學報：自然科學，2013，40（1）：33-37.

WU Yiwan， HUANG Zhi. Dynamic virtual obstacle based local path planning for intelligent vehicle[J]. Journal of Hunan University： Natural Sciences， 2013，40（1）：33-37.（In Chinese）

[10]SHAH-HOSSEINI H. Problem solving by intelligent water drops[C]// Proceedings of the Conference on Evolutionary Computation.Washington，DC： IEEE Computer Society， 2007： 3226-3231.

[11]LAVALLE S M.Rapidly-exploring random trees： A new tool for path planning[J]. Algorithmic & Computational Robotics New Directions， 1998：293-308.

[12]LAVALLE S M， KUFFNER J J. Rapidly-exploring random trees： progress and prospects[J]. Algorithmic & Computational Robotics New Directions， 2000：293-308.

[13]KUFFNER J J， LAVALLE S M. RRT-connect： An efficient approach to single-query path planning[C]// Proceedings of International Conference on Robotics and Automation.Washington，DC： IEEE Computer Society， 2003：995-1001.

[14]FERGUSON D， KALRA N， STENTZ A. Replanning with rrts [C]//Proceedings of International Conference on Robotics and Automation.Washington，DC： IEEE Computer Society， 2006： 1243-1248.

[15]ZUCKER M， KUFFNER J， BRANICKY M. Multipartite RRTs for rapid replanning in dynamic environments[C]//Proceedings of International Conference on Robotics and Automation.Washington，DC： IEEE Computer Society， 2007： 1603-1609.

[16]李紅，郭孔輝，宋曉琳.基于樣條理論的自動垂直泊車軌跡規劃[J].湖南大學學報：自然科學版，2012，39（7）：25-30.

LI Hong， GUO Konghui， SONG Xiaolin. Trajectory planning of automatic vertical parking based on spline theory[J]. Journal of Hunan University： Natural Sciences， 2012，39（7）：25-30.（In Chinese）