基于RISF融合的车辆横摆角速度估计*

廖尉华，何智成，蒋祖坚，余天龙，何逸波

（1.上汽通用五菱汽车股份有限公司，柳州 545007；2.湖南大学，汽车车身先进设计制造国家重点实验室，长沙 410082）

前言

汽车高级驾驶辅助系统，需要获取准确的车辆横摆角速度，用于车辆行驶安全状态评估、路径预测和速度控制等，以实现车辆自动驾驶过程的安全性要求和舒适性要求。

使用传感器测量获得横摆角速度是最常见的方法，但作为大规模量产的车载低成本零件，其测量信号受其性能和环境影响（如噪声、零漂等），需进行处理后才能使用，如卡尔曼滤波器KF，在处理非线性系统问题时其鲁棒性较差。再者，在滤波过程中，缺少参数的自适应环节，滤波的精度不会太高。另外，传感器作为微机电系统，其量测结果相比于实际值具有一定的滞后性。除了传感器方法外，另一类方法是通过建立车辆物理模型进行估计。文献［7］和文献［8］中建立了线性2 自由度车辆模型，使用横向加速度作为输入对横摆角速度进行估计，但这种线性模型不能反映实际车辆的非线性运动特征，估计值必然会有误差。文献［9］中建立了3 自由度非线性汽车动力学模型，仿真测试取得较好效果，但未考虑实际道路影响。

当然，横摆角速度等车身姿态估计亦有采用多源传感信息融合的估计方法。文献［5］和文献［10］均使用了线性融合方法。文献［9］中使用了自适应无迹卡尔曼滤波AUKF（adaptive unscented Kalman filter）的融合算法，但因为UKF算法存在状态不稳定和发散的潜在风险，产业界一般较少使用。上述这些融合方法，虽然在仿真测试和标准路面测试中取得较好的效果，但较少考虑实际道路状况的影响，在实际道路上的效果有待验证。

当前，ESP 控制器已成为乘用车标配，该系统配有车身运动感知传感器，可通过整车信号网络发送给驾驶辅助控制器，例如轮速传感器、前轮转角传感器、横向加速度传感器以及横摆角速度传感器等。针对上述车辆横摆角速度估计存在的问题，本文中综合传感滤波和模型估计两类方法，充分利用车载运动感知传感器，首先使用估计效果更优的ACKF算法对传感器进行滤波。ACKF 作为贝叶斯滤波框架下的次优估计算法，理论上比同框架下的KF、EKF 等具有更优的估计效果。然后通过建立车辆3自由度动力学模型，并考虑实际车辆行驶道路的倾斜角情况，使用车速、前轮转角和横向加速度信息，估计出横摆角速度。最后基于RISF 多源传感信息融合方法融合上述滤波值和模型估计值。

1 基于ACKF的传感器量测值滤波

1.1 3阶球面径向容积准则

贝叶斯滤波从理论上给出了非线性滤波的最优估计。在高斯分布假设下，贝叶斯滤波的关键问题是计算形如“非线性函数×高斯概率密度函数”的高斯加权积分，该积分可转化成如式（1）所示的积分式。其中：∈R是系统状态向量；()是任意非线性函数。

文献［13］中把式（1）转换成球径积分，即令=，=1，∈[0，∞)。为半径，为方向向量。得

式中：U={∈R|=1}为球体表面；(·)为积分域U的微元；()由单位加权函数()=1 的球面积分定义。

径向积分由m点高斯积分准则计算，球面积分由m点球面准则计算，则可得到(m× m)点的球面-径向容积准则积分式：

本文使用3 阶球面径向容积准则求解式（4）。在式（4）中，m=1，m=2，总共包含2容积点，是R的系统状态维数。

这里，若exp(-)为标准正态分布，上述高斯加权积分为

1.2 自适应容积卡尔曼滤波ACKF

算法输入为时刻的量测值z和(-1)时刻的估计结果x、P，输出为x、P，则ACKF的步骤如下。

（1）参数初始化

（2）时间更新

首先对P进行Cholesky分解：

根据分解结果可计算容积点，得

计算传播容积点，通过状态转移函数，转换后的容积点计算为

式中：=1，2，…=2。

计算时间更新步的先验估计和误差协方差：

（3）量测更新

计算容积点及传播容积点：

计算量测更新步的先验估计、新息协方差和互协方差：

（4）计算卡尔曼增益、时刻状态估计和估计方差

（5）噪声自适应

