張佳佳 魏民祥 項(xiàng)楚勇 吳樹凡 邢德鑫

（南京航空航天大學(xué)，南京 210016）

主題詞：彎道 換道避撞 擴(kuò)展卡爾曼濾波

1 前言

隨著汽車向電動(dòng)化、智能網(wǎng)聯(lián)化、共享化方向發(fā)展，先進(jìn)駕駛輔助系統(tǒng)（ADAS）為駕駛員提供了安全保障。由于車輛事故多發(fā)生在彎道、十字路口等路段，主動(dòng)安全技術(shù)在特殊路段的控制顯得尤為重要。

障礙物的信息采集是主動(dòng)安全技術(shù)的關(guān)鍵技術(shù)之一。王芬芬[1]基于機(jī)器視覺提出了一種結(jié)合像素灰度值差異的邊緣保持濾波算法，可較為精準(zhǔn)地檢測(cè)到障礙物信息，但計(jì)算量大，耗時(shí)長(zhǎng)，不適用于實(shí)時(shí)檢測(cè)。張瑞峰[2]建立了實(shí)時(shí)的雷達(dá)地圖顯示前方車輛與自車的相對(duì)速度、距離、預(yù)測(cè)的碰撞時(shí)間（Time To Collision，TTC）等，為碰撞預(yù)警控制系統(tǒng)提供了關(guān)鍵信息，但沒有提出相應(yīng)的控制策略。張鳳嬌[3]提出了一種基于模型預(yù)測(cè)控制（Model Predictive Control，MPC）的汽車緊急換道控制策略，在車輛僅采取制動(dòng)措施不能避免碰撞的條件下，采用換道實(shí)現(xiàn)避撞，但沒有考慮彎道的避撞問題。對(duì)于換道軌跡規(guī)劃，國(guó)外學(xué)者提出了利用圓弧曲線[4]、貝塞爾樣條曲線[5]、考紐螺線[6]等擬合軌跡，國(guó)內(nèi)的戚志錦[7]采用余弦函數(shù)作為換道軌跡，并設(shè)計(jì)了模糊自適應(yīng)PID控制器，但未考慮行駛路面為彎道的情況。Hatipoglu C[8]在車輛彎道行駛的研究中，確定期望橫擺角和橫擺角速度時(shí)考慮了道路的曲率，但在車輛換道過程中沒有考慮內(nèi)、外車道曲率變化的情況。

針對(duì)以上研究的不足，本文提出了一種彎道避撞控制策略，在彎道行駛中前方車輛緊急制動(dòng)的危險(xiǎn)工況下，若自車無(wú)法通過制動(dòng)避開前方車輛，采用換道路徑規(guī)劃的避撞控制算法?；赑reScan軟件搭建彎道場(chǎng)景模型，利用彎道安全距離模型實(shí)時(shí)判斷是否處于危險(xiǎn)工況；危險(xiǎn)工況下，自車依據(jù)斜坡正弦函數(shù)作為換道路徑，進(jìn)行換道避撞；采用擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）算法對(duì)車輛橫擺角速度等關(guān)鍵狀態(tài)變量進(jìn)行實(shí)時(shí)跟蹤，保證算法在滿足穩(wěn)定性的前提下安全避撞。

2 彎道建模與車輛狀態(tài)估計(jì)

2.1 彎道建模

考慮到汽車行駛的安全性，車輛進(jìn)入彎道后一般不進(jìn)行加速操作，假設(shè)汽車在彎道中低速行駛，為降低道路模型的復(fù)雜度，將道路模型簡(jiǎn)化為一段圓弧，曲率中心為OC。以外側(cè)彎道中心線l1入口為原點(diǎn)O1，以l1的切線為x軸，以垂直于x軸并指向OC的直線為y軸，彎道運(yùn)動(dòng)模型如圖1所示。

圖1 彎道運(yùn)動(dòng)模型

假設(shè)l1與l2具有相同的瞬心，l1、l2的曲率半徑分別為R、r0，d為在同一彎道軌跡中兩車的弧長(zhǎng)距離，θ為車輛航向角，ye為車輛換道完成后的側(cè)向位移，即兩車道中心線間距，換道過程中沿曲率中心方向的加速度為：

沿彎道切線方向的加速度為：

式中，v為換道過程中的車速；v0為車輛初始速度；α為質(zhì)心側(cè)偏角；ac為換道過程的側(cè)向加速度；te為換道完成時(shí)間。

2.2 3自由度模型

從本文估計(jì)車輛狀態(tài)的角度出發(fā)，需要引用縱向X、側(cè)向Y、和橫擺Z 3個(gè)自由度的狀態(tài)變量。因此，在線性2自由度車輛模型的基礎(chǔ)上引入縱向運(yùn)動(dòng)自由度，模型如圖2所示。

