一種V2X環(huán)境下基于改進(jìn)卡爾曼濾波的CACC數(shù)據(jù)精度提高方法

趙紅專，盧寧寧，陳建鵬，展 新，許恩永

(1. 桂林電子科技大學(xué) 建筑與交通工程學(xué)院，廣西 桂林 541004；2. 東風(fēng)柳州汽車有限公司 商用車技術(shù)中心，廣西 柳州 545000)

0 引 言

典型彎道區(qū)域因駕駛員視距受限和曲線線性特殊性，車輛間需要通過(guò)信息進(jìn)行頻繁的交互以獲得各自可靠信息和可信服務(wù)，實(shí)現(xiàn)有效協(xié)同。統(tǒng)計(jì)表明，彎曲、起伏路段發(fā)生的交通事故中，三分之二以上的事故發(fā)生在彎道區(qū)域；相關(guān)資料表明[1]，彎道區(qū)域的事故發(fā)生率是其他路段的兩倍以上，其機(jī)動(dòng)車事故死亡人數(shù)約占總機(jī)動(dòng)車事故死亡人數(shù)的四分之一。

V2X (vehicle to everything, V2X)通過(guò)車載傳感器設(shè)備檢測(cè)前車行駛狀態(tài)的信息，并將前方車輛行駛過(guò)程中的速度、加速度、位置等信息傳遞給本車，為CACC (cooperative adaptive cruise control, CACC)車隊(duì)中車車信息傳遞和信息交互提供技術(shù)支撐。車輛通過(guò)V2X技術(shù)實(shí)現(xiàn)車車互聯(lián)通信[2]，以便能夠有效獲取道路周邊信息和車輛實(shí)時(shí)駕駛數(shù)據(jù)，從而提高道路吞吐量和交通安全，提高駕駛員的舒適性。彎道中行駛的車輛因各種因素?zé)o法保持車道行駛，從而偏離車道，引發(fā)車輛碰撞[3-5]；CACC系統(tǒng)通過(guò)多種傳感器感知周圍車輛的行駛狀態(tài)和路況[6]，為本車獲取前方車輛實(shí)時(shí)行駛數(shù)據(jù)提供技術(shù)支持，有效減少車輛間的行駛安全距離，為車路協(xié)同控制提供保障。

CACC車輛間進(jìn)行信息頻繁交互過(guò)程中，會(huì)存在通訊延遲和信息交互不可信等問(wèn)題。筆者團(tuán)隊(duì)[7]為解決交通動(dòng)態(tài)條件下信息交互的可信性問(wèn)題，提出了一種基于車輛信息物理系統(tǒng)的車輛自組織機(jī)制的新信任模型，該信任模型可以提高車輛交互安全和駕駛安全。但是信息在數(shù)據(jù)傳輸?shù)倪^(guò)程中，仍存在環(huán)境噪聲、系統(tǒng)誤差、數(shù)據(jù)傳輸?shù)目煽啃缘仁箓鞲衅鳙@取的數(shù)據(jù)存在噪聲[8]等不利因素，進(jìn)而影響CACC車隊(duì)的安全行駛。

目前關(guān)于CACC車隊(duì)行駛特性的研究較多，如車輛跟馳、車輛滲透率等方面研究，很少關(guān)于車輛在彎道的行駛狀態(tài)的研究，目前研究也沒(méi)有考慮獲取的數(shù)據(jù)精度對(duì)車輛行駛的影響。筆者根據(jù)車載設(shè)備獲取的前方車輛信息通過(guò)V2X技術(shù)傳遞給本車，在此過(guò)程中采用基于速度方位角變化的改進(jìn)卡爾曼濾波算法處理此數(shù)據(jù)，以提高車輛行駛的數(shù)據(jù)精度。

1 CACC車輛的彎道行駛特性

研究CACC車輛的行駛狀態(tài)有利于車輛遇到突發(fā)狀況時(shí)起到預(yù)警作用。ZHU Meixin等[9]建立了前方碰撞預(yù)警系統(tǒng)，該系統(tǒng)通過(guò)車頭時(shí)距估計(jì)距離和速度的變化，以便駕駛員在行駛過(guò)程中遇到突發(fā)狀況時(shí)及時(shí)地做出反應(yīng)；Z. CHEN等[10]通過(guò)判定CACC車隊(duì)中前方車輛的3種情況，即基于位置的前方車輛、基于距離的前方車輛、基于位置和距離的前方車輛，提高了車輛的識(shí)別精度和距離；C. M. HUANG等[11]通過(guò)研究碰撞預(yù)警中車輛速度變化、曲線道路狀況、GPS(global positioning system，GPS)引起的位置中斷，提出了一種基于矢量的碰撞系統(tǒng)，試圖解決上述挑戰(zhàn)和實(shí)現(xiàn)有效預(yù)警；ZHANG Ruifeng等[12]提出了一種基于加速度的車輛碰撞預(yù)警算法，以提高碰撞檢測(cè)的準(zhǔn)確性。上述的研究均基于已有的數(shù)據(jù)或通過(guò)傳感器測(cè)得數(shù)據(jù)進(jìn)行驗(yàn)證分析，沒(méi)有對(duì)數(shù)據(jù)是否存在噪聲做詳細(xì)處理，具有理想化。

