基于前車軌跡預測的高速智能車運動規(guī)劃*

張一鳴，周 兵，吳曉建，崔慶佳，柴 天

（1.湖南大學，汽車車身先進設計制造國家重點實驗室，長沙 410082； 2.南昌大學，南昌 330031）

前言

根據國家統(tǒng)計局數據，我國2017年交通事故死亡63 772人，其中汽車交通事故死亡46 817人，占比73.4%且較往年呈現上升趨勢［1］，因此，智能車主動安全系統(tǒng)受到越來越多人的關注。運動規(guī)劃算法作為主動安全系統(tǒng)中重要一環(huán)，能夠應對不同交通場景規(guī)劃出合理軌跡且滿足環(huán)境約束、車輛運動學約束和車輛動力學約束，從而減少或者避免事故的發(fā)生。

傳統(tǒng)的運動規(guī)劃更多考慮獲取環(huán)境靜態(tài)信息，通過不斷感知更新規(guī)劃路徑。但在高速緊急避撞情況下，短時間內車輛狀態(tài)會發(fā)生較大改變，此時前車的狀態(tài)信息變化對主車規(guī)劃軌跡影響較大，如果不能提前判斷前車軌跡，僅根據前車靜態(tài)環(huán)境信息更新主車規(guī)劃軌跡會造成主車在緊急情況下過多調整行駛軌跡變化，難以完成有效避撞。Hu等［2］利用多項式方法生成路徑簇，通過構建考慮動靜態(tài)安全性和舒適性的代價函數選取最佳路徑，但動態(tài)障礙物的運動模型考慮較為簡單。Song等［3］結合勢場法和彈性繩理論，能在多種道路環(huán)境下完成路徑規(guī)劃，并保證軌跡的平滑性，但是未考慮車輛在初始時刻航向角的變化過程。Han等［4］采用構造Bezier曲線方法通過調整控制點位置實現躲避障礙物。Chen等［5］在構造Bezier曲線時對控制點位置采用序列二次規(guī)劃方法進行優(yōu)化，規(guī)劃得到曲率最優(yōu)的運動路徑，該方法能夠保證生成滿足車輛行駛的平滑軌跡，但是對于目標點位置的選取都未做進一步研究。Wang等［6］使用模型預測的方法構造引導勢場和危險勢場的代價函數，使用序列二次規(guī)劃方法進行最優(yōu)求解得到規(guī)劃路徑。黃建根等［7］提出基于D-S證據推理算法構造不同傳感器的基本概率分配函數，該方法可以在每個預測周期內選擇最佳的汽車運動模型，但是并不適合長期軌跡預測。Houenou等［8］結合基于最優(yōu)化方法和恒角速度恒加速度運動模型的方法，對不同時刻兩種預測方法的權重系數進行調整，達到對車輛短期和長期行駛軌跡的有效預測。周兵等［9］基于運動模型對交通車軌跡進行預測，并考慮運動軌跡不確定性對高速車輛采取制動避撞和轉向避撞方式進行決策，該方法能夠對未來碰撞概率進行判斷，但在高速情況下單一運動模型預測方法會造成運動后期誤差較大。Xie等［10］將基于模型和基于意圖的預測軌跡不確定性進行融合，從而減小運動模型后期預測誤差較大造成的影響。

可以看出現有文獻中著重研究車輛的軌跡預測或者軌跡規(guī)劃，但基于前車軌跡預測的主車軌跡規(guī)劃關注不足，因此本文中在考慮前車未來一段時間內運動軌跡前提下，采用5階Bezier曲線方法對高速車輛避撞進行軌跡規(guī)劃。融合基于車輛運動模型和考慮駕駛意圖對前車軌跡進行預測，并建立運動過程中碰撞安全距離模型，結合車輛運動學約束與動力學約束，采用SQP方法對Bezier曲線參數進行求解。目標函數考慮前車預測軌跡的不確定性，采用卡爾曼濾波方法求解軌跡概率密度分布函數，對運動過程中碰撞風險概率進行計算，并結合車輛運動過程中速度變化特點，規(guī)劃出保證車輛安全性與穩(wěn)定性的運動軌跡。

1 貝塞爾曲線

n階Bezier曲線的參數化表達式為

式中：B（τ）為Bezier曲線點的坐標矩陣；Pi為第i+1個控制點的坐標矩陣。

圖1為Bezier曲線示意圖，X方向為道路縱向，Y方向為道路橫向，P、P′和 P″分別表示 B、B1和 B2曲線的控制點，P0既為曲線B初始控制點，也為曲線B1、B2初始控制點，P5為曲線B、B2目標控制點，曲線B1目標控制點前移至P5′位置，從圖中可以看出不同位置控制點對曲線形狀的影響。