圖2中，假設(shè)車輛是對(duì)稱的，XOY是固定在車輛質(zhì)心處的坐標(biāo)系，規(guī)定X軸在汽車的縱向?qū)ΨQ軸上，向前為正方向，規(guī)定Y軸在水平面內(nèi)與X軸垂直，通過車輛質(zhì)心O點(diǎn)，向左為正方向。為了便于EKF算法估計(jì)，建立3自由度車輛模型的系統(tǒng)狀態(tài)方程為[9]：

圖2 3自由度非線性車輛動(dòng)力學(xué)模型

式中，r為車輛橫擺角速度；k1、k2分別為前、后輪總側(cè)偏剛度；m為汽車總質(zhì)量；a、b分別為前、后軸到質(zhì)心的距離；δ為前輪轉(zhuǎn)角；Iz為繞Z軸的轉(zhuǎn)動(dòng)慣量；β為質(zhì)心側(cè)偏角。

狀態(tài)變量為x（t）=（r,β,vx）T，量測(cè)方程為 y（t）=ay，控制變量為u（t）=（δ,ax）。

2.3 車輛狀態(tài)估計(jì)

EKF算法是將非線性函數(shù)在最佳估計(jì)點(diǎn)進(jìn)行泰勒展開，使該模型線性化，從而適用于各種非線性系統(tǒng)[10]。

建立系統(tǒng)狀態(tài)方程及量測(cè)方程：

式中，w（t）為系統(tǒng)激勵(lì)；v（t）為量測(cè)噪聲。

設(shè)Q為系統(tǒng)激勵(lì)噪聲的協(xié)方差矩陣，R為量測(cè)噪聲的協(xié)方差矩陣。分別對(duì)式（6）、式（7）的狀態(tài)變量求導(dǎo)，得到雅可比矩陣F（t）、H（t）：

設(shè)EKF算法的狀態(tài)過程噪聲方差矩陣為Q=diag（0.1,0.1,0.1），觀測(cè)協(xié)方差為Rk=[0.001]。

3 換道路徑規(guī)劃與安全距離模型

3.1 換道路徑規(guī)劃

車輛在轉(zhuǎn)向時(shí)，最易發(fā)生碰撞的部位是自車右前部與前車左后部，本文針對(duì)這種典型的碰撞情況進(jìn)行了換道過程分析。

為研究需要，對(duì)車輛轉(zhuǎn)向換道運(yùn)動(dòng)作出如下假設(shè)：

a. 自車在最大制動(dòng)強(qiáng)度條件下已下無(wú)法避開前車；

b.自車在整個(gè)換道過程中低速行駛；

c. 自車、前車車輛尺寸和性能相同；

d. 假設(shè)雙車道上只有自車和前車，兩車起始位置同在外側(cè)彎道，相鄰車道上無(wú)其他車輛，且前車緊急制動(dòng)；

e.僅考慮自車從外側(cè)彎道換道至內(nèi)側(cè)彎道的避撞路徑。

換道過程如圖3所示，其中，tc為預(yù)碰撞時(shí)刻，xe（t）為車輛換道的縱向位移。

圖3 汽車換道位置示意

本文采用斜坡正弦函數(shù)[11]作為換道路徑：

xe=v·te，帶入式（10）中，得到y(tǒng)e與t的關(guān)系（即換道軌跡），如圖4所示。

圖4 斜坡正弦函數(shù)換道軌跡

對(duì)式（10）進(jìn)行二次求導(dǎo)得到換道路徑的側(cè)向加速度：

在自車轉(zhuǎn)向換道過程中，考慮駕駛員反應(yīng)時(shí)間對(duì)安全距離的影響較大，采用文獻(xiàn)[12]的研究結(jié)果，選取駕駛員類型為男青年普通型，反應(yīng)時(shí)間τ1=0.935 s。在危險(xiǎn)工況下，若轉(zhuǎn)向盤轉(zhuǎn)角傳感器在該時(shí)間段內(nèi)未檢測(cè)到明顯變化，則默認(rèn)為駕駛員未進(jìn)行轉(zhuǎn)向操作，車輛處于極不安全的狀態(tài)，此時(shí)由換道避撞控制系統(tǒng)接管車輛，通過控制轉(zhuǎn)向盤轉(zhuǎn)角δsw完成換道操作。轉(zhuǎn)向盤轉(zhuǎn)角由逆動(dòng)力學(xué)模型[13]計(jì)算得出：

式中，l為車輛軸距；i為轉(zhuǎn)向系傳動(dòng)比；K′為穩(wěn)定性因數(shù)。

3.2 安全距離模型

