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

侯寶龍，龔柯陽，史志飛

（長安大學(xué) 汽車學(xué)院，陜西 西安 710064）

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

1 引入Frenet坐標(biāo)系

1.1 笛卡爾坐標(biāo)系的不足

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

圖1 笛卡爾坐標(biāo)系

1.2 轉(zhuǎn)換為Frenet坐標(biāo)系

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

圖2 Frenet坐標(biāo)系

本文后續(xù)的研究均基于Frenet坐標(biāo)系下進(jìn)行，此坐標(biāo)系相關(guān)參數(shù)如表1所示。

表1 Frenet坐標(biāo)系各參數(shù)含義

2 車輛運(yùn)動學(xué)模型建立

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

圖3 車輛運(yùn)動學(xué)模型

由此可建立車輛運(yùn)動學(xué)方程為

3 彎道換道軌跡規(guī)劃

3.1 軌跡規(guī)劃背景條件

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

3.2 橫向軌跡規(guī)劃

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

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

圖4 換道軌跡曲率對比

3.2.2 準(zhǔn)均勻三次B樣條曲線

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

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

圖5 準(zhǔn)均勻三次B樣條換道軌跡

3.3 縱向軌跡規(guī)劃

3.3.1 動態(tài)規(guī)劃初步得到速度序列

縱向規(guī)劃的本質(zhì)是橫向規(guī)劃得到的軌跡基礎(chǔ)上加入時間序列，計(jì)算出行駛到每一個軌跡點(diǎn)的包含時間的速度序列?？v向規(guī)劃基于圖，如圖6所示，即為Frenet坐標(biāo)系下的縱坐標(biāo)，代表時間。

圖6 s-t圖

圖中黑色區(qū)域代表了其他動態(tài)交通車在本車行駛軌跡上的占用，若將此區(qū)域視為搜索禁區(qū)，可以將縱向的最優(yōu)速度序列求解問題轉(zhuǎn)化為求解多階段決策的動態(tài)規(guī)劃問題。

3.3.2 二次規(guī)劃平滑速度序列

由動態(tài)規(guī)劃初步得到的速度序列，曲線不連續(xù)不可導(dǎo)，車輛按照此速度序列行駛會引起人極大的不適感。故利用二次規(guī)劃思想對其進(jìn)行優(yōu)化，定義平滑后的每段曲線由五次多項(xiàng)式曲線擬合，構(gòu)造如式（3）所示：

二次規(guī)劃問題代價函數(shù)構(gòu)造如式（4）所示：

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

圖7 二次規(guī)劃后平滑曲線

4 軌跡跟蹤控制

MPC整體分為三個部分：模型、預(yù)測和控制。本文通過車輛運(yùn)動學(xué)來建立模型部分，將其轉(zhuǎn)化為線型狀態(tài)空間方程。預(yù)測部分是根據(jù)狀態(tài)空間方程模型，順序遞推從而獲得狀態(tài)量。控制部分則是在每一時刻構(gòu)建最優(yōu)控制量，使得目標(biāo)函數(shù)最優(yōu)，即實(shí)現(xiàn)最優(yōu)的跟蹤控制。

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

圖8 軌跡跟蹤圖

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

圖9 橫向誤差圖

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

圖10 速度誤差圖

5 總結(jié)與不足

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