孔慧芳, 夏 露, 張 倩

(合肥工業(yè)大學(xué) 電氣與自動化工程學(xué)院,安徽 合肥 230009)

0 引 言

智能駕駛是汽車未來的發(fā)展趨勢,它的核心主要包含環(huán)境感知、決策規(guī)劃和控制執(zhí)行3個部分[1]。路徑規(guī)劃是決策規(guī)劃部分的一個重要模塊,其結(jié)果的優(yōu)劣影響著車輛行駛的舒適性和安全性。一直以來,避撞路徑規(guī)劃都是智能駕駛研究的熱點問題之一。

目前,比較常見的路徑規(guī)劃算法有人工勢場法[2]、A*算法[3]、遺傳算法[4]和快速隨機擴展樹(rapidly-exploring random trees,RRT)[5]等,其中人工勢場法以計算量小、實時性好等優(yōu)點常被用于汽車的局部路徑規(guī)劃,但是該算法沒有考慮到車輛動力學(xué)約束和道路邊界的約束,且容易陷入局部最優(yōu)值。針對這些問題,各國學(xué)者進行了大量的優(yōu)化研究。文獻[6]在傳統(tǒng)的斥力場中加入調(diào)節(jié)因子,解決了障礙物與目標(biāo)點過近的局部最優(yōu)值問題,但是沒有考慮到道路的約束;文獻[7]針對局部最優(yōu)值問題臨時增設(shè)子目標(biāo)點,通過變換目標(biāo)點位置改變勢場空間,但子目標(biāo)點的選取方法不適用于道路上的車輛;文獻[8]利用橢圓化距離代替?zhèn)鹘y(tǒng)斥力勢場中的實際距離,同時引入道路邊界斥力場模型,獲得汽車避撞局部路徑,但沒有考慮陷入局部極小值的問題;文獻[9]建立人工勢場模型,采用梯度下降算法得到路徑,但該方法規(guī)劃的路徑可能不符合車輛動力學(xué)約束。

模型預(yù)測控制最明顯的特點是能在控制的過程中增加車輛動力學(xué)的多種約束,許多學(xué)者結(jié)合人工勢場法和模型預(yù)測控制(model predictive control,MPC)進行避撞路徑規(guī)劃研究。文獻[10]設(shè)計模型預(yù)測控制器,利用粒子群算法優(yōu)化控制器參數(shù),但是只考慮了障礙物而沒有考慮道路邊界的影響;文獻[11]搭建智能汽車主動避障系統(tǒng),基于改進人工勢場模型規(guī)劃路徑,利用模型預(yù)測控制算法進行跟蹤,但不能根據(jù)車輛狀態(tài)實時調(diào)整規(guī)劃的路徑;文獻[12]提出一種分層避撞控制方法,基于模型預(yù)測控制理論構(gòu)建優(yōu)化目標(biāo)函數(shù),采用五次多項式擬合局部避撞路徑。

本文基于車輛動力學(xué)模型,考慮道路邊界的約束,為了避免局部最優(yōu)值,添加虛擬目標(biāo)點,建立道路環(huán)境模型;將模型與模型預(yù)測控制相結(jié)合,規(guī)劃層根據(jù)道路環(huán)境模型和參考路徑信息重新規(guī)劃出避開障礙物的局部路徑;再將路徑信息輸入到跟蹤控制層,實現(xiàn)對局部參考路徑的跟蹤,并將車輛狀態(tài)實時反饋給規(guī)劃層;最后通過MATLAB/Simulink與CarSim聯(lián)合仿真驗證路徑規(guī)劃及跟蹤效果。

避撞路徑規(guī)劃結(jié)構(gòu)如圖1所示。

圖1 避撞路徑規(guī)劃結(jié)構(gòu)

1 改進的人工勢場模型

1.1 方向引力勢場模型

在實際情況中,車輛正常行駛處于車道中心,完成超車或避障動作后一般會回到原車道,因此本文以被控車輛自身為避撞的起點,在前方縱向距離yd設(shè)立終點,勢場在轉(zhuǎn)向區(qū)域沿車道中心線逐漸下降,所構(gòu)建的方向引力勢場函數(shù)表達式如下:

(1)

