田國(guó)富, 張森

(沈陽(yáng)工業(yè)大學(xué)機(jī)械工程學(xué)院, 沈陽(yáng) 110870)

中國(guó)人口老齡化升高,但運(yùn)輸行業(yè)勞動(dòng)力需求卻逐漸增加,因此發(fā)展無(wú)人駕駛貨車(chē)技術(shù)有重要意義。貨車(chē)在高速行駛時(shí),當(dāng)前方發(fā)生追尾,急剎停車(chē)、甩尾等緊急情況,進(jìn)行緊急剎車(chē),可能導(dǎo)致貨物脫落、剎不住車(chē)、側(cè)翻等危險(xiǎn)情況,因此針對(duì)汽車(chē)在突發(fā)情況下進(jìn)行緊急轉(zhuǎn)向避障的決策規(guī)劃成為研究的熱點(diǎn)。

針對(duì)換道決策,目前主要分為輕決策和重決策。輕決策是將避障空間進(jìn)行離散化,設(shè)計(jì)代價(jià)函數(shù),采用動(dòng)態(tài)規(guī)劃在離散的空間進(jìn)行求解,從而得到凸空間進(jìn)行避障。這種方式可以處理復(fù)雜場(chǎng)景,但這種方式計(jì)算量很大,對(duì)感知定位要求很高。重決策主要綜合自車(chē)與障礙物的距離、相對(duì)速度、周?chē)h(huán)境等信息,根據(jù)人給定的規(guī)則判斷給出決策,主要缺點(diǎn)是場(chǎng)景太多無(wú)法覆蓋,但這重決策計(jì)算量小,決策時(shí)間短。百度的Apollo3.0之前的版本使用的決策為輕決策,Apollo3.5之后的版本采用的是重決策。

軌跡規(guī)劃分為全局和局部,全局路徑規(guī)劃有A star算法、Dijikstra算法等一些比較經(jīng)典的算法和蟻群算法等一些智能算法,局部軌跡規(guī)劃的主要方法主要有多項(xiàng)式曲線(xiàn)插值法、改進(jìn)人工勢(shì)場(chǎng)法、貝塞爾曲線(xiàn)法、B樣條曲線(xiàn)法等,文獻(xiàn)[1]采用雙五次多項(xiàng)式方法進(jìn)行軌跡規(guī)劃,相比較五次多項(xiàng)式具有較高的實(shí)時(shí)性,但該規(guī)劃方法未考慮目標(biāo)車(chē)道上行駛的車(chē)輛對(duì)自車(chē)的影響。文獻(xiàn)[2]用快速搜索隨機(jī)樹(shù)算法對(duì)五次多項(xiàng)式進(jìn)行優(yōu)化,提高了算法收斂速度,保證了軌跡的平滑。文獻(xiàn)[3]用傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行軌跡規(guī)劃,在將引斥函數(shù)進(jìn)行改進(jìn),從而達(dá)到局部避障的目的。文獻(xiàn)[4]采用基于貝葉斯優(yōu)化的XGBoot換道決策,同時(shí)采用五次多項(xiàng)式進(jìn)行規(guī)劃提升了換道效率。文獻(xiàn)[5]基于深度學(xué)習(xí)改進(jìn)決策模型,采用三次多項(xiàng)式并用代價(jià)函數(shù)求出最優(yōu)的換道軌跡。文獻(xiàn)[6]采用基于模型預(yù)測(cè)控制的動(dòng)態(tài)軌跡規(guī)劃方法,該方法可以達(dá)到較好的避障效果,但由于其在規(guī)劃中計(jì)算量大,不適于緊急轉(zhuǎn)向避障的軌跡規(guī)劃。文獻(xiàn)[7]通過(guò)建立人工勢(shì)場(chǎng)的引斥力函數(shù),規(guī)劃出一條避障軌跡。

