余嘉星，Arab Aliasghar，裴曉飛，過學(xué)迅

（1. 湖北省汽車零部件重點(diǎn)實(shí)驗(yàn)室，武漢理工大學(xué)，武漢 430070；2. 羅格斯大學(xué)機(jī)械與航空航天工程系，新澤西，皮斯卡塔韋 08854 美國）

前言

智能汽車通常采用包含感知、規(guī)劃和控制的分層架構(gòu)，其中規(guī)劃層在安全性和舒適性方面起著承上啟下的作用。當(dāng)前智能汽車已經(jīng)在低速、簡單道路的封閉場景下實(shí)現(xiàn)落地，但是在復(fù)雜的高速場景下，由于規(guī)劃問題導(dǎo)致的彎道行駛性能和類人性還有所欠缺。

通常軌跡規(guī)劃分為解耦的路徑規(guī)劃和速度規(guī)劃兩個(gè)部分，常用的路徑規(guī)劃方法包括曲線擬合法、圖搜索法、采樣搜索法、勢場法和基于優(yōu)化的方法等。其中，曲線擬合法僅能表現(xiàn)有限幾何的特征，缺少普適性。圖搜索法如A*和Dijkstra 在動(dòng)態(tài)環(huán)境中每個(gè)步長都要重新構(gòu)建地圖，從而造成過高的計(jì)算量。基于優(yōu)化的軌跡規(guī)劃法例如模型預(yù)測控制，必須將障礙物約束轉(zhuǎn)化為模型約束，較難處理多障礙物。采樣搜索法例如RRT，可快速搜索環(huán)境，構(gòu)建無碰撞的路徑，但路徑不光滑，難以保證計(jì)算實(shí)時(shí)性要求。文獻(xiàn)［5］中采用Dubin 曲線擴(kuò)展RRT 樹，再用三次樣條平滑Dubin 曲線，提高路徑平滑度。文獻(xiàn)［6］中采用RRT搜索粗糙路徑，再調(diào)用二次規(guī)劃（QP）數(shù)字優(yōu)化路徑，以達(dá)到車輛模型約束要求，但是往往耗時(shí)過長。文獻(xiàn)［7］中采用雙樹結(jié)構(gòu)，加快第二個(gè)RRT 構(gòu)建速度，返回C1 連續(xù)性的曲線，只能實(shí)現(xiàn)車輛低速避障。文獻(xiàn)［8］中通過駕駛行為引導(dǎo)、高斯采樣的方式，提高RRT 計(jì)算速度，返回曲率連續(xù)的路徑，但其速度規(guī)劃未考慮RRT 路徑的曲率，忽略了彎道曲率對車速的影響，因此無法保證智能汽車的高速過彎性能。

速度規(guī)劃對彎道行車和避撞性能也尤為重要。文獻(xiàn)［5］中采用路徑規(guī)劃和速度規(guī)劃解耦的方法，使用優(yōu)化的方法基于路徑求解速度規(guī)劃曲線，這樣更利于運(yùn)用不同的方法限制約束量。百度阿波羅（Apollo）的速度規(guī)劃分為兩步，首先通過動(dòng)態(tài)規(guī)劃得到粗糙的參考速度曲線，然后將縱向加速度及其導(dǎo)數(shù)作為二次規(guī)劃的代價(jià)函數(shù)光滑速度曲線，提高駕駛舒適性。文獻(xiàn)［8］和文獻(xiàn)［9］中設(shè)計(jì)了梯形速度規(guī)劃方法，分別考慮動(dòng)態(tài)障礙物的影響和車輛運(yùn)動(dòng)學(xué)要求。但無法保證車輛在復(fù)雜工況下的操縱穩(wěn)定性。而文獻(xiàn)［10］中的研究結(jié)果表明，在高速過彎時(shí)PGVC 能充分利用道路曲率和自車動(dòng)態(tài)信息，提高車輛對彎道的跟蹤能力。

針對現(xiàn)有文獻(xiàn)在處理彎道避撞問題上的不足，本文中提出了一種考慮規(guī)劃曲線平滑度和車輛穩(wěn)定性的軌跡規(guī)劃方法。在路徑規(guī)劃中構(gòu)建滿足車輛運(yùn)動(dòng)學(xué)模型的C2連續(xù)性曲線，同時(shí)利用并行計(jì)算提高RRT 計(jì)算效率。在速度規(guī)劃中考慮自車車速、側(cè)向加速度和規(guī)劃路徑的曲率，通過聯(lián)合速度梯形和PGVC 彌補(bǔ)了文獻(xiàn)［10］中PGVC 未考慮最高限速的問題。因此，本文首先給出了軌跡規(guī)劃的總體框架，然后分別具體介紹RRT 路徑規(guī)劃和速度規(guī)劃方法，最后通過仿真驗(yàn)證所提出方法的有效性。

