基于MPC的智能車輛路徑規(guī)劃與跟蹤控制

張麗霞,田 碩,潘福全,嚴(yán)濤峰,李寶剛

(青島理工大學(xué) 機(jī)械與汽車工程學(xué)院,山東 青島 266520)

0 引言

智能車輛是聚集了先進(jìn)感知系統(tǒng)、規(guī)劃決策系統(tǒng)、跟蹤控制系統(tǒng)、執(zhí)行系統(tǒng)、多輔助駕駛系統(tǒng)的1個(gè)綜合體[1]。目前,對(duì)路徑規(guī)劃與跟蹤控制已經(jīng)進(jìn)行了很多研究,但是在面對(duì)復(fù)雜工況和曲率較大的路況時(shí),跟蹤控制的效果仍然不理想。

研究路徑規(guī)劃算法,實(shí)現(xiàn)智能汽車在不同環(huán)境下的精準(zhǔn)避障,可為車輛提供一條安全、可靠的避障路徑[2-3]。文獻(xiàn)[4]在結(jié)合智能汽車動(dòng)力學(xué)約束條件的基礎(chǔ)上,提出一種基于蟻群算法優(yōu)化的最佳路徑設(shè)計(jì),經(jīng)過(guò)仿真驗(yàn)證,該算法可以提高全局搜索能力,減少節(jié)點(diǎn)數(shù)量,有效地降低汽車行駛能耗。文獻(xiàn)[5]提出一種基于模糊控制的避障路徑規(guī)劃器,通過(guò)超聲波測(cè)距傳感器測(cè)出左、右、前三方的距離,將其作為模糊控制的輸入,以車輛轉(zhuǎn)向角作為輸出,建立模糊控制規(guī)則。文獻(xiàn)[6]在人工勢(shì)場(chǎng)基礎(chǔ)上建立目標(biāo)點(diǎn)引力函數(shù),并引入障礙物約束和車輛動(dòng)力學(xué)約束,建立改進(jìn)的人工勢(shì)場(chǎng)模型。文獻(xiàn)[7]在人工勢(shì)場(chǎng)模型中引入道路邊界斥力勢(shì)場(chǎng)來(lái)進(jìn)行轉(zhuǎn)向避障路徑的規(guī)劃,并用橢圓形勢(shì)場(chǎng)代替了原來(lái)的斥力勢(shì)場(chǎng)。通過(guò)仿真試驗(yàn),證明了采用該方法可獲得安全的局部避障路徑。文獻(xiàn)[3]針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)(artificial potential field,APF)算法易陷入局部極值的問(wèn)題,引入Bug算法。通過(guò)搭建智能車實(shí)驗(yàn)平臺(tái)證明了改進(jìn)的APF-Bug混合算法可以有效提高智能車在行駛過(guò)程中的安全性。

路徑跟蹤控制主要通過(guò)控制車輛的前輪轉(zhuǎn)角和油門/剎車踏板,使車輛的行駛路徑與規(guī)劃路徑間的橫向偏差與橫擺角偏差盡可能小[8]。文獻(xiàn)[9]以期望航向和實(shí)際航向的偏差作為反饋,通過(guò)設(shè)計(jì)魯棒比例積分微分(proportional-integral-derivative,PID)控制器計(jì)算控制量。經(jīng)過(guò)驗(yàn)證,所提出的路徑跟蹤橫向控制策略跟蹤效果較好。文獻(xiàn)[10]建立14自由度車輛模型,提出了一種以8自由度車輛模型作為預(yù)測(cè)模型的模型預(yù)測(cè)控制(model predictive control,MPC)方法。從MPC控制器中生成最優(yōu)車輪轉(zhuǎn)向角序列,并以曲率連續(xù)變化的曲線作為參考軌跡。文獻(xiàn)[11]建立了線性時(shí)變路徑跟蹤預(yù)測(cè)模型,并在目標(biāo)函數(shù)中引入向量松弛因子,將路徑跟蹤問(wèn)題轉(zhuǎn)化為帶軟約束的二次規(guī)劃問(wèn)題。文獻(xiàn)[12]提出一種基于滑膜變結(jié)構(gòu)的控制方法,分別引入積分滑膜項(xiàng)和模糊自適應(yīng)項(xiàng),用于車輛轉(zhuǎn)向時(shí)的車速和轉(zhuǎn)向角速度的控制。

