基于車輛前方目標(biāo)運(yùn)動(dòng)模型的主動(dòng)避撞系統(tǒng)的研究

盤朝奉，丁亞強(qiáng)，江浩斌,b

(江蘇大學(xué) a.汽車工程研究院； b.汽車與交通工程學(xué)院, 江蘇 鎮(zhèn)江 212013)

近年來，汽車行業(yè)的飛速發(fā)展以及國(guó)民購(gòu)買能力的提高帶動(dòng)了汽車銷量的增加，但也不可避免地提高了交通事故發(fā)生的可能性。研究表明：如果提前0.5 s示警駕駛員，60%的追尾事故可以避免[1]。從保護(hù)人身安全和降低交通事故發(fā)生的可能性的角度出發(fā)，研制出能在兩車有碰撞危險(xiǎn)時(shí)及時(shí)提醒駕駛員采取相應(yīng)的措施，甚至對(duì)汽車進(jìn)行主動(dòng)干預(yù)、自動(dòng)采取緊急制動(dòng)措施進(jìn)行有效避撞的汽車主動(dòng)避撞系統(tǒng)(collision warning system)顯得至關(guān)重要。而如何有效、精準(zhǔn)地獲取車輛前方目標(biāo)的有效信息，并通過對(duì)前方目標(biāo)的跟蹤預(yù)測(cè)其下一步的運(yùn)動(dòng)狀態(tài)，則成為當(dāng)前研究的熱點(diǎn)。

2017年博世展出了一輛配置主動(dòng)避撞系統(tǒng)等先進(jìn)自動(dòng)駕駛技術(shù)的自動(dòng)駕駛汽車，其可以在低速狀態(tài)下實(shí)現(xiàn)主動(dòng)避撞[2]。沃爾沃公司開發(fā)的避撞控制系統(tǒng)能基于前方移動(dòng)目標(biāo)車輛信息實(shí)現(xiàn)自動(dòng)走-停巡航控制，控制車輛的自動(dòng)剎車和驅(qū)動(dòng)，實(shí)現(xiàn)主動(dòng)避撞[3]。江蘇賽博公司與高校合作也研制出了擁有多目標(biāo)識(shí)別能力的毫米波雷達(dá)，可以很好地運(yùn)用于車輛復(fù)雜環(huán)境防撞系統(tǒng)[4-6]。

汽車行駛過程中受路面形狀、附著系數(shù)以及汽車自身參數(shù)等多重因素的影響，在通過毫米波雷達(dá)跟蹤前方目標(biāo)時(shí)不可避免地會(huì)出現(xiàn)目標(biāo)識(shí)別錯(cuò)誤和漏警的情況。建立與前方機(jī)動(dòng)目標(biāo)的實(shí)際情況相符合又便于數(shù)學(xué)分析處理的目標(biāo)運(yùn)動(dòng)模型可以很好地跟蹤目標(biāo)而不會(huì)造成目標(biāo)的丟失[7-8]。本文通過對(duì)汽車運(yùn)動(dòng)規(guī)律的分析和毫米波雷達(dá)的障礙物檢測(cè)機(jī)理，對(duì)傳統(tǒng)的卡爾曼濾波算法進(jìn)行簡(jiǎn)單介紹，比較了常速度模型、常加速度模型以及當(dāng)前統(tǒng)計(jì)模型的系統(tǒng)原理及應(yīng)用場(chǎng)合。根據(jù)城市道路車輛行駛的實(shí)際情況，通過對(duì)幾種目標(biāo)機(jī)動(dòng)模型算法的仿真分析，選取了當(dāng)前統(tǒng)計(jì)模型作為機(jī)動(dòng)目標(biāo)模型，并采用了基于當(dāng)前統(tǒng)計(jì)模型的自適應(yīng)卡爾曼濾波算法對(duì)車輛前方目標(biāo)進(jìn)行跟蹤。選取一種臨界安全距離模型，在此基礎(chǔ)上建立主動(dòng)避撞仿真模型，搭建實(shí)車實(shí)驗(yàn)平臺(tái)，通過實(shí)車實(shí)驗(yàn)再一次驗(yàn)證了算法的可行性。

1 目標(biāo)跟蹤模型