若过程噪声和量测噪声能够精确定义，卡尔曼滤波可获得最优估计。由式（10）、式（15）、式（17）和式（18）可知，和影响了卡尔曼滤波增益的大小，决定了估计值在过程和量测信息间的权重。文献［4］研究了4 种自适应卡尔曼滤波方法，其中的协方差匹配法应用最广，且效果较好。本文中参照文献［4］，采用移动窗口内的新息序列对和矩阵进行自适应更新。

更新过程噪声Q和测量噪声R：

2 基于考虑路倾角自行车模型的车辆横摆角速度估计

车辆在水平路面行驶时，可通过建立自行（两轮）车模型得到车辆横向加速度与车辆横摆角速度之间的关系。但当车辆在侧倾路面行驶时，横向加速度传感器的量测值包含了车辆横向运动信息和重力沿侧倾路面的分力信息，因此不能直接用于横摆角速度的估计。虽然车辆转弯时，由于向心力导致车身横摆角速度和底盘悬架横摆角速度不等，但当车辆处于侧倾路面弯道时，重力沿侧倾路面的分力提供了部分向心加速度，车身相对底盘悬架的侧摆程度降低。另外，本文所述方法主要面向智能辅助驾驶功能的舒适性问题，例如高速领航功能和车道保持功能等。当这些功能开启时，车辆处于自动驾驶模式，系统会控制车辆按照设定速度沿着所在车道的中心线行驶。因此，本文中不考虑车辆在侧倾路面弯道上变道的情况，驾驶员主动变道所产生的车身严重侧摆不在本文考虑范围内。综上所述，本文中不考虑车身侧摆影响，认为车身横摆角速度等于底盘悬架系统的横摆角速度。

尽管自行车模型是简化的车辆模型，但模型中的参数，满足考虑侧倾路面问题的研究。本文中建立的模型如图1 和图2 所示。是车辆相对于大地坐标系的航向角，是汽车质量，是车辆质心，是车辆质心速度，是车辆质心滑移角，是重力加速度，是前轮转向角，是车辆质心到前轮中心距离，是车辆质心到后轮中心距离，是侧倾路面角。

图1 自行车模型

图2 车辆在侧倾路面的重力分解

车辆在侧倾路面弯道上行驶时，前轮转向角较小，满足车辆模型的小角度假设。可确定车辆质心横向受力平衡、质心处滑移角速度和轴力矩平衡的微分方程，分别为

式中：C为前轮侧偏刚度；C为后轮侧偏刚度；I为车辆绕车辆质心坐标系轴转动惯量。

考虑到路面侧倾角不会剧烈突变，且系统采样工作周期很短，可认为sin φ≅sin φ。对式（22）和式（26）进行离散化处理，得离散系统状态方程：

另外，根据阿克曼转向几何原理，得如下方程：

上述式（27）和式（28）可作为卡尔曼滤波的时间更新步，式（30）和式（31）作为量测更新步。递推过程与第2节相同。

值得一提的是，模型中的车速是根据轮速信号获得，由于车速估计不是本文重点，所以本文中使用最简单的轮速平均法估计，当有更好的车速估计方法时，亦能得到更好的横摆角速度估计。

3 基于RISF的横摆角速度融合估计

可靠指标传感器融合（reliability indexed sensor fusion，RISF）是一种卡尔曼滤波框架下的多源传感信息融合方法。该方法认为，可把卡尔曼滤波的量测噪声矩阵和过程噪声矩阵视为各传感器的可靠指标RI（reliability index），根据不同的场景条件依靠工程经验调试的方法确定两噪声矩阵的值。

式中是量测空间到观测空间的变换矩阵，=。

根据工程经验，当车辆横摆角速度传感器值较小时，由于低信噪比和零漂因素的存在，传感器量测值的精确度较差。在这种情况下，卡尔曼滤波更新步结果的置信度应降低，即传感器噪声矩阵的值取大些；当车辆前轮转向角较大时，由于轮胎的侧滑、变形等因素，导致小角度假设不成立，因此卡尔曼滤波预测步结果的置信度应降低，即系统过程噪声的值取大些；当车辆横向加速度传感器测量值较小时，对应的过程噪声的值取大些。

综上，本文设计如下噪声协方差更新方程。其中，c、c、c、c、c、c、c和c通过实车调试的方法确定。

4 实车验证