為避免與前車發(fā)生側(cè)碰及追尾，當(dāng)自車檢測(cè)到與前車直線距離小于安全閾值時(shí)，進(jìn)行轉(zhuǎn)向換道操作，兩車初始的弧長(zhǎng)距離d應(yīng)滿足：

式中，s0、s1分別為自車和前車運(yùn)動(dòng)的路程；a0為自車的加速度，其大小為沿彎道切線方向加速度與沿曲率中心方向加速度的矢量和的模長(zhǎng)；a1為前車的加速度；L為車長(zhǎng)；W為車寬。

由于目前車輛輔助駕駛系統(tǒng)采用的傳感器（如激光雷達(dá)、毫米波雷達(dá)）只能采集直線距離，對(duì)彎道距離的檢測(cè)較困難，故將彎道弧長(zhǎng)安全距離轉(zhuǎn)換為直線長(zhǎng)度[14]。根據(jù)弦長(zhǎng)公式，由圖3可知該彎道模型的安全距離S為：

4 控制器設(shè)計(jì)及算法驗(yàn)證

4.1 彎道避撞控制器設(shè)計(jì)

彎道避撞控制器根據(jù)車載前置攝像頭實(shí)時(shí)檢測(cè)車道線，由傳感器測(cè)得兩車實(shí)際距離，根據(jù)安全距離模型計(jì)算出安全距離S，判斷車輛是否處于危險(xiǎn)工況，一旦檢測(cè)到實(shí)際距離s小于安全距離S，開啟轉(zhuǎn)向換道控制?？刂破髡w邏輯框架如圖5所示。由于彎道避撞過程中除換道產(chǎn)生的側(cè)向加速度外，車輛還有沿彎道行駛的向心加速度ac，聯(lián)立式（1）與式（2），得到避撞過程中車輛實(shí)際側(cè)向加速度為：

式中，yd為車輛移動(dòng)過程中的側(cè)向位移。

由轉(zhuǎn)向逆動(dòng)力學(xué)模型得出理想轉(zhuǎn)向盤轉(zhuǎn)角δ0，該角度為車輛彎道行駛的轉(zhuǎn)向盤轉(zhuǎn)角δ1和換道避撞過程中的轉(zhuǎn)向盤轉(zhuǎn)角δ2的線性疊加：

圖5 控制器邏輯框架

下層控制器采用PID控制，通過比較δ0與實(shí)際轉(zhuǎn)向盤轉(zhuǎn)角δsw的偏差，調(diào)節(jié)PID值使得δsw較好地跟蹤δ0。

4.2 估計(jì)算法的虛擬驗(yàn)證

為驗(yàn)證EKF狀態(tài)估計(jì)算法的有效性，在PreScan與MATLAB/Simulink環(huán)境下建立聯(lián)合仿真平臺(tái)，以PreScan軟件的狀態(tài)輸出結(jié)果為實(shí)際值，將其與EKF算法的估計(jì)結(jié)果進(jìn)行對(duì)比。整車的參數(shù)如表1所示，設(shè)定前車速度為36 km/h，自車速度為46 km/h。

參數(shù)車寬/m質(zhì)心繞Z軸的轉(zhuǎn)動(dòng)慣量/kg·m2前輪側(cè)偏剛度/kN·rad-1后輪側(cè)偏剛度/kN·rad-1參數(shù)整備質(zhì)量/kg前輪距/m后輪距/m質(zhì)心至前軸距離/m車長(zhǎng)/m取值1 480 1.50 1.50 1.08 4.43取值1.86 2 562-181.70-198.91

模擬在附著系數(shù)μ=0.85的路面進(jìn)行蛇行試驗(yàn)，轉(zhuǎn)向盤轉(zhuǎn)角變化如圖6所示，在第2.5～7.5 s時(shí)間段內(nèi)轉(zhuǎn)向盤轉(zhuǎn)角做幅值為40°、周期為2.5 s的正弦變化。

圖6 轉(zhuǎn)向盤轉(zhuǎn)角變化

圖7 所示為縱向車速EKF估計(jì)值與實(shí)際值的對(duì)比，從圖7可以看出，EKF算法車速估計(jì)效果比較理想，估算值與實(shí)際值最大瞬態(tài)偏差僅為0.02 km/h，相對(duì)誤差為0.03%。

圖8所示為橫擺角速度EKF估計(jì)值與實(shí)際值的對(duì)比，從圖8可以看出，EKF算法估計(jì)效果較理想，最大瞬態(tài)偏差出現(xiàn)在第6.8 s，偏差為0.9°/s。以上分析表明，提出的EKF算法估計(jì)效果良好，具有較好的魯棒性。

圖7 縱向車速估算對(duì)比

圖8 橫擺角速度估算對(duì)比

5 虛擬及實(shí)車驗(yàn)證

5.1 彎道避撞控制策略的虛擬驗(yàn)證

