基于改進(jìn)人工勢場法的路徑規(guī)劃決策一體化算法研究

袁朝春 翁爍豐 何友國 SHEN Jie 陳 龍 王 桐

(1.江蘇大學(xué)汽車工程研究院， 鎮(zhèn)江 212013； 2.密歇根大學(xué)迪爾本分校， 迪爾本 MI 48128)

0 引言

路徑規(guī)劃決策算法是無人駕駛汽車的重要理論依據(jù)之一[1-3]。車輛行駛環(huán)境復(fù)雜而多變，這對無人駕駛汽車的路徑規(guī)劃決策提出了很高的實(shí)時性要求。目前常用的路徑規(guī)劃算法包括人工勢場法、網(wǎng)格法、可視圖空間法等。人工勢場法采用虛擬的斥力與引力建立梯度力場，根據(jù)梯度力場矢量方向?qū)囕v路徑進(jìn)行規(guī)劃，該算法規(guī)劃的曲線平滑，計算量較小，但易陷入局部最優(yōu)點(diǎn)[4]；網(wǎng)格法通過將當(dāng)前環(huán)境劃分為系列網(wǎng)格，并通過該網(wǎng)格是否存在障礙物判斷網(wǎng)格的可通行性，以此規(guī)劃出一條可通行路線，網(wǎng)格法需要大量的計算量，限制了算法的實(shí)時性[5]；可視圖空間法將環(huán)境中障礙物抽象為多邊形，并繪制起點(diǎn)與終點(diǎn)之間的多條連線，并在此基礎(chǔ)上篩選最優(yōu)路徑，該方法靈活性較差，在復(fù)雜變化環(huán)境下表現(xiàn)不佳[6]。

目前研究較多的路徑跟蹤算法有基于駕駛員預(yù)瞄模型的路徑跟蹤算法和基于模型預(yù)測控制算法的路徑跟蹤算法。駕駛員預(yù)瞄模型根據(jù)車輛未來的預(yù)估位置與目標(biāo)位置的差距進(jìn)行補(bǔ)償決策控制，主要分為單點(diǎn)與多點(diǎn)預(yù)瞄模型，能夠獲得較好的跟蹤控制效果[7-8]；模型預(yù)測控制算法通過設(shè)置不同的評價函數(shù)來確定路徑跟蹤過程中各參數(shù)量對車輛的重要程度，最終獲得理想的控制結(jié)果[9]。然而，現(xiàn)階段的路徑跟蹤算法環(huán)節(jié)需要較大的數(shù)據(jù)量與計算量，導(dǎo)致無人駕駛汽車的路徑規(guī)劃與決策控制環(huán)節(jié)存在著時滯，會引起檢測信息與真實(shí)行駛環(huán)境的偏差。

本文基于車輛行駛過程中的環(huán)境信息與運(yùn)動學(xué)信息對行駛碰撞風(fēng)險等級進(jìn)行評估，并且基于車輛動力學(xué)因素及環(huán)境交通參與者狀態(tài)建立改進(jìn)人工勢場模型，將無人駕駛汽車的路徑規(guī)劃與控制決策相結(jié)合建立控制與決策一體化算法，以期為無人駕駛汽車在復(fù)雜環(huán)境下的實(shí)時控制提供幫助。

1 車輛行駛環(huán)境安全性判定

車輛行駛環(huán)境動態(tài)復(fù)雜而多變。為了保證行駛過程中的安全性，本文基于車輛行駛運(yùn)動學(xué)特性與車輛行駛環(huán)境特征，建立車輛橫、縱向安全模型，以綜合反映車輛行駛過程中的安全性。

1.1 縱向安全模型

為了反映車輛在行駛過程中的縱向安全特性，本文建立了一種基于道路、車輛特征的分級安全距離模型，以體現(xiàn)車輛與環(huán)境交通參與者的不同級別的碰撞風(fēng)險，為車輛縱向速度的控制決策提供合理依據(jù)。

(1)直行工況

行駛過程中，車輛的實(shí)際制動能力受車輛性能參數(shù)、輪胎-路面摩擦因數(shù)和實(shí)際駕駛環(huán)境等的影響，假設(shè)車輛制動系統(tǒng)可以發(fā)揮道路的最大制動效能，則最大制動減速度主要受輪胎-路面摩擦因數(shù)和實(shí)際駕駛環(huán)境的影響。車輛行駛過程中，縱向方向上需保持的距離如圖1所示。

