基于改進(jìn)人工勢(shì)場(chǎng)法的智能汽車軌跡規(guī)劃算法研究

趙奉奎，葛 振，董鋒威，張 涌

(南京林業(yè)大學(xué) 汽車與交通工程學(xué)院，江蘇 南京 210037)

0 引 言

避障路徑規(guī)劃技術(shù)是自動(dòng)駕駛汽車關(guān)鍵技術(shù)之一，在環(huán)境感知層獲得行駛環(huán)境的信息后，軌跡規(guī)劃模塊可按照不同的行為決策，為自動(dòng)駕駛汽車規(guī)劃出一條從起點(diǎn)到目標(biāo)位置的最優(yōu)路徑。根據(jù)行駛場(chǎng)景的不同，路徑規(guī)劃技術(shù)主要分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃主要是根據(jù)預(yù)先存儲(chǔ)的地圖進(jìn)行路徑規(guī)劃，如在現(xiàn)有電子地圖中設(shè)定一條連接兩城市的最優(yōu)路徑。汽車在道路上行駛時(shí)，行駛環(huán)境復(fù)雜，需要利用視覺或雷達(dá)實(shí)時(shí)感知周圍環(huán)境[1]，進(jìn)行局部路徑規(guī)劃。路徑規(guī)劃算法不僅能及時(shí)準(zhǔn)確的規(guī)劃出避障路徑，而且能規(guī)劃出易于汽車跟蹤的路徑，以及具有較高的乘坐舒適性[2]。

國內(nèi)外學(xué)者對(duì)局部路徑規(guī)劃開展了廣泛的研究。王富奎[3]將避障路徑規(guī)劃問題轉(zhuǎn)換多約束優(yōu)化問題，設(shè)定了參考速度、加速度、車輛動(dòng)力學(xué)約束、車道約束等若干約束目標(biāo)，據(jù)此建立優(yōu)化模型高維圖，采用圖優(yōu)化法進(jìn)行求解；雷艷敏等[4]將最短路徑、安全性能和平滑度綜合考慮，得到了一種用于路徑規(guī)劃的全新的遺傳算法；田欣[5]基于遺傳算法提出了一種新的自適應(yīng)調(diào)整方式，提高了算法的效率，引入了模擬退火算法來克服局部最優(yōu)解的缺陷。此外，仍有基于傳統(tǒng)路徑規(guī)劃算法展開的研究，如基于采樣的RRT算法[6]和基于圖搜索的A*算法[7]，以及針對(duì)結(jié)構(gòu)化道路提出的路徑規(guī)劃算法[8]。但當(dāng)前多數(shù)算法較為復(fù)雜，求解難度較大，算法效率低，不利于實(shí)時(shí)路徑規(guī)劃。

人工勢(shì)場(chǎng)法是一種常用的路徑規(guī)劃的算法，具有算法結(jié)構(gòu)簡(jiǎn)單，實(shí)時(shí)性好，規(guī)劃出的路徑較為平滑等優(yōu)點(diǎn)。當(dāng)前對(duì)于人工勢(shì)場(chǎng)法的改進(jìn)主要集中在解決其固有的兩個(gè)缺陷——局部最優(yōu)解和目標(biāo)不可達(dá)[9-12],也有將其與其他算法相結(jié)合，段建民等[13]提出一種改進(jìn)人工勢(shì)場(chǎng)法與遺傳算法結(jié)合的方法來尋求全局最優(yōu)解，并引入填平勢(shì)場(chǎng)跳出局部極小點(diǎn)。同時(shí)也出現(xiàn)許多針對(duì)自動(dòng)駕駛汽車的改進(jìn)方案，譚寶成等[14]優(yōu)化了經(jīng)典人工勢(shì)場(chǎng)法中圓形虛擬勢(shì)場(chǎng)的作用域，能夠?qū)崿F(xiàn)結(jié)構(gòu)化道路中較小角度的避障；王樹鳳等[15]將障礙物模型改進(jìn)為橢圓模型，引入了基于前車工況的斥力系數(shù)以及局部虛擬目標(biāo)點(diǎn)；曹昊天等[16]在人工勢(shì)場(chǎng)法的基礎(chǔ)上，增加了引導(dǎo)車輛避障的引導(dǎo)勢(shì)場(chǎng)，使避障過程更為安全。當(dāng)前基于人工勢(shì)場(chǎng)法的路徑規(guī)劃算法存在很多問題，如算法未考慮車輛在不同速度下的勢(shì)場(chǎng)影響范圍，未考慮車輛運(yùn)動(dòng)學(xué)約束和車輛實(shí)際行駛的環(huán)境，以及未考慮對(duì)動(dòng)態(tài)障礙物的處理等[17]。