針對(duì)多車(chē)環(huán)境下,前方發(fā)生突發(fā)事件時(shí)的智能貨車(chē)緊急轉(zhuǎn)向避障的問(wèn)題,現(xiàn)通過(guò)建立模糊規(guī)則,比較左右車(chē)道的安全性來(lái)決定貨車(chē)的行駛動(dòng)作,同時(shí)提出使用三階貝塞爾曲線(xiàn)規(guī)劃出換道軌跡,并對(duì)換道距離和最大的側(cè)向加速度進(jìn)行約束,使智能貨車(chē)能平穩(wěn)安全地進(jìn)行緊急轉(zhuǎn)向避障。

1 緊急避障策略

當(dāng)無(wú)人駕駛貨車(chē)在高速行駛時(shí),前方發(fā)生突發(fā)工況,這時(shí)需要進(jìn)行緊急避障決策,通過(guò)對(duì)左右車(chē)道的換道條件進(jìn)行安全評(píng)估,當(dāng)左右車(chē)道只有一個(gè)滿(mǎn)足換道條件時(shí),向滿(mǎn)足換道條件的一側(cè)進(jìn)行避障,當(dāng)兩側(cè)都滿(mǎn)足換道行為時(shí),通過(guò)比較左右車(chē)道安全性大小,使貨車(chē)向更安全的一側(cè)進(jìn)行避障,如果都不滿(mǎn)足,則進(jìn)行剎車(chē)減速,等到符合轉(zhuǎn)向避障安全時(shí),進(jìn)項(xiàng)轉(zhuǎn)向避障。無(wú)人駕駛貨車(chē)避障策略如圖1所示。

SE1為左車(chē)道安全值;SE2為右車(chē)道安全值

2 建立換道安全距離

假設(shè)目標(biāo)車(chē)道的行駛車(chē)輛為人為駕駛,建立自車(chē)與目標(biāo)車(chē)道后方車(chē)輛的轉(zhuǎn)向避障安全距離[8],以后方車(chē)輛的緊急剎車(chē)距離為換道的最小安全距離,考慮人的反應(yīng)時(shí)間,設(shè)反應(yīng)時(shí)間為0.5 s,則最小安全距離為

(1)

自車(chē)與目標(biāo)車(chē)道前方車(chē)輛的緊急轉(zhuǎn)向避障最小安全距離為

(2)

式中:vr、v、vf分別為轉(zhuǎn)向時(shí)目標(biāo)車(chē)道的后方車(chē)輛速度、自車(chē)速度、轉(zhuǎn)向時(shí)目標(biāo)車(chē)道前車(chē)速度;c為前方障礙物與自車(chē)的縱向距離。

將自車(chē)換道時(shí)的安全性依據(jù)兩種最小安全距離建立模糊規(guī)則,將自車(chē)與目標(biāo)車(chē)道后車(chē)的相對(duì)距離與最小安全距離的差值D1作為模糊輸入1,語(yǔ)言變量設(shè)為5級(jí),分別為負(fù)(N),零(O),正小(PS),正中(PM),正大(PB),設(shè)目標(biāo)車(chē)道汽車(chē)的最大行駛速度限制為120 km/h,最小行駛速度限制為60 km/h,令最小安全距離與目標(biāo)車(chē)道后車(chē)的差值在范圍[-110,-10]為負(fù)(N),[-10,20]為零(O),[10,50]為正小(PS),[40,80]為正中(PM),[60,90]為正大(PB)。隸屬度函數(shù)圖像如圖2所示。

圖2 后車(chē)輸入變量的隸屬度函數(shù)

自車(chē)與目標(biāo)車(chē)道前車(chē)的相對(duì)距離與最小安全距離的差值D2作為模糊輸入2,令最小安全距離與目標(biāo)車(chē)道后車(chē)的差值在范圍[-50,-10]為負(fù)(N),[-10,30]為零(O),[20,60]為正小(PS),[50,90]為正中(PM),[80,150]為正大(PB)。隸屬度函數(shù)圖像如圖3所示。

圖3 前車(chē)輸入變量的隸屬度函數(shù)