圖1 Bezier曲線示意圖

對于路徑規(guī)劃，Bezier曲線通過確定初始兩個控制點和末尾兩個控制點位置，可以控制初末時刻軌跡切線方向，且曲線經過第1個和第n+1個控制點，可以將軌跡導向目標位置，使曲線始終位于控制點構成的凸包內，保證軌跡方向變化趨勢。另外Bezier曲線規(guī)劃方法計算量小，能夠滿足高速工況下軌跡規(guī)劃實時性要求，且曲率具有連續(xù)性，計算公式為

本文中選取5階Bezier曲線構造車輛避障軌跡，即通過設置6個控制點位置規(guī)劃出相應軌跡。結合車輛位置信息，將曲線控制點的坐標表示為

式中：P0和P5分別為初始位置和目標位置坐標，縱向坐標分別用y0和yT表示；d1和d4分別為P0點與P1點和 P4點與P5點之間距離；κ（0）和 κ（T）分別為初始時刻和結束時刻曲率；v0和vT分別為初始時刻與結束時刻速度方向矢量；i表示車道線方向單位矢量。

2 前車軌跡預測

2.1 基于運動模型的軌跡預測

運動模型主要有恒速模型（CV）、恒加速度模型（CA）、恒角速度模型（CTR）和恒角速度恒加速度模型（CTRA）。CV模型為加速度為0時的CA模型，CTR模型為加速度為0時的CTRA模型，因此本文中將上述4種模型簡化為CA模型和CTRA模型。轉向角速度對于高速行駛車輛影響較大，不同的運動模型預測軌跡會有較大的差別，因此在規(guī)劃之初需要根據前車當前狀態(tài)對運動模型進行合理決策。給定門限值ω0，當角速度大于ω0時選用CTRA模型，當角速度小于ω0時選用CA模型［11］。

采用運動學模型預測前車未來行駛軌跡時，高速車輛運動狀態(tài)的不確定性會造成模型在預測過程中的偏差，加入高斯噪聲以反映上述不確定性，從而得到預測軌跡的概率分布。利用擴展卡爾曼濾波對CA模型中車輛狀態(tài)進行遞歸估計，將狀態(tài)空間表示為 x（t）＝［x y vxvyaxay］T，其中 x、y分別為車輛在全局坐標系下X軸和Y軸位置，vx、vy分別為X軸和Y軸方向的速度分量，ax、ay分別為X軸和Y軸方向加速度分量。則t+1時刻的期望值x（t+1）和協(xié)方差矩陣 Σ（t+1）為

式中：A為狀態(tài)轉移矩陣；ωt為過程噪聲矩陣；Q為過程噪聲的協(xié)方差矩陣。

不同于CA模型，引入橫擺角速度變量，CTRA模型中位置坐標為

可以看出CTRA模型是一個高度非線性模型，此時通過擴展卡爾曼濾波方法一方面會造成計算結果嚴重離散，另一方面計算過程也較為復雜。無損卡爾曼濾波（UKF）摒棄了對非線性函數進行線性化的傳統(tǒng)做法，使用無跡變換來處理均值和協(xié)方差的非線性傳遞，因此選取UKF對軌跡進行預測，車輛未來軌跡預測不確定性結果如圖2所示。

圖2 預測軌跡不確定性

2.2 考慮駕駛意圖的軌跡預測

考慮駕駛意圖的軌跡預測首先要根據車輛信息判斷出前車的駕駛意圖，目前對于駕駛意圖的判斷方法主要有隱馬爾科夫法［12］、支持向量機法［13］和神經網絡［14］等，本文中側重對前車軌跡進行預測，在此暫不對前車駕駛意圖實時判斷做深入討論，而是將前車駕駛意圖假定為車道保持和換道兩種，將其對軌跡預測的影響加以考慮。換道軌跡在滿足車輛動力學約束前提下還需保證軌跡的平順性和連續(xù)性以及保證車輛換道效率，為了得到最優(yōu)軌跡，給定代價函數：

式中：θi（t-1）和 θi（t）分別為車輛前后時刻的航向角；si為軌跡長度；i為備選軌跡路徑序號；w1和w2分別為評價車輛平順性和換道效率部分的系數。