綜合國(guó)內(nèi)外研究發(fā)現(xiàn),針對(duì)避障路徑規(guī)劃,多數(shù)使用單個(gè)規(guī)劃算法,得到的規(guī)劃效果不是很理想,如:蟻群算法的搜索效率不高,易受到局部最優(yōu)解的影響[13];模糊控制在復(fù)雜的環(huán)境下,規(guī)則庫(kù)難以建立[14];人工勢(shì)場(chǎng)法具有效率高、適應(yīng)性好等優(yōu)點(diǎn),但也存在局部最優(yōu)等缺陷[15]。針對(duì)路徑跟蹤控制,PID控制雖然易于實(shí)現(xiàn),但其抗干擾能力和魯棒性不強(qiáng),存在對(duì)擾動(dòng)敏感、誤差計(jì)算不合理等問(wèn)題[16]。MPC控制方法雖然能夠補(bǔ)償模型誤差,控制較為精確,但MPC的權(quán)重系數(shù)為固定值,無(wú)法滿足某些工況對(duì)其精度和穩(wěn)定性的要求[17]。綜上,本文采用基于MPC和改進(jìn)的APF算法,既解決了傳統(tǒng)APF的缺陷,又在考慮車輛動(dòng)力學(xué)特性下,有效地進(jìn)行動(dòng)態(tài)避障路徑規(guī)劃。同時(shí)根據(jù)車輛橫向偏差和參考路徑曲率,引入模糊控制對(duì)MPC橫向路徑跟蹤控制器權(quán)重系數(shù)進(jìn)行優(yōu)化,提高了跟蹤精度及穩(wěn)定性。

1 車輛動(dòng)力學(xué)模型

建立三自由度車輛動(dòng)力學(xué)模型,如圖1所示。圖1中,坐標(biāo)系X-O-Y為大地坐標(biāo)系,坐標(biāo)系x-o-y為車輛坐標(biāo)系,x軸沿車輛縱軸向前為正,y軸垂直于車輛縱軸指向駕駛員左側(cè)為正。

根據(jù)圖1可得車輛動(dòng)力學(xué)方程:

(1)

其中:m為整車質(zhì)量,kg;vx為車輛縱向速度,m/s;vy為車輛橫向速度,m/s;φ為車輛橫擺角速度,rad/s;Fxf、Fxr為車輛坐標(biāo)系下x軸方向前、后輪胎受到的力,N;Flf、Flr為前、后輪胎受到的縱向力,N;δ為前輪轉(zhuǎn)角,rad;Fyf、Fyr為車輛坐標(biāo)系下y軸方向前、后輪胎受到的力,N;Fcf、Fcr為前、后輪胎受到的側(cè)向力,N;Iz為車輛繞z軸的轉(zhuǎn)動(dòng)慣量,kg·m2;a為前軸到質(zhì)心處的距離,m;b為后軸到質(zhì)心處的距離,m。

輪胎的縱向力和側(cè)向力可用線性函數(shù)近似表示為:

(2)

其中:cli為輪胎縱向剛度,N/m;cci為輪胎側(cè)偏剛度,N/rad;si為輪胎滑移率;αi為輪胎側(cè)偏角,rad。

通過(guò)對(duì)以上各式進(jìn)行簡(jiǎn)化和推導(dǎo),可得到基于小角度和線性輪胎模型假設(shè)條件下的車輛非線性系統(tǒng)動(dòng)力學(xué)模型:

(3)

2 基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器設(shè)計(jì)

針對(duì)傳統(tǒng)APF中存在的目標(biāo)不可達(dá)、局部最優(yōu)等缺陷,本文改進(jìn)障礙物斥力勢(shì)場(chǎng),添加道路邊界約束、速度斥力約束、車道引力約束等多個(gè)約束條件,建立改進(jìn)APF;同時(shí)考慮到車輛動(dòng)力學(xué)特性,結(jié)合MPC,設(shè)計(jì)了基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器。