輸出為換道安全值SE,論域?yàn)閇-1,1],語(yǔ)言變量設(shè)為7級(jí),分別為負(fù)(N)、零(O)、正小(PS)、正中小(PMS)、正中(PM)、正中大(PMB)和正大(PB),隸屬度函數(shù)圖像如圖4所示。通過(guò)輸入輸出構(gòu)建模糊規(guī)則如表1所示。

表1 模糊規(guī)則表

圖4 輸出安全值的隸屬度函數(shù)

在換道決策時(shí)將左右目標(biāo)車(chē)道上的前車(chē)和后車(chē)與自車(chē)的相對(duì)距離分別輸入,然后查找模糊規(guī)則表,通過(guò)解模糊求出相應(yīng)的安全值,比較安全值的大小,然后控制車(chē)輛向安全值較大的一側(cè)轉(zhuǎn)向避障。

3 局部軌跡規(guī)劃

貝塞爾曲線(xiàn)[9]具有的幾何特性不隨著坐標(biāo)系的變化而變化,曲線(xiàn)的始末控制點(diǎn)分別為曲線(xiàn)的起始點(diǎn)和終止點(diǎn)的特點(diǎn),使貝塞爾曲線(xiàn)控制簡(jiǎn)單,易于跟蹤,經(jīng)常應(yīng)用于無(wú)人駕駛汽車(chē)的局部避障[10]的規(guī)劃軌跡中,并且還可以通過(guò)改變貝塞爾曲線(xiàn)控制點(diǎn)的坐標(biāo)進(jìn)而達(dá)到控制曲線(xiàn)曲率和曲線(xiàn)彎曲方向目的,由于在緊急轉(zhuǎn)向時(shí)的軌跡不僅需要具有平滑的曲線(xiàn)和連續(xù)的曲率,而且要求在規(guī)劃時(shí)需要更少的計(jì)算量來(lái)減少計(jì)算時(shí)間,因?yàn)槿A貝塞爾曲線(xiàn)與其他高階貝塞爾曲線(xiàn)相比僅需要4個(gè)控制點(diǎn)就可以規(guī)劃出一條連續(xù)平滑的軌跡,可以減少規(guī)劃軌跡的計(jì)算量,所以選擇三階貝塞爾曲線(xiàn)作為避障規(guī)劃軌跡的曲線(xiàn)。

3.1 貝塞爾曲線(xiàn)公式

設(shè)P0、P1為曲線(xiàn)的兩個(gè)控制點(diǎn),t的取值為0～1,則一階貝塞爾曲線(xiàn)可表示為

p1(t)=(1-t)p0+tp1

(3)

設(shè)P0、P1、P2為曲線(xiàn)的3個(gè)控制點(diǎn),t的取值為0～1,由于P0、P1構(gòu)成一階貝塞爾曲線(xiàn),P1、P2構(gòu)成一階貝塞爾曲,可得

p1,1(t)=(1-t)p0+tp1

(4)

p1,2(t)=(1-t)p1+tp2

(5)

則在此基礎(chǔ)上可生成二階貝塞爾曲線(xiàn)為

p2(t)=(1-t)p1,1+tp1,2

(6)

通過(guò)一二階貝塞爾曲線(xiàn)公式可推出,當(dāng)P0、P1、P2、P3共4個(gè)控制點(diǎn)已知時(shí),P0和P1、P1和P2、P2和P3都構(gòu)成一階貝塞爾曲線(xiàn),即

p1,1(t)=(1-t)p0+tp1

(7)

p1,2(t)=(1-t)p1+tp2

(8)

p1,3(t)=(1-t)p2+tp3

(9)

由3個(gè)一階曲線(xiàn)可推出如下的兩個(gè)二階曲線(xiàn)

p2,1(t)=(1-t)p1,1+tp1,2

(10)

p2,2(t)=(1-t)p1,2+tp1,3

(11)

由此可推出三階貝塞爾曲線(xiàn)為

p3(t)=(1-t)p2,1+tp2,2

(12)

將式(3)～式(11)代入(12)可得三階貝塞爾曲線(xiàn)的表達(dá)式為

(13)