相同運動距離條件下，車輛航向角變化值累加量越大，則車輛平順性越差，而運動距離變大則會增加車輛平順性，但同時降低了車輛避撞通過效率。式（12）中不同的權重代表不同的駕駛員風格，本文中在此不做過多研究，只討論通過該方法對長期軌跡進行預測。通過代價函數最小化，優(yōu)化得到考慮前車駕駛意圖的局部最優(yōu)預測軌跡。

2.3 軌跡預測融合

假定基于運動模型的預測軌跡為Tmdl，考慮駕駛意圖的預測軌跡為Tman，基于模型的預測軌跡能夠反映車輛短時間內運動趨勢，考慮意圖的軌跡預測方法可以反映長期預測軌跡，因此構造兩種預測方法隨時間變化的權重函數，權重函數采用三次Bezier曲線形式表示，設置0～1.5 s內Tmdl權重值較高，1.5～4 s內 Tman權重值較高，權重函數 f（t）如圖3所示。

圖3 權重函數

加權融合后的預測軌跡為

3 碰撞概率計算

將預測軌跡離散化，定義主車與前車軌跡分別為 xSE（xse，yse，θse）和 xTR（xtr，ytr，θtr），如圖4所示，分別對兩車縱向距離與側向距離進行計算。

圖4 安全距離模型示意圖

不發(fā)生碰撞的安全條件為

式中：l和w分別為車長和車寬；ls和ws分別為縱向和側向安全距離。

當車輛間距小于該安全距離時，判定車輛處于危險范圍，不再考慮軌跡不確定性；大于該安全距離時，需要考慮不確定性進一步計算碰撞概率。采用蒙特卡洛法求解近似概率，將某一時刻前車狀態(tài)變量根據概率密度函數離散采樣，通過以下公式計算碰撞概率P（Ct）期望值：

當 IC（xSE，xTR）＝1時，說明主車和前車幾何圖形所占空間存在交集，即發(fā)生碰撞；當IC（xSE，xTR）＝0時，表示不會發(fā)生碰撞。

4 主車軌跡規(guī)劃

考慮到前車未來的運動軌跡，若主車目標點在道路縱向位置距離較大，在運動過程中會增大與前車碰撞概率，而目標點在道路縱向位置距離較小時減小了碰撞概率，但是會增大車輛側向加速度，極易造成車輛側向失穩(wěn)。圖5所示為P5點在Y方向距離P0點3.75 m時，前車保持直道前行時目標點位置對于車輛最大加速度和碰撞概率影響，此時側向加速度絕對值變化不大，隨著P5點在X方向坐標位置變大，縱向加速度絕對值減小較為明顯。圖6和圖7為P5點在X方向距離P0點72 m，道路縱向坐標位置52 m時橫向距離對側向加速度絕對值和縱向加速度絕對值與碰撞概率的影響?？梢钥闯鋈裟繕它c在道路橫向位置距離P0點較大，此時側向加速度受影響變化較為明顯，容易造成高速行駛車輛側滑失穩(wěn)，在碰撞概率較小的安全裕度內，可以通過適當減小目標點縱向位置，既保證車輛穩(wěn)定，又保證車輛不發(fā)生碰撞。

圖5 P0點縱向位置對加速度與最大碰撞概率影響

圖6 P0點橫向位置對側向加速度與最大碰撞概率影響

圖7 P0點橫向位置對縱向加速度與最大碰撞概率影響

考慮在高速緊急避撞過程中，車輛進行制動操作不可避免，而避撞規(guī)劃的目的又是在車輛盡可能減少速度變化的同時保證安全，這樣既可以提高車輛運動效率，又可以減小車輛縱向加速度，提高車輛穩(wěn)定性，基于此構造關于車輛運動速度的目標函數，采用序列二次規(guī)劃方法對參數 P5、d4和x3進行求解。

車輛側向加速度ay常用來表征側向穩(wěn)定性，假定預期最大側向加速度值為aymax，則其表達式為

ay＝vt2κ≤aymax（18）式中 aymax＝min（μg，ayuser），ayuser為駕駛員期望最大側向加速度值。

主車在變道過程中速度發(fā)生變化，縱向及橫向加速度受到輪胎和地面摩擦力的限制：

式中：Ft為輪胎力；m為車輛質量；ax為車輛縱向加速度；μ為地面附著率；axuser為駕駛員期望最大縱向加速度。

