城市工況下基于改進RRT的無人車運動規(guī)劃算法*

余卓平 衛(wèi)燁 熊璐 李奕姍

（同濟大學(xué)，上海 201804）

主題詞：無人駕駛汽車 運動規(guī)劃 偏向性采樣 鄰近點搜索

1 前言

運動規(guī)劃是無人駕駛汽車的關(guān)鍵技術(shù)，是在決策給出目標(biāo)點的位姿信息后，利用車載傳感器獲得自車狀態(tài)以及周圍環(huán)境信息，在滿足運動系統(tǒng)微分約束的基礎(chǔ)上，及時生成一條指導(dǎo)汽車在接下來短時間內(nèi)運動的安全路徑。運動規(guī)劃最初開始于機器人領(lǐng)域，車輛可看成是一種特殊的輪式機器人，因此車輛的運動規(guī)劃算法大多由機器人領(lǐng)域的規(guī)劃算法發(fā)展而來。較有代表性的算法有兩大類：基于網(wǎng)格搜索的算法和基于隨機采樣的算法。基于網(wǎng)格搜索的算法如A*[1]、AD*算法[2]，能保證找到可行解，后者還能在動態(tài)障礙物環(huán)境下應(yīng)用，但生成安全、可行的路徑需要較精確的環(huán)境地圖以及成熟的代價確定準(zhǔn)則。基于隨機采樣的算法以快速隨機擴展樹[3]（Rapidly-exploring Random Tree，RRT）法為代表，此類方法不需要對狀態(tài)空間顯式建模，因此搜索速度快，并且可以在搜索過程中考慮車輛自身客觀存在的約束。但此方法也有一些不足之處。以RRT算法為例：首先，雖然RRT算法具有概率完備性，但每次搜索的結(jié)果隨機性較大；其次，標(biāo)準(zhǔn)RRT算法均勻隨機采樣過程中大量的碰撞檢測嚴(yán)重影響效率；另外，車輛構(gòu)型空間中的度量距離沒有閉合形式表達式，計算困難。國內(nèi)外的學(xué)者針對這些缺陷提出了一些解決辦法。美國斯坦福大學(xué)的Kuffner和愛荷華州立大學(xué)的LaValle提出了RRT-Connect算法[4]，同時構(gòu)建兩棵分別起于初始狀態(tài)和目標(biāo)狀態(tài)的樹，以加快RRT搜索。隨后LaValle又提出Goal-biasing RRT算法[5]，在隨機采樣序列中以一定比例插入目標(biāo)狀態(tài)，引導(dǎo)樹向目標(biāo)狀態(tài)擴展。英國劍橋大學(xué)的Shkolnik和Walter等人[6]提出了RG-RRT（Reachability Guided RRT）算法以消除不準(zhǔn)確的度量距離對RRT探索能力的影響。美國伊利諾伊大學(xué)的Cheng[7]提出RC-RRT（Resolution Complete RRT）算法降低障礙物周邊節(jié)點獲得擴展的概率，從而加速碰撞檢測。盡管各種改進措施能有效提高求解質(zhì)量，但仍難保證獲得最優(yōu)解，于是近年來出現(xiàn)了許多求漸進最優(yōu)解的算法，如RRT*[8]、LBT-RRT算法[9]等。

另外，也有部分研究人員僅采用幾何曲線進行路徑生成，如螺旋曲線[10]、B樣條曲線[11]、貝塞爾曲線[12]等，但通常需要在生成路徑時與障礙物進行繁瑣復(fù)雜的幾何分析，以保證路徑無碰撞，因此效率較低。

本文針對城市結(jié)構(gòu)化道路環(huán)境下的無人車運動規(guī)劃問題，提出了一種快速偏向性RRT（Fast-Biasing RRT，F(xiàn)B-RRT）算法。該算法首先基于幾何分析結(jié)果初始化RRT，隨后針對城市工況下最常見的換道和轉(zhuǎn)向行為設(shè)計了不同的偏向性采樣方法，并結(jié)合KD樹加速鄰近點搜索。搜索成功后對RRT進行裁剪重構(gòu)并利用B樣條曲線平滑路徑。最后，通過仿真以及實車試驗驗證了該算法的有效性和實用性。

