自動駕駛車輛彎道處換道軌跡規劃與跟蹤控制

侯寶龍，龔柯陽，史志飛

（長安大學 汽車學院，陜西 西安 710064）

近年來關于自動駕駛汽車的規劃與控制的研究成為實現自動駕駛的核心點之一。規劃模塊從整體上分為全局的導航規劃與局部的路徑規劃，局部的路徑規劃也稱為局部的軌跡規劃。局部軌跡規劃多采用直接構造法，例如直線圓弧段構造、多項式曲線法、樣條曲線插值法等。軌跡的跟蹤控制階段便是控制車輛沿著路徑規劃層的路徑行駛，同時在保證操縱安全和平穩的前提下盡量減少跟蹤偏差。常用的跟蹤方法有純跟蹤法、Stanley法、比例-積分-微分法、模型預測控制（Model Predictive Control, MPC）法。彎道路況相比直線道路的規劃更加復雜，不僅需要軌跡的曲率連續性來保證傳至人體的加速度變化，從而保證舒適性，還需要考慮換道過程中的動態障礙物，從而確保安全性。本文從智能車輛的規劃與跟蹤控制算法出發，重點研究彎道處變道過程的軌跡規劃與軌跡跟蹤控制。

1 引入Frenet坐標系

1.1 笛卡爾坐標系的不足

圖1為笛卡爾坐標系，車輛在彎道處的行駛路徑如圖1右側所示。不難發現，普通的笛卡爾坐標系不易描述曲線道路，對于彎道上行駛的車輛更難以確定道路邊界與車輛的位置，更不便于規劃換道軌跡。

圖1 笛卡爾坐標系

1.2 轉換為Frenet坐標系

圖2展示了車輛在Frenet坐標系下彎道處的行駛路徑。它由軸和軸構成，軸沿著道路前進方向，也被稱為縱向位移，向前為正值；軸垂直于道路前進方向，稱為橫向位移，向左為正值。李萌提到在規劃問題中可以將笛卡爾坐標系轉換為Frenet坐標系，可以準確定位車輛在道路上的坐標與坐標，便于將橫向的規劃與縱向的規劃問題解耦，使得復雜的三維規劃問題易于解決。

圖2 Frenet坐標系

本文后續的研究均基于Frenet坐標系下進行，此坐標系相關參數如表1所示。

表1 Frenet坐標系各參數含義

2 車輛運動學模型建立

前文提到的Frenet坐標系簡化了彎道換道規劃的復雜度，而在后文中為實現對規劃軌跡的跟蹤，需要對車輛的運動過程在笛卡爾坐標系下建模。如圖3所示建立運動學模型。圖中是車輛的縱向速度，為前輪的轉向角，為前后軸軸距，為車輛航向角。其基于以下假設：（1）不考慮汽車的垂直運動，即只有平面的運動；（2）假設左右輪胎相同，即在任意時刻都擁有相同的輪速和轉角，可以合并為一個輪胎；（3）整個車身和懸架都視為剛性模型；（4）車輛的運動和轉向都是由前輪驅動和轉向。

圖3 車輛運動學模型

由此可建立車輛運動學方程為

3 彎道換道軌跡規劃

3.1 軌跡規劃背景條件

局部的軌跡規劃與普通的路徑規劃不同，它是利用一系列帶有時間序列的路徑點生成最終的行駛軌跡。吳樹凡等人提出對于彎道路段，傳統的多項式曲線構造軌跡存在數值計算復雜，難以保證實時性，且不易考慮動態障礙物等困難。故本文采用將彎道處換道的軌跡解耦為橫向的軌跡規劃與縱向的速度規劃。橫向的規劃輸出軌跡曲線，決定行駛軌跡的形狀，縱向規劃輸出平滑的速度序列，同時應考慮動態障礙物。

3.2 橫向軌跡規劃

3.2.1 四種橫向換道軌跡曲線對比