在路徑規(guī)劃過(guò)程中,將車輛簡(jiǎn)化為質(zhì)點(diǎn)。車輛行駛環(huán)境狀況中的目標(biāo)點(diǎn)位置、障礙物位置和車輛自身位置均由感知層確定。車輛的位置為q(x,y)T,障礙物的位置為qob(xob,yob)T,目標(biāo)點(diǎn)的位置為qg(xg,yg)T。

引力勢(shì)場(chǎng)函數(shù)可表示為:

(4)

其中:κ為引力勢(shì)場(chǎng)尺度因子;d(qg,q)為車輛與目標(biāo)點(diǎn)間的歐幾里德距離,方向?yàn)檐囕v指向目標(biāo)點(diǎn),m。

在路徑規(guī)劃過(guò)程中,不僅考慮躲避障礙物,還要考慮車輛駛出道路邊界影響安全的因素。故建立道路邊界斥力勢(shì)場(chǎng)為:

(5)

其中:ηedge1、ηedge2為道路邊界斥力勢(shì)場(chǎng)增益系數(shù);yl、yr分別為左右車道中心線的橫向位置,m;L為道路寬度,m。

在傳統(tǒng)人工勢(shì)場(chǎng)法中,障礙物處于其對(duì)車輛產(chǎn)生作用的最大距離區(qū)域d0內(nèi)時(shí),行駛車輛才受到斥力作用,但障礙物形狀多樣,固定的參數(shù)無(wú)法滿足實(shí)際行駛的場(chǎng)景,所以改進(jìn)為橢圓形斥力勢(shì)場(chǎng)。因此本文建立改進(jìn)的障礙物斥力勢(shì)場(chǎng)函數(shù)為:

(6)

在車輛的整個(gè)行駛環(huán)境中,不僅有靜態(tài)障礙物,還存在動(dòng)態(tài)障礙物,在建立障礙物斥力勢(shì)場(chǎng)時(shí),不僅要考慮被控車輛距障礙物的距離,還應(yīng)考慮障礙物速度影響。圖2為被控車輛與障礙物的速度方向示意圖。

圖2 速度方向示意圖

速度斥力勢(shì)場(chǎng)函數(shù)可表達(dá)為:

(7)

其中:ηvel為速度斥力勢(shì)場(chǎng)尺度因子。

車輛在行駛和轉(zhuǎn)向避障時(shí),主要沿著車道中心線行駛。本文以車道中心線為引力點(diǎn)建立車道引力勢(shì)場(chǎng)模型,以前方車道中心線上各點(diǎn)為引力點(diǎn),構(gòu)建車道引力勢(shì)場(chǎng)函數(shù),其表達(dá)式為:

Ulane=kroad(|y-y0|2+|x-xd|),

(8)

其中:kroad為車道引力勢(shì)場(chǎng)增益系數(shù);y0為車輛起始橫向位置坐標(biāo),m;xd為車輛轉(zhuǎn)向區(qū)域的最大縱向距離,m。

綜合上述分析,車輛在合力勢(shì)場(chǎng)作用下的勢(shì)能為:

Uall=Urep+Uroad+Uvel+Ulane+Uatt。

(9)

將改進(jìn)APF建立的勢(shì)場(chǎng)模型函數(shù)引入到MPC的目標(biāo)函數(shù)和約束中,建立基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器。

首先將式(3)轉(zhuǎn)換為狀態(tài)空間方程:

(10)

設(shè)計(jì)的目標(biāo)函數(shù)如下:

(11)

其中:λφ為輸出量權(quán)重矩陣;λU為環(huán)境勢(shì)場(chǎng)權(quán)重矩陣;λu為控制增量權(quán)重矩陣。

各約束條件的取值參考文獻(xiàn)[18]?；贛PC和改進(jìn)APF的路徑規(guī)劃問(wèn)題可轉(zhuǎn)化為以下帶約束問(wèn)題:

(12)

通過(guò)對(duì)式(16)進(jìn)行求解,可得到當(dāng)前時(shí)刻的控制增量,進(jìn)而確定當(dāng)前時(shí)刻的控制量,如此循環(huán)迭代,得到預(yù)測(cè)時(shí)域內(nèi)的最優(yōu)路徑點(diǎn)序列。