本文中基于某SUV 车型进行验证（见图3），在测试车上安装中汽院智能网联的i-TESTER AVE 2100 测试系统，用于获取车辆运动姿态真值，如图4所示。该系统配备有i-TESTER主机、组合定位模块等，包括GPS-RTK 和IMU 等传感器，可精确测量车身姿态参数。该系统可把测量数据通过CAN 方式发出。另外使用Speedgoat 公司的Mobile 型快速控制原型机RCP（见图5）实时运行Matlab/Simulink 算法模型，该原型机可通过网线与上位机PC 连接通信。

图3 某SUV车型测试车

图4 i-TESTER AVE 2000 车身姿态真值采集系统

测试车设备连接原理如图5 所示，智能驾驶控制器与ESP 控制器通过车载网关交互信号，使用CAN 通信方式。在智能辅助驾驶控制器和网关之间，制作三通线，连接Speedgoat 实时机。真值测试系统的组合定位传感器安装在车身上，系统主机与上位机PC 通过CANoe 连接。最后Speedgoat 实时机的运行结果和i-TESTER 真值系统测量结果汇总在上位机PC上显示。

图5 试验车设备连接原理

该车车身参数和轮胎参数如表1和表2所示。

表1 车身参数表

表2 轮胎参数表

根据JTG B01—2014《公路工程技术标准》，高等级弯道横坡坡度设计推荐为2%～3%。本文中选取柳州市北进路自南向北一段较长弯路作为测试道路，该弯道的设计侧倾坡度为3%，如图6 所示。测试车辆以（65±2）km/h的车速行驶，对所设计算法进行验证。

图6 测试道路

4.1 横摆角速度传感器滤波

测试弯道从入弯到出弯长约550 m，整个过程约30 s。根据采集的数据结果，如图7（a）所示，车载传感器量测值与i-TESTER 测试系统输出真值曲线的走势基本一致。同时使用传统KF 算法和本文设计的ACKF 算法，对车载传感器的输出滤波进行处理，结果如图7（b）所示。

图7 车载横摆角速度传感器输出及滤波

滤波曲线的走势表明，在传感信号振荡处，ACKF 比KF 具有更好的消抖表现；在传感信号持续上升过程中，ACKF 也比KF 表现出更好的跟随性。另外，把i-TESTER 测试系统输出值作为参考真值，使用均方误差（MAE）和均方根误差（RMSE）对KF和ACKF 两种算法的估计精度进行定量比较，结果如表3 所示。从表中数据可看出，虽然车载传感器与i-TESTER测量系统存在固有误差，导致两滤波算法的MAE 和RMSE 结果相差不大，但ACKF 算法的MAE 和RMSE 依旧比KF 的小，反映了ACKF 更好的估计精度。

表3 KF和ACKF的MAE和RMSE指标

4.2 车辆模型的估计结果

基于3 自由度自行车模型的横摆角速度估计如图8 所示，绿色曲线为模型估计值，其走势基本与真值测试系统的输出曲线保持一致。转动转向盘是产生车辆横摆角速度的原因，而传感器本身是一个微机电系统，因此理论上模型估计值应比传感器量测值“早一些”。实际的测试结果也验证了这一现象，即图8 中的绿色曲线均比真值测试系统的红色曲线和车载传感器滤波黑色曲线“靠左”。

图8 基于车辆模型的横摆角速度和道路侧倾角估计

蓝色曲线为道路侧倾角估计，基本能反映出道路侧倾角，在0.03 rad值附近。

4.3 基于RISF的融合估计

图9 中蓝色曲线为最终的RISF 融合曲线，相比于传感器滤波曲线和模型估计曲线，融合曲线更加平滑，波峰波谷区域亦消除了传感器滤波曲线的抖动。另外，融合曲线相比于传感器滤波曲线“靠左”，说明融合曲线更能反映出车辆实时真实的姿态值。

图9 基于RISF的横摆角速度融合估计

5 结论

本文中设计了一种基于RISF 多源传感信息融合的车辆横摆角速度估计方法。在RISF 自适应卡尔曼滤波算法框架下，首先使用ACKF 算法获得了较好的横摆角速度传感器滤波值，然后建立考虑道路侧倾角的自行车模型，通过动力学公式估计出横摆角速度，最后在RISF-AKF框架下进行融合。

实车道路数据表明，RISF 融合曲线相比于传感器滤波曲线，消除了波峰波谷区域的信号抖动，与此同时引入了模型估计对车辆真实横摆角速度的预测信息，解决了使用单一传感器信息源带来的横摆角速度估计波动大、量测滞后等问题。