2 車輛模型建立

無人車的運動規(guī)劃問題是要求找出從起始點到終止區(qū)域的一條可行路徑上的控制輸入，也就是說需要先規(guī)劃運動的軌跡，再根據(jù)軌跡輸出和系統(tǒng)狀態(tài)變量的各階導(dǎo)數(shù)求出系統(tǒng)狀態(tài)變量和控制輸入。阿克曼轉(zhuǎn)向模型為常見的具有此種性質(zhì)的車輛模型，如圖1所示。

圖1 車輛模型

該模型將車輛視為平面剛體，具有3個自由度，選取后軸中心點R作為參考點。圖1中，b為車輛的軸距，?為前輪轉(zhuǎn)角，車輛在大地坐標(biāo)系下的坐標(biāo)為(X,Y)，橫擺角為θ。車輛狀態(tài)方程為：

式中，v為車速；κ為轉(zhuǎn)彎曲率。

車輛模型受動力學(xué)約束，因此需滿足：

式中，?max為最大前輪轉(zhuǎn)角；vmax為最大車速；κmax為最大轉(zhuǎn)彎曲率。

3 快速偏向性RRT算法

3.1 基本RRT算法

RRT算法由S.M.LaValle提出[3]，是一種基于隨機采樣的增量式運動規(guī)劃算法，屬于典型的樹狀結(jié)構(gòu)搜索算法，通過在狀態(tài)空間中不斷搜索采樣生成一棵樹，整棵樹包括樹上所有節(jié)點組成的節(jié)點集N和所有“樹枝”組成的邊界集E，擴展過程如圖2所示。

圖2 RRT擴展示意

整棵樹T從初始點ninit開始擴展，利用采樣策略對狀態(tài)空間隨機采樣得到采樣點nrand，利用鄰近點搜索算法在已有樹T中搜索“距離”nrand最近的節(jié)點nnear，之后根據(jù)設(shè)置的擴展機制（如確定的擴展步長d）連接nnear和nrand，并得到新節(jié)點nnew，檢測nnew和連接線段enew的安全性，如果滿足要求，那么將nnew和enew分別插入節(jié)點集N和邊界集E中，樹T得到擴展。整個擴展過程循環(huán)進行，直到新節(jié)點擴展到目標(biāo)區(qū)域或達到最大循環(huán)次數(shù)為止。最后，由目標(biāo)節(jié)點Ngoal反向回溯到根節(jié)點ninit，就可得到規(guī)劃路徑。

3.2 RRT的初始化

由于RRT搜索具有隨機性，因此在搜索前先用幾何分析法生成曲線控制線段和控制點，經(jīng)碰撞檢測后保留無碰撞的控制點與線段，用以初始化RRT，這樣可有效提高RRT算法的搜索效率。城市化道路具有結(jié)構(gòu)性，在此類環(huán)境下典型城市駕駛工況可分為直行、換道、轉(zhuǎn)向和掉頭[13]。本文以換道和轉(zhuǎn)向為例，介紹控制點和控制線段的獲得。

3.2.1 B樣條曲線

