基于車路協(xié)同的車輛狀態(tài)估計(jì)方法*

謝伯元，王建強(qiáng)，秦曉輝，李克強(qiáng)

(1. 清華大學(xué)汽車工程系，北京 100084； 2. 總裝汽車試驗(yàn)場，南京 210028)

前言

車輛狀態(tài)信息是行車危險(xiǎn)辨識和車輛安全控制算法的基礎(chǔ)，是主動橫擺力矩控制、電子穩(wěn)定程序、換道輔助等車輛安全控制技術(shù)開發(fā)的關(guān)鍵。輪速、車速、加速度、橫擺角速度、轉(zhuǎn)向盤轉(zhuǎn)角等參數(shù)可以通過傳感器直接測量獲取，然而質(zhì)心側(cè)偏角、輪胎/地面力、路面附著系數(shù)等關(guān)系到車輛動力學(xué)建模與反饋控制的重要參數(shù)則無法直接測量，只能通過參數(shù)估計(jì)的方法獲得。

車輛狀態(tài)參數(shù)估計(jì)方法主要有兩種：一是動力學(xué)方法，聯(lián)合慣性傳感器與車輛動力學(xué)模型估計(jì)器進(jìn)行狀態(tài)估計(jì)，通過建立車輛動力學(xué)模型，用線性觀測器[1]、非線性Luenberger觀測器[2]、滑模觀測器(sliding model observer，SMO)[3]、擴(kuò)展卡爾曼濾波器(extended kalman filter，EKF)[4]、無味卡爾曼濾波器(unscented kalman filter，UKF)[5]等方法對車輛的質(zhì)心側(cè)偏角、輪胎/地面力和摩擦因數(shù)進(jìn)行估計(jì)，車輛模型、輪胎動力學(xué)模型[4,6]以及慣性傳感器的數(shù)量[7]對估計(jì)的精度和魯棒性有重要的影響；二是運(yùn)動學(xué)方法，聯(lián)合慣性傳感器與車輛運(yùn)動學(xué)模型估計(jì)器進(jìn)行狀態(tài)估計(jì)，通過建立質(zhì)心側(cè)偏角、縱向、橫向車速與加速度、橫擺角速度、轉(zhuǎn)向盤轉(zhuǎn)角之間的車輛運(yùn)動學(xué)模型，用閉環(huán)非線性觀測器(closed loop nonlinear observer)[8]、模糊邏輯方法[9]、神經(jīng)網(wǎng)絡(luò)方法[10]來估計(jì)車輛縱向速度、橫向速度和質(zhì)心側(cè)偏角，該方法不受車輛動力學(xué)狀態(tài)變化的影響，魯棒性較好，但是嚴(yán)重依賴傳感器的測量精度，要及時(shí)消除積分累積誤差的影響。近年來，隨著高精度GPS的應(yīng)用，信息融合的方法得到廣泛應(yīng)用[11-13]，目前的信息融合方法主要以多天線GPS測量(2天線或3天線)為主，通過載體姿態(tài)測量的方法直接獲得車輛橫擺角[12]或者質(zhì)心側(cè)偏角[11]，多天線的布局方案，對GPS精度和更新頻率的要求較高。

隨著智能交通系統(tǒng)(intelligent transport systems，ITS)的發(fā)展， GPS和無線通信技術(shù)開始廣泛應(yīng)用于交通管理與車輛控制領(lǐng)域。近年來，國際上對車路協(xié)同(V2I)技術(shù)進(jìn)行了廣泛的研究[14-19]。本文中以車路協(xié)同技術(shù)為基礎(chǔ)，車輛通過信息交互獲取路側(cè)的GPS基站位置信息和差分改正信息，結(jié)合車載慣性傳感器和車載GPS測量數(shù)據(jù)，在車輛運(yùn)動學(xué)模型的基礎(chǔ)上，通過設(shè)計(jì)二階離散卡爾曼濾波器估計(jì)車輛的橫擺角和質(zhì)心側(cè)偏角。

1 車路協(xié)同系統(tǒng)框架

車路協(xié)同系統(tǒng)一般分為三層：一是信息感知層，通過各種傳感器采集交通渠化信息、動態(tài)交通信息和車輛運(yùn)動信息等；二是信息交互層，通過專用短程通信設(shè)備(dedicated short range communication, DSRC)或者WiFi等無線通信設(shè)備進(jìn)行車-路之間的信息雙向?qū)崟r(shí)交互；三是應(yīng)用層，車輛可根據(jù)獲取的各種信息進(jìn)行動態(tài)信息服務(wù)和主動安全控制，交通管理部門可根據(jù)獲取的信息對各種交通要素進(jìn)行實(shí)時(shí)管理，引導(dǎo)交通，從而提高整個(gè)交通系統(tǒng)的安全性、運(yùn)行效率和節(jié)能環(huán)保。

