智能車輛局部避障軌跡規(guī)劃研究

姚 芳，鄭 帥，吳正斌

(1.河北省電磁場與電器可靠性重點實驗室(河北工業(yè)大學)，天津 300132；2.省部共建電工裝備可靠性與智能化國家重點實驗室(河北工業(yè)大學)， 天津 300132；3.中國科學院大學 深圳先進技術研究院， 廣東 深圳 518055)

1 引言

避障軌跡規(guī)劃是智能車輛主動安全技術中的重要一環(huán)[1-4]，其目的是尋求車輛在當前車道環(huán)境及車輛運動約束條件下的最優(yōu)避障運動解[5-6]。當車道前方存在障礙車輛，如駕駛員未及時作出轉(zhuǎn)向判斷決策，則需主動安全系統(tǒng)快速根據(jù)前方障礙車輛位置、車速等信息規(guī)劃并跟蹤避障軌跡[7-8]。在局部避障軌跡規(guī)劃策略中，需保證車輪側(cè)偏角、橫向加速度等車輛動力學參數(shù)控制在約束范圍內(nèi)，以保證規(guī)劃軌跡的可靠性及穩(wěn)定性[15]，在此基礎上提升車輛避障過程駕駛舒適性是軌跡規(guī)劃策略的重要研究方向。

在智能車輛軌跡規(guī)劃策略的研究中，人工勢場法的應用較為廣泛，其中，文獻[9]基于道路和障礙車輛的信息構(gòu)建了一個三維危險潛力場，然后生成實時無碰撞軌跡用于軌跡跟蹤。文獻[10]在軌跡規(guī)劃模塊中設計了非對稱車道勢場函數(shù)范圍，并得到無碰撞路徑。因此，人工勢場法可有效應用于全局工況下的避障軌跡規(guī)劃決策。

但是在局部避障過程中，為了提高避障過程的車輛穩(wěn)定性，還需考慮如側(cè)向加速度、質(zhì)心側(cè)偏角和輪胎側(cè)偏角等車輛動力學約束，因此，現(xiàn)有研究成果將人工勢場法與MPC策略相結(jié)合，提高避障軌跡規(guī)劃可靠性。2種算法的結(jié)合研究中，主要可分為2個研究方向，其一為通過人工勢場法求解二次規(guī)劃參考軌跡，隨后通過MPC算法重規(guī)劃出帶約束的理想避障軌跡[11]。其二為通過人工勢場法，優(yōu)化設計MPC策略目標函數(shù)中的避障懲罰項，提升避障可靠性。其中，文獻[12]定義了斥力勢場的橫向安全范圍，并將斥力函數(shù)作為避障懲罰項引入MPC目標函數(shù)中。文獻[13]提出可穿越型障礙物及不可穿越型障礙物斥力勢場，并將合斥力函數(shù)作為避障懲罰項引入MPC目標函數(shù)中。但文獻[12-13]中，障礙車輛斥力勢場的作用范圍均為圓型區(qū)域，當車輛完成避障動作，在相鄰車道安全行駛時，仍受到斥力函數(shù)作用，使得軌跡規(guī)劃層求解出遠離障礙車輛的目標點，影響車輛穩(wěn)定性及避障安全性。對此，文獻[14]將斥力勢場作用范圍改進為橢圓型區(qū)域，得到更加合理的規(guī)劃軌跡。但是，當自主車輛處于大幅轉(zhuǎn)向過程中，持續(xù)受到向外的斥力，則可能導致車輛轉(zhuǎn)向過度或車身失穩(wěn)。因此，固定范圍的斥力勢場不能很好地適用于自主車輛的避障決策中。

對此，為提升避障懲罰項的靈活性、可靠性，文中根據(jù)自主車輛的避障動態(tài)過程，定義碰撞風險區(qū)域及轉(zhuǎn)向失調(diào)風險區(qū)域，并結(jié)合人工勢場理論，對自主車輛所受引斥力進行優(yōu)化設計，并將引斥合力引入軌跡規(guī)劃層目標函數(shù)作為避障懲罰項。在保證整車質(zhì)心側(cè)偏角、側(cè)向加速度、輪胎側(cè)偏角遠離約束邊界的同時，得到規(guī)劃軌跡最優(yōu)解，提高整車行駛穩(wěn)定性與舒適性。

2 基于MPC的避障路徑規(guī)劃

2.1 點質(zhì)量模型

在避障軌跡規(guī)劃中，為便于理論分析及控制器設計，忽略輪胎受力，并將整車簡化為點質(zhì)量模型，采用的點質(zhì)量模型如圖1所示。