本文選擇B樣條曲線作為最終路徑的平滑曲線。B樣條曲線是樣條曲線的一種特殊表示形式，由(n+1)個控制點及參數(shù)節(jié)點向量Tn,k=(t≤tii+1的(k-1)確定次B樣條曲線為：

式中，Ni,k(t)為Tn,k上(k-1)次B樣條基函數(shù)，由deBoox-Cox遞推關(guān)系[14]確定：

B樣條曲線具有連續(xù)性、凸包性和局部性等優(yōu)點，曲線控制點個數(shù)和階數(shù)沒有必然聯(lián)系，不需要復(fù)雜的數(shù)值計算。另外，通過限定控制點連線間的夾角可將曲線的曲率維持在一定界限內(nèi)，如圖3所示。

圖3 曲線控制線段模型

控制點A1、O、B1決定的線段夾角的最小值αmin與曲率最大值以及控制點連線的最短線段長度Lmin滿足[11]：

只要α>αmin，即可滿足曲率要求。為使最終路徑較好地貼合控制線段，將控制線段的中點A2、B2也納入RRT的初始節(jié)點集。

3.2.2 換道工況

給定初始點和目標(biāo)點的位姿后，換道工況可以簡單抽象為車輛坐標(biāo)系中的3條控制線段，如圖4所示。

圖4 換道參數(shù)示意

假設(shè)3條線段長度分別為l1、l2和l3，總長度為L，經(jīng)圖4所示幾何分析得線段長度關(guān)系為：

式中，(x1,y1)、(x2,y2)、(x3,y3)分別為車輛坐標(biāo)系下線段交點X1、X2以及終點X3的坐標(biāo)。

借助優(yōu)化工具，以總路徑長度L最短為優(yōu)化目標(biāo)即可得到3段線段的長度，通過計算即可得7個控制點的坐標(biāo)。

3.2.3 轉(zhuǎn)向工況

為使曲線更接近于轉(zhuǎn)向工況時的圓弧曲線，設(shè)定控制線段長度及控制線段間的夾角都相同，如圖5所示。

圖5 轉(zhuǎn)向參數(shù)示意

假設(shè)從初始點位姿變換到目標(biāo)點位姿共需n條長為l的控制線段，相鄰控制線段間夾角均為α。根據(jù)邊界條件可得約束方程：

式中，(xg,yg,θg)為終點狀態(tài)；(xi,yi,θi)為各控制點的狀態(tài)。

根據(jù)式（7）即求解得到3個未知量n、l、α，控制點的坐標(biāo)即可求出。

3.3 采樣策略

隨機采樣直接關(guān)系到樹的擴展質(zhì)量和搜索效率，本節(jié)結(jié)合決策給出的駕駛行為設(shè)計偏向性采樣策略，引導(dǎo)RRT的擴展。

3.3.1 目標(biāo)偏向性采樣

采樣過程中概率分布函數(shù)最為重要。本文采用常見的高斯分布，便于根據(jù)已知條件進行偏向性采樣。高斯分布遵循的采樣策略為：

式中，(x0,y0)、(sx,sy)分別為參考采樣點和采樣點坐標(biāo)；(r,θ)為高斯分布決定的采樣相對值：

式中，(r0,θ0)為相對于(x0,y0)的補償參數(shù)，即高斯分布的均值；(σr,σθ)為對應(yīng)的高斯分布標(biāo)準(zhǔn)差；(rrand,θrand)為隨機變量。

因此可以通過高斯采樣將采樣點限制在一個環(huán)形區(qū)域內(nèi)，如圖6所示。

針對決策給出的駕駛行為和目標(biāo)點，結(jié)合高斯采樣的性質(zhì)，可以設(shè)置合適的(r0,θ0)和(σr,σθ)，引導(dǎo)搜索向著目標(biāo)點的方向進行。

圖6 高斯采樣點分布規(guī)律

3.3.2 強制擴展策略

所謂強制擴展，就是按照一定比例將目標(biāo)點和樹中距離目標(biāo)點最近的節(jié)點利用一定的方法連接，隨后結(jié)合碰撞檢測判斷路徑的安全性，并將路徑上有效部分插入到現(xiàn)有的樹中，從而提高搜索效率。因此本文利用ε貪婪算法，按照一定比例采用3.2節(jié)所述幾何分析法進行強制擴展。

偏向性采樣雖然加快了搜索進程，但同時也限制了RRT算法的探索能力，因此，本文按照一定比例采用普通采樣策略進行采樣，保證了算法在障礙物較密集的環(huán)境中的搜索能力。整體采樣策略如圖7所示，其中，r1、r2為隨機數(shù)。

圖7 采樣策略架構(gòu)

除了采樣策略，RRT的擴展步長d也直接影響了搜索速度。本文的擴展步長根據(jù)駕駛行為選擇：在換道時，根據(jù)初始點和目標(biāo)點的距離來選擇不同的擴展步長，如果距離較大，選用較大步長加快搜索，反之選用較小步長提高成功率；在轉(zhuǎn)向時，路徑的曲率變化較大，因此采用較小的擴展步長以保證最終搜索結(jié)果的質(zhì)量，防止采樣步長過大導(dǎo)致路徑的曲率不滿足要求。

3.4 最鄰近點搜索策略

利用采樣策略得到安全的采樣點后，需要在已有的樹中尋找“距離”采樣點最近的節(jié)點，然后將采樣點與其最鄰近點連接，確定安全后將采樣點插入樹中。因此，最鄰近點搜索關(guān)系到樹的生長方向，從而關(guān)系到最終路徑的質(zhì)量。本文考慮到研究對象為無人駕駛汽車，結(jié)合歐氏距離與車輛的轉(zhuǎn)向能力生成度量函數(shù)，如圖8所示。n1到nrand的歐式距離比n2到nrand的歐氏距離小，但n2到nrand朝向角的變化更小，得到的路徑將更平緩，因此，本文將采樣點到鄰近點之間的度量函數(shù)定義為：

式中，D、H分別為歐式距離和角度歸一化之后的值；w1=w2=0.5為對應(yīng)的權(quán)重，可以根據(jù)實際應(yīng)用進行設(shè)定。

圖8 度量基準(zhǔn)示意

距離和角度量綱不同，因此將這兩個變量進行歸一化處理，D、H分別為：

若將采樣點與樹中節(jié)點一一進行度量距離的比較，代價非常大，因此在計算度量距離前，先利用KD樹快速搜索歐式距離最近的幾個鄰近點作為最鄰近點的備選點，然后從備選點中選擇式（10）最小的點作為最鄰近點，有效提高最鄰近點的搜索效率。

3.5 碰撞檢測

通過環(huán)境地圖與車輛構(gòu)型做卷積校驗的碰撞檢測方法不受限于環(huán)境的復(fù)雜度，普適性較強，因此本文采用此方法進行碰撞檢測，將車輛構(gòu)型為若干半徑為r的圓形，如圖9所示。另外，為了保證安全，將根據(jù)實際車輛的尺寸加上安全閾值rerr（本文設(shè)為0.1 m）。判斷每個圓形在環(huán)境地圖中所占據(jù)的柵格是否安全，有1個圓形不安全則判定此檢測點不安全。

圖9 車輛構(gòu)型示意

圓形相關(guān)參數(shù)為：

在幾何分析部分需要對控制線段進行碰撞檢測，為了在保證安全的情況下盡可能少地檢測，將相鄰檢測點間的距離設(shè)置為車長，一旦線段上有檢測點與障礙物發(fā)生碰撞，就將此線段及后續(xù)線段從初始化邊界集中除去。在采樣部分需要對采樣點進行碰撞檢測，檢測結(jié)果安全的采樣點才被加入樹中。

3.6 應(yīng)對搜索未成功的情況

RRT算法具有概率完備性，因此一定存在搜索不成功的情況。采樣策略具有目標(biāo)偏向性，因此即使搜索沒有成功，也可將已搜到的路徑輸出。在之后的規(guī)劃周期中，不斷接收實時地圖檢測路徑安全性的同時，從上一次輸出的路徑終點出發(fā)搜索到目標(biāo)點的路徑。如果依然沒有搜索成功，則利用最鄰近點搜索策略，搜索已有樹中距離目標(biāo)點最近的樹節(jié)點作為前一段路徑的終點，逆向搜索得到這一段路徑的控制點。

3.7 RRT的修剪

由于RRT算法搜索具有隨機性，因此若直接根據(jù)回溯得到的RRT生成路徑，路徑必會非常曲折，很可能無法滿足車輛的最大曲率約束，路徑長度也會出現(xiàn)不必要的增加。可通過將樹中安全的節(jié)點刪去的方式對其進行剪裁。如圖10所示，若從n1→n2到n7→n8中間都是安全的，則可以將節(jié)點從n2到n7都省略，直接將n1與n8連接，同理，可將n8與n10連接。當(dāng)修剪后的樹在生成曲線時不滿足車輛的最大曲率約束時，可根據(jù)式（5）插入控制點。

圖10 RRT的修剪示意

4 仿真與試驗驗證

本文針對避障和轉(zhuǎn)向工況分別進行仿真和實車試驗，所涉及的車輛參數(shù)如表1所示。

表1 車輛參數(shù)

仿真與實車試驗中速度設(shè)定為滿足最大車速以及最大側(cè)向加速度約束的最大速度：

4.1 仿真驗證

為了證明FB-RRT算法的實時性和安全性，本文將只采用隨機采樣的基本RRT算法、RRT*算法以及同樣帶有導(dǎo)向性采樣的Goal-baising RRT算法作為對比算法，設(shè)Goal-biasing RRT算法中目標(biāo)狀態(tài)被擴展的比率為10%。整個算法用C++語言編寫，仿真驗證在處理器是2.60 GHz Intel?Core? i7-6500U，7.9 GB內(nèi)存的筆記本電腦上完成，分別仿真1 000次，考察算法的平均表現(xiàn)。

4.1.1 避障/換道工況

不同算法的對比結(jié)果如表2所示。由表2可知，F(xiàn)B-RRT算法在時間效率和成功率上都明顯優(yōu)于其余3種算法。由于FB-RRT算法會對最終生成的RRT進行修剪，因此在路徑長度上FB-RRT算法與其余3種算法相差不多，甚至更短。RRT*由于每次新加入節(jié)點時都要搜索最佳父節(jié)點，加入節(jié)點后還要對已有的RRT進行重新連接，因此耗時較長。另外，考慮到車輛轉(zhuǎn)向系統(tǒng)的限制，F(xiàn)B-RRT最終搜索到的樹都需要在滿足3.2節(jié)所述曲率約束的條件上生成路徑，因此成功生成滿足曲率約束的路徑概率高于其余3種算法。

表2 避障工況算法對比結(jié)果

避障/換道工況下，為了更快搜索到終點，將采樣區(qū)域固定在一個扇形區(qū)域內(nèi)，如圖11a所示。該區(qū)域的中心線是假設(shè)以起始點和目標(biāo)點為端點的線段，此處設(shè)擴展步長d=2 m，圖11b為采樣結(jié)果。

圖11 避障工況采樣示意

圖12所示為多障礙物的換道仿真結(jié)果。車輛前方12 m、距目標(biāo)點15 m處各有一靜止障礙物，車輛換道時須先躲避障礙物并回到目標(biāo)車道。由圖12a可見，路徑能夠保證安全約束；由圖12b可見路徑曲率連續(xù)，并保證在所限制的曲率范圍之內(nèi)。

圖12 避障工況仿真

4.1.2 轉(zhuǎn)向工況

轉(zhuǎn)向工況下，曲線的曲率和車輛航向角變化相對避障/換道工況更劇烈。表3所示為算法對比結(jié)果，可以看出，F(xiàn)B-RRT算法在成功率、算法效率和路徑長度上都要優(yōu)于另外3種算法。

表3 轉(zhuǎn)向工況算法對比結(jié)果

轉(zhuǎn)向工況下，根據(jù)循環(huán)采樣的次數(shù)，可將采樣過程分為3個階段，如圖13a所示：初始階段車輛需要進入路口，因此采樣點集中在細(xì)長的A區(qū)域，獲得充分采樣后，車輛從路口進入目標(biāo)車道，采樣點集中在扇形區(qū)域B內(nèi)，該區(qū)域的采樣參考點nmid是初始航向所確定直線和目標(biāo)航向所確定直線的交點，區(qū)域中心線是nmid和ngoal的連線。這樣的兩段采樣理論上能夠連接初始點和終止點。但如果環(huán)境中障礙物較多，RRT需向其他方向擴展以避開障礙物，因此一定比例地將范圍較廣的區(qū)域C作為采樣區(qū)域，該扇形的半徑為ninit到ngoal的距離，其中一個扇形邊界平行于ngoal的航向，另一個邊界和第1階段的扇形邊界重合。轉(zhuǎn)向工況下設(shè)置步長為1 m以避免曲率超出邊界約束。從圖13b的采樣效果可見，采樣點均出現(xiàn)在預(yù)設(shè)的采樣區(qū)域中，在第2段采樣區(qū)域末尾，采樣點幾乎可以擬合為一條曲線，可見強制擴展策略奏效。

圖13 轉(zhuǎn)向工況采樣示意

圖14所示為多障礙轉(zhuǎn)向工況仿真環(huán)境。起點(0,0)處左方2個車道為逆向車道，路徑起點處距離路口車道停止線3.5 m，同理，終點處下方2個車道也是逆向車道，設(shè)置終點處距離路口車道停止線2 m，路口處人行橫道寬為5 m。在起點處對面的反向直行車道上有一輛障礙車停在左上方，其尾部與目標(biāo)車道的一條車道線齊平。車輛在轉(zhuǎn)向時需要先避開障礙車，然后進入目標(biāo)車道。

圖14 轉(zhuǎn)向工況仿真

由仿真圖14a可見，路徑可以滿足安全約束。車輛在轉(zhuǎn)向條件不理想的情況下有可能將轉(zhuǎn)向盤轉(zhuǎn)至極限位置，因此圖14b中曲線的曲率雖然達到了車輛的最大曲率，但依然符合實際情況。

4.2 實車試驗驗證

試驗平臺為一輛改裝智能車，如圖15所示。通過控制2個前輪電機的力矩實現(xiàn)線控驅(qū)動，控制轉(zhuǎn)向系統(tǒng)的驅(qū)動電機力矩實現(xiàn)線控轉(zhuǎn)向，以及控制一套電子液壓制動系統(tǒng)實現(xiàn)線控制動。

圖15 無人駕駛試驗平臺

試驗平臺所配備的傳感器以及控制器見表4。

表4 試驗平臺相關(guān)配置

本文所有程序都在工控機中完成，不同模塊間借助輕量級通信與數(shù)據(jù)封送庫（Lightweight Communications and Marshalling，LCM）進行通信。環(huán)境感知模塊根據(jù)傳感器探測到的信號，實時建立一個車輛坐標(biāo)系下350格×350格的柵格環(huán)境地圖，柵格邊長為20 cm。得到的環(huán)境地圖被封裝在LCM通訊包中傳遞給運動規(guī)劃模塊。運動規(guī)劃模塊通過GPS獲得車輛位置進行規(guī)劃，并將規(guī)劃結(jié)果也封裝在LCM通訊包中傳遞給軌跡跟蹤模塊。

基于4.1節(jié)的仿真環(huán)境，構(gòu)造了圖16所示的實車試驗環(huán)境，避障工況下在道路兩側(cè)分別設(shè)置路障以構(gòu)造多障礙環(huán)境，轉(zhuǎn)向工況為車輛從雙車道路口左轉(zhuǎn)彎進入匝道。圖17所示為試驗結(jié)果，圖17a所示的避障工況中右側(cè)切換點為換道起點，可見路徑是安全的，并且期望路徑與實際路徑幾乎完全重合，效果較理想，圖17b中車輛跟蹤期望路徑效果不如避障工況，這是因為本文是基于阿克曼轉(zhuǎn)向模型進行規(guī)劃，所考慮的動力學(xué)特性是最簡單的穩(wěn)態(tài)特性，但實際上車輛的動力學(xué)特性非常復(fù)雜，因此在大曲率情況下跟蹤期望路徑存在偏移，但車輛依然能滿足功能需求，安全轉(zhuǎn)向。

圖16 試驗場景

圖17 實車跟蹤效果

5 結(jié)束語

運動規(guī)劃在無人駕駛研究問題中銜接著感知和控制，是使無人駕駛成為完整系統(tǒng)的關(guān)鍵技術(shù)。本文提出的FB-RRT規(guī)劃算法不僅利用了城市環(huán)境下結(jié)構(gòu)化道路的特點來加速算法，同時保留了RRT算法處理復(fù)雜環(huán)境的能力，也滿足了無人駕駛汽車的曲率連續(xù)有界約束。仿真和實車試驗同時表明，該算法能有效解決無人駕駛汽車在城市化道路下的復(fù)雜運動規(guī)劃問題。另外，本文暫未涉及對軌跡性能的評價，后續(xù)研究將對軌跡的避障性能、穩(wěn)定性、舒適性等方面進行評價研究。本文提出的算法目前致力于解決低速工況下的運動規(guī)劃問題，下一步將嘗試解決高速運動規(guī)劃問題，進一步提高算法的魯棒性和普適性，更好地滿足實際工程需要。