CACC車隊(duì)在行駛過(guò)程中通過(guò)雷達(dá)或傳感器實(shí)時(shí)獲取周圍環(huán)境(道路、建筑、行人、車輛等)和運(yùn)行控制中端等信息，決定自身的行駛路徑、加速度和速度等因素[13]。通過(guò)分析CACC車隊(duì)在彎道行駛的軌跡，以車輛間幾何關(guān)系為基礎(chǔ)分析車隊(duì)的彎道行駛狀態(tài)。

選取CACC車隊(duì)中某一車輛i為坐標(biāo)原點(diǎn)建立直角坐標(biāo)系，平行于車輛行駛方向?yàn)閤軸，如圖1。

圖1 CACC車輛在彎道行駛中坐標(biāo)系的建立Fig. 1 Establishment of coordinate system of CACC vehicles driving on curves

根據(jù)微積分原理，將車輛在行使過(guò)程中的軌跡分割成多個(gè)線段(例如0.01 s內(nèi)的行駛軌跡)，則這些線段可以近似看作直線。通過(guò)車輛安裝的雷達(dá)傳感器測(cè)量前車相對(duì)本車的方位角和距離，再通過(guò)三角函數(shù)得出車輛間的幾何關(guān)系，進(jìn)而預(yù)測(cè)前方車輛相對(duì)自車的坐標(biāo)位置：

(1)

式中：f(xi+1，yi+1)為目標(biāo)車輛相對(duì)自車的坐標(biāo)；αi為自車到目標(biāo)車輛坐標(biāo)系方位角；Li為車輛間直線距離。

根據(jù)遞歸思想，目標(biāo)車輛k相對(duì)自車k-1的坐標(biāo)為：

(2)

式中：f(xk,yk)為第k輛車相對(duì)第k-1 輛車的坐標(biāo)；αk為第k輛車相對(duì)第k-1 輛車的位置方位角；Lk為第k輛車與第k-1 輛車之間的直線距離。

做垂直于車輛速度行駛方向的垂線，兩車之間的垂線相交于O′點(diǎn)，對(duì)應(yīng)的夾角為θi(圖1)，對(duì)應(yīng)的半徑設(shè)為:

(3)

根據(jù)自車和目標(biāo)車輛的速度矢量關(guān)系，得到：

(4)

式中：θi=βi=φ+φ0。

根據(jù)三角函數(shù)關(guān)系可知：

(5)

根據(jù)三角函數(shù)關(guān)系，當(dāng)αi，φ，φ0，βi足夠小時(shí)，可以得到近似值：

(6)

(7)

|α0|=φ0

(8)

車輛在彎道行駛的幾何特性可得：

(9)

車輛之間通過(guò)θi進(jìn)行位置判斷，車輛行駛過(guò)程中t-1時(shí)刻橫向變化為：

(10)

(11)

xer,t=xk,t-xreal,k-1,t

(12)

式中：xreal,k-1,t為兩車之間的縱向真實(shí)距離；xev,k-1,t為兩車之間的期望距離；xk,t為第k輛車在t時(shí)刻相對(duì)于自車k-1的橫向坐標(biāo)；xk-1,t-1為第k-1輛車在t-1 時(shí)刻相對(duì)于自車k-2的橫向坐標(biāo)；vk-1,t-1為第k-1 輛車在t-1時(shí)刻的速度；T為t-1到t時(shí)刻的時(shí)間間隔；xer,t為兩車之間的間距誤差。

2 改進(jìn)卡爾曼濾波算法

通過(guò)分析車輛在彎道行駛的幾何關(guān)系，在原有卡爾曼濾波基礎(chǔ)上考慮了車輛速度方位角的變化，將車輛行駛過(guò)程中的變化與改進(jìn)卡爾曼濾波算法相結(jié)合，使改進(jìn)的卡爾曼濾波算法適用于彎道行駛的CACC車輛，進(jìn)而提高中低速環(huán)境下CACC車輛在彎道行駛的數(shù)據(jù)精度，保障車輛在彎道行駛的穩(wěn)定性和駕駛員舒適性。

