劉夢奇，王維強，田良宇

（武漢科技大學(xué) 汽車與交通工程學(xué)院，武漢 430065）

0 引 言

汽車產(chǎn)銷量的持續(xù)增長，使得城市交通控制任務(wù)面臨嚴(yán)峻形勢，而城市交通系統(tǒng)中有關(guān)交通擁堵、安全和環(huán)境污染等問題也不容小覷。隨著計算機云計算等新技術(shù)的快速普及，無人駕駛技術(shù)得到了重視與發(fā)展。無人駕駛車輛不僅可以降低事故的發(fā)生率，還可以提高汽車的出行效率。作為自動駕駛核心體系中的路徑規(guī)劃，一直都是無人駕駛領(lǐng)域的熱點與難點。

傳統(tǒng)的路徑規(guī)劃算法主要分為4類，分別是：基于圖搜索的算法，如A算法，D算法等；基于插值擬合的軌跡生成算法，如β樣條曲線等；以MPC為代表的用于局部路徑規(guī)劃的最優(yōu)控制算法；基于采樣的算法，如PRM，RRT算法等。

其中，RRT是一種在完全已知的環(huán)境中通過采樣擴展搜索的算法，相較于其它的算法，最突出的優(yōu)點就是快，而且其搜索過程不需要構(gòu)建顯式的任務(wù)空間，計算簡單，且具有概率完備性。但同時也有著比較明顯的缺點，比如常常得不到最優(yōu)解、規(guī)劃的路徑并不平滑等，因而后續(xù)提出了很多對RRT算法的改進策略。如，賀伊琳等人針對RRT算法盲目采樣的缺點，基于目標(biāo)偏向策略，限制采樣區(qū)域，以一定的概率將目標(biāo)點作為隨機點進行隨機樹擴張；針對RRT算法缺乏穩(wěn)定性和收斂速度慢的問題，提出了一種改進的雙向搜索路徑規(guī)劃算法；Bormann等人提出了一種漸進最優(yōu)版本的RRT改進算法RRT，該算法探索狀態(tài)空間的過程與RRT相似，但增加了父節(jié)點重新選擇和重新布線的步驟來保證最優(yōu)性，且在可行解找到后，擴展過程并不停止，而是繼續(xù)不斷迭代找到更優(yōu)的解，當(dāng)?shù)螖?shù)越來越多時，RRT會產(chǎn)生最優(yōu)解或者幾乎接近最優(yōu)的解。劉猛等人將RRT算法與三次曲線規(guī)劃相結(jié)合，通過三次曲線規(guī)劃使路徑趨向平滑，減少汽車自主泊車時的橫向移動。

為了解決RRT產(chǎn)生大的采樣空間且采樣時間長的問題，則隨即提出了Informed RRT算法。該算法在利用RRT找到初始解后，立即生成由起點、目標(biāo)點、當(dāng)前路徑長度決定的橢圓狀態(tài)空間區(qū)域，之后進行啟發(fā)式直接采樣，即把采樣范圍僅控制在這個能改善當(dāng)前可行解的啟發(fā)式橢圓子集區(qū)域內(nèi)，從而加速收斂到最優(yōu)解。但Informed算法產(chǎn)生的路徑會產(chǎn)生明顯的拐角，使生成的路徑不平滑、質(zhì)量差，本文擬進一步采用B樣條曲線函數(shù)來擬合生成的路徑點，從而生成曲率連續(xù)的平滑路徑。

1 相關(guān)算法與原理

1.1 RRT算法原理

RRT算法是由美國愛荷華州立大學(xué)的LaValle教授于1998年提出的，是基于概率采樣的運動規(guī)劃算法，能夠快速有效地搜索高維空間。算法運算流程如圖1所示。下面將闡釋分析RRT算法的核心操作。

圖1 RRT算法流程圖Fig.1 Flow chart of RRT algorithm

（1）初始化。由于RRT需要知道完整的環(huán)境地圖，所以在初始化時要提供環(huán)境地圖，以進行后續(xù)的碰撞檢測，再將起點作為樹的根節(jié)點進行存儲。

（2）隨機采樣。確定起點和地圖后，在整個地圖中進行隨機采樣。