在車路協(xié)同系統(tǒng)框架下，通過路側(cè)DSRC設(shè)備發(fā)送GPS基站偽距差分改正信息，車載GPS獲得較高精度的車輛動態(tài)位置、車速和航向角信息；通過車載橫擺角速度、縱向加速度和橫向加速度傳感器采集自車行駛狀態(tài)信息；在此基礎(chǔ)上，通過信息融合的方法，估計(jì)車輛橫擺角和質(zhì)心側(cè)偏角。系統(tǒng)框架如圖1所示。

2 車輛狀態(tài)參數(shù)估計(jì)方法

2.1 車輛運(yùn)動學(xué)模型

以路側(cè)基站為坐標(biāo)原點(diǎn)，建立大地坐標(biāo)系，以車輛質(zhì)心為原點(diǎn)，建立車輛運(yùn)動坐標(biāo)系，根據(jù)2自由度車輛運(yùn)動微分方程建立兩個(gè)坐標(biāo)系間的運(yùn)動學(xué)關(guān)系，如圖2所示。建立車輛的運(yùn)動學(xué)方程為

(1)

式中：vx為車輛縱向車速；vy為車輛橫向速度；ωt為橫擺角速度；axt為縱向加速度；ayt為橫向加速度，可通過車載的慣性傳感器實(shí)時(shí)采集。

以GPS基站位置為原點(diǎn)，通過DSRC無線通信設(shè)備發(fā)送GPS基站的偽距差分改正信息，在移動站上進(jìn)行差分，獲取車輛對基站的相對位置(Xp,Yp)，建立車輛在大地坐標(biāo)系下的運(yùn)動軌跡方程為

(2)

以式(1)為狀態(tài)方程，以式(2)為輸出方程，聯(lián)立式(1)和式(2)建立線性時(shí)變系統(tǒng)的狀態(tài)空間方程為

(3)

對于線性時(shí)變系統(tǒng)(式(3))，由于慣性傳感器的采樣周期很短，可對其進(jìn)行近似離散化處理。

(4)

式中：X(k+1)為狀態(tài)向量，是通過慣性傳感器采集到的縱向、橫向加速度和橫擺角速度信號；Y(k)為觀測量，是車輛在大地坐標(biāo)系X和Y方向的速度分量，通過DGPS系統(tǒng)獲取。

(5)

式中：Ts為傳感器采樣周期。狀態(tài)向量和觀測向量的數(shù)據(jù)來源于不同的信息采集系統(tǒng)。

2.2 車輛狀態(tài)信息融合算法

基于運(yùn)動學(xué)的參數(shù)估計(jì)方法嚴(yán)重依賴于傳感器數(shù)據(jù)的精度，本文中采用卡爾曼濾波器分別對數(shù)據(jù)進(jìn)行濾波和估計(jì)，其算法結(jié)構(gòu)框圖如圖3所示。

建立系統(tǒng)的二次卡爾曼濾波模型。通過一次離散卡爾曼濾波器對汽車橫擺角速度、縱向加速度和橫向加速度信號進(jìn)行實(shí)時(shí)濾波，降低信號噪聲，消除信號漂移，提高測量精度。

在此基礎(chǔ)上，通過分析橫擺角速度和橫向加速度測量值，判斷車輛的直線行駛狀態(tài)，并以此為系統(tǒng)的初始狀態(tài)，利用二次離散卡爾曼濾波器對式(4)的縱向車速和橫向車速估計(jì)值進(jìn)行時(shí)間更新；以橫擺角和DGPS的車速、航向角作為系統(tǒng)輸入變量，對縱向車速和橫向車速進(jìn)行測量更新；通過信息融合的方法估計(jì)車輛質(zhì)心側(cè)偏角。

2.3 基于信息融合的車輛橫擺角估計(jì)

要對式(4)進(jìn)行觀測，須確定車輛的橫擺角φt，無法通過單個(gè)車載GPS對橫擺角進(jìn)行直接測量，在車路協(xié)同系統(tǒng)下，可采用信息融合的方法計(jì)算φt。

當(dāng)車輛直線行駛時(shí)，車輛的航向角等于橫擺角。不考慮大滑移的極端條件，設(shè)定車輛直線行駛的橫向加速度和橫擺角速度傳感器測量閾值為[aylineωline]。當(dāng) [ayω]<[aylineωline]時(shí)，可認(rèn)為車輛直線行駛。此時(shí)，φinitial=γline，γline是車輛直線行駛時(shí)的航向角，GPS航向角的精度與車輛的定位精度密切相關(guān)，本文中通過車路協(xié)同系統(tǒng)獲得較高精度的車輛航向角。