汽車(chē)在直角坐標(biāo)系中的坐標(biāo)為(x,y),需要將曲線(xiàn)公式轉(zhuǎn)換為x,y分別關(guān)于t的函數(shù)。

設(shè)x,y關(guān)于t的參數(shù)方程為

(14)

將式(13)轉(zhuǎn)換為矩陣表達(dá)式為

(15)

將式(14)代入式(15)得

(16)

(17)

回代系數(shù)a、b后整理可得道直角坐標(biāo)系下的三階貝塞爾曲線(xiàn)。

(18)

由曲率的定義可知

(19)

式(19)中:x′、y′、x″、y″分別為汽車(chē)的縱向速度、橫向速度、縱向加速度、橫向加速度。

3.2 確定4個(gè)控制點(diǎn)的坐標(biāo)

設(shè)第一個(gè)控制點(diǎn)為初始點(diǎn)(0,0),則P0(x0,y0)=(0,0),第二個(gè)控制點(diǎn)為P1(x1,y1)假設(shè)車(chē)輛初始航向角為0°,則P1(x1,0),第三個(gè)控制點(diǎn)為P2(x2,y2),第四個(gè)控制點(diǎn)為P3(x3,y3),再換道避障時(shí),為了安全的考慮通常將換道的結(jié)束點(diǎn)的橫坐標(biāo)設(shè)置為道路中心線(xiàn),由中國(guó)道路的標(biāo)準(zhǔn)寬度為3.75 m,可知P3(x3,3.75),P2(x2,3.75),為了更快地計(jì)算控制坐標(biāo),規(guī)劃避障軌跡將4個(gè)點(diǎn)設(shè)計(jì)為中心對(duì)稱(chēng)的形式,如圖5所示,可以得到x2=x1,x3=2x1,P2(x2,3.75)=(x1,3.75),P3(x2,3.75)=(2x1,3.75),由4個(gè)控制點(diǎn)坐標(biāo)的表達(dá)式可知,它們都與x1相關(guān)聯(lián),因此只需求出x1的大小就可以將三階貝塞爾曲線(xiàn)的形狀確定。

圖5 三階貝塞爾曲線(xiàn)

3.3 規(guī)劃軌跡的邊界約束

針對(duì)貨車(chē)在緊急轉(zhuǎn)向避障時(shí)可能發(fā)生的危險(xiǎn),將對(duì)貨車(chē)在緊急轉(zhuǎn)向時(shí)規(guī)劃軌跡的邊界進(jìn)行約束,如圖6所示。

圖6 規(guī)劃軌跡邊界約束

貨車(chē)在進(jìn)行緊急轉(zhuǎn)向時(shí)由于自身質(zhì)量大,質(zhì)心高,如果轉(zhuǎn)向時(shí)橫向加速度過(guò)大,可能會(huì)導(dǎo)致貨車(chē)發(fā)生側(cè)翻,擺尾等危險(xiǎn)情況發(fā)生,為了避免這種危險(xiǎn),要對(duì)貨車(chē)的橫向加速度進(jìn)行約束。車(chē)輛靜態(tài)穩(wěn)定性公式為

(20)

可以推導(dǎo)出車(chē)輛的瞬時(shí)平衡方程為

(21)

式中:Fzl為左側(cè)車(chē)輪的垂直載荷;ay為轉(zhuǎn)向時(shí)的橫向加速度;lw為車(chē)輪輪距;h為車(chē)輛質(zhì)心高度。

由式(21)得到左側(cè)車(chē)輪的垂直載荷為

(22)

由式(22)可以得出,當(dāng)橫向加速度為0時(shí),左側(cè)車(chē)輪的垂直載荷為mg/2,當(dāng)橫向加速度不斷增大,直到左側(cè)垂直載荷為0時(shí),此時(shí)的橫向加速度為

(23)