2.1 數(shù)據(jù)處理方法概述

CACC車隊(duì)在行駛過(guò)程中通過(guò)傳感器獲取周圍車輛行駛信息并通過(guò)V2X技術(shù)將獲取的信息傳遞給本車，獲取的信息包括車輛間的距離和車輛行駛速度、加速度、車輛周圍行駛環(huán)境等特性。但車輛響應(yīng)、傳感器、信息傳遞延誤等因素使獲取的數(shù)據(jù)存在噪聲，為了提高CACC數(shù)據(jù)精度，需要對(duì)數(shù)據(jù)進(jìn)行濾波處理。

目前車輛狀態(tài)信息估計(jì)研究方法主要為卡爾曼濾波算法及其各種改進(jìn)算法。據(jù)調(diào)查發(fā)現(xiàn)[14-15]，卡爾曼濾波算法廣泛應(yīng)用于評(píng)估交通密度，使之獲得比原始數(shù)據(jù)更高的精度；同時(shí)在車輛跟蹤方面，卡爾曼濾波通過(guò)此刻獲取的數(shù)據(jù)對(duì)下一刻進(jìn)行預(yù)測(cè)和減少長(zhǎng)期跟蹤帶來(lái)的誤差問(wèn)題[16]?？柭鼮V波算法是一種最優(yōu)化的遞歸數(shù)字處理的算法，不僅是一般的濾波器，更像是一種觀測(cè)器，具有較高的精度。

2.2 基于速度方位角的改進(jìn)卡爾曼濾波算法

車輛在彎道行駛過(guò)程中速度方位角和傳感器獲取的車輛橫向坐標(biāo)為xk,t，根據(jù)式(10)和式(12)可得：

xk,t=xk-1,t-1+(1-cosθ)vk-1,t-1T

(13)

車輛在t-1時(shí)刻到t時(shí)刻的加速度為ak,t-1，則車輛在間隔T時(shí)刻的位置、速度為：

(14)

(15)

式中：加速度ak,t-1由自車加速度uk,t-1和隨機(jī)加速度wk,t-1兩部分組成；uk,t-1、wk,t-1分別符合標(biāo)準(zhǔn)正態(tài)分布，協(xié)方差矩陣分別為Q，R。

ak,t-1=uk,t-1+wk,t-1

(16)

式中：uk,t-1表示CACC車輛在t-1時(shí)刻自身產(chǎn)生的加速度，該值在T時(shí)刻內(nèi)可以看作是一個(gè)常量，在此過(guò)程中會(huì)產(chǎn)生過(guò)程誤差wk,t-1；wk,t-1為外力產(chǎn)生的干擾加速度。

CACC車隊(duì)行駛狀態(tài)如圖2。設(shè)采樣時(shí)刻k處車輛運(yùn)動(dòng)的狀態(tài)方程為：

(17)

圖2 CACC車隊(duì)行駛狀態(tài)Fig. 2 Driving status of CACC fleet

觀測(cè)方程：

(18)

車輛狀態(tài)空間模型為：

yt=Φxt-1+But-1+Γwt-1

(19)

xreal,t=xt-1+xt-1Tcosθ

(20)

故根據(jù)車輛速度方位角θ建立的改進(jìn)卡爾曼濾波模型得出：

1)先驗(yàn)狀態(tài)預(yù)測(cè)方程：

(21)

2)先驗(yàn)誤差協(xié)方差：

(22)

式中：

3)經(jīng)計(jì)算化簡(jiǎn)得卡爾曼濾波增益方程：

(23)

4)后驗(yàn)狀態(tài)更新方程：

(24)

5)更新誤差協(xié)方差方程：

(25)

3 實(shí)驗(yàn)驗(yàn)證

為提高CACC車輛在彎道的行駛安全性和舒適度，在保證數(shù)據(jù)可信的情況下，將獲取的數(shù)據(jù)經(jīng)過(guò)改進(jìn)卡爾曼濾波后模擬CACC車輛在彎道行駛的過(guò)程，進(jìn)而分析車輛間行駛數(shù)據(jù)經(jīng)過(guò)有無(wú)改進(jìn)卡爾曼濾波后對(duì)車輛行駛速度、加速度等狀態(tài)的影響。

3.1 數(shù)值仿真參數(shù)設(shè)置