圖1 點質(zhì)量模型示意圖Fig.1 Point quality model

此時車輛運動模型可表示為

(1)

(2)

(3)

(4)

(5)

2.2 非線性MPC避障軌跡規(guī)劃控制器

將式(1)～式(5)離散化，可得

vx(k+1)=vx(k)+Tax

(6)

vy(k+1)=vy(k)+Tay

(7)

(8)

X(k+1)=X(k)+T[vx(k)cos(ψ(k))-vy(k)sin(ψ(k))]

(9)

Y(k+1)=Y(k)+T[vx(k)sin(ψ(k))+vy(k)cos(ψ(k))]

(10)

其中：T為離散時間；k為離散遞推整數(shù)；Vy(k)、Vx(k)、ψ(k)、X(k)、Y(k)均表示當前時刻對應相關變量。

假設車輛避障過程中縱向狀態(tài)保持穩(wěn)定，即vx為常數(shù)、ax=0此時有

vx(k+1)=vx(k)

(11)

選取側(cè)向加速度ay為非線性MPC軌跡規(guī)劃控制量，選取Vy(k)、Vx(k)、ψ(k)、X(k)、Y(k)為非線性MPC軌跡規(guī)劃狀態(tài)變量，定義非線性MPC軌跡規(guī)劃控制輸出函數(shù)為

η(k+i|t)=Fi[η(k|k),ay(j)]

(12)

式中，

(13)

式中：Np為預測時域；Nc為控制時域，且Nc

(14)

最終得非線性MPC軌跡規(guī)劃控制器的預測表達式為

(15)

式中:η(k+i|k)為k時刻時，k+i時刻離散化后的車輛狀態(tài)變量的預測值。

為使臨時路徑盡可能縮小與全局靜態(tài)參考路徑的跟蹤誤差，同時考慮側(cè)向加速度及其增量約束、道路邊界約束以保證避障過程車身轉(zhuǎn)向穩(wěn)定性及避障可靠性，設計非線性MPC軌跡規(guī)劃控制器的目標函數(shù)為

(16)

式中：Qi、Rj分別為輸出量、控制量加權矩陣，其中i=1,2,…,Np，j=1,2,…,Nc。

3 基于障礙車輛引斥力的NMPC軌跡規(guī)劃控制器設計

3.1 改進車輛局部避障速度引斥函數(shù)

實際工況中，障礙車輛以一定速度行駛，若忽視障礙車輛的相對運動狀態(tài)，則可能導致避障失敗。本文車輛避障過程所受速度斥力情況如圖2所示。

圖2 避障車輛所受速度斥力示意圖Fig.2 Schematic diagram of the velocity repulsion of a barrier avoidance vehicle

圖2中，F(xiàn)rev為相對速度斥力，q為車輛當前質(zhì)心位置，q0為障礙車輛當前質(zhì)心位置，D為初始時刻車輛質(zhì)心至道路中線的縱向距離，λd1為無碰撞風險因子，λd2為無轉(zhuǎn)向失穩(wěn)風險因子，且0<λd1<1、λd1<λd2<2，yp為初始時刻車輛質(zhì)心的縱向位置，D=|yp|，ypnow為避障車輛當前質(zhì)心縱向位置，車輛質(zhì)心縱向位置變化量為|ypnow-yp|。

當車輛避障過程中，定義碰撞風險臨界時刻(如圖2中紅色線框位置)，此時避障車輛邊界距障礙車輛邊界距離最短，此刻|ypnow-yp|=λd1D；定義轉(zhuǎn)向失調(diào)風險臨界時刻(如圖2中藍色線框位置)，此時|ypnow-yp|=λd2D。

定義速度引斥函數(shù)為

(21)

式中:Urev為速度斥力勢能；krev>0為速度斥力勢能增益；q、v分別為車輛當前質(zhì)心位置、車速；q0、v0分別為障礙車輛當前質(zhì)心位置、車速；ρ(q,q0)為q至q0的距離；ρ0為斥力勢場作用范圍。

經(jīng)過重新定義的速度引斥函數(shù)特點為：

1) 需保證ρ(q,q0)小于斥力場范圍ρ0時斥力函數(shù)才有可能作用；

2) 需保證兩車質(zhì)心距離qq0變化量為負數(shù)時，即兩車相對距離不斷減小時斥力函數(shù)才有可能作用；