其中:katt為引力勢能增益系數(shù);xs為車輛起點橫向坐標(biāo)位置;yd為轉(zhuǎn)向區(qū)域最大縱向距離。

根據(jù)(1)式得到引力勢能場的三維分布,如圖2所示。

圖2 引力勢場三維分布

1.2 障礙物斥力勢場模型

斥力勢場的建立保證被控車輛遠離障礙物,在障礙物斥力勢場作用域D之內(nèi),距離越小,被控車輛所受的斥力越大,離開作用域時,所受斥力為0,一般在橫向和縱向的作用距離并不相同,將車輛的作用域膨脹為橢圓,橢圓方程為:

(2)

其中:(x0,y0)為障礙物所在的中心位置;L1、L2分別為橢圓的長半軸和短半軸。L1、L2的數(shù)學(xué)表達式為:

(3)

L2=vrt+Smin

(4)

其中:G為安全因子;L為車道的寬度;vr為被控車輛相對于障礙物的相對速度;t為制動前的反應(yīng)時間;Smin為設(shè)定的最小安全距離。

根據(jù)被控車輛到障礙物的距離與勢場大小成反比,遠距離時隨著距離減小上升平緩、近距離上升迅速的特點,采用類高斯函數(shù)模擬障礙物勢能[13],建立障礙物斥力勢場函數(shù)為:

Urep=

(5)

其中:Krep為斥力勢能增益系數(shù);d為被控車輛到障礙物的距離。

根據(jù)(5)式可以得到斥力勢能場的三維分布,如圖3所示。

圖3 斥力勢場三維分布

1.3 道路勢場模型

車輛在道路上行駛不僅需要考慮障礙物的影響,還需要考慮道路邊界和車道線的影響。道路邊界用于防止車輛駛出道路,給車輛一種斥力的作用,因此在道路兩邊勢場取最大值;車道線限制車輛不必要的換道,因此在車道線位置勢場取極大值,并向兩邊遞減,變化較為平緩。

根據(jù)道路特點構(gòu)建道路勢場函數(shù)如下:

Uroad=

(6)

其中:Kroad為道路邊界勢能增益系數(shù);Kline為車道線勢能增益系數(shù);xborder為離車輛最近的道路邊界線的位置;xmid為道路中心線的位置;δ為分道線勢場收斂系數(shù),由車道寬度決定。

根據(jù)(6)式可以得到道路上不同位置勢能的三維分布,如圖4所示。

圖4 道路勢場三維分布

1.4 虛擬目標(biāo)點

為了解決人工勢場法中易出現(xiàn)的局部最優(yōu)值問題,本文擬設(shè)立虛擬目標(biāo)點來擺脫陷入局部最優(yōu)的困境。虛擬目標(biāo)點選取在縱向上與障礙物保持同一水平線,橫向上距離障礙物為M的位置,其計算方法為:

M=λL2

(7)

其中,λ為距離擴大系數(shù)。

根據(jù)障礙物的危險程度,通過改變λ的值可以動態(tài)調(diào)整安全距離。

根據(jù)虛擬目標(biāo)點的位置信息構(gòu)建勢場函數(shù):

(8)

其中,kvir為虛擬目標(biāo)點勢能增益系數(shù)。

當(dāng)車輛根據(jù)傳感器獲取周圍環(huán)境信息,計算并判斷車輛在勢場中是否陷入局部最優(yōu)值。若是,則開始受到虛擬目標(biāo)點勢場的作用,當(dāng)車輛運行到虛擬目標(biāo)點處,虛擬目標(biāo)點消失,繼續(xù)原目標(biāo)點的作用,直至到達目標(biāo)點。虛擬目標(biāo)點選取方式如圖5所示。

圖5 虛擬目標(biāo)點選取方式

車輛在道路上主要受到方向引力勢場、障礙物斥力勢場和道路勢場的作用,當(dāng)車輛陷入局部最優(yōu)困境時,受到虛擬目標(biāo)點勢場的作用,幾種勢場共同構(gòu)成道路環(huán)境模型,即

UAPF=Uatt+Urep+Uroad+Uvir

(9)

通過改進后的人工勢場模型,獲取車輛行駛的周圍環(huán)境信息,結(jié)合MPC算法重新規(guī)劃局部避撞路徑。

2 基于MPC的局部路徑規(guī)劃