圖1 車輛縱向安全距離Fig.1 Longitudinal safety distance model

結(jié)合車輛運(yùn)動學(xué)特征并綜合道路環(huán)境、車輛因素影響，利用車輛最大制動減速度、自車車速、前車車速、前車減速度等信息建立縱向分級安全距離模型[10-12]，為

(1)

(2)

式中Dbr1、Dbr2——1、2級安全距離閾值，m

vx——車輛當(dāng)前行駛縱向速度，m/s

ar-max——車輛最大制動減速度，m/s2

vlf——當(dāng)前車道前方交通參與者速度，m/s

alf——當(dāng)前車道前方交通參與者加速度，m/s2

Tbr——自車制動響應(yīng)時間，s

(2)彎道工況

如圖2所示，彎道行駛過程中，車輛的實(shí)際行駛路徑的距離與傳感器檢測的直線距離有所偏差。

圖2 彎道行駛工況示意圖Fig.2 Vehicle driving on a bend

傳感器的測量結(jié)果Sobstacle與車輛的預(yù)期行駛軌跡存在偏差，無法準(zhǔn)確地描述自車行駛到前車位置過程中實(shí)際行駛的距離。為了保證安全判斷的準(zhǔn)確性，本文按照圖3所示的方法對交通參與者與自車之間的距離作了修正計算。

圖3 車輛在彎道行駛工況中的修正距離Fig.3 Corrected distance on a bend

自車與前方交通參與者之間的修正距離為

(3)

式中S′obstacle——修正后自車與前方交通參與者距離，m

α——車輛行駛軌跡扇形的圓心角，(°)

δ——前輪轉(zhuǎn)角，(°)

l——前后軸距之和，m

R——車輛轉(zhuǎn)向半徑，m

根據(jù)前方交通參與者修正距離與分級安全距離模型分級閾值的關(guān)系，得到車輛目前行駛環(huán)境縱向安全級別為

(4)

1.2 橫向換道安全模型

車輛行駛過程中，不僅需要合理控制縱向速度以保證車輛不會與前方交通參與者發(fā)生碰撞，還需要在當(dāng)前車道存在擁堵或靜止障礙物時控制車輛執(zhí)行換道操作。為了保證換道過程中的安全性，建立了車輛橫向換道安全模型[13-15]。

換道過程中，為了保證車輛不會與前方車輛發(fā)生碰撞事故，結(jié)合無人駕駛汽車與前向環(huán)境交通參與者的運(yùn)動學(xué)信息，建立前向換道防碰撞模型為

(5)

式中fjud——換道過程前方交通參與者碰撞風(fēng)險判別參數(shù)

Tchange——換道所需時間，s

vf——鄰車道前方交通參與者車速，m/s

af——鄰車道前方交通參與者加速度，m/s2

Sremain——預(yù)留安全車距，m

Ssf——車輛與鄰車道前方障礙物縱向距離，m

同時，為了保證車輛不干涉相鄰車道后方車輛的正常行駛，結(jié)合無人駕駛汽車與后向環(huán)境交通參與者的運(yùn)動學(xué)信息，建立了后向換道防追尾模型

(6)

式中bjud——換道過程后方交通參與者碰撞風(fēng)險判別參數(shù)

vb——鄰車道后方交通參與者車速，m/s

ab——鄰車道后方交通參與者加速度，m/s2

當(dāng)滿足條件：fjud>0和bjud>0時，車輛安全換道狀態(tài)參數(shù)Csafe為1，不滿足時，Csafe為0。

2 車輛7自由度動力學(xué)模型

為了更好地分析車輛動力學(xué)主要特征，作如下假設(shè)：直接將前輪轉(zhuǎn)角作為方向盤輸入，不考慮轉(zhuǎn)向系統(tǒng)影響；不考慮車輛垂直運(yùn)動；假設(shè)車輛為剛性系統(tǒng)，忽略懸架系統(tǒng)；忽略側(cè)向風(fēng)對車輛產(chǎn)生的影響。根據(jù)車輛特性，建立圖4所示的車輛7自由度動力學(xué)模型[16-18]。