根據實際駕駛經驗，在高速緊急避撞工況下，如果車輛發(fā)現有碰撞風險，會最大程度采取制動操作，在短時間內充分利用地面附著力進行避撞，躲避危險之后降低減速度，使得車輛快速通過危險區(qū)域。本文中采取兩種避撞策略，主車與前車行駛意圖不同時，主車會在避撞前半段采取較大制動減速度，后半段降低減速度；主車與前車行駛意圖相同時，主車全程采取較大制動減速度來避免發(fā)生碰撞。另外考慮前車運動軌跡的不確定性，在可行目標點區(qū)域內對車輛避障過程中的碰撞概率進行計算，并且車輛的橫縱向加速度大小影響到乘客舒適性與穩(wěn)定性。對規(guī)劃軌跡在后半段均勻選取n個點，給出求解的目標函數形式為式中：i為n個采用點序號；w1為速度規(guī)劃權值；w2為最大縱向加速度alon權值；w3為避撞過程中最大碰撞概率Pcom的權值。

I＝0表示主車與前車直行或換道意圖不同，此時 t∈［0，T／2］，若意圖相同，則 t∈［T／2，T］。對該非線性優(yōu)化問題采用序列二次規(guī)劃方法進行求解，求解步驟如下［15］：

（1）初始化Hessian矩陣以及變量x0；

（2）利用主動集法求解子二次規(guī)劃獲得求解搜尋方向dk；

（3）通過 xk+1＝xk+αkdk獲取下一個迭代值xk+1，其中αk需要保證目標函數能充分減小，重復直至Δφ（xk）＜τ；

（4）采用擬牛頓法更新Hessian矩陣。

5 仿真分析

道路場景選取單向雙車道，針對高速行駛車輛面臨前方障礙車輛變道和直行兩種典型工況，采用Matlab對本文中提出方法進行仿真驗證。

5.1 前車直行工況

主車車速為 30 m／s，前車速度為 20 m／s，兩車距離為20 m，前車加速度為0，橫擺角速度為0，因此運動模型選用CA運動模型，根據對前車未來的預測軌跡規(guī)劃出主車避障運動軌跡如8所示。

圖8 主車避撞軌跡規(guī)劃

仿真前車直行工況下，主車若繼續(xù)減速直行，從圖9看出，在23 m之后碰撞概率開始上升并在26 m處達到100%，可以肯定會與前車發(fā)生碰撞，此時選擇換道避撞方式降低了碰撞風險。如圖10所示，主車在換道躲避碰撞過程中速度持續(xù)減小，換道前期縱向加速度絕對值持續(xù)增大，后期開始減小，從而保證安全前提下快速通過危險區(qū)域，側向加速度絕對值和縱向加速度絕對值波峰錯開，分配地面附著力以提高車輛穩(wěn)定性，僅在車道縱向位置27 m處有最大2%的碰撞概率，保證了車輛通過安全性。

圖9 主車直行與換道碰撞概率

圖10 主車換道加速度與速度規(guī)劃

5.2 前車變道工況

主車車速為 30 m／s，前車速度為 20 m／s，兩車距離為20 m，前車加速度為0.85 m／s2，橫擺角速度為0.04 rad／s，此時車輛運動模型選用CTRA模型。從圖11中可以看出，基于運動模型的方法在短距離0～18 m內能夠較好接近實際軌跡，考慮駕駛意圖的方法則能夠較準確地預測長期運動軌跡，融合兩種方法能夠較準確地預測前車未來運動軌跡，圖12為主車軌跡規(guī)劃結果。

圖11 前車軌跡預測

圖12 主車避撞軌跡規(guī)劃

若此時主車采取換道方式，全程采取較大制動減速度，從圖13看出在道路縱向位置23 m處碰撞概率達到100%。而通過提前預判前車未來運動軌跡，如圖14所示，前半段制動減速度開始上升，當躲避前車后降低制動減速度，快速脫離危險區(qū)域，提高了車輛通過效率，碰撞概率僅在道路縱向位置38 m處達到4%，保證了車輛安全性，制動加速度在允許范圍之內，避免了轉向產生的側向加速度，從而在側向具有穩(wěn)定性。

圖13 主車換道與直行碰撞概率

圖14 主車直行加速度與速度規(guī)劃

6 結論

考慮到高速情況下主車執(zhí)行避撞動作受前車狀態(tài)變化影響較大，提出基于前車軌跡預測的主車運動規(guī)劃算法。構造目標函數包括主車規(guī)劃軌跡與前車在運動過程中碰撞概率以及主車在運動過程中加速度變化特點，并結合車輛動力學與運動學約束，采用SQP方法對Bezier曲線控制點位置進行求解，能夠規(guī)劃出主車最優(yōu)避撞運動軌跡。使用Matlab仿真前車直行和換道兩種意圖下主車避撞運動規(guī)劃，結果驗證了基于前車軌跡預測的主車運動規(guī)劃方法的可行性與有效性。