車輛行駛過程中自車與前方障礙物目標(biāo)是存在相對(duì)運(yùn)動(dòng)的，目標(biāo)跟蹤算法需要實(shí)時(shí)得到前方障礙物目標(biāo)的位置、速度及加速度等狀態(tài)信息，精確地識(shí)別前方目標(biāo)并準(zhǔn)確判斷目標(biāo)距離，這有利于防碰撞功能的實(shí)現(xiàn)。

目前主要的跟蹤模型主要有目標(biāo)類似勻速運(yùn)動(dòng)的勻速運(yùn)動(dòng)模型(CV)、目標(biāo)類似勻加速的勻加速運(yùn)動(dòng)模型(CA)和能應(yīng)用于多種目標(biāo)運(yùn)動(dòng)模型的 Singer 模型，后來在 Singer 模型的基礎(chǔ)上又提出了當(dāng)前統(tǒng)計(jì)模型,下面主要介紹常用于車輛跟蹤的當(dāng)前統(tǒng)計(jì)模型[9-11]。

勻速運(yùn)動(dòng)模型和勻加速運(yùn)動(dòng)模型都是研究目標(biāo)相對(duì)規(guī)律運(yùn)動(dòng)的狀態(tài)，對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)的研究都有一定的局限性。如果當(dāng)目標(biāo)做不規(guī)律運(yùn)動(dòng)時(shí)，它的加速度在緊鄰的下一刻不會(huì)發(fā)生太大的突變，即只能取在“當(dāng)前”的加速度的某一個(gè)鄰域內(nèi)的有限值，這即為當(dāng)前統(tǒng)計(jì)模型提出的根據(jù)。其表達(dá)式如下：

由此可以得出當(dāng)前統(tǒng)計(jì)模型的狀態(tài)空間方程：

(4)

選取狀態(tài)變量

(5)

假設(shè)采樣周期為T，需要對(duì)上式進(jìn)行離散化，由此可得到當(dāng)前統(tǒng)計(jì)模型的離散狀態(tài)方程：

(6)

相應(yīng)的觀測(cè)方程為：

Z(k)=HZ(k)+V(k)

(7)

狀態(tài)轉(zhuǎn)移矩陣為：

式中：Z(k)為觀測(cè)向量；H為觀測(cè)矩陣。

輸入矩陣：

2 卡爾曼濾波算法

目前用于目標(biāo)跟蹤的算法很多，其中，卡爾曼( Kalman )濾波算法因能結(jié)合車輛行駛的特點(diǎn)，對(duì)車輛跟蹤的效果比較穩(wěn)定，所以普遍應(yīng)用于車輛目標(biāo)跟蹤領(lǐng)域。

設(shè)跟蹤系統(tǒng)過程的離散隨機(jī)差分方程描述為

X(k)=AX(k-1)+BU(k-1)+W(k-1)

(16)

系統(tǒng)的觀測(cè)方程為：

Z(k)=HX(k)+V(k)

(17)

式中：Z(K)為k時(shí)刻的量測(cè)值；H為系統(tǒng)的測(cè)量矩陣；V(K)為系統(tǒng)量測(cè)高斯白噪聲。

卡爾曼濾波的數(shù)學(xué)模型如下：

1) 狀態(tài)變量預(yù)測(cè)方程：

X(k|k-1)=AX(k-1|k-1)+BU(k)

(18)

2) 協(xié)方差矩陣預(yù)測(cè)方程：

P(k|k-1)=AP(k-1|k-1)AT+Q

(19)

3) Kalman增益矩陣方程：

Kg(k)=P(k|k-1)HT/[HP(k|k-1)HT+R]

(20)

4) 狀態(tài)變量最優(yōu)化方程：

X(k|k)=X(k|k-1)+Kg(k)[Z(k)-HX(k|k-1)]

(21)

5)協(xié)方差最優(yōu)更新方程：

P(k|k)=[1-Kg(k)H]P(k|k-1)

(22)