圖4 7自由度車輛動力學(xué)受力示意圖Fig.4 Forces of seven degrees of freedom model

(1)動力學(xué)方程

7自由度車輛動力學(xué)模型包括2個平動(縱向、側(cè)向運(yùn)動)和5個轉(zhuǎn)動(橫擺運(yùn)動、4個車輪轉(zhuǎn)動)，動力學(xué)方程為

(7)

式中vy——橫向車速，m/s

r——橫擺角速度，rad/s

Fxi、Fyi、Fzi——輪胎縱向力、側(cè)向力、垂向力，i分別對應(yīng)左前輪fl、右前輪fr、左后輪rl、右后輪rr，N

m——整車質(zhì)量，kg

a、b——前后軸到質(zhì)心的距離，m

Iz——整車?yán)@Z軸的轉(zhuǎn)動慣量，kg·m2

tw1——前軸輪距，m

tw2——后軸輪距，m

Fw——車輛行駛過程中空氣阻力

(2)約束方程建立

采用魔術(shù)公式建立輪胎模型，考慮到車輛在行駛過程中由于加速或減速引起各車輪的垂直載荷偏移，建立各輪胎垂向載荷力求解公式為

(8)

式中hg——整車質(zhì)心高度，m

各輪胎側(cè)偏角計算公式為

(9)

各車輪輪心在車輪坐標(biāo)系下縱向速度為

(10)

式中vt_fl、vt_fr、vt_rl、vt_rr——輪胎坐標(biāo)系下的輪胎縱向速度，m/s

(3)Simulink模型建立

如圖5所示，使用Simulink搭建7自由度整車動力學(xué)模型。

圖5 整車動力學(xué)模型Simulink框圖Fig.5 Simulink block diagram of vehicle dynamics model

3 路徑規(guī)劃決策一體化模型

局部路徑規(guī)劃算法的核心是在全局路徑規(guī)劃算法的基礎(chǔ)上尋找2個節(jié)點(diǎn)之間的行駛最優(yōu)路徑[19]。現(xiàn)有算法需要先根據(jù)環(huán)境情況規(guī)劃一條行駛路線，通過控制器控制車輛在此路徑上進(jìn)行跟蹤。然而，智能車輛行駛過程中，環(huán)境信息與車輛自身信息處于不斷的更新狀態(tài)，傳統(tǒng)路徑規(guī)劃決策算法在規(guī)劃層與決策層的脫離可能引起無人車控制的不穩(wěn)定性。

在傳統(tǒng)路徑規(guī)劃與路徑跟蹤的基礎(chǔ)上，利用多類約束下的虛擬斥力與引力模型體現(xiàn)智能車在交通法規(guī)、交通事故風(fēng)險、車輛動力學(xué)等方面所受的約束。用路徑規(guī)劃決策一體化模型代替?zhèn)鹘y(tǒng)路徑規(guī)劃算法與路徑跟蹤控制器，為車輛提供合理的局部路徑的同時提供當(dāng)前所需的執(zhí)行機(jī)構(gòu)的最優(yōu)控制值，為復(fù)雜工況的智能車輛主動避撞規(guī)劃提供更加合理高效的方法。算法流程如圖6所示。

圖6 改進(jìn)人工勢場算法流程示意圖Fig.6 Flow chart of improved artificial potential field algorithm

3.1 人工引力建模

人工虛擬引力代表智能車輛行駛過程中對其具有吸引作用的環(huán)境因素，主要有全局規(guī)劃虛擬引力與車輛換道虛擬引力，分別用于提供智能車輛行駛過程中克服環(huán)境阻力正常行駛所需的牽引力及換道操作對車輛的牽引作用。

3.1.1全局規(guī)劃虛擬引力建模

全局規(guī)劃路線虛擬引力用于提供車輛正常行駛所需的牽引力，其大小與當(dāng)前行駛道路的最高限速、車輛在該路面行駛的滾動阻力系數(shù)以及車輛自身質(zhì)量有關(guān)。該設(shè)計虛擬引力是為保證智能車輛在無交通參與者或交通參與者碰撞威脅級別為0級的道路上能夠在該虛擬引力的作用下持續(xù)加速，并在車輛速度與當(dāng)前道路限速相同為止之后達(dá)到受力平衡狀態(tài)并保持勻速行駛，直到前方出現(xiàn)交通參與者或道路限速發(fā)生變化為止。而在前方道路有其他交通參與者時，車輛將根據(jù)該交通參與者對自車的威脅級別進(jìn)行處理，當(dāng)威脅級別處于1級時，全局規(guī)劃引力將僅提供保持車輛在當(dāng)前速度下正常行駛的力，當(dāng)威脅級別處于0級時，全局規(guī)劃虛擬引力為0，釋放車輛油門。

