基于車路協同的車輛狀態估計方法*

謝伯元，王建強，秦曉輝，李克強

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

前言

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

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

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

1 車路協同系統框架

車路協同系統一般分為三層：一是信息感知層，通過各種傳感器采集交通渠化信息、動態交通信息和車輛運動信息等；二是信息交互層，通過專用短程通信設備(dedicated short range communication, DSRC)或者WiFi等無線通信設備進行車-路之間的信息雙向實時交互；三是應用層，車輛可根據獲取的各種信息進行動態信息服務和主動安全控制，交通管理部門可根據獲取的信息對各種交通要素進行實時管理，引導交通，從而提高整個交通系統的安全性、運行效率和節能環保。

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

2 車輛狀態參數估計方法

2.1 車輛運動學模型

以路側基站為坐標原點，建立大地坐標系，以車輛質心為原點，建立車輛運動坐標系，根據2自由度車輛運動微分方程建立兩個坐標系間的運動學關系，如圖2所示。建立車輛的運動學方程為

(1)

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

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

(2)

以式(1)為狀態方程，以式(2)為輸出方程，聯立式(1)和式(2)建立線性時變系統的狀態空間方程為

(3)

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

(4)

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

(5)

式中：Ts為傳感器采樣周期。狀態向量和觀測向量的數據來源于不同的信息采集系統。

2.2 車輛狀態信息融合算法

基于運動學的參數估計方法嚴重依賴于傳感器數據的精度，本文中采用卡爾曼濾波器分別對數據進行濾波和估計，其算法結構框圖如圖3所示。

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

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

2.3 基于信息融合的車輛橫擺角估計

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

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

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

(6)

車輛直線行駛時，側偏角βinitial=0。通過判斷車輛直線行駛狀態，將其作為狀態估計的初始狀態，以減少觀測中存在的積分誤差。

2.4 二次卡爾曼濾波器的設計

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

(7)

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

二次離散卡爾曼濾波器以式(4)為處理對象，其中：狀態轉移矩陣為Gk，輸入矩陣為Hk，當車輛做回轉運動時，輸出矩陣為Ck，其計算參見式(5)。

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

(8)

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

3 實驗驗證

采用圖1介紹的車路協同系統進行穩態回轉實驗，由DSRC向車輛傳輸路側基站的偽距差分改正信息，獲取車輛的動態差分GPS信息，通過CAN總線采集車載慣性傳感器信息，信息基本情況見表1。

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

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

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

表1 信息基本屬性表

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

(9)

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

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

通過估計得到的橫擺角與側偏角，利用式(10)計算得到車輛航向角估計值，再與GPS測量點的航向角測量值進行比較，驗證算法的準確性，結果如圖10所示。

(10)

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

4 結論

在車路協同系統中，利用DSRC發送路側GPS基準站的差分信息，基于車輛運動學模型，通過建立二次離散卡爾曼濾波器，融合DGPS的航向角、車速、位置信息與車載傳感器的縱向和橫向加速度、橫擺角速度信息，估計車輛橫擺角和質心側偏角，通過比較GPS測量點的航向角估計值與測量值，驗證了算法的準確性，穩態回轉實驗的結果表明，即使在橫向加速度較大的情況下，該方法仍具有較好的估計精度。

[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, 等. 基于模糊邏輯的車輛側偏角估計方法[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] 孔祥元, 郭際明, 劉宗泉.大地測量學基礎[M]. 湖北：武漢大學出版社, 2005.