式(18)～(22)中：X(k|k-1)為通過前一狀態(tài)預(yù)測(cè)的結(jié)果；X(k-1|k-1)為前一狀態(tài)通過運(yùn)算得到的最優(yōu)結(jié)果；U(k) 為當(dāng)前狀態(tài)的控制量；P(k|k-1) 為X(k|k-1) 為當(dāng)前狀態(tài)相對(duì)應(yīng)的協(xié)方差矩陣；P(k-1|k-1) 為X(k-1|k-1) 對(duì)應(yīng)的協(xié)方差矩陣；AT為A的轉(zhuǎn)置；X(k|k) 表示現(xiàn)在狀態(tài)的最優(yōu)估計(jì)值；Kg(k) 為當(dāng)前狀態(tài)的卡爾曼增益；HT為H的轉(zhuǎn)置。

卡爾曼濾波主要由2部分組成：一為時(shí)間更新方程，即步驟 1)和 2)，計(jì)算得出狀態(tài)變量的一步預(yù)測(cè)以及相應(yīng)的預(yù)測(cè)協(xié)方差矩陣，得到的協(xié)方差矩陣可作為下一時(shí)刻的狀態(tài)估算；二為狀態(tài)更新方程，即 第3)～ 5)步，通過第1部分中得到的狀態(tài)變量的預(yù)測(cè)以及相應(yīng)的預(yù)測(cè)協(xié)方差矩陣和當(dāng)前狀態(tài)的觀測(cè)值，計(jì)算得出當(dāng)前狀態(tài)的后驗(yàn)估計(jì)。

3 仿真模型的建立

車輛在道路上直線行駛時(shí)，一般有以下幾種行駛模式：勻速或接近勻速、勻加速或勻減速(或接近勻加速勻減速)以及變加速變減速。因?yàn)檎虻臏p速可以當(dāng)作反向的加速[13]，因此本文將減速運(yùn)動(dòng)與加速運(yùn)動(dòng)看作同一種運(yùn)動(dòng)情況，統(tǒng)一以加速代替。所以主要設(shè)計(jì)了以下幾種仿真工況：車輛勻速運(yùn)動(dòng)、車輛勻加速運(yùn)動(dòng)以及車輛變加速運(yùn)動(dòng)。分別對(duì)常速度模型、常加速度模型以及當(dāng)前統(tǒng)計(jì)模型的卡爾曼濾波算法估計(jì)結(jié)果進(jìn)行了對(duì)比分析。圖1為目標(biāo)運(yùn)動(dòng)仿真模型。

圖1 目標(biāo)運(yùn)動(dòng)仿真模型

3.1 車輛勻速運(yùn)動(dòng)

將車輛看作勻速運(yùn)動(dòng)，分別對(duì)車輛的當(dāng)前統(tǒng)計(jì)模型、CA模型和CV模型的卡爾曼濾波算法進(jìn)行仿真分析，設(shè)置兩車仿真距離為60 m，以相同的速度同向直線行駛，隨著時(shí)間的變化，可得到兩車的位移和速度曲線，見圖2、3。

圖2 勻速運(yùn)動(dòng)時(shí)各模型跟蹤相對(duì)距離的仿真結(jié)果

圖3 勻速運(yùn)動(dòng)時(shí)各模型跟蹤相對(duì)速度的仿真結(jié)果

由仿真結(jié)果可得：車輛在勻速跟車時(shí)，當(dāng)前統(tǒng)計(jì)模型和常速度模型跟蹤算法較為精確，常加速度模型跟蹤算法誤差較大。

3.2 車輛勻加速運(yùn)動(dòng)

將車輛看作勻加速運(yùn)動(dòng)，分別對(duì)車輛的當(dāng)前統(tǒng)計(jì)模型、CA模型和CV模型的卡爾曼濾波算法進(jìn)行仿真分析，設(shè)置兩車仿真距離為60 m,同向直線行駛時(shí)，自車速度為30 km/h，加速度為1 m/s2，前車速度為20 km/h，加速度為1.5 m/s2，隨著時(shí)間的變化，可得到兩車的位移和速度曲線，見圖4、5。

由仿真結(jié)果可得:車輛在勻加速跟車時(shí)，當(dāng)前統(tǒng)計(jì)模型和常加速度模型跟蹤算法較為精確，常速度模型跟蹤算法誤差較大。