3 基于模糊MPC的車輛橫向路徑跟蹤控制器設(shè)計(jì)

3.1 MPC控制器設(shè)計(jì)

根據(jù)文獻(xiàn)[19],建立如下目標(biāo)函數(shù):

(13)

結(jié)合目標(biāo)函數(shù)與約束條件,路徑跟蹤控制問(wèn)題轉(zhuǎn)化為求解如下帶約束的二次規(guī)劃問(wèn)題:

(14)

最后通過(guò)對(duì)路徑跟蹤控制器的目標(biāo)函數(shù)取最小,在每個(gè)控制周期內(nèi),可得到控制時(shí)域內(nèi)一系列的控制增量序列,并將這一序列的第1個(gè)元素作為實(shí)際的控制輸入增量作用到系統(tǒng)上。如此循環(huán)迭代,實(shí)現(xiàn)車輛對(duì)期望軌跡的跟蹤。

3.2 模糊控制器設(shè)計(jì)

將橫向偏差和參考路徑曲率作為模糊控制的輸入量,權(quán)重系數(shù)qφ和Ru的修正量Δqφ和ΔRu作為模糊控制的輸出量,建立模糊規(guī)則。同時(shí)對(duì)于輸入量選擇三角形隸屬度函數(shù),輸出量選擇高斯隸屬度函數(shù),進(jìn)而對(duì)MPC橫向路徑跟蹤控制器的權(quán)重系數(shù)進(jìn)行優(yōu)化。

首先,需對(duì)模糊控制輸入量進(jìn)行歸一化處理,有:

(15)

(16)

其中:eY,max、eY,min為最大、最小橫向路徑偏差,m;kmax、kmin為最大、最小參考路徑曲率。

定義權(quán)重系數(shù)的調(diào)整算式為:

qφ=qφ0×100Δqφ;

(17)

Ru=Ru0×10ΔRu,

(18)

其中:qφ、Ru為模糊MPC控制器當(dāng)前階段計(jì)算出的權(quán)重系數(shù);qφ0、Ru0為MPC控制器的權(quán)重系數(shù),Δqφ、ΔRu為權(quán)重系數(shù)當(dāng)前階段的修正量。

表1 Δqφ模糊控制規(guī)則表

表2 ΔRu模糊控制規(guī)則表

經(jīng)過(guò)模糊推理得出的結(jié)果不能直接作用于被控車輛,需經(jīng)過(guò)反模糊化輸出精確值,從而對(duì)權(quán)重系數(shù)進(jìn)行調(diào)節(jié)。考慮到控制穩(wěn)定性、抗干擾性和靈敏性的要求,本文采用面積重心法進(jìn)行解模糊處理[20]。

為驗(yàn)證模糊MPC路徑跟蹤控制器的控制性能,選擇權(quán)重系數(shù)為常量的MPC路徑跟蹤控制器和模糊MPC路徑跟蹤控制器進(jìn)行仿真對(duì)比。選擇雙移線工況作為參考路徑,車速設(shè)定為60 km/h。不同附著系數(shù)的路面會(huì)提供輪胎不同的側(cè)向力,直接影響車輛的控制性能,因此本文分別選擇μ=0.85 的干燥路面和μ=0.40的濕滑路面2種不同工況進(jìn)行仿真實(shí)驗(yàn),MPC路徑跟蹤控制器參數(shù)設(shè)置如表3所示。

表3 MPC路徑跟蹤控制器參數(shù)

測(cè)試工況一:車速60 km/h,路面附著系數(shù)0.85。