為驗(yàn)證彎道避撞控制策略的有效性，在PreScan軟件中搭建彎道避撞場(chǎng)景，如圖9所示。設(shè)置仿真工況為：前車在外車道上進(jìn)行緊急制動(dòng)，運(yùn)行一段時(shí)間后停止，自車在外車道上勻速行駛，且內(nèi)車道無(wú)車輛通過。

圖9 彎道避撞場(chǎng)景建模

圖10 所示為避撞過程中自車與前車的行駛軌跡曲線，仿真開始時(shí)，自車起始位置為外車道25 m處，前車在外車道57 m處。

圖10 車輛行駛軌跡

仿真開始后，在25～56 m路段，自車一直在外側(cè)車道勻速運(yùn)動(dòng)，同樣在外側(cè)行駛的前車突然緊急制動(dòng)，自車通過傳感器檢測(cè)到與前方車輛的距離小于安全距離后，通過制動(dòng)已經(jīng)不能避免碰撞，自車采取換道操作進(jìn)行避撞，并保持勻速行駛。從圖中可以看出，自車行駛軌跡曲線在58～70 m路段的軌跡曲率發(fā)生明顯變化，表明在此處進(jìn)行了轉(zhuǎn)向操作，換道至內(nèi)側(cè)車道。前車緊急制動(dòng)行駛至X=66 m處停止，避撞過程中兩條曲線沒有交點(diǎn)，證明沒有發(fā)生碰撞。

5.2 彎道避撞控制策略的實(shí)車驗(yàn)證

在外側(cè)車道曲率半徑為96 m、內(nèi)側(cè)車道曲率半徑為81 m的瀝青彎道路面進(jìn)行測(cè)試，采用GPS采集車速、航向角、經(jīng)緯度等信息，采用毫米波雷達(dá)采集距離信息?？紤]實(shí)際行車過程中多數(shù)駕駛員的彎道行駛速度大于30 km/h，本文針對(duì)該情況進(jìn)行彎道避撞的實(shí)車驗(yàn)證。設(shè)置前車最高車速30 km/h，自車最高車速為36 km/h。

試驗(yàn)過程中，傳感器信號(hào)通過串口通訊發(fā)送至上位機(jī)，控制器計(jì)算出安全距離，若實(shí)際距離小于該值，采用彎道換道避撞控制算法，依據(jù)逆動(dòng)力學(xué)模型計(jì)算出轉(zhuǎn)向盤轉(zhuǎn)角δ0，通過車身CAN發(fā)送轉(zhuǎn)角信號(hào)，車輛接收到信號(hào)后進(jìn)行換道，測(cè)試設(shè)備如圖11所示。

圖11 測(cè)試設(shè)備

將GPS采集到的經(jīng)緯度點(diǎn)生成KML格式導(dǎo)入高清地圖軟件，其避撞路徑如圖12所示。

圖12 兩車的運(yùn)動(dòng)軌跡

其中A-B-C-D-E線段為自車的運(yùn)動(dòng)軌跡，F(xiàn)-G-HJ線段為前車（障礙車）的運(yùn)動(dòng)軌跡。自車在A點(diǎn)開始勻加速行駛，加速至36 km/h后到達(dá)B點(diǎn)，勻速行駛；自車在C點(diǎn)處檢測(cè)到與前車的距離小于安全距離，開始轉(zhuǎn)向換道，至D點(diǎn)完成換道操作，E點(diǎn)為自車避撞結(jié)束點(diǎn)。前車由F點(diǎn)開始加速，加速至30 km/h到達(dá)G點(diǎn)后勻速行駛，模擬前車在H點(diǎn)發(fā)生故障采取緊急制動(dòng)，至J點(diǎn)停止。

試驗(yàn)過程中，自車從外側(cè)車道換至內(nèi)側(cè)車道，避免了與前車的相撞，在低速避撞過程中自車無(wú)側(cè)滑、甩尾等不穩(wěn)定的情況，滿足車輛行駛的穩(wěn)定性要求。

6 結(jié)束語(yǔ)

本文提出一種在自車通過制動(dòng)無(wú)法完成彎道避撞時(shí)的換道避撞算法。設(shè)計(jì)了基于斜坡正弦函數(shù)的避撞換道路徑，利用彎道安全距離模型實(shí)時(shí)判斷自車是否采取換道轉(zhuǎn)向避撞措施，換道避撞過程中采用擴(kuò)展卡爾曼濾波算法對(duì)車輛橫擺角速度等關(guān)鍵狀態(tài)變量進(jìn)行實(shí)時(shí)跟蹤，以保證彎道避撞的穩(wěn)定性。仿真和實(shí)車驗(yàn)證結(jié)果表明，車輛在彎道行駛中通過換道有效避開了前方緊急制動(dòng)的障礙車輛。