當(dāng)檢測到車輛以直線行駛時(shí)，將其作為系統(tǒng)初始狀態(tài)，通過式(6)融合慣性傳感器測量的橫擺角速度計(jì)算其橫擺角。同時(shí)，通過初始狀態(tài)的更替，消除系統(tǒng)的積分累積誤差。

(6)

車輛直線行駛時(shí)，側(cè)偏角βinitial=0。通過判斷車輛直線行駛狀態(tài)，將其作為狀態(tài)估計(jì)的初始狀態(tài)，以減少觀測中存在的積分誤差。

2.4 二次卡爾曼濾波器的設(shè)計(jì)

在式(4)中，角速度、加速度等慣性傳感器信息是系統(tǒng)狀態(tài)時(shí)間更新的狀態(tài)矩陣和輸入向量；DGPS信息是系統(tǒng)狀態(tài)測量更新的輸出向量。由于兩者的信息來源不同，采樣周期也不一樣，通過設(shè)計(jì)離散卡爾曼濾波器，實(shí)現(xiàn)兩種異構(gòu)信息的融合，對車輛縱向、橫向加速度和質(zhì)心側(cè)偏角進(jìn)行實(shí)時(shí)估計(jì)。其中，第一階卡爾曼濾波器用于處理慣性傳感器的采樣數(shù)據(jù)；第二階卡爾曼濾波器用于融合DGPS和慣性傳感器數(shù)據(jù)，估計(jì)車輛的狀態(tài)參數(shù)。

(7)

一次離散卡爾曼濾波器主要處理輸入信號，Xk=[axkaykωk]T，G=I，C=I。

二次離散卡爾曼濾波器以式(4)為處理對象，其中：狀態(tài)轉(zhuǎn)移矩陣為Gk，輸入矩陣為Hk，當(dāng)車輛做回轉(zhuǎn)運(yùn)動時(shí)，輸出矩陣為Ck，其計(jì)算參見式(5)。

通過該方法計(jì)算出(vxk,vyk)，再根據(jù)下式計(jì)算質(zhì)心側(cè)偏角：

(8)

綜上所述，通過二階卡爾曼濾波器能夠得到車輛在k時(shí)刻的運(yùn)動學(xué)狀態(tài)參數(shù)向量vStatus(k)=[axkaykωkφkvxkvykβk]。其中，一次離散卡爾曼濾波器能夠得到[axkaykωk]的濾波值；二次離散卡爾曼濾波器能夠得到[φkvxkvykβk]的估計(jì)值。

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

采用圖1介紹的車路協(xié)同系統(tǒng)進(jìn)行穩(wěn)態(tài)回轉(zhuǎn)實(shí)驗(yàn)，由DSRC向車輛傳輸路側(cè)基站的偽距差分改正信息，獲取車輛的動態(tài)差分GPS信息，通過CAN總線采集車載慣性傳感器信息，信息基本情況見表1。

利用高斯-克呂格公式將WGS-84經(jīng)緯度坐標(biāo)轉(zhuǎn)換為高斯平面坐標(biāo)[20]，在基站坐標(biāo)系下描述車輛的行駛軌跡，如圖4所示。

利用一次離散卡爾曼濾波器處理采集到的車載傳感器原始數(shù)據(jù)，處理結(jié)果如圖5～圖7所示。

車輛航向角和橫擺角都是車輛行駛方向與正北方向的夾角，以順時(shí)針方向旋轉(zhuǎn)為正。因此，車輛直線行駛時(shí)，兩者有以下關(guān)系：φinitial=γline。 當(dāng)系統(tǒng)檢測到車輛直線行駛時(shí)，可以確定車輛的橫擺角初始值，通過式(6)估計(jì)車輛的橫擺角φes，計(jì)算結(jié)果如圖8所示。

表1 信息基本屬性表

在式(3)中，得到橫擺角估計(jì)值后，利用式(9)，求得在GPS測量點(diǎn)的側(cè)偏角βGPS。

(9)

然后，利用二次離散卡爾曼濾波器估計(jì)車輛的縱向車速和橫向車速，求出車輛轉(zhuǎn)彎時(shí)的橫向速度和縱向速度，通過式(9)求出車輛質(zhì)心側(cè)偏角估計(jì)值βes，將其與βGPS進(jìn)行對比，結(jié)果如圖9所示。

由圖可見，在最大橫向加速度達(dá)到0.35g的情況下，側(cè)偏角估計(jì)值與GPS測量點(diǎn)側(cè)偏角的平均偏差為0.569°，標(biāo)準(zhǔn)差為2.51°。由于DGPS測量頻率較低，差分精度為0.45m，航向角測量值存在一定程度的跳變，側(cè)偏角估計(jì)值比GPS測量點(diǎn)側(cè)偏角具有更高的可信度。