在無坡道的道路上，該虛擬引力表示為

(11)

式中μroll——車輛滾動阻力系數(shù)

A——車輛的等效迎風(fēng)面積,m2

Cw——車輛的風(fēng)阻系數(shù)

vlimit——車輛行駛最高速度,m/s

Kattract-plan——全局規(guī)劃虛擬引力系數(shù)

3.1.2智能車輛換道引力建模

智能車輛行駛過程中，當(dāng)遇到存在前方交通參與者或其他阻礙車輛正常行駛的狀態(tài)時，車輛需要進(jìn)行換道行駛。該引力為智能車輛的換道提供決策與執(zhí)行依據(jù)。

車輛換道引力受當(dāng)前行駛路面的滑動摩擦因數(shù)、車輛行駛速度、車輛已完成的換道距離、車輛質(zhì)量等因素決定。車輛換道引力計算式為

(12)

式中Schange——車輛已完成的橫向位移距離，m

Slane1-2——車輛與所在車道及換行車道的中線距離,m

μslide——路面滑動摩擦因數(shù)

3.2 人工斥力建模

虛擬斥力代表車輛行駛過程中對車輛行駛具有排斥作用的環(huán)境因素，用于引導(dǎo)車輛規(guī)避交通參與者(車輛、行人、道路兩側(cè)灌木等)與車輛自身特性約束(動力學(xué)特性、運(yùn)動學(xué)特性)。主要分為：前向交通參與者斥力與前方傳感器檢測盲區(qū)虛擬斥力。

3.2.1前向交通參與者斥力建模

前向交通參與者斥力用于表征前方交通參與者對于車輛產(chǎn)生的影響，能充分反映前方交通參與者對于自車的威脅，可以保證車輛在發(fā)生碰撞之前完成制動或減速。

該斥力與車輛當(dāng)前行駛速度、車輛與前方交通參與者理論安全距離、當(dāng)前路面附著力系數(shù)、整車質(zhì)量、前方交通參與者運(yùn)動速度有關(guān)。以建立的交通參與者篩選算法與車輛安全距離模型作為依據(jù)，對車輛運(yùn)動學(xué)特性進(jìn)行合理分析，建立前方交通參與者斥力模型為

(13)

式中Kr-obstacle——前向斥力系數(shù)

μroad——當(dāng)前路面最大附著力系數(shù)

Srem——行駛過程中車輛與前方交通參與者預(yù)留安全距離，m

3.2.2前方傳感器檢測盲區(qū)斥力建模

傳感器檢測盲區(qū)斥力用于反映車輛在前方存在傳感器檢測盲區(qū)時的制動減速度特性。該模型根據(jù)傳感器檢測的盲區(qū)邊界，在該邊界的基礎(chǔ)上控制車輛進(jìn)行制動減速，以保證車輛在該力的作用下能夠避免盲區(qū)內(nèi)突然出現(xiàn)交通參與者引起制動距離不足而導(dǎo)致的交通事故。

該虛擬斥力主要由前方傳感器盲區(qū)邊界與自車距離、自車行駛速度、當(dāng)前道路附著力系數(shù)確定，計算式為

(14)

式中Kr-blind——前方傳感器檢測盲區(qū)斥力系數(shù)

Sblind——盲區(qū)邊界與車輛當(dāng)前距離，m

4 Carsim/Simulink聯(lián)合仿真分析

為了驗(yàn)證提出模型的有效性與優(yōu)越性，使用Carsim/Simulink建立了多個仿真工況，對算法有效性與時滯性進(jìn)行了分析。

4.1 算法有效性驗(yàn)證

仿真設(shè)車輛迎風(fēng)面積為1.6 m2，風(fēng)阻系數(shù)為0.3，質(zhì)量為1 100 kg，車輛滾動系數(shù)為0.014，道路附著力系數(shù)設(shè)為0.85，方向盤與前輪轉(zhuǎn)角傳動系數(shù)為20。