（3）樹節(jié)點擴展。尋找距離采樣點最近的樹中的節(jié)點，將其作為新擴展的節(jié)點的父節(jié)點，再以該父節(jié)點為基礎(chǔ)，向采樣點的方向延伸一定距離，將延伸后的節(jié)點作為新節(jié)點，并加入樹。

（4）終止條件。在樹節(jié)點擴展的方法基礎(chǔ)上，可以將擴展的新節(jié)點是終點作為終止條件。

1.2 RRT*算法原理

RRT算法的規(guī)劃結(jié)果通常不會最優(yōu)，RRT就是針對RRT的規(guī)劃結(jié)果并非最優(yōu)的局限性提出來的。在原始RRT的基礎(chǔ)上，RRT主要有2點改進，即：為新節(jié)點選擇代價最小的父節(jié)點、重布線。

RRT算法重選父節(jié)點和重布線示意圖如圖2所示。圖2中，首先，當(dāng)采樣節(jié)點X加入節(jié)點樹后，以采樣點為圓心加一個小圓，考慮該圓圈內(nèi)是否有更近的父節(jié)點使起點到該節(jié)點的距離最近；若存在更合適的父節(jié)點，就將其連接起來，并去除原來的連線，然后判斷圈內(nèi)是否存在某種節(jié)點，該節(jié)點經(jīng)過X節(jié)點到初始節(jié)點的代價比該節(jié)點到初始節(jié)點原路徑的代價小，若存在該類型節(jié)點，則將X節(jié)點作為該節(jié)點的父節(jié)點，并斷開該節(jié)點與原父節(jié)點的連接，最后重新進行連線。

圖2 RRT*算法重選父節(jié)點和重布線示意圖Fig.2 Schematic diagram of RRT*algorithm for reselecting parent nodes and rewiring

1.3 Informed RRT*算法的原理

Informed RRT是在RRT的基礎(chǔ)上對采樣空間進行了優(yōu)化，大大縮短了搜索時間。其偽代碼參見算法1。黑體部分為改進的地方，其中表示起點到終點的距離，C表示規(guī)劃路徑的長度，初始狀態(tài)下C為0。圖3為Informed RRT算法的采樣示意圖，將已經(jīng)搜索到的最短路徑作為C，C作為橢圓的2個頂點2，作為橢圓的2個焦點，將采樣空間限制在橢圓內(nèi)，以此來構(gòu)建橢圓，如圖4所示。隨著路徑長度的不斷縮短，逐漸縮小該橢圓形區(qū)域。

圖3 Informed RRT*采樣示意圖Fig.3 Schematic diagram of Informed RRT*sampling

圖4 Informed RRT*搜索過程示意圖Fig.4 Schematic diagram of the Informed RRT*search process

Informed RRT(X，X)

2 B樣條曲線擬合路徑點

用B樣條規(guī)劃路徑可以滿足規(guī)劃路徑時對局部路徑進行修改而不改變整個路徑形狀的要求，本文正是應(yīng)用B樣條曲線的這些優(yōu)點來完成路徑點的后期擬合，以生成光滑路徑。

設(shè)一共有1個控制點，這些控制點用于定義樣條曲線的走向、界限范圍，則階B樣條曲線的定義為：

其中，B（）是第個階B樣條基函數(shù)，與控制點相對應(yīng)，≥1；是自變量，基函數(shù)有以下推導(dǎo)式：

一個非減的實數(shù)序列［，…，］稱為節(jié)點向量，B樣條基函數(shù)B（）在區(qū)間［u，u］之外為0，對所有，和都為非負(fù)。

一般來說，B樣條曲線分為均勻B樣條曲線和準(zhǔn)均勻B樣條曲線，本文選擇準(zhǔn)均勻B樣條曲線，因為這是兩端節(jié)點具有重復(fù)度、中間節(jié)點非遞減的序列，故而有著更為廣泛的應(yīng)用。還需指出的是，B樣條曲線公式中的值代表曲線的平滑度，值越大，曲線的平滑度越高，但計算復(fù)雜度也越高；值過小，曲線的平滑度就差。為兼顧無人車軌跡的平滑度與計算復(fù)雜度，本文最終選擇三階B樣條曲線，即3，將起始點和目標(biāo)點都設(shè)置為3重節(jié)點。