1 總體規(guī)劃框架

如圖1 所示，軌跡規(guī)劃方案采用了縱橫向解耦的軌跡規(guī)劃方式，從而更加靈活地給出速度規(guī)劃的限制條件。全局路徑不考慮是否有障礙物在車道內(nèi)，并將其作為參考路徑。局部路徑規(guī)劃RRT 在參考路徑的車道和旁車道內(nèi)采樣，采樣點(diǎn)與預(yù)估代價(jià)值最小的節(jié)點(diǎn)連接。然后在節(jié)點(diǎn)附近尋找是否有代價(jià)值更小的節(jié)點(diǎn)，優(yōu)化預(yù)估代價(jià)值最小節(jié)點(diǎn)的路徑，最后輸出包括坐標(biāo)點(diǎn)位置、橫擺角、曲率和精確的路徑長度的螺旋線路徑。原始的速度規(guī)劃采用路徑上節(jié)點(diǎn)之間的線段距離總和，估計(jì)梯形速度規(guī)劃的加速和減速時(shí)長，而通過精確的路徑長度可得到精確的加速和減速時(shí)長。速度規(guī)劃首先判斷車輛應(yīng)處于的加速、減速或勻速狀態(tài)，生成速度梯形。然后用三次多項(xiàng)式優(yōu)化速度梯形得到連續(xù)的縱向加速度曲線，最后利用速度決策融合速度梯形和PGVC 的加速度，輸出期望縱向加速度。

圖1 軌跡規(guī)劃方案

軌跡跟蹤作為下層控制器，由基于預(yù)瞄線性模型的模型預(yù)測控制器和加速度跟隨控制器組成。前者將規(guī)劃的路徑和自車狀態(tài)轉(zhuǎn)化為跟蹤誤差，基于模型預(yù)測控制將此誤差作為初始狀態(tài)量構(gòu)建二次規(guī)劃，并把最佳控制序列的第一個(gè)值作為前輪轉(zhuǎn)角控制量輸出。而加速度跟隨器預(yù)先定義了當(dāng)前速度與期望加速度的關(guān)系，判斷此時(shí)應(yīng)給出制動(dòng)壓力還是節(jié)氣門開度，在根據(jù)PID 反查表得出自車實(shí)際的制動(dòng)壓力或者節(jié)氣門開度。

2 軌跡規(guī)劃

2.1 RRT路徑規(guī)劃

本文從車輛模型約束和計(jì)算實(shí)時(shí)性兩個(gè)方面對傳統(tǒng)RRT 加以改進(jìn)。文獻(xiàn)［12］中指出只要規(guī)劃曲線的曲率和曲率變化率低于門限值，曲線光滑度可滿足車輛運(yùn)動(dòng)學(xué)模型。因此，本文采用G插值法生成具有C2連續(xù)性曲線，再通過限制其曲率和曲率變化率使得所構(gòu)建的路徑符合車輛運(yùn)動(dòng)學(xué)模型。此外，采用并行計(jì)算提高RRT 的計(jì)算速度，具體在3.3節(jié)中討論。

改進(jìn)的RRT 算法偽代碼算法如表1 所示。首先，根據(jù)車輛位置和航向角對RRT 樹初始化，并將初始化為False，表示還未找到可行解。Sample() 是采用探索和優(yōu)化的方式在參考路徑附近采樣，為False 時(shí)采用預(yù)定義的路徑，為True 時(shí)選擇可行解路徑或者預(yù)定義的路徑作為參考路徑。BestNearest采用深度神經(jīng)網(wǎng)絡(luò)作為度量函數(shù)近似兩節(jié)點(diǎn)之間的曲率變化量，如圖2 所示。Steer 使用G插值法生成螺旋線，返回曲率、曲率變化率和螺旋線的長度，并將從根節(jié)點(diǎn)至x的曲率變化量標(biāo)記為。如果曲線沒有與障礙物碰撞，且滿足曲率和曲率變化量要求，則在附近搜索每個(gè)節(jié)點(diǎn)，記錄下其曲率變化量。通過比較各節(jié)點(diǎn)曲率變化量和，找到最小的曲率變化量節(jié)點(diǎn)，輸出對應(yīng)曲線的中間節(jié)點(diǎn)。若有中間節(jié)點(diǎn)位于目標(biāo)區(qū)域，則將改為True。

表1 改進(jìn)的RRT

圖2 BestNearest神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)

2.2 梯形速度規(guī)劃