這時(shí)的車(chē)輛狀態(tài)為貨車(chē)發(fā)生側(cè)翻的初始狀態(tài),aymax為車(chē)輛準(zhǔn)靜態(tài)下側(cè)翻的閾值[11]。在實(shí)際駕駛中,車(chē)輛的瞬態(tài)側(cè)翻閾值明顯小于準(zhǔn)靜態(tài)閾值,一般僅為準(zhǔn)靜態(tài)側(cè)翻閾值的30%～50%[12]。通過(guò)仿真實(shí)驗(yàn)的車(chē)輛模型數(shù)據(jù)可得到aymax=0.9g,選取動(dòng)態(tài)側(cè)翻閾值的大小為準(zhǔn)靜態(tài)的30%,則

aymax≤0.27g

(24)

由圖6可知,此時(shí)的R1和X1為

(25)

(26)

式中:v為自車(chē)行駛速度;Xl為控制點(diǎn)P1(x1,y1)中x1的最小距離。

在避障時(shí)需要考慮在換道時(shí)是否會(huì)與前方障礙物發(fā)生碰撞,對(duì)車(chē)輛換道時(shí)的碰撞邊界進(jìn)行約束。在與前方障礙車(chē)輛不發(fā)生碰撞的極限車(chē)距進(jìn)行避障,如圖3的碰撞邊界所示,以車(chē)輛自身為圓心,r為半徑建立圓形車(chē)輛幾何形狀模型進(jìn)行約束分析。

(27)

(28)

(29)

(30)

(31)

式中:ay2為碰撞邊界的最大橫向加速度;y為道路寬度;r為車(chē)輛半徑;w為車(chē)輛寬度;l為車(chē)輛長(zhǎng)度;d為換道時(shí)不發(fā)生碰撞的兩車(chē)的最小橫向距離;v為自車(chē)縱向速度;Xr為控制點(diǎn)P1(x1,y1)中x1的最大距離。

穩(wěn)定性邊界和碰撞邊界的約束只能規(guī)劃出保證車(chē)輛不發(fā)生側(cè)傾和碰撞軌跡,為了規(guī)劃出最合理的軌跡,做下列約束。

(1)在軌跡的初始點(diǎn)應(yīng)使軌跡曲率盡可能趨近于0,這樣可以使車(chē)輛更好地對(duì)規(guī)劃的軌跡進(jìn)行跟蹤。

(2)由式(27)可知,在換道過(guò)程中車(chē)輛的轉(zhuǎn)向半徑與車(chē)輛的橫向加速度成反比,同時(shí)軌跡的曲率與轉(zhuǎn)向半徑成正比,因此在換道過(guò)程中對(duì)最大橫向加速度的約束可以轉(zhuǎn)為對(duì)軌跡的最大曲率的約束。

(3)換道中間位置車(chē)輛朝向與車(chē)道線(xiàn)之間的夾角θ,應(yīng)滿(mǎn)足0°<θ<45°的條件[13],此約束條件可以變?yōu)楫?dāng)參數(shù)t=0.5時(shí)的道路曲率約束。

通過(guò)上述約束可設(shè)計(jì)代價(jià)函數(shù)為

J=min(J1+J2+J3)

(32)

式(32)中:J1=kt=0;J2=kmax;J3=dy/dx(當(dāng)t=0.5時(shí))。各個(gè)約束條件[14]如下。

(33)

對(duì)上述代價(jià)函數(shù)求解[15],采用MATLAB中自帶的fmincon函數(shù)來(lái)求解非線(xiàn)性函數(shù)有約束優(yōu)化問(wèn)題的極小值。

(34)

由式(34)可以求出不同車(chē)速下,三階貝塞爾曲線(xiàn)中控制點(diǎn)P1(x1,0)中x1的最優(yōu)解。其中S為距離前車(chē)安全的轉(zhuǎn)向車(chē)距,一般取值為(2～3)v。根據(jù)P1(x1,0)然后得到控制點(diǎn)P2(x2,y2),P3(x3,y3)的坐標(biāo),從而得到不同車(chē)速下對(duì)應(yīng)的貝塞爾換道曲線(xiàn)如圖7所示。

圖7 不同車(chē)速下的避障軌跡

