基于改進RRT算法的無人車路徑規劃

李偉東, 李 樂

(大連理工大學 汽車工程學院,遼寧 大連 116024)

0 引言

隨著自動駕駛技術的快速發展，路徑規劃作為無人車領域的核心技術之一，也逐漸成為研究熱點。路徑規劃可以分為全局路徑規劃和局部路徑規劃。全局路徑規劃要求地圖信息已知并且不考慮動態障礙物，其能夠規劃出一條從起點到終點的可行路徑。局部路徑規劃算法是指無人車在沿著全局路徑行駛時，如果全局路徑上出現障礙物導致車輛無法正常通過，需要規劃出用于局部避障的路徑[1]。與高精度地圖相結合，全局路徑規劃算法可以在整個位姿空間內找到一條從初始位姿到目標位姿的無碰撞路徑，同時該路徑還滿足環境約束、搜索時間約束和車輛運動學約束等約束條件。全局路徑規劃算法可以大致分為以下幾類：(1)基于仿生學的路徑規劃算法，如遺傳算法[2]、蟻群算法等[3]；(2)基于搜索的路徑規劃算法[3-4]；(3)基于采樣的路徑規劃算法[5]。應用最廣泛的基于采樣的算法便是由Lavalle提出的RRT[6]算法。 它最顯著的優點是不需要對空間進行預處理并且具備概率完備性。但是，基本的RRT算法在規劃路徑時存在幾個缺點[7]：(1)采用隨機采樣算法，搜索過程具有盲目性和隨機性；(2)路徑曲折，不能應用于無人車全局路徑規劃領域；(3)收斂速度慢，搜索效率低。

很多學者都致力于改進RRT算法的缺陷。LaValle和Kuffner提出了RRT-Connect[7]算法，該算法會從起始節點和目標節點并行生成隨機樹來提高搜索速度。Karaman 和Frazzoli提出的RRT*[9]算法引入了路徑代價信息和重布線操作，雖然得到的路徑質量最優，但算法的收斂時間也隨之增加。此外還有很多RRT系列算法在不斷地涌現，例如Dynamic-RRT[10]算法、Informed RRT*[11]算法和批處理信息樹[12]( BIT*, batch informed trees)算法等。與此同時RRT系列算法也開始與其他算法結合。馮來春[13]提出將A*算法和RRT算法結合的方法，利用A*算法在格柵圖中生成的最短路徑來引導RRT算法的擴展。Huang Shunyu[14]將人工勢場法引入到RRT算法中，通過建立障礙物斥力場來限制障礙物附近的搜索區域，并在隨機樹的擴展中添加引力分量加快算法的收斂速度，但是可能會出現局部最小值問題。Ivojevic等人[15]針對類汽車機器人的路勁規劃問題，將RRT算法的節點間的直線連接替換為Dubins曲線，雖然能規劃出滿足汽車運動學約束的路徑，但是路徑過于冗余且轉彎太多。朱冰[16]提出基于安全場的改進RRT*智能汽車路徑規劃算法，通過建立安全場模型引導車輛避障行駛，但是路徑的搜索具有較大盲目性。

綜合已有研究成果，針對上述提到的RRT算法的缺點，本文提出一種應用于無人車全局路徑規劃領域的改進RRT算法。本文算法首先使用人工勢場法和目標動態概率采樣策略來減少算法在搜索過程中的盲目性以及在連接終點位姿時使用Reeds-Shepp曲線，減少目標點處額外的位姿調整，從而減少算法消耗的時間；然后算法考慮了車輛的非完整性約束，并對規劃的路徑進行轉角約束檢測和碰撞檢測，保證汽車跟蹤行駛的安全性；最后對路徑進行后處理優化，最終得到的全局路徑更適合無人車跟蹤行駛。

1 汽車運動學模型

標準RRT算法僅可以尋找一條能夠連接起點和終點路徑，但是規劃的路徑曲折且無目的性，不能滿足無人車的運動學約束，從而車輛無法跟隨規劃的全局路徑行駛。因此本文算法在進行路徑規劃時考慮車輛的運動學約束，車輛運動學模型如圖1。

圖1 車輛運動學模型