在改進(jìn)的RRT 獲得無碰撞的目標(biāo)路徑基礎(chǔ)上，通過速度規(guī)劃進(jìn)一步構(gòu)建梯形速度曲線。根據(jù)RRT的規(guī)劃路徑長度和最高限速，確定梯形速度規(guī)劃的加速和減速時(shí)長，此時(shí)可構(gòu)建原始的速度曲線。由于在分段線組成的速度梯形中車輛加速度不連續(xù)，故為了得到連續(xù)的加速度曲線，基于三次多項(xiàng)式對梯形速度進(jìn)行平滑處理。輸出連續(xù)的加速度和速度曲線如式（1）所示，圖3中對比光滑前后的梯形速度曲線。

圖3 梯形速度曲線

式中：是當(dāng)前加速度；是當(dāng)前速度；是終端速度時(shí)刻；是終端加速度。

2.3 加速度決策

上述梯形速度曲線只考慮了RRT 的路徑長度、當(dāng)前車輛速度和加速度，忽略了速度對路徑曲率的敏感程度，故未考慮車輛過彎時(shí)的操縱性能。PGVC針對不同曲率和曲率變化率，可動(dòng)態(tài)調(diào)整相應(yīng)的加/減速度，還可以通過速度變化率，動(dòng)態(tài)調(diào)整相應(yīng)的加/減速度，以維持車輛的過彎操縱性能。PGVC由速度預(yù)瞄跟蹤器和GVC 組成，速度預(yù)瞄跟蹤器基于預(yù)瞄點(diǎn)處曲率變化率判斷出車輛的縱向加/減速度，如式（2）所示，GVC 基于車輛側(cè)向加速度判斷車輛的縱向加/減速度。為了除去因轉(zhuǎn)向盤轉(zhuǎn)角抖動(dòng)造成的˙抖動(dòng)，式（3）還定義一個(gè)開啟GVC 的閾值。最終由PGVC 輸出的加速度是兩個(gè)部分疊加而成，即=+。式中：是速度預(yù)瞄跟蹤器增益；是預(yù)瞄時(shí)間；˙是預(yù)瞄點(diǎn)處曲率變化率；a是車輛側(cè)向加速度；˙是側(cè)向加速度導(dǎo)數(shù)Jerk；C是GVC增益。

由PGVC 公式可知，是目標(biāo)加速度，而式（1）也可產(chǎn)生理想的加速度曲線，兩者在預(yù)瞄點(diǎn)曲率變化處會(huì)產(chǎn)生不同的期望加速度，故需要加速度控制邏輯給出唯一的期望加速度。加速度控制邏輯希望車輛安全行駛的同時(shí)保持速度優(yōu)勢。本文采用的加速度控制邏輯為：若PGVC 的減速度大于速度規(guī)劃的減速度，或者PGVC 加速度大于速度規(guī)劃的加速度且縱向車速小于最高車速時(shí)，將PGVC 的加速度作為最終的期望加速度，否則輸出速度規(guī)劃的加速度作為最終的期望加速度。梯形速度曲線和PGVC均能產(chǎn)生平滑的縱向加速度，因此線性加權(quán)后的期望加速度仍可保證平滑性和連續(xù)性。

3 仿真測試

本節(jié)不僅給出在S 彎道中智能汽車的軌跡規(guī)劃效果，同時(shí)驗(yàn)證RRT 算法的實(shí)時(shí)性。本文中采用ROS、SIMULINK 和CARSIM 聯(lián)合仿真的形式，臺(tái)式機(jī)的配置是12 線程Intel i7-8700。RRT 路徑規(guī)劃和速度規(guī)劃的控制頻率均設(shè)定為10 Hz，而PGVC 和軌跡跟蹤控制頻率設(shè)為100 Hz。

3.1 RRT性能驗(yàn)證

本節(jié)將BestNearest 與常用的歐幾里得度量、RRT*在如圖4所示的環(huán)境中進(jìn)行50次測試，對比其路徑的平滑程度和收斂速度。為評(píng)估是否發(fā)生碰撞，采用將車輛的形狀分解為多個(gè)連續(xù)的圓形，其圓形的半徑設(shè)置為0.95 m。

圖4 路徑平滑性比較

圖5 路徑曲率變化量

圖6 收斂速度

3.2 彎道穩(wěn)定性測試

圖7 彎道超車及避障軌跡

圖7（f）展示了智能汽車彎道超車和避撞的全部軌跡，現(xiàn)分析本文軌跡規(guī)劃方法的跟蹤誤差和速度優(yōu)勢。因彎道和換道最高限速是18 m/s，因此在0-2 s 內(nèi)車速都有所下降，PGVC 介入使得車速下降至17 m/s，如圖8（b）所示。而從圖8（a）和圖8（c）可看出，缺少PGVC 方案在自車第一次換道時(shí)（0-2 s）前輪已達(dá)到其最大轉(zhuǎn)角17°，但側(cè)向偏差依然達(dá)到0.9 m，缺少PGVC 方案的質(zhì)心側(cè)偏角達(dá)到10.9°。相比而言，本文的規(guī)劃方法側(cè)向偏差只有0.5 m，質(zhì)心側(cè)偏角只有8°，分別降低了80%和36.2%。