圖4對比了彎道的四種換道軌跡規劃方法，對于彎道路段的軌跡主要應考慮曲線曲率的連續性。Dubins曲線的軌跡曲率出現了兩次跳變，不滿足行駛標準。sin正弦函數曲線和bessel曲線雖然曲率連續，但曲率在開始換道時和換道結束后不為0，這將導致車輛換道結束后方向盤不能復位，會繼續偏離車道行駛，故這兩種換道軌跡也不滿足行駛標準。三次B樣條曲線不但保證曲率連續，還解決了以上幾種曲線存在的缺點。

圖4 換道軌跡曲率對比

3.2.2 準均勻三次B樣條曲線

B樣條曲線是所有B-樣條基函數的線性組合。定義階B樣條函數為

式中，B()是第個階B樣條基函數，與控制點P相對應。三次（四階）B樣條曲線可由6個控制點生成，故式中為5，為4。上文已經提到三次B樣條曲線能保證曲率連續，而準均勻三次B樣條函數不但所有基函數都是相同的形式，而且函數兩端具有重復的性質，這降低了計算的復雜度。所以選取其進行在彎道處橫向規劃軌跡，結果如圖5所示。

圖5 準均勻三次B樣條換道軌跡

3.3 縱向軌跡規劃

3.3.1 動態規劃初步得到速度序列

縱向規劃的本質是橫向規劃得到的軌跡基礎上加入時間序列，計算出行駛到每一個軌跡點的包含時間的速度序列?？v向規劃基于圖，如圖6所示，即為Frenet坐標系下的縱坐標，代表時間。

圖6 s-t圖

圖中黑色區域代表了其他動態交通車在本車行駛軌跡上的占用，若將此區域視為搜索禁區，可以將縱向的最優速度序列求解問題轉化為求解多階段決策的動態規劃問題。

3.3.2 二次規劃平滑速度序列

由動態規劃初步得到的速度序列，曲線不連續不可導，車輛按照此速度序列行駛會引起人極大的不適感。故利用二次規劃思想對其進行優化，定義平滑后的每段曲線由五次多項式曲線擬合，構造如式（3）所示：

二次規劃問題代價函數構造如式（4）所示：

其中，、為兩個優化目標的權重比例。f代表規劃軌跡的加速度，優化后應使其盡可能小。s-代表二次優化后軌跡與原動態規劃軌跡的誤差，優化目的是誤差應盡可能小。最終二次規劃平滑后速度序列曲線如圖7所示，可看出規劃曲線平滑并且可以躲避動態障礙物。

圖7 二次規劃后平滑曲線

4 軌跡跟蹤控制

MPC整體分為三個部分：模型、預測和控制。本文通過車輛運動學來建立模型部分，將其轉化為線型狀態空間方程。預測部分是根據狀態空間方程模型，順序遞推從而獲得狀態量。控制部分則是在每一時刻構建最優控制量，使得目標函數最優，即實現最優的跟蹤控制。

圖8為利用MPC對彎道處換道軌跡的跟蹤圖，可以看到跟蹤效果良好。

圖8 軌跡跟蹤圖

圖9為軌跡跟蹤過程的橫向誤差圖，可知整個彎道換道過程的橫向誤差控制在±0.04 m之內。

圖9 橫向誤差圖

圖10為軌跡跟蹤過程中的速度誤差圖，由圖可知實際軌跡的行駛速度略高于規劃軌跡的速度，誤差在+0.2 m/s之內。但前文在縱向速度規劃 過程中已經避讓了動態障礙物，故確保了行駛的安全性。

圖10 速度誤差圖

5 總結與不足

本研究基于Frenet坐標系，重點研究了彎道處變道的軌跡規劃與跟蹤控制的問題。所提出的橫縱向解耦的軌跡規劃方法能規劃出滿足曲率連續，安全避開動態障礙物的換道路徑曲線。模型預測控制能精準跟蹤規劃路徑，并有良好的魯棒性。不足在于采用了簡單的車輛運動學模型，但實際的換道過程中車速一般很高，應考慮車輛動力學模型更符合真實情況。