圖4 勻加速運(yùn)動(dòng)時(shí)各模型跟蹤相對(duì)距離的仿真結(jié)果

圖5 勻加速運(yùn)動(dòng)時(shí)各模型跟蹤相對(duì)速度的仿真結(jié)果

3.3 車輛變加速運(yùn)動(dòng)

由上述兩個(gè)仿真模型分析可以看到：常速度模型的卡爾曼濾波跟蹤算法適用于車輛勻速運(yùn)動(dòng)，常加速度模型的卡爾曼濾波跟蹤算法適用于車輛勻加速運(yùn)動(dòng)，而當(dāng)前統(tǒng)計(jì)模型的卡爾曼濾波跟蹤算法則可適用于兩種形式的車輛運(yùn)動(dòng)狀態(tài)。車輛在道路上行駛時(shí)，不止會(huì)有前兩種運(yùn)動(dòng)狀態(tài)，還可能會(huì)進(jìn)行變加速運(yùn)動(dòng)等不規(guī)則運(yùn)動(dòng)，因此本文建立相關(guān)跟蹤模型進(jìn)行進(jìn)一步的仿真分析。

將車輛運(yùn)動(dòng)看作變加速運(yùn)動(dòng)，分別對(duì)車輛的當(dāng)前統(tǒng)計(jì)模型、CA模型和CV模型的卡爾曼濾波算法進(jìn)行仿真分析，設(shè)置兩車仿真距離為50 m，同向直線行駛時(shí)，自車做速度為30 km/h 的勻速運(yùn)動(dòng)，前車初速度為20 km/h，在前5 s內(nèi)做勻速運(yùn)動(dòng)，在5～10 s內(nèi)做加速度為2 m/s2的勻加速運(yùn)動(dòng)，在 10～15 s內(nèi)做加速度為-1 m/s2的勻減速運(yùn)動(dòng)，然后在 15～20 s內(nèi)做勻速運(yùn)動(dòng)，隨著時(shí)間的變化，可得到兩車的位移和速度曲線,見圖6、7。

圖6 變加速運(yùn)動(dòng)時(shí)各模型跟蹤相對(duì)距離的仿真結(jié)果

圖7 變加速運(yùn)動(dòng)時(shí)各模型跟蹤相對(duì)速度的仿真結(jié)果

通過對(duì)車輛的3種運(yùn)動(dòng)狀態(tài)的仿真結(jié)果可以看出：相對(duì)于常速度模型和常加速度模型的卡爾曼濾波算法，基于當(dāng)前統(tǒng)計(jì)模型的卡爾曼濾波跟蹤算法可以更精確地跟蹤前方目標(biāo)，在車輛正常行駛時(shí)可以很好地達(dá)到穩(wěn)定的跟蹤效果，且相對(duì)誤差較小。

3.4 安全距離模型

假設(shè)車輛在制動(dòng)時(shí)能全力制動(dòng)，引入臨界安全距離模型，對(duì)跟蹤效果進(jìn)行進(jìn)一步分析。安全距離模型見圖8。

圖8 安全距離模型示意圖

如圖8所示，當(dāng)兩車同向行駛且前車在后車的正前方時(shí)，兩車在A車制動(dòng)的剎那相距為D。A車制動(dòng)一段時(shí)間后，行駛距離為Sa，B車的行駛距離為Sb，這時(shí)兩車間的距離為d0。安全距離的公式為

D=Sa-Sb+d0

(23)

臨界安全距離可表示為

式中:amax為自車最大制動(dòng)減速度,一般數(shù)值取6～8 m/s2；d1為自車停止后與前方目標(biāo)車輛之間的車間距,一般取1～2 m;v1和vrel分別為自車制動(dòng)前的初始速度和相對(duì)前車的速度[14-15]。

3.5 主動(dòng)避撞仿真模型

為了進(jìn)一步驗(yàn)證本算法的雷達(dá)的跟蹤效果，基于本算法建立了主動(dòng)避撞模型，見圖9。

圖9 主動(dòng)避撞仿真模型