圖8 路徑跟蹤和穩(wěn)定性能

進(jìn)入S 彎的第一個(gè)彎道時(shí)（3-5 s），由于PGVC輸出減速度與梯形速度規(guī)劃對比后，采用PGVC 減速度，故車速會(huì)繼續(xù)下降。本文的規(guī)劃方法路徑跟蹤誤差相對較小，前輪轉(zhuǎn)角穩(wěn)定在4°左右，而非PGVC 控制的前輪轉(zhuǎn)角接近于最大轉(zhuǎn)角。而在車輛出彎時(shí)（6-10 s），由于速度規(guī)劃和PGVC 聯(lián)合作用，使得車輛加速至20.6 m/s 以較大的加速度出彎，從而獲得速度優(yōu)勢。車輛在S 彎的第二個(gè)彎道時(shí)（12-15 s），本文規(guī)劃方法的前輪轉(zhuǎn)角穩(wěn)定在4°左右，側(cè)向偏差保持在0.1 m內(nèi)，均遠(yuǎn)小于非PGVC控制的方案。因此，所提出的規(guī)劃方法可維持智能汽車以較高的車速過彎，同時(shí)路徑跟蹤性能和操縱性能更好。

3.3 算法實(shí)時(shí)性驗(yàn)證

本文中的路徑規(guī)劃RRT基于C++和ROS系統(tǒng)實(shí)現(xiàn)。RRT首次訂閱自車的姿態(tài)和周圍車輛的姿態(tài)信息，并輸出以自車為起點(diǎn)的路徑。在下一次訂閱時(shí)，將路徑的第二個(gè)節(jié)點(diǎn)定為根節(jié)點(diǎn)，保留第一個(gè)節(jié)點(diǎn)至根節(jié)點(diǎn)的路徑。若自車行駛過根節(jié)點(diǎn)，則刪除第一個(gè)節(jié)點(diǎn)，第三個(gè)節(jié)點(diǎn)定為根節(jié)點(diǎn)，保留第二個(gè)節(jié)點(diǎn)至根節(jié)點(diǎn)的路徑，以此反復(fù)循環(huán)。 RRT 的BestNearest 采用Open MP 并行計(jì)算，Open MP 是由#paragma 組成控制多線程執(zhí)行程序，每次采樣后BestNearest需要搜尋RRT樹上的所有節(jié)點(diǎn)。由于各個(gè)節(jié)點(diǎn)之間沒有數(shù)據(jù)依賴，Open MP 的For循環(huán)更適合運(yùn)用于大量神經(jīng)網(wǎng)絡(luò)的計(jì)算。此外，速度規(guī)劃也需要RRT 的路徑信息，因此兩者寫入同一個(gè)節(jié)點(diǎn)內(nèi)，并確定最終的期望加速度。

RRT 算法的BestNearest 采用深度神經(jīng)網(wǎng)絡(luò)，神經(jīng)網(wǎng)絡(luò)調(diào)用的頻率隨著節(jié)點(diǎn)數(shù)量增加而上升。因本文采用神經(jīng)網(wǎng)絡(luò)的層數(shù)和神經(jīng)元個(gè)數(shù)較多，故BestNearest 會(huì)消耗較多的計(jì)算資源，需測試RRT 的實(shí)時(shí)性。圖9 是分別進(jìn)行640 次采樣試驗(yàn)的測試結(jié)果。圖9（a）是采用并行計(jì)算的更新頻率，更新頻率居中在6.25-7.25 Hz 之間；圖9（b）是未采用并行計(jì)算，更新頻率居中在0.8-1.6 Hz 之間，可看出并行計(jì)算大大提高了RRT算法的實(shí)時(shí)性。

圖9 RRT計(jì)算效率

4 結(jié)論

本文提出一種智能汽車的軌跡規(guī)劃方法，可實(shí)現(xiàn)車輛穩(wěn)定、安全、快速地超過前車或避開障礙物。RRT 以犧牲少量最優(yōu)解為代價(jià)，換取了快速的收斂速度，且保證了車輛路徑的平滑程度。同時(shí)，引入并行計(jì)算提高了RRT 計(jì)算速度，解決了RRT 實(shí)時(shí)性問題。在有曲率不連續(xù)的S 彎道路上，RRT 返回曲率連續(xù)的路徑，便于車輛控制。速度規(guī)劃與PGVC 聯(lián)合控制將路徑的曲率考慮在內(nèi)，輸出的期望加速度更加符合過彎和換道要求，提高車輛過彎的操縱性能。未來考慮引入基于機(jī)器學(xué)習(xí)的決策，適用于更加復(fù)雜的道路環(huán)境。