以車輛的后軸中心為車輛的參考點，b為車的軸距，φ為前輪轉角，θ為航向角，r為轉彎半徑，k為路徑曲率。假設車輛的速度為v，那么根據圖1就能得到車輛的運動學方程，如式(1)：

(1)

車輛受到的運動學約束需要滿足下式：

(2)

式中,vmax為最大車速，φmax為最大等效前輪轉向角，kmax為最大路徑曲率，Rmin為最小轉彎半徑。

2 改進的RRT算法

2.1 地圖及障礙物柵格化

為了能夠精確表示地圖信息、障礙物信息以及定位信息，需要將無人車所處環境的高精度地圖作為算法的基礎。全局路徑規劃可以使用柵格地圖，二維柵格地圖使用占用柵格的形式存儲環境中的靜態障礙物信息，為全局路徑規劃提供信息[17]。本文使用已知的二維柵格地圖作為地圖信息輸入，在全局路徑規劃算法開始時初始化地圖信息。

2.2 基本RRT算法

當得到場景的柵格地圖后，便可以根據目標位姿和起始位姿構造搜索樹了。RRT算法示意圖如圖2所示，以起始點作為隨機樹的根節點，通過隨機采樣，將隨機樹的擴展引導到可行區域直到搜索到目標點，從而生成一條從起始點到目標點的全局規劃路徑。

圖2 RRT算法示意圖

基本步驟如下：

1)以狀態空間的起點Xinit作為根節點構造隨機樹；

2)在搜索空間內產生隨機采樣點Xrand，用于引導隨機樹的擴展；

3)遍歷整個隨機樹上已產生的節點，選出距離隨機點Xrand最近的樹節點Xnear；

4)從Xnear節點沿著Xrand節點的方向以擴展步長生成新的節點Xnew作為新的樹節點。如果擴展過程中遇到障礙物則取消本次擴展。重復上述迭代過程直到目標節點變為葉子節點或者超過規定的迭代次數時搜索結束。從目標點回溯到起點便可得到規劃的路徑。

2.3 目標動態概率采樣