因此提出了一種改進(jìn)型的人工勢(shì)場(chǎng)法，在進(jìn)行路徑規(guī)劃時(shí)加入車輛運(yùn)動(dòng)學(xué)約束，使規(guī)劃的路徑滿足當(dāng)前速度下車輛前輪最大轉(zhuǎn)角約束。研究了超車場(chǎng)景路徑規(guī)劃，設(shè)計(jì)了基于安全車距的不規(guī)則障礙物模型，在設(shè)定的道路約束下，實(shí)現(xiàn)該算法對(duì)動(dòng)態(tài)障礙物的避障路徑規(guī)劃處理。

1 理論基礎(chǔ)

1.1 經(jīng)典人工勢(shì)場(chǎng)法

目標(biāo)點(diǎn)產(chǎn)生的引力勢(shì)場(chǎng)記為Uatt，假設(shè)車輛為一質(zhì)點(diǎn)，引力勢(shì)能的大小與目標(biāo)點(diǎn)和車輛所在位置間的距離有關(guān)，距離越遠(yuǎn)，車輛受引力勢(shì)能的影響就越強(qiáng)，距離越近，車輛受引力勢(shì)能的影響就越小，目標(biāo)點(diǎn)處的引力勢(shì)能為0。將車輛的運(yùn)動(dòng)空間抽象成二維平面，在整個(gè)運(yùn)動(dòng)空間內(nèi)的引力勢(shì)能可表示為：

(1)

式中：Katt為引力勢(shì)能比例增益因子；X為車輛當(dāng)前位置的坐標(biāo)；Xg為目標(biāo)點(diǎn)位置坐標(biāo)；m為引力勢(shì)場(chǎng)因子，一般取值為2；ρ為車輛當(dāng)前位置距目標(biāo)點(diǎn)的距離。

引力勢(shì)場(chǎng)對(duì)車輛產(chǎn)生的引力記為Fatt，那么Fatt為引力勢(shì)能的負(fù)梯度，即沿引力勢(shì)能下降最快的方向，其可表示為：

Fatt(X)=-Katt·ρ(X,Xg)

(2)

與引力勢(shì)場(chǎng)相似，車輛在行駛過程中還會(huì)受到由障礙物產(chǎn)生的斥力勢(shì)場(chǎng)的作用，其對(duì)車輛的影響也與車輛與障礙物的距離有關(guān)。當(dāng)車輛與障礙物越近時(shí)，障礙物產(chǎn)生的斥力勢(shì)場(chǎng)對(duì)車輛的影響越大，超過一定距離時(shí)，斥力勢(shì)場(chǎng)便不再對(duì)車輛產(chǎn)生影響。與引力勢(shì)能函數(shù)類似，構(gòu)建函數(shù)為：

(3)

式中：Krep為斥力勢(shì)能比例增益因子；Xo為障礙物位置坐標(biāo)；ρo為車輛受障礙物影響的最大距離。

與引力的求法同理，斥力勢(shì)場(chǎng)對(duì)車輛產(chǎn)生的斥力記為Frep，那么Frep為斥力勢(shì)能的負(fù)梯度，即沿斥力勢(shì)能下降最快的方向，可表示為：

(4)

由于運(yùn)動(dòng)空間中每個(gè)位置處的引力勢(shì)能大小和斥力勢(shì)能大小為標(biāo)量，根據(jù)疊加定理，運(yùn)動(dòng)空間中的總勢(shì)能大小如式(5):

U(X)=Uatt(X)+Urep(X)

(5)

從而，車輛所受到的合力F(X)為：

F(X)=Fatt(X)+Frep(X)

(6)

1.2 線性二自由度車輛運(yùn)動(dòng)學(xué)模型

由于車輛運(yùn)動(dòng)學(xué)系統(tǒng)極為復(fù)雜，為了便于分析，將四輪車輛簡(jiǎn)化為兩輪的單車模型，需做如下假設(shè)：

