考慮路徑平滑性和避撞穩定性的智能汽車彎道軌跡規劃研究*

余嘉星，Arab Aliasghar，裴曉飛，過學迅

（1. 湖北省汽車零部件重點實驗室，武漢理工大學，武漢 430070；2. 羅格斯大學機械與航空航天工程系，新澤西，皮斯卡塔韋 08854 美國）

前言

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

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

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

針對現有文獻在處理彎道避撞問題上的不足，本文中提出了一種考慮規劃曲線平滑度和車輛穩定性的軌跡規劃方法。在路徑規劃中構建滿足車輛運動學模型的C2連續性曲線，同時利用并行計算提高RRT 計算效率。在速度規劃中考慮自車車速、側向加速度和規劃路徑的曲率，通過聯合速度梯形和PGVC 彌補了文獻［10］中PGVC 未考慮最高限速的問題。因此，本文首先給出了軌跡規劃的總體框架，然后分別具體介紹RRT 路徑規劃和速度規劃方法，最后通過仿真驗證所提出方法的有效性。

1 總體規劃框架

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

圖1 軌跡規劃方案

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

2 軌跡規劃

2.1 RRT路徑規劃

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

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

表1 改進的RRT

圖2 BestNearest神經網絡結構

2.2 梯形速度規劃

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

圖3 梯形速度曲線

式中：是當前加速度；是當前速度；是終端速度時刻；是終端加速度。

2.3 加速度決策

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

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

3 仿真測試

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

3.1 RRT性能驗證

本節將BestNearest 與常用的歐幾里得度量、RRT*在如圖4所示的環境中進行50次測試，對比其路徑的平滑程度和收斂速度。為評估是否發生碰撞，采用將車輛的形狀分解為多個連續的圓形，其圓形的半徑設置為0.95 m。

圖4 路徑平滑性比較

圖5 路徑曲率變化量

圖6 收斂速度

3.2 彎道穩定性測試

圖7 彎道超車及避障軌跡

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

圖8 路徑跟蹤和穩定性能

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

3.3 算法實時性驗證

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

RRT 算法的BestNearest 采用深度神經網絡，神經網絡調用的頻率隨著節點數量增加而上升。因本文采用神經網絡的層數和神經元個數較多，故BestNearest 會消耗較多的計算資源，需測試RRT 的實時性。圖9 是分別進行640 次采樣試驗的測試結果。圖9（a）是采用并行計算的更新頻率，更新頻率居中在6.25-7.25 Hz 之間；圖9（b）是未采用并行計算，更新頻率居中在0.8-1.6 Hz 之間，可看出并行計算大大提高了RRT算法的實時性。

圖9 RRT計算效率

4 結論

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