V2X環(huán)境下CACC車輛在行駛過(guò)程中，通過(guò)改進(jìn)卡爾曼濾波算法處理CACC車輛自身傳感器獲取前方車輛的速度、加速度、車輛間的距離等數(shù)據(jù)。設(shè)計(jì)數(shù)值仿真實(shí)驗(yàn)時(shí)，考慮到CACC車輛在行駛過(guò)程中，車車之間信息傳遞存在通訊延遲，文獻(xiàn)[17]研究端到端通信延遲為20 ms，故此次仿真過(guò)程中將CACC車輛通訊延遲設(shè)置為20 ms。根據(jù)彎道限速和文獻(xiàn)[18]仿真工況，仿真實(shí)驗(yàn)場(chǎng)景設(shè)置為車輛在中低速情況下行駛，車輛在初始時(shí)刻設(shè)置40 km/h的行駛速度，以a=sin(0.3t)模擬車輛在彎道中不斷變化的加速度，m/s2，具體參數(shù)設(shè)置如表1。

表1 交通仿真中參數(shù)取值Table 1 Parameters value in traffic simulation

3.2 數(shù)值仿真結(jié)果分析

由于通過(guò)車載傳感器獲取的數(shù)據(jù)存在噪聲，所以在仿真過(guò)程中采用傳統(tǒng)卡爾曼濾波和筆者建立的改進(jìn)卡爾曼濾波算法處理的數(shù)據(jù)與真實(shí)值xreal進(jìn)行比較。主要分析CACC車輛在彎道行駛過(guò)程中有何變化，進(jìn)而判斷改進(jìn)卡爾曼濾波算法是否提高數(shù)據(jù)精度。為易于判斷車輛行駛過(guò)程中橫向位移的變化和改進(jìn)卡爾曼濾波處理數(shù)據(jù)噪聲是否有效，選取部分時(shí)間內(nèi)的車輛橫向位移變化進(jìn)行分析。

通過(guò)對(duì)比改進(jìn)前后卡爾曼濾波對(duì)車輛行駛橫向位移的影響，進(jìn)而判斷改進(jìn)后的卡爾曼濾波對(duì)車輛行駛是否有影響。從圖3可知，真實(shí)值、濾波前、濾波后相對(duì)平緩，沒(méi)有較大的變動(dòng)，但是濾波后的數(shù)值更接近真實(shí)值；而圖4中，濾波前的數(shù)值在真實(shí)值附近上下波動(dòng)，波動(dòng)范圍較大，濾波后的數(shù)值接近于真實(shí)值，具有較好的濾波效果。

圖3 卡爾曼濾波后車輛行駛橫向位移曲線變化Fig. 3 Variation of vehicle lateral displacement curve after Kalman filtering

圖4 改進(jìn)卡爾曼濾波后車輛行駛橫向位移變化曲線Fig. 4 Variation curve of vehicle lateral displacement after improved Kalman filtering

采用均方誤差(mean squared error，MSE)和均方根誤差(root mean squared error, RMSE)對(duì)數(shù)據(jù)經(jīng)過(guò)有無(wú)改進(jìn)卡爾曼濾波后的精度進(jìn)行評(píng)價(jià)。

(26)

(27)

通過(guò)式(26)、式(27)計(jì)算的數(shù)據(jù)結(jié)果如表2。

采用MSE和RMSE評(píng)價(jià)數(shù)據(jù)精度是否有提高，通過(guò)對(duì)比有無(wú)改進(jìn)卡爾曼濾波處理測(cè)量值，其相對(duì)應(yīng)的提高值如表2。由表2可知，改進(jìn)卡爾曼濾波算法使車輛行駛數(shù)據(jù)更加精確，更接近理想值。

表2 有無(wú)改進(jìn)卡爾曼濾波算法數(shù)值分析Table 2 Numerical analysis of improved Kalman filtering algorithm

將濾波后的數(shù)據(jù)帶入CACC車輛行駛軌跡預(yù)測(cè)算法中，通過(guò)對(duì)比CACC車輛的加速度、速度變化，分析有無(wú)改進(jìn)卡爾曼濾波對(duì)車輛行駛狀態(tài)的影響。圖5反映了車輛在行駛過(guò)程中不同濾波方法處理后的數(shù)據(jù)帶入CACC車輛中加速度的變化。

圖5 車輛加速度對(duì)比曲線Fig. 5 Comparison curve of vehicle acceleration