2.1 車輛點質(zhì)量模型

車輛在道路上行駛時,遇到障礙物需要迅速根據(jù)傳感器獲取的障礙物信息進行局部路徑規(guī)劃,實時性要求較高。為了減少計算量,規(guī)劃層采用點質(zhì)量模型。

車輛模型可以表示為:

(10)

2.2 路徑規(guī)劃器設(shè)計

將(10)式轉(zhuǎn)化為狀態(tài)空間形式,即

(11)

(12)

其中:Jf(ξ)為f相對于ξ的雅克比矩陣;Jf(u)為f相對于u的雅克比矩陣。將以上兩式相減可以得到:

(13)

將新的狀態(tài)方程離散化處理可得:

(14)

為了使規(guī)劃的路徑能夠安全平穩(wěn)地避開障礙物,將道路環(huán)境模型作為一部分引入到目標(biāo)函數(shù)中,新的目標(biāo)函數(shù)如下所示:

(15)

其中:ηref為全局參考路徑信息;Δu(k+i|k)為k時刻第i步的預(yù)測控制增量;UAPF為道路環(huán)境勢能值;Np、Nc分別為預(yù)測時域和控制時域;Q、R為各部分的權(quán)重。

考慮到車輛動力學(xué)的約束,加入的約束條件如下:

|ay|<μg

(16)

其中,μ為道路附著系數(shù)。

因為路徑規(guī)劃器的預(yù)測方程給出的都是離散的點,跟蹤控制層難以完成跟蹤,所以需要對離散點進行擬合。本文采用五次多項式擬合曲線,即

φrep=a1x5+a2x4+a3x3+a4x2+a5x+a6,

Yrep=b1x5+b2x4+b3x3+b4x2+b5x+b6

(17)

其中:φrep為擬合后的橫擺角;Yrep為擬合后的路徑;ai,bi(i=1,2,…,6)為多項式擬合系數(shù)。

3 車輛避撞路徑跟蹤控制

3.1 車輛動力學(xué)模型

本文主要考慮車輛的平面運行,只有縱向、橫向和橫擺3個方向的運動,因此跟蹤控制層選擇三自由度的車輛單軌模型。根據(jù)牛頓第二定律,分別得到沿x軸、y軸和繞z軸的受力平衡方程為:

(18)

在輪胎力的計算過程中,為了簡化計算,基于小角度假設(shè)建立車輛動力學(xué)模型,即滿足cosθ=1和sinθ=θ,θ表示各個角,包括前輪轉(zhuǎn)角和側(cè)偏角等。

通過簡化,可以得到前、后輪胎側(cè)偏角的計算式分別為:

(19)

(20)

前、后輪的輪胎力可以表示為:

(21)

其中:Fcf、Fcr分別為前、后輪受到的側(cè)向力;Flf、Flr分別為前、后輪受到的縱向力;Ccf、Ccr分別為前、后輪側(cè)偏剛度;Clf、Clr分別為前、后輪縱向剛度;sf、sr分別為前、后輪滑移率。

結(jié)合(10)式、(18)～(21)式可以得到車輛的動力學(xué)模型如下:

(22)

3.2 跟蹤控制器設(shè)計

在對重規(guī)劃的路徑進行跟蹤過程中,應(yīng)該使實際路徑與參考路徑的偏差和控制輸入增益盡可能小,設(shè)計控制器的目標(biāo)函數(shù)為:

(23)

其中:ηrep為路徑規(guī)劃器輸出的局部路徑;ρ為權(quán)重系數(shù);ε為松弛因子,防止出現(xiàn)目標(biāo)函數(shù)無法得到可行解的情況。

保證車輛跟蹤過程中的平穩(wěn)性和安全性,需要滿足的控制量、控制增量和橫向加速度的約束條件如下:

(24)

根據(jù)博世公司的車輛穩(wěn)定性研究結(jié)果,在正常路面下保證汽車的穩(wěn)定性和舒適性,質(zhì)心側(cè)偏角β的約束為[-12°,12°],結(jié)合目標(biāo)函數(shù)和約束條件求解得到每個控制周期內(nèi)最優(yōu)控制增量序列為:

ΔUt=[ΔutΔut+1… Δut+Nc-1]

(25)

