劉思超,楊鐵梅
(太原科技大學(xué) 電子信息工程學(xué)院,太原 030024)
智能車輛可以自動完成一些基本的駕駛操作行為,從而可以避免人為因素引起的交通事故,不僅交通事故率可以大大降低,而且道路擁堵問題也能得到改善[1]。并道行為會影響交通通行效率和道路安全,研究智能車輛的并道行為有重要意義。
軌跡規(guī)劃模塊主要是智能車根據(jù)車輛所處的環(huán)境、上層決策任務(wù)與自身所處位置,在滿足運動學(xué)約束與安全性條件下,規(guī)劃出一段時間內(nèi)車輛期望的運動軌跡,包括行駛速度、方向和路徑等[2]。王趙輝等[3]分析了目標車道上是否有行駛的車輛,在無車換道工況下選取梯形加速度換道軌跡,有車工況下則選擇五次多項式換道軌跡。張新鋒等[4]使用貝塞爾曲線作為參考軌跡,利用遺傳算法實現(xiàn)了最優(yōu)避障路徑規(guī)劃。潘兵宏等[5]通過對高速出口車輛換道軌跡數(shù)據(jù)的分析,使用雙曲正切函數(shù)擬合出換道軌跡。范柄堯等[6]利用人工勢場與差分進化算法相結(jié)合的方法,實現(xiàn)了移動機器人的軌跡規(guī)劃問題。使用五次多項式作為并道軌跡平滑且連續(xù),不存在曲率突變等問題,而且只需要已知智能車輛起點和終點狀態(tài),能夠直接有效的規(guī)劃出并道軌跡。
路徑跟蹤是指智能汽車控制器對規(guī)劃出的參考軌跡跟蹤行駛的能力。控制器的設(shè)計大多與汽車模型有關(guān)。跟蹤控制目前較為常用的算法有比例積分微分控制算法,滑??刂扑惴ǎ琇QR控制算法,MPC算法等。李偉等[7]采用滑??刂品椒▽崿F(xiàn)了無人駕駛汽車路徑跟蹤控制。尹曉麗等[8]基于橫向位置和參考軌跡的橫擺角誤差設(shè)計模糊控制器,實現(xiàn)了無人駕駛車輛的軌跡跟蹤控制。季杰等[9]將車輛動力學(xué)模型和運動學(xué)模型相結(jié)合,設(shè)計多約束模型預(yù)測控制器實現(xiàn)無人駕駛汽車路徑跟蹤控制。模型預(yù)測方法對外界干擾和模型不確定性具有較強的魯棒性,但目前模型預(yù)測方法設(shè)計的預(yù)測時域與控制時域大多是基于經(jīng)驗選取,在參數(shù)選取過程中,還需要考慮系統(tǒng)對參考軌跡的跟蹤能力。
本文建立最優(yōu)五次多項式并道軌跡,提出基于橫擺角變化的自適應(yīng)模型預(yù)測控制器,并證明算法的有效性。
智能車輛本身是一個非常復(fù)雜的系統(tǒng),為使簡化模型更加準確、方便的描述車輛運動狀態(tài),對車輛本身作如下假設(shè):
(1)車輛在平坦無傾角的水平路面上行駛;
(2)忽略車體本身的振動及內(nèi)部零件的相對運動;
(3)只考慮輪胎純側(cè)偏特性,并且工作在線性區(qū);
(4)橫向運動只由前輪轉(zhuǎn)向引起,且縱向速度不變。
為了滿足輪胎側(cè)偏特性滿足在線性區(qū)工作,對前輪偏角作小角度假設(shè)。車輛二自由度動力學(xué)模型如圖1所示。其中oxy為車輛坐標系,OXY為地面慣性坐標系。
圖1 智能車輛二自由度動力學(xué)模型Fig.1 Two-degree-of-freedom dynamic model of intelligent vehicle
對車輛模型進行受力分析,結(jié)合偏角小角度假設(shè)條件及車輛坐標系與地面慣性坐標系之間的關(guān)系,可以得到簡化后的二自由度前輪小偏角假設(shè)智能車輛動力學(xué)模型如式(1)所示。
(1)
智能車輛的并道行為就是沿著一條已經(jīng)規(guī)劃好的軌跡穩(wěn)定安全的從本車道駛向目標車道。為了保證車輛并道過程的穩(wěn)定性,一般性的認為車輛以恒定速度完成并道過程。
在軌跡規(guī)劃模塊中,首先根據(jù)本車道與目標車道車輛速度,預(yù)測一段時間內(nèi)前方與目標車道的車距,計算安全距離,保證并道過程中無碰撞,這是安全并道的前提;其次,規(guī)劃出合適的軌跡,不僅要符合車輛速度,角速度,前輪偏角等運動學(xué)約束,還要考慮質(zhì)心側(cè)偏角,輪胎側(cè)偏角,橫向加速度等動力學(xué)的約束,保證車輛的穩(wěn)定性;最后,根據(jù)并道類型,安全距離模型及駕駛員模型,動態(tài)調(diào)整并道時間,盡量減少交通干擾的同時,保證駕駛者與乘車人的舒適性。
并道軌跡選取五次多項式,保證車輛平順行駛,不會發(fā)生突然轉(zhuǎn)向。在慣性坐標系下建立五次多項式:
y=a0+a1x1+a2x2+a3x3+a4x4+a5x5
(2)
假設(shè)已知車輛變道起始點坐標A(XA,YA)與終點坐標B(XB,YB).根據(jù)車輛運動學(xué)模型,有
(3)
在起點和終點,車輛的縱向速度與縱向加速度均為0.
y′|(XA,YA)=0,y′|(XB,YB)=0
y″|(XA,YA)=0,y″|(XB,YB)=0
(4)
根據(jù)上述條件,可以得到一條完整的五次多項式軌跡。如圖2可以看出,加速度、曲率連續(xù)無突變,軌跡平滑,適合作為智能車輛的并道參考軌跡。
圖2 五次多項式軌跡圖(左)及曲率圖(右)Fig.2 Fifth degree polynomial trajectory graph(left)and curvature graph(right)
考慮在并道過程中,智能車輛始終能夠以恒定的速度沿著期望軌跡完成并道行為。在本文中,智能車輛作勻速曲線運動,在每一個參考點上,車輛的縱向速度恒定。車輛的橫向運動只由前輪方向的決定。
在并道過程中,橫坐標X是與時間t成正比關(guān)系,即:
X=vreft
(5)
式(5)中,vref為慣性坐標系下的縱向參考速度。本文假設(shè)vref=(1-0.05)vx.
假設(shè)高速匝道入口處,小車P準備并道,前方大卡車Q與后方小轎車O保持勻速行駛,車P并道過程中不會影響主道車輛的正常行駛,智能車輛向左并道示意圖如圖3所示。
圖3 智能車并道示意圖Fig.3 Schematic diagram of intelligent vehicle merging
考慮本車P與后車O在并道終點處的最小安全距離分別為:
DPMRS=0.3vQ-0.5vP
DOMRS=0.3vP-0.5vO
(6)
根據(jù)相對距離公式建立安全性代價函數(shù):
(7)
式(7)中,系數(shù)k由車輛的相對速度與車型有關(guān)及并道類型相關(guān)。若智能車輛向左并道,后車為小型車輛,則k=0.7,保證與后車的相對安全距離。若智能車輛向右并道,前車為大型車輛,則k=0.4,要增大與前車的間距,防止并道終點前車急剎車的情況發(fā)生。
同時,人體的舒適度用j來定義,表示加速度的變化率,用如下公式表示:
(8)
智能車輛的橫向加速度與曲率之間的關(guān)系為:
(9)
五次多項式任一點曲率可由式(10)表示:
(10)
智能車輛作勻速曲線運動,只有法向加速度,由式(9)可知,曲率越大,橫向加速度越小,人體的舒適感越好。因此,要保證乘車人的舒適程度,約束函數(shù)還需要與j相關(guān)。
舒適度代價函數(shù)由式(11)表示。
(11)
式(11)中,μ為舒適度系數(shù),ρmax為最大曲率,Δρmax為最大曲率變化率。
同時,考慮車輛并道時間對并道效率的影響,建立并道效率代價函數(shù):
(12)
根據(jù)式(7)、(11)、(12),建立最優(yōu)目標函數(shù):
(13)
式(13)中,λi為權(quán)重系數(shù),且λ1+λ2+λ3=1.目標函數(shù)同時考慮了智能車輛并道安全性,乘車人舒適性及并道效率。
將目標函數(shù)轉(zhuǎn)化為關(guān)于并道時間t的多約束優(yōu)化問題:
s.t. 3≤t≤6
vPt |a|≤amax ρ≤ρmax j≤jmax w1+w2+w3+w4=1 (14) 式(14)中,Dmax表示并道所允許的最大距離,amax,ρmax,jmax分別表示最大橫向加速度,最大曲率及最大橫向加加速度。 遺傳算法是一種基于概率的群體優(yōu)化算法,具有并行,高效的優(yōu)點,采用Matlab中的遺傳算法工具箱對上述優(yōu)化問題進行求解,得到最優(yōu)并道軌跡。 設(shè)置并道時間上下限(3~6)s,初始化種群數(shù)目20,個體數(shù)目5,交叉概率0.4,變異概率0.1,最大進化代數(shù)20.求解結(jié)果如圖4所示,并道時間t取3.366 s時,并道軌跡目標函數(shù)取值最小。 圖4 遺傳算法求最優(yōu)軌跡圖Fig.4 Genetic algorithm for optimal trajectory graph 模型預(yù)測控制具有前饋-反饋的功能,能夠處理非線性多約束優(yōu)化問題,根據(jù)整個預(yù)測內(nèi)的參考輸入以及狀態(tài)量的約束求解輸出最優(yōu)的控制量。其主要有三個模塊組成,構(gòu)建預(yù)測模型,優(yōu)化問題求解,之后將解的第一個元素作用于下一時刻控制量。 根據(jù)第一節(jié)所建立的動力學(xué)模型,建立車輛非線性狀態(tài)方程: (15) 由于非線性模型計算非常復(fù)雜,參考文獻[10]的方法對狀態(tài)方程進行線性化及離散化,得到離散的狀態(tài)空間表達式: η(k)=Cξ(k) (16) 其中,A(k)=I+TA(t),B(k)=I+TB(t). 目標函數(shù)的設(shè)計要保證智能車快速且平穩(wěn)的追蹤上期望軌跡。本文采用如下形式的目標函數(shù): J(ξ(t),U(t-1),ΔU(t))= (17) 式(17)中,第一項為預(yù)測時域內(nèi)系統(tǒng)狀態(tài)與參考軌跡的反饋誤差,第二項為控制時域內(nèi)對控制量的約束,能夠使保證控制有效性,防止控制量的突變與超出控制范圍,最后一項為軟約束,避免了無解的情況發(fā)生。 同時,優(yōu)化函數(shù)還需滿足如下約束: (1)控制量約束 (2)控制增量約束 (3)質(zhì)心側(cè)偏角約束 -12°<β<12° (4)附著條件約束 ay,min+ε≤ay≤ay,max+ε (5)輪胎側(cè)偏角約束 -2.5°<αf,t<2.5° 綜合上述的目標函數(shù)和約束條件,基于動力學(xué)模型的模型預(yù)測控制器需解決如下優(yōu)化問題: s.t. ΔUmin≤ΔU≤ΔUmax Umin≤AΔU+U≤Umax yhmin≤yh≤yhmax ysmin-ε≤ys≤ysmax+ε ε>0 (18) 式(18)中,yh為硬約束輸出;ys為軟約束輸出,可動態(tài)調(diào)節(jié)約束范圍。 上述優(yōu)化問題可轉(zhuǎn)化為對二次規(guī)劃問題求解,得到控制時域內(nèi)一系列的控制增量,將第一個元素作為實際的控制增量作用于系統(tǒng),即: ut+1=ut+ΔU(1) (19) 對每個控制周期重復(fù)上述過程,即可實現(xiàn)軌跡跟蹤。 由多次實驗發(fā)現(xiàn),改變預(yù)測時域Np和控制時域Nc對控制效果有很大的影響。如果Np過大,可能導(dǎo)致求解時間過長,在控制周期內(nèi)不能完成優(yōu)化問題的求解,出現(xiàn)控制器失效的問題,Nc過大,則會導(dǎo)致系統(tǒng)的輸出超過預(yù)期,產(chǎn)生過擬合現(xiàn)象。因此,本文提出橫擺角系數(shù)自適應(yīng)的模型預(yù)測控制器,根據(jù)車身的橫擺角自適應(yīng)調(diào)整Np,Nc. Np=k1NP,Nc=k2NC (20) (21) 在開始并道階段,車身橫擺角小于軌跡切線,Nc應(yīng)該增大,保證跟蹤精度。在并道中期,車身橫擺角與軌跡切線斜率幾乎相等,控制時域和預(yù)測時域應(yīng)相對減小。在并道末期,智能車處于軌跡下方,橫擺角大于切線斜率,要減小Nc,防止出現(xiàn)過擬合現(xiàn)象。 本文提出了基于五次多項式的并道軌跡及曲率自適應(yīng)模型預(yù)測控制。左并道一般從低速車道變向較高車速車道,多見于高速入口及道路合流等情形。本文采用向左并道工況,對上述所提出的跟蹤控制算法與斯坦利方法[11]進行Carsim/Simulink聯(lián)合仿真對比實驗。Carsim提供智能車輛的速度,位置,橫擺角等狀態(tài)量,軌跡規(guī)劃模提供參考軌跡,再經(jīng)過跟蹤控制模塊求解出下一時刻車輛的前輪偏角輸入到Carsim中,形成閉環(huán)控制系統(tǒng),驗證算法的有效性。 Carsim中車輛模型參數(shù)表1所示。 表1 車輛模型參數(shù) 本文使用Carsim中的b-class,輪胎選擇225/60 R18,車輛速度72 km/h,并道距離84 m.仿真結(jié)果如下圖所示。 圖5 Carsim/Simulink聯(lián)合仿真示意圖Fig.5 Carsim/Simulink co-simulation diagram 圖6表示控制器的跟蹤效果圖,圖7表示控制器的誤差對比圖。可以看出,三種控制器都有著很好的跟蹤效果,文獻[11]斯坦利方法在并道終點處會有較小的超調(diào),之后會逐漸穩(wěn)定。比傳統(tǒng)的MPC控制器而言,本文提出的控制算法有更小的誤差,更好的跟蹤精度。 圖6 軌跡跟蹤效果圖Fig.6 Trajectory tracking effect diagram 圖7 軌跡跟蹤誤差圖Fig.7 Trajectory tracking error graph 圖8表示并道過程中車輛橫擺角的變化圖,文獻[11]斯坦利方法是基于幾何跟蹤的控制方法,在終點處會有一段時間的微調(diào)。圖9反映了變道過程中前輪偏轉(zhuǎn)角變化,可以看出三種控制器均滿足小角度約束條件,本文提出的控制器所解得的前輪偏角連續(xù)且平穩(wěn),能夠有效的實現(xiàn)智能車輛的并道行為。綜上所述,本文提出的控制器保證了并道的安全性,穩(wěn)定性,相比具有更好的跟蹤性能。 圖8 橫擺角變化圖Fig.8 Yaw angle variation diagram 圖9 前輪偏角變化圖Fig.9 Variation diagram of front wheel deflection angle (1)基于五次多項式提出了一種最優(yōu)并道軌跡,在最優(yōu)軌跡規(guī)劃中綜合考慮并道類型,車輛的安全性、乘車人舒適度及并道效率,設(shè)置不同的權(quán)重系數(shù),使用遺傳算法優(yōu)化得到最優(yōu)變道軌跡。 (2)提出了基于橫擺角的自適應(yīng)MPC,詳細分析了并道過程中橫擺角的變化對控制器參數(shù)的影響,并且通過仿真結(jié)果說明了本文算法的有效性。3 跟蹤控制
3.1 模型預(yù)測控制器
3.2 基于橫擺角系數(shù)自適應(yīng)的MPC
4 仿真
5 結(jié)論