4 實(shí)車實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證所引用的安全距離模型的有效性以及本算法的跟蹤效果，基于中距離雷達(dá)對(duì)主動(dòng)避撞系統(tǒng)進(jìn)行實(shí)車實(shí)驗(yàn)。根據(jù)中國(guó)新車評(píng)價(jià)規(guī)程標(biāo)準(zhǔn)測(cè)試工況，在天氣晴朗的情況下，進(jìn)行了實(shí)車驗(yàn)證。實(shí)驗(yàn)設(shè)備見表1。

表1 實(shí)驗(yàn)設(shè)備

目標(biāo)車試驗(yàn)?zāi)繕?biāo)車運(yùn)動(dòng)目標(biāo)車車載設(shè)備毫米波雷達(dá)執(zhí)行器控制器其他設(shè)備Canape11.0線束CancaseOBD線

上位機(jī)采用 MicroAutoBox II 控制器作為通信網(wǎng)關(guān)，實(shí)現(xiàn)上位機(jī)數(shù)據(jù)收發(fā)。測(cè)控上位機(jī) PC 采用光纖以太網(wǎng)與 MicroAutoboxII 連接，并擁有CANCase及其他設(shè)備的預(yù)留接口。CANCase用于實(shí)時(shí)在線觀測(cè)雷達(dá)數(shù)據(jù)，完成數(shù)據(jù)采錄工作，可通過canape讀取數(shù)據(jù)，且可以轉(zhuǎn)換為.mat格式，通過Matlab 分析數(shù)據(jù)。本次實(shí)驗(yàn)采用的是國(guó)內(nèi)某款車型，該車匹配有博世第 9 代 ESP 系統(tǒng)，能作為自動(dòng)緊急制動(dòng)系統(tǒng)的執(zhí)行器。圖10 為移動(dòng)目標(biāo)車。

圖10 移動(dòng)目標(biāo)車

實(shí)驗(yàn)中，運(yùn)動(dòng)目標(biāo)是支撐架下的反射棱鏡，該三棱鏡有3個(gè)面，3個(gè)面之間角度為 90°，這樣的尖銳直角結(jié)構(gòu)對(duì)于雷達(dá)波有非常強(qiáng)的反射效應(yīng)。實(shí)驗(yàn)測(cè)試證明，該三角棱鏡可以代替車輛作為雷達(dá)反射目標(biāo)。使用該移動(dòng)目標(biāo)進(jìn)行測(cè)試實(shí)驗(yàn)的優(yōu)點(diǎn)是測(cè)試方便、反復(fù)使用性強(qiáng)。但基于安全起見，只進(jìn)行了自車30 km/h對(duì)前車10 km/h以及20 km/h的工況實(shí)驗(yàn)，所得結(jié)果與仿真結(jié)果的對(duì)比分析如圖11、12所示。

圖11 前車速度為10 km/h時(shí)隨時(shí)間變化的距離

圖12 前車速度為20 km/h時(shí)隨時(shí)間變化的距離

兩個(gè)實(shí)驗(yàn)結(jié)果均表明：基于當(dāng)前統(tǒng)計(jì)模型的卡爾曼濾波算法的實(shí)車實(shí)驗(yàn)結(jié)果與仿真結(jié)果趨勢(shì)完全一致，車輛在到達(dá)臨界安全距離時(shí)制動(dòng)，避免了碰撞，表明所建立的仿真模型是可靠的。

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

本文對(duì)車輛前方目標(biāo)的跟蹤采用了3種仿真工況，最后仿真對(duì)比分析得到了當(dāng)前統(tǒng)計(jì)模型卡爾曼濾波跟蹤算法能實(shí)現(xiàn)各種工況下的精確追蹤的結(jié)論。使用該算法，建立一種臨界安全距離模型，通過實(shí)車實(shí)驗(yàn)，對(duì)該算法進(jìn)行進(jìn)一步的驗(yàn)證。對(duì)實(shí)驗(yàn)所得數(shù)據(jù)進(jìn)行分析，證明該算法能精確跟蹤前車，且避免了車輛碰撞，再次驗(yàn)證了該算法的可行性。但本文只驗(yàn)證了前方移動(dòng)目標(biāo)的跟蹤效果，接下來對(duì)車輛前方靜止目標(biāo)的主動(dòng)避撞效果還需進(jìn)一步的驗(yàn)證。