1)汽車的縱向行駛速度u恒定不變。

2)忽略轉(zhuǎn)向系統(tǒng)的影響，以前輪轉(zhuǎn)角作為輸入，后輪轉(zhuǎn)角始終為0。

3)忽略懸架系統(tǒng)的影響，認(rèn)為車輛僅做平行于地面的平面運(yùn)動(dòng)，即忽略汽車的垂向運(yùn)動(dòng)、俯仰運(yùn)動(dòng)以及側(cè)傾運(yùn)動(dòng)。

4)不考慮載荷的左右轉(zhuǎn)移，即前軸和后軸的左右輪分別等效為一個(gè)輪。

5)忽略橫縱向空氣動(dòng)力學(xué)。

6)側(cè)向加速度小于0.4g，以保證側(cè)偏剛度為常數(shù)。

于是汽車被簡(jiǎn)化成僅有側(cè)向和橫擺兩自由度的單車模型，如圖1。根據(jù)余志生[18]的《汽車?yán)碚摗?，?dāng)汽車運(yùn)動(dòng)時(shí)，汽車質(zhì)心加速度ay沿Oy軸方向上的分量和線型二自由度模型的運(yùn)動(dòng)微分方程分別為：

(7)

(8)

式中：k1，k2分別為前輪側(cè)偏剛度、后輪側(cè)偏剛度；β為質(zhì)心側(cè)偏角；v1為車輛質(zhì)心速度；u，v分別為車輛質(zhì)心速度在車輛坐標(biāo)系中沿x軸和y軸的分量；a，b分別為質(zhì)心至前軸的距離和質(zhì)心至后軸的距離；δ為前輪轉(zhuǎn)角；ωr為車輛橫擺角速度；a1，a2分別為前輪側(cè)偏角和后輪側(cè)偏角；m為車輛的質(zhì)量；IZ為車輛的轉(zhuǎn)動(dòng)慣量。

(9)

式(9)便是車輛前輪轉(zhuǎn)角和速度的運(yùn)動(dòng)學(xué)約束，現(xiàn)假設(shè)K=0，車輛為中性轉(zhuǎn)向，軸距L為3 m，代入式(9)，以前輪最大轉(zhuǎn)角為因變量，速度為自變量，作出關(guān)系圖，如圖2。由圖2可知，前輪最大轉(zhuǎn)角與車速呈反比關(guān)系。因此，車輛在某一速度下，只需前輪轉(zhuǎn)角的值位于圖2的曲線下方，即可滿足車輛的運(yùn)動(dòng)學(xué)約束。

圖1 二自由度車輛運(yùn)動(dòng)學(xué)模型Fig. 1 Two-degree-of-freedom vehicle kinematics model

圖2 前輪角度隨速度的變化曲線Fig. 2 Variation curve of front wheel angle changing with speed

2 改進(jìn)型人工勢(shì)場(chǎng)法

2.1 引入道路約束

道路約束是指由右車道中心線產(chǎn)生的引力勢(shì)場(chǎng)，如圖3。在該勢(shì)場(chǎng)內(nèi)的車輛會(huì)受到指向右車道中心線的引力，以保持沿右車道行駛，同時(shí)防止越過道路邊界。因此，距右車道中心線越遠(yuǎn)，引力勢(shì)能的值越大。

為滿足上述要求，采用基于二次函數(shù)的勢(shì)場(chǎng)分布模型。如圖3(a)，當(dāng)車輛位于右車道中心線的左側(cè)時(shí)，記車輛質(zhì)心所在位置為點(diǎn)A，dLM為點(diǎn)A距右車道中心線的距離，ULroad(dLM) 為右車道中心線在其左側(cè)dLM處產(chǎn)生的引力勢(shì)能，簡(jiǎn)稱左引力勢(shì)能，?。?/p>

(10)

式中：KLroad為左側(cè)道路約束系數(shù)，用于左引力勢(shì)能的縮放。同理，如圖3(b)，右引力勢(shì)能為：

(11)

式中：KRroad為右側(cè)道路約束系數(shù)，用于右引力勢(shì)能縮放；dRM為點(diǎn)A距右車道中心線的距離。

圖3 道路約束示意Fig. 3 Schematic diagram of road restraint