將序列的第1個元素作為實際控制增量作用于系統(tǒng),得到下一時刻的控制量,重復(fù)求解計算,完成對路徑的跟蹤控制。

4 聯(lián)合仿真驗證

為了驗證路徑規(guī)劃器的有效性,采用MATLAB/Simulink與CarSim 進行聯(lián)合仿真實驗。在CarSim中建立車輛模型,模型具體參數(shù)見表1所列。

表1 車輛模型參數(shù)

環(huán)境模型選擇城市道路下寬度7 m的單向雙車道,通過設(shè)置靜態(tài)障礙物和動態(tài)障礙物來驗證車輛避撞能力,在車輛前方50 m處設(shè)置一輛障礙物車輛。

4.1 靜態(tài)障礙物環(huán)境

為了驗證本文方法能夠更好地符合車輛動力學(xué)性能,將其與人工勢場法[9]對比仿真,設(shè)置被控車輛車速為54 km/h,仿真結(jié)果如圖6所示。

圖6 靜態(tài)障礙物下車輛避撞過程仿真結(jié)果

由圖6a可以看出,相較于文獻[9]方法,本文規(guī)劃器規(guī)劃的路徑更為平滑,車輛行駛過程中穩(wěn)定性更高,且提前6 m開始避障行為,安全性更高;由圖6b～圖6e可以看出,本文方法規(guī)劃的路徑下,車輛的前輪轉(zhuǎn)角、橫擺角、橫擺加速度和橫擺角速度變化均小于文獻[9]方法,且曲線變化平穩(wěn)沒有明顯突變,車輛能夠很好地進行避障跟蹤。

為了驗證車輛在不同速度下的避撞效果,分別在72、54、36 km/h的車速下進行仿真驗證,仿真結(jié)果如圖7所示。從圖7a中可以看出,在不同車速下,車輛均可以規(guī)劃出安全的局部避撞路徑,速度為72、54、36 km/h時分別從10、13、17 m處開始避撞行為。從圖7b～圖7d可以看出:車輛橫擺角變化在±5°內(nèi),前輪轉(zhuǎn)角變化在±2°內(nèi),變化范圍較小,滿足基于小角度假設(shè)的模型建立;質(zhì)心側(cè)偏角變化在±0.5°內(nèi),滿足[-12°,12°]的約束條件;橫向加速度變化在±0.2 m/s2內(nèi),滿足μg的約束條件,符合車輛運動約束,車輛在整個避撞過程中處于平穩(wěn)狀態(tài)。

4.2 動態(tài)障礙物環(huán)境

為了驗證路徑規(guī)劃器對動態(tài)障礙物的避障效果,在右車道前50 m處放置一輛動態(tài)障礙物車輛,以36 km/h速度前進,仿真結(jié)果如圖8所示。

圖8a所示為車輛以72 km/h的速度在3、6、9 s時刻障礙物車輛和被控車輛的位置關(guān)系。

從圖8a可以看出,在不同時刻下被控車輛與障礙物車輛都不會發(fā)生碰撞。從圖8b～圖8d可以看出動態(tài)障礙物環(huán)境下車輛的狀態(tài)變化:前輪轉(zhuǎn)角變化在±1°以內(nèi),橫擺角變化在±4°以內(nèi),橫向加速度變化在±0.2 m/s2內(nèi),符合車輛運動約束條件;曲線變化平穩(wěn)沒有明顯突變,符合實際駕駛習(xí)慣;質(zhì)心側(cè)偏角變化在±0.4°以內(nèi),變化較小,說明車輛在避撞過程中行駛具有良好的穩(wěn)定性。

5 結(jié) 論

本文設(shè)計了一種基于道路環(huán)境模型和MPC的避撞路徑規(guī)劃控制器。對人工勢場法進行改進,加入道路邊界的約束,添加虛擬目標(biāo)點擺脫局部最優(yōu)困境;將道路環(huán)境模型引入路徑規(guī)劃層的目標(biāo)函數(shù)中,規(guī)劃出局部最優(yōu)路徑;再將路徑信息輸入到跟蹤控制層進行實時跟蹤。仿真結(jié)果表明,對于動、靜態(tài)障礙物,車輛在不同的速度下本文避撞路徑規(guī)劃均可以實時規(guī)劃出安全平穩(wěn)的局部避撞路徑。