圖3為路面附著系數(shù)0.85,60 km/h車速下2種控制器的跟蹤結(jié)果對(duì)比。由圖3a可以看出:2種控制器控制的車輛都能夠較為精確地跟蹤參考路徑,但車輛在曲率較大的路段時(shí),MPC路徑跟蹤控制器的最大橫向偏差約為0.350 m,而模糊MPC路徑跟蹤控制器的最大橫向偏差約為0.283 m,最大橫向偏差減少19.14%,這是由于曲率較大的路段難以跟蹤,模糊MPC路徑跟蹤控制器中的權(quán)重系數(shù)qφ和Ru變小,減小在該路段的橫向偏差,使得車輛能夠跟蹤參考路徑。由圖3b可知:在 40 m的縱向范圍內(nèi)MPC控制器的橫擺角軌跡跟蹤最大偏差為 0.031 rad,模糊MPC控制器的最大偏差為 0.009 rad,因此模糊MPC控制器的軌跡跟蹤更準(zhǔn)確。從圖3c和圖3d可以看出:模糊MPC路徑跟蹤控制器的最大橫擺角速度和前輪轉(zhuǎn)角雖然比MPC路徑跟蹤控制器的最大值略有增加,穩(wěn)定性有所降低,但保證了車輛的行駛安全性;同時(shí)整個(gè)軌跡跟蹤過(guò)程中變化平緩,而且都處于約束范圍內(nèi),說(shuō)明此時(shí)車輛的穩(wěn)定性良好。

(a) 車輛行駛路徑

測(cè)試工況二:車速60 km/h,路面附著系數(shù)0.40。

圖4為路面附著系數(shù)0.40,60 km/h車速下兩種控制器的跟蹤結(jié)果對(duì)比。由圖4a和圖4b可以看出:由于輪胎與地面摩擦力的下降,MPC路徑跟蹤控制器和模糊MPC路徑跟蹤控制器的軌跡跟隨都存在一定的滯后性,在50 m的縱向范圍內(nèi)MPC控制器的橫向偏差為0.651 m,模糊MPC控制器的橫向偏差為0.1 m,相比較減少了0.551 m。同時(shí)在40 m范圍內(nèi)MPC控制器的橫擺角軌跡跟蹤最大偏差為 0.021 rad,模糊MPC控制器的最大偏差為 0.005 rad,由此模糊MPC控制器軌跡跟蹤精度更高。從圖4c與圖4d可以看出:模糊MPC路徑跟蹤控制器的橫擺角速度與前輪轉(zhuǎn)角變化范圍都因地面附著系數(shù)的減小而變大,但是沒(méi)有發(fā)生突變,曲線變化始終平順光滑,從始至終一直處于約束范圍之內(nèi),車輛行駛穩(wěn)定性良好。

(a) 車輛行駛路徑

4 避障路徑動(dòng)態(tài)規(guī)劃與跟蹤控制試驗(yàn)驗(yàn)證

將設(shè)計(jì)的路徑規(guī)劃器和路徑跟蹤控制器進(jìn)行聯(lián)合,搭建避障路徑規(guī)劃與跟蹤控制聯(lián)合仿真模型,并進(jìn)行動(dòng)態(tài)障礙物不同速度下的避障路徑規(guī)劃與跟蹤試驗(yàn)。

搭建的避障路徑規(guī)劃與跟蹤控制聯(lián)合仿真模型如圖5所示。

圖5 避障路徑規(guī)劃及跟蹤控制聯(lián)合仿真模型

基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器參數(shù)如表4所示。

表4 基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器參數(shù)

第1種工況距智能車輛約90 m處設(shè)有10 km/h的動(dòng)態(tài)障礙物;第2種速度工況距智能車輛約130 m處設(shè)有20 km/h的動(dòng)態(tài)障礙物;第3種工況距智能車輛約200 m處設(shè)有40 km/h的動(dòng)態(tài)障礙物,沿著車道中心線行駛。動(dòng)態(tài)障礙物不同速度下的避障路徑規(guī)劃及跟蹤行駛曲線見(jiàn)圖6,車輛行駛路徑與規(guī)劃路徑的橫向偏差見(jiàn)圖7。智能車輛先沿著車道中心線行駛,檢測(cè)到動(dòng)態(tài)障礙物后,路徑規(guī)劃器規(guī)劃出向左換道的路徑,控制智能車輛開(kāi)始向左側(cè)換道避障,此時(shí)3種速度工況下的動(dòng)態(tài)障礙物位置分別為圖中紅色、黑色、綠色矩形框位置;避障換道后,系統(tǒng)規(guī)劃出沿著車道中心線,控制車輛沿著該車道中心線繼續(xù)行駛,而動(dòng)態(tài)障礙物繼續(xù)沿著原車道車輛中心線繼續(xù)行駛。

