基于自适应模糊扩展卡尔曼滤波的车辆运动状态联合估计*

刘明春 彭志波 吴晓建

（1.南昌大学，南昌 330031；2.金龙联合汽车工业（苏州）有限公司，苏州 215000）

主题词：车辆状态估计 扩展卡尔曼滤波 模糊控制 联合估计 车辆动力学模型

1 前言

车辆驾驶过程中的状态估计是底盘主动控制系统和车载故障诊断系统的关键技术之一。车辆的某些重要参数难以直接测量或测量所用的传感器成本较高，如纵向速度、横摆角速度和质心侧偏角等。对上述重要参数的获取，可利用低成本传感器得到易获取的状态信息，再通过估计算法获得。

目前，车辆状态信息估计的研究方法主要为卡尔曼滤波（Kalman Filter，KF）算法及其各种相关改进算法，如扩展卡尔曼滤波（Extended Kalman Filter，EKF）和无迹卡尔曼滤波（Unscented Kalman Filter，UKF）等，此外还有模糊逻辑和神经网络等估计算法。文献[4]、文献[5]采用扩展卡尔曼滤波算法，以非线性3自由度车辆动力学模型为基础，通过泰勒级数展开略去高阶项达到线性化的效果进行状态参数的估计。文献[6]、文献[7]为增强控制系统的鲁棒性和准确度，将无迹变换（Unscented Transform）与标准卡尔曼滤波体系相结合，通过无迹变换的方式使非线性车辆模型适用于线性的标准卡尔曼滤波形式。文献[8]、文献[9]提出一种基于模糊逻辑与车辆运动学模型的滤波框架，开发了模糊决策模块以实现更好的融合性能。文献[10]～文献[12]采用神经网络算法提出一种有监督的机器学习方案，通过惯性测量单元（Inertial Measurement Unit，IMU）等低成本传感器获取易测量的参数，以此获得所需的变量。上述车辆状态信息估计方法并未充分考虑传感器获取测量参数时，测量参数受环境等因素干扰所产生的噪声影响，在估算效果的稳定性和精确性方面有待提高。另外，上述估计算法主要针对单一车辆状态进行估算，存在估计对象单一、难以同时估算多个状态的问题。

针对上述问题，本文针对存在测量噪声情况下的车辆横、纵向运动状态信息进行联合估计，提出一种基于自适应模糊扩展卡尔曼滤波（Adaptive Fuzzy Extended Kalman Filter，AFEKF）的车辆状态联合估计算法，估算车辆纵向速度、横摆角速度和质心侧偏角，并通过仿真和实车试验对联合估计算法的有效性进行验证。

2 车辆动力学建模

2.1 车辆模型

本文所建立的车辆动力学模型包括14 自由度车辆动力学模型和3 自由度车辆动力学模型。其中，14自由度车辆动力模型用于模拟真实车辆运动状态，并得到估计算法所需的输入状态量和被估计状态的比较状态量。3 自由度车辆动力学模型通过微分方程变换、离散化后与扩展卡尔曼滤波算法结合，建立估计算法。

2.2 14自由度车辆动力学模型

为了较全面地反映车辆各方向的动力学状态，所建立的14自由度车辆动力学模型包括车身的纵向、侧向、垂向、侧倾、俯仰和横摆运动，4 个车轮的回转运动和4个簧下质量的垂向运动，如图1所示。

图1 14自由度车辆动力学模型示意

车身的6自由度运动微分方程如下：

纵向、侧向和垂向动力学方程分别为：

式中，v、v、v分别为车辆质心沿、、坐标轴的纵向、侧向和垂向运动速度；ω、ω、ω分别为侧倾、俯仰和横摆角；为重力加速度；为整车质量；为车身质量；为车辆在行驶过程中受到的阻力之和；为坡道阻力；为滚动阻力；为空气阻力；为迎风面积；为空气密度；为坡道角度；为空气阻力系数；为滚动阻力系数；F、F、F分别为各车轮受到的纵向力、侧向力、垂向力；=1,2 分别表示车辆前、后轴；=l,r分别表示左、右侧。

侧倾、俯仰和横摆动力学方程分别为：

式中，为轮距；M、M分别为悬挂质量的离心力和重力引起的侧倾力矩、俯仰力矩；M、M分别为第轴左、右侧轮胎的回正力矩；I、I、I分别为车辆绕、、轴的转动惯量；、分别为质心到前、后轴的距离；F、F分别为第轴左、右侧悬架的垂向力。

轮胎力在车辆坐标系和轮胎坐标系间的转换关系为：

式中，δ为各车轮的转向角；F、F分别为轮胎坐标系上第轴侧的纵向力和侧向力。

悬架运动微分方程为：

式中，、分别为车辆静态平衡条件下第轴侧的悬架力、轮胎载荷；、分别为车辆第轴侧动态悬架力、轮胎载荷；F、F分别为综合静态和动态条件下作用于悬架与轮胎上的力，得到的第轴侧悬架系统的垂向力和车轮的静态垂向载荷。

对悬架系统进行受力分析，当车辆处于侧倾和俯仰运动时，悬架会产生一定程度的形变，导致载荷转移，通过左、右侧悬架和前、后轴变形量可以得到悬架动态载荷。