3 仿真結(jié)果與分析

為了驗證算法改進的有效性，擬基于Matlab軟件進行路徑規(guī)劃仿真實驗。實驗仿真環(huán)境為800 m*800 m的矩形區(qū)域，設(shè)置起始坐標(biāo)（0，0），目標(biāo)點（700，700），在Matlab中對RRT、RRT、Informed RRT、基于B樣條優(yōu)化的Informed RRT四種算法進行仿真。仿真實驗對比的評價指標(biāo)包括平均采樣的數(shù)目、平均規(guī)劃時間和平均路徑長度等。算法生成隨機樹上的采樣節(jié)點越少，規(guī)劃時間越短，則說明算法規(guī)劃路徑的效率越高，規(guī)劃的優(yōu)越性更好。

3.1 RRT、RRT*、Informed RRT*三種算法的仿真分析

圖5為RRT、RRT、Informed RRT三種算法在障礙物環(huán)境下的仿真實驗規(guī)劃結(jié)果對比圖。

圖5 3種算法仿真結(jié)果實驗圖Fig.5 Experimental diagram of simulation results of three algorithms

3種算法50次實驗的平均采樣時間、平均采樣長度、平均采樣節(jié)點數(shù)的數(shù)據(jù)對比見表1。

表1 算法對比指標(biāo)數(shù)據(jù)Tab.1 Indicator data comparison of three algorithms

由表1可知，基本RRT算法的平均采樣時間，平均采樣長度和平均采樣節(jié)點較大，RRT算法和Informed RRT算法在各項評價指標(biāo)上則有較大的改進，特別是在采樣時間和采樣節(jié)點上均有顯著的下降。與RRT算法相比，Informed RRT算法的大多數(shù)分支集中在起始點和終止點的空間，反復(fù)實驗過程中，最終路徑并無太大變化，而RRT樹支基本遍布整個空間，每次運行生成的路徑都不相似，隨機性較大。同時，其采樣時間提高了大約28%，采樣長度和平均采樣節(jié)點并無太大變化。

3.2 基于B樣條曲線的優(yōu)化仿真

進行B樣條曲線的優(yōu)化仿真，圖6為基礎(chǔ)的Informed RRT仿真結(jié)果圖，圖7為加入B樣條曲線平滑后的路徑對比，圖8為局部優(yōu)化對比圖，圖9為基于三次B樣條曲線軌跡優(yōu)化后路徑的曲率變化圖。

圖6 Informed RRT*仿真結(jié)果Fig.6 Simulation results graph of Informed RRT*

圖7 B-Informed RRT*平滑路徑對比圖Fig.7 Comparison of B-Informed RRT*smooth path

圖8 局部優(yōu)化圖Fig.8 Local optimization diagram

圖9 基于三次B樣條曲線的軌跡優(yōu)化曲率變化圖Fig.9 Trajectory optimization curvature change diagram based on cubic B-spline curve

由圖6～圖9可知，加入了B樣條曲線擬合后的路徑平滑性更高，經(jīng)B樣條曲線擬合后，所得曲線能夠沿給定趨勢變化，剔除了控制曲線中的尖點及曲率半徑較小處的點，曲線的平滑度有了一定程度的提高。其中，三次B樣條擬合曲線的曲率半徑更小，尖點處的過渡更為平滑，從車輛最大轉(zhuǎn)彎半徑求得允許最大曲率為0.14，優(yōu)化路徑的最大曲率小于0.12，經(jīng)過對比可以發(fā)現(xiàn)B樣條曲線優(yōu)化后的曲率連續(xù)且滿足車輛的曲率范圍，適用于車輛規(guī)劃軌跡。

4 結(jié)束語

綜上所述，通過對比RRT、RRT、Informed RRT算法，本文提出的B-Informed RRT算法生成的路徑平均曲率降低，路徑平滑點增加，同時采樣時間和采樣節(jié)點的降低，使得軌跡的生成速率有了較大的提升，從而提高了軌跡規(guī)劃的實時性，能夠在一定程度上滿足車輛在高速行駛時的軌跡規(guī)劃要求；而且加入了三次B樣條曲線，在無人駕駛汽車軌跡規(guī)劃中具有較強的實用性。