3) 當車輛處于碰撞風險區(qū)域，即|ypnow-yp|<λd1D，速度引斥函數(shù)有可能作用，此時表現(xiàn)為斥力；當輛處于轉(zhuǎn)向失調(diào)風險區(qū)域，即λd1D<|ypnow-yp|<λd2D，這時速度引斥函數(shù)有可能作用，此時表現(xiàn)為引力，助力車身回穩(wěn)，避免轉(zhuǎn)向過度；當|ypnow-yp|超過λd2D，速度引斥函數(shù)不起作用，避免避障車輛回穩(wěn)時轉(zhuǎn)向過度。

當(1)(2)(3)點同時滿足時，則速度引斥函數(shù)作用，此時速度引斥力為

(22)

3.2 改進障礙車輛引斥力

沿用3.1節(jié)設計思想，定義障礙車輛引斥勢場函數(shù)為

(23)

式中:Urep為障礙車輛引斥勢能；k1>0為障礙車輛引勢能增益。

對應障礙車輛引斥力為

(24)

綜上，改進避障車輛引斥函數(shù)及所受引斥力為

(25)

3.3 基于引斥合力的局部避障軌跡規(guī)劃目標函數(shù)

將改進后障礙物引斥力函引入式(16)中，同時引入松弛項ρε2，保證二次規(guī)劃存在最優(yōu)解，最終可得避障軌跡規(guī)劃層目標函數(shù)為

(26)

式中，Qi、Rj為權重矩陣。

4 CarSim/Simulink聯(lián)合仿真

聯(lián)合仿真試驗中，設計自主車輛車速分別為36 km/h、72 km/h、108 km/h，障礙車輛于橫向位置100 m處靜止?；谡系K車輛引斥合力的NMPC軌跡規(guī)劃控制器控制效果如圖3—圖5所示。

圖3 36 km/h軌跡規(guī)劃控制器輸出效果曲線Fig.3 Output of the 36 km/h trajectory planning controller

圖4 72 km/h軌跡規(guī)劃控制器輸出效果曲線Fig.4 Output of the 72 km/h trajectory planning controller

圖5 108 km/h軌跡規(guī)劃控制器輸出效果曲線Fig.5 Output of the 108 km/h trajectory planning controller

由圖3—圖5可見，當車輛開始避障時，自主車輛處于碰撞風險區(qū)域，2個控制器規(guī)劃軌跡相近。當自主車輛躲開障礙車輛后，處于無碰撞風險且車身回正區(qū)域時，改進的NMPC控制器規(guī)劃出平緩軌跡，避免車身回正急轉(zhuǎn)向操作時轉(zhuǎn)向不足從而出現(xiàn)側(cè)翻的問題，且車輛橫擺角速度峰值更小，車身穩(wěn)定性更佳。避障過程橫向位移單位時間增量小于傳統(tǒng)NMPC橫向位移單位時間增量，即避障過程所需車輛側(cè)向加速度較小，車輛穩(wěn)定性及駕駛舒適性均更佳。

為驗證基于障礙車輛引斥合力NMPC軌跡規(guī)劃層的有效性，建立軌跡規(guī)劃及跟蹤雙層控制CarSim/Simulink聯(lián)合仿真平臺，觀測四輪側(cè)偏角及側(cè)向加速度變化情況。將本文所設計的基于障礙車輛引斥合力NMPC軌跡規(guī)劃層命名為控制器1，傳統(tǒng)NMPC軌跡規(guī)劃層命名為控制器2，試驗結(jié)果如圖6—圖8所示。

由圖6—圖8可知，在橫向加速度觀測值中，采用控制器1的車輛在保證避障安全的前提下，低中高車速下的橫向加速度峰值更小，駕駛舒適性更佳。36～108 km/h的避障車輛四輪側(cè)偏角范圍為-1.3°<α<1.3°，質(zhì)心側(cè)偏角范圍為0°<β<0.6°，滿足動力學約束，避免了車身甩尾及打滑等危險狀況發(fā)生。

圖6 36 km/h避障效果曲線Fig.6 36 km/h obstacle avoidance results

圖7 72 km/h避障效果曲線Fig.7 72 km/h obstacle avoidance results

圖8 108 km/h避障效果曲線Fig.8 108 km/h obstacle avoidance results

5 結(jié)論

引入速度引斥力及障礙車輛引斥力，定義了避障風險區(qū)域及轉(zhuǎn)向失調(diào)風險區(qū)域，據(jù)此改進自主車輛所受引斥合力作用區(qū)域，優(yōu)化懲罰避障項的作用效果，在保證避障安全的前提下，提升駕駛舒適性及操縱穩(wěn)定性。將基于障礙車輛引斥合力的懲罰避障項引入非線性MPC軌跡規(guī)劃策略的目標函數(shù)中，CarSim/Simulink試驗表明了本軌跡規(guī)劃策略的有效性。