為保證道路的左右邊界具有相同的約束作用，道路左右邊界處的引力勢(shì)能的大小應(yīng)相等，即：

(12)

化簡(jiǎn)可得：

9×KLroad=KRroad

(13)

于是可得到道路約束引力勢(shì)場(chǎng)的總分布圖，如圖4。

圖4 道路約束引力勢(shì)場(chǎng)分布Fig. 4 Distribution of road constrained gravitational potential field

2.2 引入二自由度運(yùn)動(dòng)學(xué)約束

在經(jīng)典人工勢(shì)場(chǎng)法中，自車遇到障礙物進(jìn)行避障時(shí)，人工勢(shì)場(chǎng)法會(huì)規(guī)劃出現(xiàn)近乎直角的路徑，如圖5，顯然車輛很難跟蹤該路徑，因此需要在路徑規(guī)劃時(shí)引入車輛模型約束。車輛按照某一速度行駛時(shí)，可根據(jù)圖2得到此時(shí)的最大前輪轉(zhuǎn)角，算法所規(guī)劃的路徑點(diǎn)如圖6，任意相鄰的3個(gè)路徑點(diǎn)角度差Δθi不能大于當(dāng)前速度最大前輪轉(zhuǎn)角[19]，如式(14):

Δθ≤δmax

(14)

圖5 經(jīng)典人工勢(shì)場(chǎng)法路徑規(guī)劃結(jié)果Fig. 5 Path planning results of the classical artificial potential field method

圖6 相鄰路徑點(diǎn)示意Fig. 6 Schematic diagram of adjacent path points

2.3 改進(jìn)障礙物模型

在車輛實(shí)際行駛過程中，遇到的障礙物多為靜止或運(yùn)動(dòng)的汽車，其外形可近似為矩形。在人工勢(shì)場(chǎng)法中，當(dāng)車輛前方存在障礙車輛需要進(jìn)行超車路徑規(guī)劃時(shí)，由于障礙車輛后方的勢(shì)場(chǎng)影響范圍在沿車輛橫向方向(如圖7，Y方向)各點(diǎn)大小相等，不利于引導(dǎo)自車進(jìn)行變道，據(jù)此，有學(xué)者提出了Yukawa車輛模型。Yukawa模型將車身后部按照距離衰減的斥力勢(shì)場(chǎng)用一個(gè)楔形勢(shì)場(chǎng)來表示，其分布規(guī)律按照某一特定函數(shù)來約束，如圖8。

圖7 車輛-障礙物坐標(biāo)Fig. 7 Vehicle-obstacle coordinates

圖8 Yukawa車輛障礙物模型Fig. 8 Yukawa vehicle obstacle model

為了簡(jiǎn)化Yukawa模型，采用橢圓形的改進(jìn)斥力勢(shì)場(chǎng)模型，從而便于引導(dǎo)后方車輛進(jìn)行超車或避障時(shí)的變道。勢(shì)場(chǎng)分布圖如圖9。該模型由車輛矩形、斥力勢(shì)能峰值區(qū)和斥力勢(shì)場(chǎng)影響范圍3個(gè)同心圖形組成。車輛矩形描述了車輛的二維空間尺寸。

經(jīng)典人工勢(shì)場(chǎng)法中的單個(gè)障礙物是由若干“作用點(diǎn)”及其勢(shì)能作用范圍組成的，由式(3)可知，“作用點(diǎn)”中心處的斥力勢(shì)能為峰值(趨近于正無窮)。將前述的“作用點(diǎn)”轉(zhuǎn)化為單個(gè)車輛的及其對(duì)應(yīng)的安全范圍，即斥力勢(shì)能峰值不再是存在于“作用點(diǎn)”的中心處，而是“作用點(diǎn)”周圍的一整個(gè)區(qū)域內(nèi)，即斥力勢(shì)能峰值區(qū)。由圖9可知，該斥力勢(shì)能峰值區(qū)由前部半橢圓區(qū)、中部矩形區(qū)和后部半橢圓區(qū)共3部分組成。后半橢圓區(qū)是為了引導(dǎo)車輛超車時(shí)第1次變道而設(shè)定的，相比矩形和圓形的勢(shì)場(chǎng)范圍，其曲率較小且沿道路方向的變化較為緩慢，極易引導(dǎo)車輛進(jìn)行變道。此外，車輛與前方障礙車輛具有最小的跟車距離，即安全車距。因此，取后半橢圓長(zhǎng)軸長(zhǎng)AR為：

