智能車輛局部避障軌跡規劃研究

姚 芳，鄭 帥，吳正斌

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

1 引言

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

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

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

對此，為提升避障懲罰項的靈活性、可靠性，文中根據自主車輛的避障動態過程，定義碰撞風險區域及轉向失調風險區域，并結合人工勢場理論，對自主車輛所受引斥力進行優化設計，并將引斥合力引入軌跡規劃層目標函數作為避障懲罰項。在保證整車質心側偏角、側向加速度、輪胎側偏角遠離約束邊界的同時，得到規劃軌跡最優解，提高整車行駛穩定性與舒適性。

2 基于MPC的避障路徑規劃

2.1 點質量模型

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

圖1 點質量模型示意圖Fig.1 Point quality model

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

(1)

(2)

(3)

(4)

(5)

2.2 非線性MPC避障軌跡規劃控制器

將式(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為離散遞推整數；Vy(k)、Vx(k)、ψ(k)、X(k)、Y(k)均表示當前時刻對應相關變量。

假設車輛避障過程中縱向狀態保持穩定，即vx為常數、ax=0此時有

vx(k+1)=vx(k)

(11)

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

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

(12)

式中，

(13)

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

(14)

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

(15)

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

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

(16)

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

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

3.1 改進車輛局部避障速度引斥函數

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

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

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

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

定義速度引斥函數為

(21)

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

經過重新定義的速度引斥函數特點為：

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

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

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

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

(22)

3.2 改進障礙車輛引斥力

沿用3.1節設計思想，定義障礙車輛引斥勢場函數為

(23)

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

對應障礙車輛引斥力為

(24)

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

(25)

3.3 基于引斥合力的局部避障軌跡規劃目標函數

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

(26)

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

4 CarSim/Simulink聯合仿真

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

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

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

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

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

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

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

圖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 結論

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