圖6 動(dòng)態(tài)障礙物不同速度下的避障路徑規(guī)劃及跟蹤行駛曲線

圖7 車輛行駛路徑與規(guī)劃路徑的橫向偏差

圖8為智能車輛與動(dòng)態(tài)障礙物的相對(duì)距離與偏離車道中心線、橫擺角的關(guān)系曲線圖。從圖8a和圖8b可以看出:隨著動(dòng)態(tài)障礙物速度的增加,智能車輛與障礙物間的相對(duì)速度越來(lái)越小,此時(shí)系統(tǒng)控制車輛跟蹤所規(guī)劃路徑的橫擺角也在減小。

(a) 偏離車道

圖9為動(dòng)態(tài)障礙物不同速度下的路徑跟蹤結(jié)果圖。從圖9可以看出:障礙物速度為10 km/h時(shí),車輛前輪轉(zhuǎn)角、橫擺角速度、質(zhì)心側(cè)偏角、側(cè)向加速度的最大值分別為0.026 1 rad、0.071 7 rad/s、0.002 4 rad、0.518 m/s2;障礙物速度為20 km/h時(shí),車輛前輪轉(zhuǎn)角、橫擺角速度、質(zhì)心側(cè)偏角、側(cè)向加速度的最大值分別為0.023 8 rad、0.069 1 rad/s、0.023 9 rad、0.473 m/s2;障礙物速度為40 km/h時(shí),車輛前輪轉(zhuǎn)角、橫擺角速度、質(zhì)心側(cè)偏角、側(cè)向加速度的最大值分別為0.022 9 rad、0.062 7 rad/s、0.001 9 rad、0.404 m/s2,各參數(shù)曲線變化平穩(wěn),沒(méi)有出現(xiàn)較大波動(dòng),其值均處于合理范圍內(nèi),并且隨著障礙物速度的增加,車輛的前輪轉(zhuǎn)角、橫擺角速度、質(zhì)心側(cè)偏角、側(cè)向加速度的值在減小。因?yàn)檎系K物速度的增加,智能車輛與障礙物間的相對(duì)速度在減小,進(jìn)而智能車輛受到的勢(shì)場(chǎng)作用減弱,致使系統(tǒng)規(guī)劃的路徑更為平滑,系統(tǒng)能夠平穩(wěn)、精確地控制車輛進(jìn)行路徑跟蹤。

(a) 前輪轉(zhuǎn)角

5 結(jié)束語(yǔ)

本文考慮智能車輛的路徑規(guī)劃,將改進(jìn)APF建立的勢(shì)場(chǎng)模型函數(shù)引入到MPC的目標(biāo)函數(shù)和約束中,設(shè)計(jì)了基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器。經(jīng)過(guò)仿真驗(yàn)證分析,避障路徑規(guī)劃器在跟蹤規(guī)劃路徑過(guò)程中最大橫向偏差約為0.170 m,證明改進(jìn)的APF和MPC能夠在動(dòng)態(tài)障礙物環(huán)境下合理規(guī)劃出安全的避障路徑。同時(shí)隨著障礙物速度的增加,車輛的前輪轉(zhuǎn)角、橫擺角速度、質(zhì)心側(cè)偏角、側(cè)向加速度的值在減小,因此基于MPC和改進(jìn)APF的避障路徑動(dòng)態(tài)規(guī)劃器規(guī)劃的路徑更為平滑,系統(tǒng)能夠平穩(wěn)、精確地控制車輛進(jìn)行路徑跟蹤。

本文針對(duì)智能車輛的跟蹤控制,建立了基于模糊MPC的車輛橫向路徑跟蹤控制器,與傳統(tǒng)MPC相比,模糊MPC路徑跟蹤控制方法可使控制輸出平滑,更加滿足車輛的穩(wěn)定性,同時(shí)模糊MPC控制器在干燥路面時(shí)最大橫向偏差減少19.14%,在濕滑路面下橫向偏差減少0.55 m,因此在不同路面下模糊MPC能更有效協(xié)調(diào)路徑跟蹤準(zhǔn)確性和橫向穩(wěn)定。