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

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

主題詞：無人駕駛汽車 運(yùn)動(dòng)規(guī)劃 偏向性采樣 鄰近點(diǎn)搜索

1 前言

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

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

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

2 車輛模型建立

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

圖1 車輛模型

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

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

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

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

3 快速偏向性RRT算法

3.1 基本RRT算法

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

圖2 RRT擴(kuò)展示意

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

3.2 RRT的初始化

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

3.2.1 B樣條曲線

本文選擇B樣條曲線作為最終路徑的平滑曲線。B樣條曲線是樣條曲線的一種特殊表示形式，由(n+1)個(gè)控制點(diǎn)及參數(shù)節(jié)點(diǎn)向量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)點(diǎn)，曲線控制點(diǎn)個(gè)數(shù)和階數(shù)沒有必然聯(lián)系，不需要復(fù)雜的數(shù)值計(jì)算。另外，通過限定控制點(diǎn)連線間的夾角可將曲線的曲率維持在一定界限內(nèi)，如圖3所示。

圖3 曲線控制線段模型

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

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

3.2.2 換道工況

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

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

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

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

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

3.2.3 轉(zhuǎn)向工況

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

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

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

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

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

3.3 采樣策略

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

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

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

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

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

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

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

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

3.3.2 強(qiáng)制擴(kuò)展策略

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

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

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

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

3.4 最鄰近點(diǎn)搜索策略

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

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

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

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

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

3.5 碰撞檢測(cè)

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

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

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

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

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

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

3.7 RRT的修剪

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

圖10 RRT的修剪示意

4 仿真與試驗(yàn)驗(yàn)證

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

表1 車輛參數(shù)

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

4.1 仿真驗(yàn)證

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

4.1.1 避障/換道工況

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

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

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

圖11 避障工況采樣示意

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

圖12 避障工況仿真

4.1.2 轉(zhuǎn)向工況

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

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

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

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

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

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

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

4.2 實(shí)車試驗(yàn)驗(yàn)證

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

圖15 無人駕駛試驗(yàn)平臺(tái)

試驗(yàn)平臺(tái)所配備的傳感器以及控制器見表4。

表4 試驗(yàn)平臺(tái)相關(guān)配置

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

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

圖16 試驗(yàn)場(chǎng)景

圖17 實(shí)車跟蹤效果

5 結(jié)束語

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