基于改進人工勢場法的智能車輛避撞路徑規(guī)劃

孔慧芳, 夏 露, 張 倩

(合肥工業(yè)大學 電氣與自動化工程學院,安徽 合肥 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ī)劃,但是該算法沒有考慮到車輛動力學約束和道路邊界的約束,且容易陷入局部最優(yōu)值。針對這些問題,各國學者進行了大量的優(yōu)化研究。文獻[6]在傳統(tǒng)的斥力場中加入調(diào)節(jié)因子,解決了障礙物與目標點過近的局部最優(yōu)值問題,但是沒有考慮到道路的約束;文獻[7]針對局部最優(yōu)值問題臨時增設子目標點,通過變換目標點位置改變勢場空間,但子目標點的選取方法不適用于道路上的車輛;文獻[8]利用橢圓化距離代替?zhèn)鹘y(tǒng)斥力勢場中的實際距離,同時引入道路邊界斥力場模型,獲得汽車避撞局部路徑,但沒有考慮陷入局部極小值的問題;文獻[9]建立人工勢場模型,采用梯度下降算法得到路徑,但該方法規(guī)劃的路徑可能不符合車輛動力學約束。

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

本文基于車輛動力學模型,考慮道路邊界的約束,為了避免局部最優(yōu)值,添加虛擬目標點,建立道路環(huán)境模型;將模型與模型預測控制相結(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設立終點,勢場在轉(zhuǎn)向區(qū)域沿車道中心線逐漸下降,所構(gòu)建的方向引力勢場函數(shù)表達式如下:

(1)

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

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

圖2 引力勢場三維分布

1.2 障礙物斥力勢場模型

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

(2)

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

(3)

L2=vrt+Smin

(4)

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

根據(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 虛擬目標點

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

M=λL2

(7)

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

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

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

(8)

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

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

圖5 虛擬目標點選取方式

車輛在道路上主要受到方向引力勢場、障礙物斥力勢場和道路勢場的作用,當車輛陷入局部最優(yōu)困境時,受到虛擬目標點勢場的作用,幾種勢場共同構(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ī)劃器設計

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

(11)

(12)

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

(13)

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

(14)

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

(15)

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

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

|ay|<μg

(16)

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

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

φ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 車輛動力學模型

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

(18)

在輪胎力的計算過程中,為了簡化計算,基于小角度假設建立車輛動力學模型,即滿足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)式可以得到車輛的動力學模型如下:

(22)

3.2 跟蹤控制器設計

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

(23)

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

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

(24)

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

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

(25)

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

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

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

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

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

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

為了驗證本文方法能夠更好地符合車輛動力學性能,將其與人工勢場法[9]對比仿真,設置被控車輛車速為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),變化范圍較小,滿足基于小角度假設的模型建立;質(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)沒有明顯突變,符合實際駕駛習慣;質(zhì)心側(cè)偏角變化在±0.4°以內(nèi),變化較小,說明車輛在避撞過程中行駛具有良好的穩(wěn)定性。

5 結(jié) 論

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