基于车路协同的车辆状态估计方法*

谢伯元，王建强，秦晓辉，李克强

(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.