AR=KERDmin

(15)

式中：KER為后半橢圓的長(zhǎng)軸縮放系數(shù)，取值為1；Dmin為安全車距，根據(jù)游峰等[20]提出的基于縱向安全距離的超車預(yù)警模型(圖10)，可確定不同相對(duì)速度下的安全距離Dmin。

前部半橢圓區(qū)用于在一定程度上引導(dǎo)后方車輛超車后進(jìn)行并道，同時(shí)為了保持一定安全車距。由此確定該半橢圓的長(zhǎng)軸長(zhǎng)AF為：

AF=KEFDmin

(16)

式中：KEF為前半橢圓的長(zhǎng)軸縮放系數(shù)。因?qū)嶋H駕駛時(shí)超車車輛的速度較快，故超車后可再為抵達(dá)安全車距的位置時(shí)提前返回右向車道，因此可以取KEF為0.5。

圖9 改進(jìn)障礙物斥力作用域模型Fig. 9 Improved obstacle repulsion scope model

圖10 恒速條件下自車與障礙車輛的碰撞區(qū)域Fig. 10 The collision area between the own vehicle and the obstacle vehicle under constant speed conditions

由于在結(jié)構(gòu)化道路上，同一車道僅允許1輛車行駛，因此可以取中部矩形區(qū)的寬度為車道寬度，長(zhǎng)度為車長(zhǎng)，兩個(gè)半橢圓的短軸長(zhǎng)與中部矩形區(qū)的寬度相等，即半橢圓的短軸長(zhǎng)為車道寬度。

斥力勢(shì)能峰值區(qū)外部即為障礙物產(chǎn)生的斥力勢(shì)場(chǎng)的作用范圍，大于此范圍，斥力勢(shì)能為0?？紤]駕駛環(huán)境為雙向車道和實(shí)際超車環(huán)境，確定斥力勢(shì)場(chǎng)作用范圍邊界與斥力勢(shì)能峰值區(qū)邊界之間的距離為2 m。

3 綜合仿真

3.1 仿真效果分析

車輛在實(shí)際避障和超車時(shí)應(yīng)考慮諸多因素，如障礙物(車流)密度、與障礙物的安全距離以及對(duì)向是否有車輛等。為了簡(jiǎn)化模型，筆者設(shè)定駕駛環(huán)境為雙向車道，且僅存在自車和1輛行駛中的障礙車輛，在該環(huán)境下模擬超車路徑規(guī)劃。車輛和道路尺寸參數(shù)來自表1。設(shè)定自車車速為勻速25 m/s，障礙車輛車速為勻速15 m/s，相對(duì)速度大小為10 m/s，根據(jù)圖2可得到該車速下前輪最大轉(zhuǎn)角約為π/200 rad(約為0.9°)。根據(jù)圖10，確定縱向安全車距Dmin為23 m。設(shè)定目標(biāo)點(diǎn)為車輛前方Dmin處，而車輛在實(shí)際運(yùn)行過程中，自車和障礙車輛的位置是不斷變化的，因此，在設(shè)定好目標(biāo)點(diǎn)后，自車每到達(dá)一個(gè)位置都要更新目標(biāo)點(diǎn)的位置，以保持車輛在避障后仍然與周圍車輛保持安全車距，然后重新計(jì)算到達(dá)新的目標(biāo)點(diǎn)的路徑。

表1 仿真參數(shù)Table 1 Simulation parameter table

在MATLAB中腳本文件中編寫仿真程序，運(yùn)行結(jié)果如圖11。從圖11中可以看出，改進(jìn)的人工勢(shì)場(chǎng)法能較好的完成針對(duì)運(yùn)動(dòng)障礙物的避障路徑規(guī)劃，其所規(guī)劃的路徑更加接近車輛實(shí)際行駛的路徑，規(guī)劃出的路徑較為光滑，說明改進(jìn)的障礙物模型能較好的引導(dǎo)車輛做出變道行為。

圖11 仿真結(jié)果的6個(gè)時(shí)間節(jié)點(diǎn)Fig. 11 Six time nodes of the simulation results