通過估計(jì)得到的橫擺角與側(cè)偏角，利用式(10)計(jì)算得到車輛航向角估計(jì)值，再與GPS測量點(diǎn)的航向角測量值進(jìn)行比較，驗(yàn)證算法的準(zhǔn)確性，結(jié)果如圖10所示。

(10)

從圖中可以看出，在GPS測量點(diǎn)，航向角估計(jì)值與測量值非常接近，平均偏差為0.569°，標(biāo)準(zhǔn)差為2.52°，可以看出，其偏差基本由GPS測量點(diǎn)的跳變引起，二次離散卡爾曼濾波器在一定程度上抑制了這一現(xiàn)象，使側(cè)偏角估計(jì)值更加有效、準(zhǔn)確。

4 結(jié)論

在車路協(xié)同系統(tǒng)中，利用DSRC發(fā)送路側(cè)GPS基準(zhǔn)站的差分信息，基于車輛運(yùn)動學(xué)模型，通過建立二次離散卡爾曼濾波器，融合DGPS的航向角、車速、位置信息與車載傳感器的縱向和橫向加速度、橫擺角速度信息，估計(jì)車輛橫擺角和質(zhì)心側(cè)偏角，通過比較GPS測量點(diǎn)的航向角估計(jì)值與測量值，驗(yàn)證了算法的準(zhǔn)確性，穩(wěn)態(tài)回轉(zhuǎn)實(shí)驗(yàn)的結(jié)果表明，即使在橫向加速度較大的情況下，該方法仍具有較好的估計(jì)精度。

[1] Kiencke U, Dail A. Observation of Lateral Vehicle Dynamics[J]. Control Engineering Practice, 1997, 5(8):1145-1150.

[2] Kiencke U, Nielsen L. Automotive Control System [M]. Berlin: Springer-Verlag, 2000.

[3] Stephant J, Charara A, Meizel D. Experimental Validation of Vehicle Sideslip Angle Observers[C]. Intelligent Vehicles Symposium, 2004.

[4] Daknlallah J, Glaser S, Mammar S, et al. Tire-road Force Estimation Using Extended Kalman Filter and Sideslip Angle Evaluation[C]. American Control Conference, 2008.

[5] Gerard M. Tire-road Friction Estimation Using Slip-based Observers [D]. Sweden: Lund University, 2006.

[6] Baffet G, Charara A, Stephant J. Side Slip Angle, Lateral Tire Force and Road Friction Estimation in Simulation and Experiments[C]. IEEE International Conference on Control Applications, 2006.

[7] Stephant J, Charara A, Meizel D. Linear Observers for Vehicle Sideslip Angle: Experimental Validation[C]. Industrial Electronics, 2004.

[8] Hac A, Simpson M, Estimation of Vehicle Side Slip Angle and Yaw Rate[C]. SAE Paper 2000-01-0696.

[9] 施樹明,Henk Lupker, Paul Bremmer, 等. 基于模糊邏輯的車輛側(cè)偏角估計(jì)方法[J]. 汽車工程,2005, 27 (4):426-430.

[10] Du Xiaoping,Sun Huamei, Qian Kun,et al. A Prediction Model for Vehicle Sideslip Angle Based on Neural Network[C]. ICIFE, 2010.

[11] Zhang Jinzhu, Zhang Hongtian. Vehicle Stability Control System Based on Direct Measurement of Body Sideslip Angle[C]. Power Electronics and Intelligent Transportation System, 2009.

[12] Daily R,Bevly D M. The Use of GPS for Vehicle Stability Control System [J]. Industrial Electronics, 2004, 51( 2) : 270-277.

[13] Tu Lai, Huang Chungming . Forwards: A Map-Free Intersection Collision-Warning System for All Road Patterns[C]. IEEE Transactions on Vehicular Technology, 2010.

[14] Ashwin A. Overview of IntelliDrive / Vehicle Infrastructure Integration (VII) [R]. Virginia tech: Transportation Institute, 2009.

[15] Row S,Schagrin M, Briggs V. The Future of VII [R]. US Department of Transportation, 2008.

[16] ITS Joint Program Office. Achieving the Vision: From VII to IntelliDrive Policy White Paper[R]. Washington, D.C.: Department of Transportation,2010.

[17] ITS Joint Program Office. IntelliDriveSM Governance Needs Summary a Summarization of Research from 2004-2009[R].Washington, D.C.: Department of Transportation,2009.

[18] Hiroshi Makino. Smartway Project Cooperative Vehicle Highway Systems[C]. TRB Annual Meeting, 2006.

[19] Mitsuo Arino. ITS Policy in Japan and Smartway[R]. Tokyo: ITS Policy and Program Office, 2007.

[20] 孔祥元, 郭際明, 劉宗泉.大地測量學(xué)基礎(chǔ)[M]. 湖北：武漢大學(xué)出版社, 2005.