通過(guò)表1中設(shè)置的加速度參數(shù)可知，改進(jìn)后的卡爾曼濾波更符合正弦曲線的變化，而卡爾曼濾波后的加速度變化在某一時(shí)間點(diǎn)趨于穩(wěn)定，沒(méi)有產(chǎn)生較大的波動(dòng)，故對(duì)應(yīng)的速度為均加/減速行駛。通過(guò)對(duì)比分析車輛加速度數(shù)值變化可知，未經(jīng)改進(jìn)卡爾曼濾波算法處理數(shù)據(jù)帶入系統(tǒng)后其加速度波動(dòng)較大，經(jīng)過(guò)改進(jìn)卡爾曼濾波算法處理后的數(shù)據(jù)帶入系統(tǒng)后其加速度較穩(wěn)定，其車輛加速度主要在-2～2 m/s2內(nèi)變化，保證了駕駛員和乘客的舒適性。

圖6為車輛速度對(duì)比曲線。

圖6 車輛速度對(duì)比曲線Fig. 6 Comparison curve of vehicle speed

根據(jù)微積分定理，加速度是速度對(duì)時(shí)間的一階導(dǎo)數(shù)，所以圖6和圖5的變化趨勢(shì)一致，即速度變化曲線(圖6)與加速度變化曲線(圖5)相對(duì)應(yīng)。在 40 s 后兩種速度大小差距越來(lái)越大，兩種方法處理的數(shù)據(jù)帶入CACC系統(tǒng)中的速度變化如此大，其原因如下：首先，圖5中經(jīng)卡爾曼濾波后的加速度幾乎為定值變化，而經(jīng)改進(jìn)卡爾曼濾波后的加速度幾乎在區(qū)間內(nèi)進(jìn)行波動(dòng)且變化規(guī)律符合設(shè)定的加速度變化曲線；其次，車輛在彎道行駛過(guò)程中速度方位角在不斷變化，間接證明筆者建立的基于速度方位角的改進(jìn)卡爾曼濾波算法對(duì)彎道行駛的車輛有效；最后，由圖6可知，車輛行駛速度通過(guò)卡爾曼濾波處理后，其波動(dòng)范圍較大且車輛速度在不斷增加，究其原因可能車輛響應(yīng)存在噪音，產(chǎn)生誤差，車車之間的通訊延遲、道路崎嶇變化也是極有可能造成較大范圍的波動(dòng)。改進(jìn)后的卡爾曼濾波速度變化較平穩(wěn)，同加速度變化曲線較一致，具有規(guī)律性和較好的穩(wěn)定性、舒適性。

經(jīng)過(guò)仿真實(shí)驗(yàn)分析，改進(jìn)卡爾曼濾波算法使車輛行駛具有較好的穩(wěn)定性，提高駕乘人員的舒適性。改進(jìn)卡爾曼濾波算法處理后的數(shù)據(jù)其位移更接近真實(shí)值，使數(shù)據(jù)更加準(zhǔn)確；其加速度和速度浮動(dòng)范圍相比改進(jìn)前卡爾曼濾波算法較小，從而說(shuō)明了改進(jìn)卡爾曼濾波后數(shù)據(jù)應(yīng)用到車輛行駛過(guò)程中在同等時(shí)間內(nèi)速度變化波動(dòng)較小，保證了車輛在彎道行駛的平穩(wěn)性，更有利于駕駛?cè)撕统丝统俗孢m性。

4 結(jié) 語(yǔ)

通過(guò)研究CACC車輛在彎道行駛過(guò)程中傳感器獲取車輛信息存在噪聲對(duì)車輛行駛產(chǎn)生誤差的問(wèn)題，利用改進(jìn)卡爾曼濾波算法對(duì)傳感器獲取的數(shù)據(jù)進(jìn)行處理，并使用CarSim和Simulink對(duì)該算法進(jìn)行仿真，在仿真過(guò)程中將有無(wú)改進(jìn)卡爾曼濾波算法對(duì)車輛行駛狀態(tài)的影響進(jìn)行對(duì)比。仿真結(jié)果表明，經(jīng)改進(jìn)卡爾曼濾波處理后的數(shù)據(jù)具有更高的精度和駕乘人員舒適性，更接近于車輛在彎道行駛的真實(shí)情況。研究結(jié)果為車輛在彎道行駛過(guò)程中提供一種數(shù)據(jù)處理方法和參考依據(jù)。

接下來(lái)研究如何將該算法應(yīng)用于實(shí)車和分析該數(shù)據(jù)在實(shí)車驗(yàn)證過(guò)程中出現(xiàn)的情況，同時(shí)研究CACC車隊(duì)在彎道行駛過(guò)程中如何保持巡航等情況和基于CACC數(shù)據(jù)精度的車輛模型預(yù)測(cè)控制軌跡在不同工況下的行駛狀態(tài)。