4.1.1全局規(guī)劃虛擬引力模型驗(yàn)證

為了驗(yàn)證該模型的有效性，設(shè)置了不同的道路限速工況。工況1，車輛以初速度40 km/h在直線車道上行駛，將道路速度分別限制為60、80 km/h。車輛速度與加速度變化曲線如圖7所示。

圖7 車輛速度與加速度變化曲線(工況1)Fig.7 Diagram of vehicle speed and acceleration (condition 1)

由圖7可以看出，在全局規(guī)劃虛擬引力的作用之下，車輛在無交通參與者道路上行駛時將保持一個逐漸減少的加速度進(jìn)行持續(xù)加速，并在車輛速度達(dá)到當(dāng)前限速值之后保持勻速直線行駛。

工況2，車輛在直線車道上行駛，道路前1 km的限速為60 km/h,后續(xù)道路限速為80 km/h。車輛的速度與加速度變化曲線如圖8所示。

由圖8可以看出，在全局規(guī)劃虛擬引力的作用下，前半段道路上，車輛將持續(xù)加速行駛，向60 km/h的速度持續(xù)逼近，并在道路限速改變之后改變加速度，控制車速向80 km/h的速度逼近。

工況3，車輛在直線車道上行駛，道路前1 km的限速為80 km/h,后續(xù)道路限速為60 km/h。車輛的速度與加速度變化曲線如圖9所示。

由圖9可以看出，在全局規(guī)劃虛擬引力的作用下，前半段道路上，車輛將持續(xù)加速行駛，向80 km/h的速度持續(xù)逼近。道路限速改變之后，車輛的全局規(guī)劃虛擬引力小于車輛當(dāng)前所受的行駛阻力的，車輛將減速，車速向60 km/h的速度逼近。

通過上述3種工況的仿真，可以看出，全局規(guī)劃虛擬引力可以引導(dǎo)車輛速度向當(dāng)前路面的限速條件不斷逼近，直到車輛行駛速度達(dá)到當(dāng)前道路允許的最大行駛速度。

圖8 車輛速度與加速度變化曲線(工況2)Fig.8 Diagram of vehicle speed and acceleration (condition 2)

圖9 車輛速度與加速度變化曲線(工況3)Fig.9 Diagram of vehicle speed and acceleration (condition 3)

4.1.2換道引力模型驗(yàn)證

為了驗(yàn)證該模型的有效性，設(shè)置了3種行駛工況，車輛分別以60、80、100 km/h的速度行駛，在換道引力與車輛行駛引力的綜合作用下進(jìn)行換道操作，方向盤轉(zhuǎn)角變化、車輛側(cè)向加速度變化、車輛位置變化、車輛質(zhì)心側(cè)偏角變化如圖10所示。

圖10 方向盤轉(zhuǎn)角、車輛側(cè)向加速度、車輛位置與車輛質(zhì)心側(cè)偏角變化曲線Fig.10 Diagram of steering angle, lateral acceleration, vehicle position and side slip angle

如圖10所示，不同行駛速度下，車輛的方向盤轉(zhuǎn)角峰值隨車速的增加而減??；不同車速下，車輛側(cè)向加速度的峰值均可控制在1.5 m/s2以內(nèi)，可保證良好的乘坐感受；車輛位置變化為平滑曲線，轉(zhuǎn)向過渡自然；車輛質(zhì)心側(cè)偏角未超過0.2°，保證了良好的車輛穩(wěn)定性。

4.1.3前向交通參與者斥力模型驗(yàn)證

為了驗(yàn)證該模型的有效性，針對不同的前方交通參與者場景與自車速度進(jìn)行了仿真分析。設(shè)定前方交通參與者行駛速度為20 km/h，車輛與前方交通參與者初始距離為70 m，車輛以60、80 km/h的速度沿道路行駛，車輛速度、車輛與前方交通參與者距離變化曲線如圖11所示。

圖11 車輛速度及車輛與前方交通參與者距離變化曲線Fig.11 Diagram of vehicle speed and distance from obstacle ahead