车轮垂向运动和旋转运动的微分方程为：

式中，为侧轮胎滚动半径；为各轮心的垂向位移；为各轮的质量；、Δ分别为车轮在回转滚动过程中产生弹性迟滞现象形成的滚动阻力矩和偏向接地几何中心一侧的距离；、、分别为各车轮的转动惯量、输出力矩和旋转角速度。

此外，14 自由度车辆动力学模型还包含了“PAC2002”魔术公式轮胎模型、驾驶员模型和电机模型等，使车辆模型各部件之间的耦合关系和动力学特性更为接近真实车辆，以适用于车辆主动安全控制的研究。

2.3 3自由度车辆动力学模型

为了考虑估计算法的准确性和鲁棒性需求，建立包含纵向、侧向和横摆运动的非线性3自由度车辆模型如图2所示，以此作为车辆状态信息的估计模型。

图2 3自由度车辆动力学模型示意

根据牛顿第二定律，得到车辆动力学平衡方程为：

式中，a、a分别为纵向、横向加速度；、分别为质心到前、后轴的距离；为前轮转角；为横摆角速度；分别为前、后轮轮距。

由上述动力学平衡方程计算可得3 自由度模型的状态方程和测量方程为：

式中，为质心侧偏角；为转向盘转角；、分别为前、后轮胎的侧偏刚度；为转向盘与前轮的角转向传动比。

设二维控制输入向量为=[ a]，三维状态向量为=[γ β v]，一维量测输出向量为=[a]。

3 基于AFEKF估计算法的车辆状态联合估计

3.1 AFEKF算法总体方案设计

本文设计的车辆横、纵向状态联合估计算法方案如图3所示。以14自由度车辆动力学模型的输出状态参数作为车辆的真实运动状态，、a和a分别为14自由度动力学模型输出的前轮转角、纵向加速度和横向加速度，经过高斯噪声、的叠加后，模拟得到带有噪声的测量值。

图3 行驶状态联合估计算法总体方案

AFEKF 联合估计算法包含1 个扩展卡尔曼滤波器和1个模糊控制器，扩展卡尔曼滤波器通过输入的前轮转角、纵向加速度和横向加速度来估算质心侧偏角、横摆角速度和纵向速度，模糊控制器可以在估计过程中对观测噪声协方差矩阵进行实时调整，使得扩展卡尔曼滤波算法具有自适应性，提高算法精度。

3.2 扩展卡尔曼滤波离散化

由于3自由度车辆模型具有非线性的特点，EKF算法的核心是通过线性化方法将非线性系统转换为近似的线性系统，围绕滤波值将非线性函数略去二阶及以上项的泰勒级数展开，得到一个近似线性化的模型，并通过3自由度车辆模型的动力学方程式（18）～式（20）建立状态空间方程：

式中，()为三维状态向量；()为二维控制输入向量；()为一维量测输出向量；()、()分别为相互独立且均值为零的系统噪声和测量噪声，二者的协方差矩阵分别为、。

函数和均为非线性函数，为了得到线性化模型，分别对()求偏导得到雅克比矩阵()和()：

时刻状态转移矩阵为：

式中，Δ为采样周期；为单位矩阵。

根据上述公式，采用欧拉法将3自由度车辆的状态空间方程进行离散化：

3.3 自适应模糊扩展卡尔曼滤波算法

车辆在实际行驶过程中环境复杂多变，传感器测量的精度水平会受不同工况等因素的影响，从而使得测量噪声协方差矩阵相应地变化，本文通过模糊控制对其进行动态调整，提高算法的适应性。设计离散对象的系统状态方程和测量方程分别为：

式中，X、Y分别为时刻系统的状态变量和输出变量；F为状态矩阵，为式（22）中的雅克比矩阵()；B为控制矩阵；H为测量矩阵，为式（23）中的雅克比矩阵()；对应第个采样时刻。

扩展卡尔曼滤波算法可分为时间更新和测量更新2个部分：

时间更新是通过时刻的X预测(+1)时刻的X：

根据式（27）可知，预测值存在系统噪声，因此需求出时刻的预测误差协方差矩阵：

测量更新需要通过增益矩阵对X进行修正，以得到(+1)时刻的估计值X：

然后更新(+1)时刻的协方差矩阵来表示估计误差，用于开展下一阶段的估算：

本文将模糊控制与EKF 算法相结合，以达到动态调整测量噪声协方差矩阵的效果。设(+1)时刻的测量噪声协方差矩阵为(+1)，且有(+1)=(1+Δ)()，其中Δ为的调节量。

定义测量状态Y的实际协方差()和理论协方差()分别为：

将理论协方差与实际协方差间的差值作为模糊控制器的输入，输出为：

结合相关理论和大量仿真分析，实时调整模糊控制器的滤波参数和的值，积累和参考足够的专家经验，制定模糊规则集，less N 表示小于0，equal AZ 表示约等于0，more P 表示大于0，解模糊化的方法使用重心法，二维曲线如图4 所示，模糊规则集为：

图4 输入-输出关系

