多車環(huán)境下智能貨車的緊急轉向決策及軌跡規(guī)劃

田國富, 張森

(沈陽工業(yè)大學機械工程學院, 沈陽 110870)

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

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

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

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

1 緊急避障策略

當無人駕駛貨車在高速行駛時,前方發(fā)生突發(fā)工況,這時需要進行緊急避障決策,通過對左右車道的換道條件進行安全評估,當左右車道只有一個滿足換道條件時,向滿足換道條件的一側進行避障,當兩側都滿足換道行為時,通過比較左右車道安全性大小,使貨車向更安全的一側進行避障,如果都不滿足,則進行剎車減速,等到符合轉向避障安全時,進項轉向避障。無人駕駛貨車避障策略如圖1所示。

SE1為左車道安全值;SE2為右車道安全值

2 建立換道安全距離

假設目標車道的行駛車輛為人為駕駛,建立自車與目標車道后方車輛的轉向避障安全距離[8],以后方車輛的緊急剎車距離為換道的最小安全距離,考慮人的反應時間,設反應時間為0.5 s,則最小安全距離為

(1)

自車與目標車道前方車輛的緊急轉向避障最小安全距離為

(2)

式中:vr、v、vf分別為轉向時目標車道的后方車輛速度、自車速度、轉向時目標車道前車速度;c為前方障礙物與自車的縱向距離。

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

圖2 后車輸入變量的隸屬度函數

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

圖3 前車輸入變量的隸屬度函數

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

表1 模糊規(guī)則表

圖4 輸出安全值的隸屬度函數

在換道決策時將左右目標車道上的前車和后車與自車的相對距離分別輸入,然后查找模糊規(guī)則表,通過解模糊求出相應的安全值,比較安全值的大小,然后控制車輛向安全值較大的一側轉向避障。

3 局部軌跡規(guī)劃

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

3.1 貝塞爾曲線公式

設P0、P1為曲線的兩個控制點,t的取值為0～1,則一階貝塞爾曲線可表示為

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

(3)

設P0、P1、P2為曲線的3個控制點,t的取值為0～1,由于P0、P1構成一階貝塞爾曲線,P1、P2構成一階貝塞爾曲,可得

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

(4)

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

(5)

則在此基礎上可生成二階貝塞爾曲線為

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

(6)

通過一二階貝塞爾曲線公式可推出,當P0、P1、P2、P3共4個控制點已知時,P0和P1、P1和P2、P2和P3都構成一階貝塞爾曲線,即

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個一階曲線可推出如下的兩個二階曲線

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

(10)

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

(11)

由此可推出三階貝塞爾曲線為

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

(12)

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

(13)

汽車在直角坐標系中的坐標為(x,y),需要將曲線公式轉換為x,y分別關于t的函數。

設x,y關于t的參數方程為

(14)

將式(13)轉換為矩陣表達式為

(15)

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

(16)

(17)

回代系數a、b后整理可得道直角坐標系下的三階貝塞爾曲線。

(18)

由曲率的定義可知

(19)

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

3.2 確定4個控制點的坐標

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

圖5 三階貝塞爾曲線

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

針對貨車在緊急轉向避障時可能發(fā)生的危險,將對貨車在緊急轉向時規(guī)劃軌跡的邊界進行約束,如圖6所示。

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

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

(20)

可以推導出車輛的瞬時平衡方程為

(21)

式中:Fzl為左側車輪的垂直載荷;ay為轉向時的橫向加速度;lw為車輪輪距;h為車輛質心高度。

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

(22)

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

(23)

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

aymax≤0.27g

(24)

由圖6可知,此時的R1和X1為

(25)

(26)

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

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

(27)

(28)

(29)

(30)

(31)

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

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

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

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

(3)換道中間位置車輛朝向與車道線之間的夾角θ,應滿足0°<θ<45°的條件[13],此約束條件可以變?yōu)楫攨祎=0.5時的道路曲率約束。

通過上述約束可設計代價函數為

J=min(J1+J2+J3)

(32)

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

(33)

對上述代價函數求解[15],采用MATLAB中自帶的fmincon函數來求解非線性函數有約束優(yōu)化問題的極小值。

(34)

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

圖7 不同車速下的避障軌跡

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

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

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

4 仿真檢驗

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

圖9 左側避障仿真

圖10 左側避障過程中的橫向加速度變化

圖11 左側避障軌跡的跟蹤效果

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

圖12 右側避障仿真

圖13 右側避障過程中的橫向加速度變化

圖14 右側避障軌跡的跟蹤效果

5 結論

通過仿真結果可以得出以下結論。

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

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

(3)通過兩次仿真得出的不同速度下轉向時的最大橫向加速度均小于規(guī)劃軌跡的最大橫向加速度,滿足在緊急轉向時減小貨車側翻的風險的目的。