如圖11所示，在車輛與前方交通參與者的距離小于安全距離模型所規(guī)定的閾值之后，此時，車輛前向交通參與者斥力大于道路行駛引力，車輛在合力的作用下減速，在車輛與前方交通參與者的距離大于安全距離模型所規(guī)定的閾值之后，車輛又會在道路行駛引力的作用下加速，最終，車輛與前方交通參與者的距離穩(wěn)定在安全距離模型所允許的值附近。綜上，車輛前向交通參與者斥力可以保證行駛中的前向碰撞安全性。

4.1.4傳感器盲區(qū)斥力模型驗(yàn)證

為了驗(yàn)證該模型的有效性，設(shè)定了一種傳感器盲區(qū)突然出現(xiàn)交通參與者的工況，該工況為：車輛以60 km/h的速度在道路上行駛，前方100 m處車輛右前方有一傳感器盲區(qū)，傳感器盲區(qū)于5.5 s時突然出現(xiàn)交通參與者，分別在未考慮傳感器盲區(qū)斥力與考慮傳感器盲區(qū)斥力的基礎(chǔ)上分別進(jìn)行仿真分析，車輛速度與加速度曲線如圖12所示。

由圖12可以看出，考慮盲區(qū)邊界條件的情況下，車輛會較早地對盲區(qū)風(fēng)險做出預(yù)判而提前減速，在交通參與者出現(xiàn)后，車輛由于預(yù)判減速的存在，留有足夠的制動距離，從而規(guī)避碰撞事故的發(fā)生，而未考慮盲區(qū)的情況下，車輛在發(fā)現(xiàn)前方交通參與者之后才開始制動減速，制動距離過短，易導(dǎo)致碰撞事故的發(fā)生。

圖12 車輛與盲區(qū)障礙物距離、速度、加速度變化曲線Fig.12 Diagram of vehicle distance from blind area, vehicle speed and vehicle acceleration

4.2 算法時滯分析

仿真實(shí)驗(yàn)所使用的運(yùn)算平臺CPU處理器為Intel G4560，RAM為4 GB。Matlab版本為R2014a，Carsim版本為8.1。

選用基于MPC的軌跡規(guī)劃算法與基于MPC的路徑跟蹤算法作為參照對象，建立圖13所示的聯(lián)合仿真模型[20]，規(guī)劃量輸出頻率為50 Hz，設(shè)定仿真時間為5 s，使用基于Matlab中profile函數(shù)對算法進(jìn)行了運(yùn)行時間分析，重復(fù)進(jìn)行10次，不計Carsim動力學(xué)模型耗時，得到的結(jié)果如表1所示。綜合多次實(shí)驗(yàn)結(jié)果可知，該算法單次運(yùn)行所需時間為0.06 s。

搭建圖14所示的基于改進(jìn)人工勢場法的路徑規(guī)劃決策一體化算法模型，使用profile函數(shù)對其進(jìn)行時滯分析。控制量輸出頻率為1 000 Hz，運(yùn)行時間為5 s，重復(fù)10次的程序運(yùn)行耗時數(shù)據(jù)如表2所示，由表2可知，單次規(guī)劃決策平均運(yùn)行時間為5.5×10-5s。

由表1、2可知，本文提出的路徑?jīng)Q策規(guī)劃一體化算法在時滯方面表現(xiàn)更優(yōu)。

圖13 基于MPC的軌跡規(guī)劃算法與基于MPC的路徑跟蹤算法模型Fig.13 Model of MPC-based trajectory planning algorithm and MPC-based path tracking

s

圖14 基于改進(jìn)人工勢場法的路徑規(guī)劃決策一體化算法模型Fig.14 Integrated algorithm model of path planning and decision-making based on improved artificial potential field method

s

5 結(jié)論

(1)針對車輛行駛環(huán)境特征與自身運(yùn)動學(xué)特征對智能車行駛過程中橫縱向風(fēng)險進(jìn)行了分析，綜合反映了智能車行駛過程中的環(huán)境安全性。

(2)針對智能車輛傳感器盲區(qū)作了盲區(qū)斥力分析，避免傳感器盲區(qū)帶來的車輛碰撞風(fēng)險。

(3)提出了路徑?jīng)Q策規(guī)劃一體化算法的概念，根據(jù)車輛與環(huán)境信息建立了改進(jìn)人工勢場模型，利用模型在規(guī)劃車輛行駛路徑的同時給出車輛控制量，減小了規(guī)劃決策過程中時滯的影響。