模糊控制的隶属度函数如图5所示。

图5 模糊控制的隶属度函数

4 算法的仿真验证和实车验证

4.1 基于MATLAB/Simulink的仿真验证

为了验证所提出算法的有效性，在MATLAB/Simu⁃link 环境中进行仿真验证，采用双移线工况，将自适应模糊扩展卡尔曼滤波的效果分别与扩展卡尔曼滤波和14自由度车辆动力学模型输出的参考状态进行对比分析，车辆的主要参数如表1所示。

表1 仿真车辆主要参数

在MATLAB/Simulink 中设置路面附着系数为0.8，车速为100 km/h，采样周期为0.01 s，误差协方差矩阵初值为=0.1，系统噪声协方差矩阵=0.1，观测噪声协方差矩阵初值=[50 000]。

双移线工况的行驶轨迹如图6所示，对应的转向盘转角经角转向传动比转换后，得到转向轮转角如图7所示，图8、图9所示分别为纵向加速度和横向加速度的14自由度参考值和加上高斯噪声的模拟值。将转向盘转角、带噪声的横向加速度和纵向加速度状态作为估算器的输入，经过算法估计后得到如图10～图12所示的估计结果。图10、图11 的AFEKF 的质心侧偏角、横摆角速度和纵向速度的估计曲线比EKF的更接近14自由度车辆动力学模型的参考状态，尤其是曲线的拐角处较为显著。造成此现象的原因可能是，曲线拐角处车辆状态变动更加频繁和卡尔曼滤波估计算法估计误差在一阶离散化的过程中随时间的增加累积增大，而AFEKF 对协方差矩阵的调整、减小累积误差和测量噪声等因素的影响，使估计结果更准确。

图6 行驶轨迹随时间变化

图7 前轮转角随时间变化

图8 纵向加速度随时间变化

图9 横向加速度随时间变化

图10 质心侧偏角估计

图11 横摆角速度估计

图12 纵向速度估计

相较于EKF 估计值更为接近14 自由度车辆动力学模型的参考值，估计效果较好，尤其在横摆角速度峰值处AFEKF 的估计精度明显提高，因此可知AFEKF 对测量噪声矩阵进行了自适应调整，使得质心侧偏角、横摆角速度和纵向速度的估计效果要优于EKF。

4.2 实车试验验证

基于某量产车型进行实车试验。试验车安装有CANoe、Kistler 和MicroAutoBox 等相关设备和传感器来采集车辆关键参数，包括转向盘转角、纵向加速度a、横向加速度a、质心侧偏角、横摆角速度和纵向车速v，试验现场如图13、图14所示，控制车速为40 km/h，采样周期为0.01 s，标桩间距=50 m。

图13 GB/T 6323—2014蛇行绕桩工况

图14 实车试验场地

实车试验方案如图15 所示，利用Kistler 车载传感采集测试车辆的运动状态信息，AFEKF 估计算法运行于MicroAutoBox控制器，在计算机中将实车采集的数据结果与AFEKF 联合估计算法进行比较，验证算法的有效性。

图15 实车试验估计流程

图16～图18所示分别为转向盘转角、纵向加速度和横向加速度状态量随时间的变化情况；图19～图21所示分别为质心侧偏角、横摆角速度和纵向速度状态量的实车试验值与AFEKF算法估计值的对比结果。

图16 转向盘转角随时间变化

图17 纵向加速度随时间变化

图18 横向加速度随时间变化

图19 质心侧偏角估计对比分析

图20 横摆角速度估计对比分析

图21 纵向速度估计对比分析

由图19～图21可知：传感器采集的车辆真实状态值与估计值存在一定的偏差，尤其是起步阶段偏差相对较大，这是因为车辆需要一定的加速起动时间才能达到设定车速和试验路段，此时纵向加速度和横向加速度不稳定以及噪声较大所致，质心侧偏角真实值受Kistler等车载传感器的安装位置和测量误差等因素的影响，也是造成其真实值与AFEKF 估计值曲线存在差距的重要因素；随后估计值与车辆真实值较为接近，逼近度较好，说明AFEKF算法得到的估计结果具有较好的准确性。

5 结束语

本文针对车辆行驶过程中的横、纵向动力学状态联合估计问题展开研究，基于自适应模糊扩展卡尔曼滤波（AFEKF）理论建立了联合估计算法。首先建立了14自由度车辆动力学整车模型模拟真实车辆行驶状态；然后根据3 自由度车辆动力学模型的微分方程建立基于扩展卡尔曼滤波（EKF）的估计算法模型，将所测量的转向盘转角和带噪声的纵向加速度、横向加速度状态信息作为算法输入，实现对质心侧偏角、横摆角速度和纵向速度的联合估计，并在EKF 算法中应用模糊控制器来自适应调整测量噪声，降低测量噪声对估计结果的影响，使估计精度更高。仿真和实车测试结果表明，在存在测量噪声的情况下，针对车辆横、纵向运动状态的联合估计，自适应模糊扩展卡尔曼滤波（AFEKF）估计算法具有较好的准确性，可以准确地估算车辆的纵向车速、横摆角速度和质心侧偏角。