3.2 實(shí)驗(yàn)對(duì)比分析

通過對(duì)比基于Yukawa模型和改進(jìn)模型的仿真規(guī)劃路徑來驗(yàn)證所提出的模型的優(yōu)越性，對(duì)比效果如圖12。由于改進(jìn)模型的后部采用橢圓，使得規(guī)劃出的路徑更接近于左側(cè)車道中心位置，超車車輛與被超車車輛之間的橫向距離相對(duì)較大，增加了超車的安全性；改進(jìn)模型的前部也采用橢圓，從圖12中可以看出在超車即將完成，改進(jìn)模型使得超車車輛回到右側(cè)車道的時(shí)間比較晚，兩車之間的縱向距離明顯較大，能更有效避免后車突然加速導(dǎo)致的交通事故，并且規(guī)劃出的路徑更為平滑。

圖12 不同模型規(guī)劃路徑對(duì)比Fig. 12 Comparison of planning paths of different models

為了驗(yàn)證引進(jìn)二自由度運(yùn)動(dòng)學(xué)約束的有效性，在保證其它條件不變的情形下，對(duì)沒有引進(jìn)二自由度運(yùn)動(dòng)學(xué)約束進(jìn)行仿真，規(guī)劃出的路徑如圖13，前輪轉(zhuǎn)角如圖14(a)，盡管能實(shí)現(xiàn)超車的任務(wù)，但是可以看出在超車任務(wù)后半程，車頭會(huì)劇烈晃動(dòng)，經(jīng)過計(jì)算最大晃動(dòng)角度可達(dá)到1.14 rad(約為65.39°)，嚴(yán)重影響了車輛行駛的穩(wěn)定性和乘坐舒適性。圖14(b)為引入二自由度運(yùn)動(dòng)學(xué)約束時(shí)各路徑點(diǎn)處的車輛前輪轉(zhuǎn)角分布圖，車輛前輪轉(zhuǎn)角被控制在π/200內(nèi)，轉(zhuǎn)角變化較小，最大晃動(dòng)角度只有0.016 rad(約為0.9°),車輛行駛較為穩(wěn)定。經(jīng)過對(duì)比和分析，發(fā)現(xiàn)引進(jìn)二自由度運(yùn)動(dòng)學(xué)約束極大得增加了車輛行駛的穩(wěn)定性，驗(yàn)證了引進(jìn)二自由度運(yùn)動(dòng)學(xué)約束的有效性。

圖13 無運(yùn)動(dòng)學(xué)約束規(guī)劃路徑Fig. 13 Planning path without kinematic constraints

圖14 前輪轉(zhuǎn)角分布Fig. 14 Front wheel angle distribution

為了驗(yàn)證引進(jìn)道路約束得有效性，筆者在保證其它條件不變的情形下，刪除道路約束對(duì)仿真的影響，規(guī)劃出的路徑如圖15。對(duì)比圖12和圖15可以看出當(dāng)超車車輛在左側(cè)車道時(shí)，在無道路約束的條件下車輛與道路左側(cè)邊界的距離非常近，容易造成車輛脫離道路的風(fēng)險(xiǎn)。通過對(duì)比和分析，說明引進(jìn)道路約束是有效的。

圖15 無道路約束規(guī)劃路徑Fig. 15 Planning path without road constraints

4 結(jié) 語

對(duì)經(jīng)典人工勢(shì)場(chǎng)法進(jìn)行了改進(jìn)，引入道路約束和運(yùn)動(dòng)學(xué)約束，使規(guī)劃的路徑更加符合車輛實(shí)際行駛的特性；依據(jù)超車安全距離模型改進(jìn)了障礙物模型，調(diào)整了障礙物模型的結(jié)構(gòu)以及作用范圍，使其便于引導(dǎo)后方車輛進(jìn)行變道；建立了針對(duì)移動(dòng)障礙物中特殊情況——超車避障的路徑規(guī)劃仿真模型。仿真結(jié)果表明：采用改進(jìn)的人工勢(shì)場(chǎng)法具有良好的避障路徑規(guī)劃效果，能引導(dǎo)車輛做出滿足車輛運(yùn)動(dòng)學(xué)約束的變道行為，且規(guī)劃出的避障路徑更為光滑。