為了減少RRT算法搜索過程的盲目性，解決盲目搜索的問題，本文算法在隨機采樣時引入了目標動態概率采樣。動態概率采樣策略首先會設置一個目標采樣閾值pbias(0

(3)

當隨機樹擴展過程中遇到的障礙物較少時，直接使用目標點作為采樣點往往會加快搜索速度。但是當環境中障礙物較多時，在目標點方向上進行簡單的概率擴展，可能會陷入局部困境問題。因此為了應對多種不同的場景，目標點采樣概率閾值pbias應該被設置為動態變化的值，Niter為當前迭代次數，Ncurr為當前所生成的有效節點個數，目標點采樣概率閾值pbias公式如式(4)：

(4)

式中,k代表目標概率采樣閾值的最大值。Ncurr和Niter的比值代表柵格地圖中障礙物的稠密程度。比值越大代表當前有效節點占比越多，障礙物越少，隨機樹向目標節點擴展的可能性越大。相反，意味著無效節點數目較多，障礙物越稠密，隨機樹向隨機點擴展的可能性更大。

2.4 基于人工勢場的節點擴展優化

RRT算法由于其搜索的隨機性強，會在空間進行搜索時產生許多無用節點，收斂速度低，搜索時間過長。同時，在人工勢場法路徑規劃中，無人車總是朝向虛擬勢場的負梯度方向運動，在復雜和特殊的環境可能會出現勢能最小處在某個受到的引力和斥力相互抵消的非目標點位置而不是目標點位置的情況，出現局部最小值的情況，導致目標節點不可達[18]。

針對上述問題，本文將人工勢場法的目標引力作用和障礙物斥力作用融入到RRT算法的擴展節點過程中，根據先驗格柵地圖構造地圖的人工勢場，并使隨機樹的擴展能夠在勢場的引導下朝著目標點進行擴展，加快算法的收斂速度。同時依靠目標動態概率采樣和RRT算法的擴展隨機性，可以有效避免傳統的人工勢場法在進行路徑規劃時會陷入局部最小值的問題。人工勢場的引力場Ua(p)、斥力場Ur(p)以及合力勢場Utotal的函數如下所示：

(5)

(6)

Utotal(p)=Ua(p)+Ur(p)

(7)

其中:ka是目標引力場增益系數，kr是斥力場增益系數。ρ0是一個距離閾值，當節點到最近的障礙物距離大于該閾值則不產生斥力。ρ(p,pgoal)和ρ(p,pobs)分別代表當前節點到目標點和最近障礙物的距離。如圖3所示，圖(a)的環境從起點到終點產生的總勢場圖如圖(b)所示。

圖3 總勢場示意圖

因此算法擴展過程中，受到的目標引力函數與障礙物斥力函數分別如式(8)所示：

Fa(p)=kaρ(p,pgoal)

(8)

(9)

Ftotal(p)=Fa(p)+Fr(p)

(10)

在人工勢場中，目標產生的引力和障礙物產生的斥力的合力Ftotal的方向就是總勢場的負梯度方向。在新節點Xnew擴展過程中融合人工勢場的作用，使節點擴展的方向不僅沿著Xrand，還會考慮勢場合力Ftotal的引導作用。擴展節點由兩個向量疊加而成，擴展節點的計算公式如式(11)所示：

Xnew=Xnear+d(wrnrand+(1-wr)nf)

(11)

式中,d為擴展步長，wr為隨機點的擴展偏向權重，nrand為Xrand方向的單位向量，nf為合力Ftotal方向的單位向量。

新節點擴展過程示意圖如圖4所示。

圖4 節點擴展示意圖

2.5 路徑約束條件

由前文分析可知，由于汽車的動力學約束，生成路徑中的轉角應該小于最大前輪轉角φmax，因此在生成新節點和生成路徑時需要考慮角度約束。轉角的計算如圖5所示，路徑中3個連續節點X1，X2，X3及其夾角角度φ，計算公式如式(12)所示：

(12)

在擴展過程中會根據公式判斷擴展的新節點Xnew和Xnear的連線與Xnear及其父節點Xparent的連線之間的夾角是否大于前輪最大轉角，如果大于約束，則放棄本次擴展。

圖5 轉角約束示意圖

除了路徑轉角約束以外，為了保證所規劃出的路徑對于無人車來說是安全無碰撞的，還需要對路徑進行碰撞檢測。本文采用方向包圍盒(OBB, oriented bounding box)的碰撞檢測方法，一個對象的OOB指的是包含該對象且相對于坐標軸方向任意的最小長方體[19]。如果兩個凸多邊形不發生碰撞，則一定存在一條分離軸，使得兩物體在該分離軸上的投影不相交。在格柵地圖中，OBB的檢測原理如圖6所示，矩形A和B分別代表車輛和障礙物的OOB。Ax、Ay為以矩形A中心點OA為原點坐標的局部坐標系下的單位向量，Bx、By為以矩形B中心點為OB原點坐標的局部坐標系下的單位向量。α為全局坐標系下車輛的航向角，φ為兩矩形中心連線與格柵地圖的全局坐標系x軸正方向的夾角。

圖6 OBB碰撞檢測示意圖

在實際應用中只需要判斷兩矩形分別在Ax、Ay、Bx、By4個方向的投影是否有重疊即可。如果矩形A和矩形B沒有碰撞的話，應該滿足下式4個條件：

(13)

式中，L為矩形中心點的距離LA、LB分別代表矩形A、B的長度；WA、WB分別代表矩形A、B的寬度。

2.6 目標點貪心連接

Reeds-Shepp[20]曲線是由多段半徑為最小轉彎半徑的圓弧曲線或直線拼接而成，能夠在一個位姿到另一個位姿之間生成滿足運動學約束的前進或倒車軌跡。在原RRT算法中，當隨機樹新擴展的節點到目標節點的距離小于擴展步長時，就宣告搜索結束找到路徑，但如果有終點位姿要求，RRT算法就需要反復搜索以保證滿足終點的位姿要求。本文采用Reeds-Shepp曲線來代替原RRT算法的目標點連接方式，當新擴展節點進入到目標點的可直接連接范圍后，每次搜索都嘗試使用Reeds-Shepp曲線對目標點連接，從而避免反復搜索以滿足終點位姿要求。通常目標點的連接范圍可以設置為10倍的擴展步長，這樣以來就能減少大量搜索時間和擴展的節點，避免多余的位姿調整。

2.7 路徑后處理優化

假設使用該算法規劃得到的初始全局路徑點的集合為P{P1,P2,P3,…,Pn}，如圖7所示，假設節點P1與節點P4之間能夠通過直線連接并且連線又同時滿足轉角約束和無碰撞約束，則P1和P4之間的P2和P3節點均為冗余節點，可以將其從路徑節點集合中刪除。在得到初始路徑后進行路徑剪枝，從起點開始，遍歷整個節點集合，找到并去除所有冗余節點，能夠減少路徑長度并使車輛駕駛的平順性提高。

圖7 路徑剪枝示意圖

但是此時得到路徑仍由若干線段連接而成，不滿足車輛對平順性和安全性的要求，則需對路徑進行平滑處理。由于三階貝塞爾曲線能夠保證始末位姿要求和曲率連續要求，則對3個節點形成的兩段直線采用兩段三階貝塞爾曲線進行平滑處理。n階貝塞爾曲線表達式如式(14)所示：

(14)

式中,Pi代表貝塞爾曲線的控制點，u代表貝塞爾曲線的參數且0≤u≤1。Bi,n為n次Bernstein基函數，且滿足：

(15)

則原來由3個隨機樹節點確定的兩段直線路徑經過兩段三階貝塞爾曲線平滑后的結果示意圖如圖8所示。

圖8 貝塞爾曲線應用示意圖

圖中P1、P2、P3分別為路徑相連的兩直線的節點。A1、A2、A3、P為第一段三階貝塞爾曲線的控制點。B1、B2、B3、P為第二段三階貝塞爾曲線的控制點。其中控制點P為A3和B3連線的中點。

本文的改進RRT算法的流程圖如圖9所示。

圖9 改進RRT算法流程圖

3 仿真實驗對比分析

本文使用Matlab對本文所提出的改進RRT算法進行仿真實驗，測試主機的處理器為Intel(R) Core(TM) i7-9700，主頻為3.0 GHz，內存大小為16 GB。

為了驗證本文的改進RRT算法具有更好的性能，將RRT算法、RRT*算法和改進RRT算法在簡單障礙物環境、復雜障礙物環境和單一通道環境3種二維仿真柵格地圖環境下進行全局路徑規劃對比試驗，3種仿真地圖大小均為500×500個像素的柵格地圖表示，起點均為(50,50)，終點均為(450,450)。在3種環境下每種算法均進行100次重復實驗，并統計算法各個指標的平均值。3種環境下各算法的規劃結果如圖10～12所示，其中(a)圖為基本RRT算法規劃效果，(b)圖為基本RRT*算法規劃效果，(c)圖為改進RRT算法得到的路徑優化前的效果，(d)圖為改進RRT算法對初始路徑優化后的效果。

圖10為簡單障礙物環境的算法規劃對比圖，地圖中障礙物較大，環境較為簡單。圖(a)為基本RRT算法規劃的一條可行路徑，但由于RRT算法的盲目搜索特性，生長樹會隨機分布在無障礙區域，算法效率較低并且得到路徑質量很差。圖(b)為RRT*算法規劃的一條可行路徑，由于RRT*算法具有漸進最優的特性，所以RRT*能夠得到較優的路徑長度，但是需要大量反復迭代，節點利用率和搜索效率較差，并且不滿足路徑安全約束。圖(c)和圖(d)是本文改進RRT算法在進行路徑優化前后的規劃路徑，由圖可見路徑剪枝優化效果顯著。

圖10 簡單環境下算法效果對比

從表1的實驗數據中可以得出，由于地圖環境比較簡單，目標概率采樣和人工勢場的引導作用可以被充分利用，減少了大量無用節點的擴展，明顯加快了搜索速度，相比于RRT算法和RRT*算法，擴展節點數目分別減少了43.50%和83.37%，搜索時間分別減少了62.12%和77.46%。并且在經過節點剪枝和路徑優化后，改進RRT算法的路徑長度較優，其路徑長度相比于RRT算法的路徑長度縮短13.86%，但是由于考慮了運動學約束和碰撞約束，相比于路徑長度最優的RRT*的算法增加了3.39%。

表1 簡單障礙物環境實驗結果對比

圖11為復雜環境地圖的規劃效果，環境中存在大量大小不一的障礙物，從起點到終點可以有多條路徑可以選擇。圖(a)中RRT算法的規劃效果圖，可以看出隨機樹的擴展雜亂無章，路徑曲折。圖(b)中RRT*算法能夠利用自身的重布線特性在復雜的環境中規劃出一條較優路線，但是擴展樹所需節點較多，搜索效率較差。圖(c)和圖(d)中的規劃效果明顯優于RRT算法和RRT*算法，并總能找到較優路徑，并且在需要多次轉折的場景下，路徑優化的效果仍顯著。

圖11 復雜環境下算法效果對比

表2的實驗結果得知，本文改進的RRT*算法的節點擴展數目相比于RRT算法和RRT*算法分別減少了67.36%和85.07%，規劃時間分別減少了63.14%和77.46%。規劃得到的路徑長度相比RRT算法縮短了23.12%，并且和路徑長度最優的RRT*算法差距不大。因此本文的改進RRT算法該環境下能夠保證路徑長度達到相對較小的同時提高了算法的規劃效率，路徑也變得更加平滑。

表2 復雜障礙物環境實驗結果對比

圖12為單一通道障礙物環境，特點是從起點到終點只有一條狹窄的通道可以經過。3種算法的規劃效果對比來看，RRT和RRT*在尋找通道入口前需要經過大量的隨機擴展，節點利用率和搜索效率非常低。而本文改進的RRT算法能夠根據人工勢場的引導作用，快速找到通道的入口，搜索時間相比于RRT算法和RRT*算法大幅降低，并且能夠依靠障礙物斥力作用和碰撞檢測機制，能夠在狹窄通道內較快得到一條安全無碰撞的可行的全局路徑。

圖12 單一通環境下算法效果對比

從表3的實驗數據中可以得出，改進RRT算法的平均擴展節點數為 41.25個，比 RRT減少 65.97%，比RRT*減少87.21%；平均規劃時長為1.05 s，比RRT縮短58.33%，比RRT*縮短76.87%；平均路徑長度為616.83 m，比RRT減少14.80%，比RRT*少幅度增加了2.32%。

表3 單一通道環境實驗結果對比

根據上述的仿真圖和實驗結果可以明顯的看出，本文的改進RRT算法無論是在規劃時長、擴展節點數還是路徑長度上都相比標準RRT算法具有明顯優勢。雖然本文的RRT算法得到的路徑略長于RRT*算法得到的路徑長度，但是花費時間和無用節點的數量明顯減少。除了規劃時間、節點數量和路徑成本等指標以外，RRT和RRT*算法得到的路徑曲折并且不滿足碰撞約束，汽車的行駛安全和平順性不能得到保障。并且由圖10到圖12的(c)圖和(d)圖可以發現，本文提出的路徑后剪枝和優化方法效果顯著。因此，本文所提出的改進RRT算法在3種復雜的障礙物環境下，具有規劃效率更高，路徑長度較優，路徑光滑且安全的特點。

4 結束語

本文針對RRT算法在無人車全局路徑規劃領域存在的問題，提出一種綜合改進算法。首先引入目標動態概率采樣和人工勢場引導隨機樹擴展的策略，提高了算法的搜索效率，減少冗余節點的產生，內存占用更少；然后定義了路徑的轉角約束和碰撞約束，使路徑能夠滿足汽車的運動學約束并使無人車安全無碰撞行駛；其次當隨機樹擴展到目標點可連接范圍后，每次迭代都嘗試使用Reeds-Shepp曲線連接目標點，減少多余的位姿調整，進一步提高算法效率；最后在路徑初步規劃完成后，提出一種路徑修剪的方法，對路徑進行簡化，并使用兩段三階貝塞爾曲線對路徑轉角進行平滑連接，去除冗余節點的同時還能使路徑更加光滑。通過與RRT算法和RRT*算法仿真實驗對比，結果表明本文提出的改進RRT算法在多種環境下的規劃得到的路徑在搜索效率、路徑質量上都更具優越性。該算法對無人車的全局路徑規劃具有一定的參考意義。