低速行駛時(shí)貨車(chē)轉(zhuǎn)向時(shí)發(fā)生側(cè)翻的可能性遠(yuǎn)遠(yuǎn)小于高速行駛時(shí)側(cè)翻的可能性,因此對(duì)20、25、30 m/s 3種行駛速度所規(guī)劃出的道路曲率進(jìn)行計(jì)算,如圖8所示。

圖8 不同軌跡下的道路曲率

計(jì)算得到3種速度對(duì)應(yīng)的最大道路曲率分別為0.001 5、0.000 97、0.000 66,均小于對(duì)應(yīng)的最大道路曲率值Kmax,則規(guī)劃軌跡滿(mǎn)足轉(zhuǎn)向要求。

4 仿真檢驗(yàn)

情景一:當(dāng)前方緊急情況,此時(shí)自車(chē)速度為100 km/h,右側(cè)車(chē)道后車(chē)速度為90 km/h,與自車(chē)距離為20 m,右側(cè)車(chē)道前車(chē)速度為90 km/h,與自車(chē)距離為10 m,左側(cè)車(chē)道后車(chē)速度為100 km/h,與自車(chē)距離為80 m,左側(cè)車(chē)道前車(chē)速度為100 km/h,與自車(chē)距離為70 m。此時(shí)右側(cè)不滿(mǎn)足避障條件,左側(cè)滿(mǎn)足避障條件。使用carsim仿真軟件中自帶的utility truck貨車(chē)模型進(jìn)行仿真,結(jié)果如圖9～圖11所示。

圖9 左側(cè)避障仿真

圖10 左側(cè)避障過(guò)程中的橫向加速度變化

圖11 左側(cè)避障軌跡的跟蹤效果

情景二:前方發(fā)生緊急情況時(shí),此時(shí)自車(chē)速度為90 km/h,右側(cè)車(chē)道后車(chē)速度為90 km/h,與自車(chē)距離為60 m,右側(cè)車(chē)道前車(chē)速度為90 km/h,與自車(chē)距離為50 m,左側(cè)車(chē)道后車(chē)速度為100 km/h,與自車(chē)距離為20 m,左側(cè)車(chē)道前車(chē)速度為100 km/h,與自車(chē)距離為10 m。此時(shí)左側(cè)不滿(mǎn)足避障條件,右側(cè)滿(mǎn)足避障條件,結(jié)果如圖12～圖14所示。

圖12 右側(cè)避障仿真

圖13 右側(cè)避障過(guò)程中的橫向加速度變化

圖14 右側(cè)避障軌跡的跟蹤效果

5 結(jié)論

通過(guò)仿真結(jié)果可以得出以下結(jié)論。

(1)貨車(chē)在前方出現(xiàn)突發(fā)情況下,采用根據(jù)人的經(jīng)驗(yàn)建立的模糊策略生成的決策,可以有效地應(yīng)對(duì)多車(chē)環(huán)境下的緊急轉(zhuǎn)向是向左側(cè)還是右側(cè)轉(zhuǎn)向避障的問(wèn)題,避免了可能與目標(biāo)車(chē)道行駛車(chē)輛碰撞的風(fēng)險(xiǎn),提高了貨車(chē)在緊急轉(zhuǎn)向時(shí)的安全性。

(2)然后根據(jù)在轉(zhuǎn)向初始時(shí)自車(chē)不同的速度,規(guī)劃出該速度下最合適的三階貝塞爾局部避障曲線(xiàn),經(jīng)過(guò)規(guī)劃軌跡和實(shí)際軌跡的對(duì)比,該規(guī)劃軌跡可以讓貨車(chē)較好地跟蹤,從而實(shí)現(xiàn)躲避障礙物。

(3)通過(guò)兩次仿真得出的不同速度下轉(zhuǎn)向時(shí)的最大橫向加速度均小于規(guī)劃軌跡的最大橫向加速度,滿(mǎn)足在緊急轉(zhuǎn)向時(shí)減小貨車(chē)側(cè)翻的風(fēng)險(xiǎn)的目的。