基于迭代扩展卡尔曼滤波的车辆运动状态估计

赵强 范思远 唐政林

摘 要：采用低成本传感器并借助卡尔曼滤波方法实现车辆运动状态的高精度估计。首先考虑车辆侧向运动、横摆运动以及侧倾运动，建立非线性三自由度的动力学车辆模型，通过对其线性化，实现扩展卡尔曼滤波设计，进一步针对线性化带来的截断误差问题，利用贝叶斯估计建立极大后验状态估计最小二乘表达式，通过进一步求解最终设计完成了迭代扩展卡尔曼滤波算法。通过不同行驶条件下仿真，验证迭代扩展卡尔曼滤波过滤噪声和追踪实际值的能力。仿真结果表明：在复杂的行驶条件下，迭代扩展卡尔曼滤波能大幅过滤噪聲，并有效追踪车辆质心侧偏角和横摆角速度的实际状态。

关键词：车辆运动状态估计;迭代扩展卡尔曼滤波;三自由度;质心侧偏角;横摆角速度

中图分类号：U463.6    文献标识码：A   文章编号：1006-8023（2021）01-0066-07

Abstract：The paper discusses to use low-cost sensors and Kalman filter to achieve high-precision vehicle motion state estimation. Firstly， considering the lateral， yaw motion and roll motion of the vehicle， a nonlinear 3-DOF dynamic vehicle model is established and further linearized to realize extended Kalman filter design. To solve the problem of truncation error caused by linearization， the least square expression of largest posterior state estimation is established by Bayesian estimation， and the iterative extended Kalman filter （IEKF） algorithm is finally completed by further solving. Through simulations under different driving conditions， the IEKF algorithm is verified on filtering noise and tracking the actual value. The simulation results show that under complex driving conditions the IEKF algorithm can effectively filter noise and track the actual state of the vehicle slip angle and yaw rate.

Keywords：Vehicle motion state estimation; iterative extend Kalman filter; 3-DOF; slip angle; yaw rate

0 引言

实时准确获取汽车行驶状态是汽车主动安全控制的必要条件[1]。由于成本的限制，高精度但价格昂贵的状态测量传感器通常较难装车应用，实际的车辆一般采用低成本传感器结合滤波算法进行行驶状态的估计[2]。状态估计大致分为运动学估计和动力学估计两类[3-4]。

Koibuchi等[5]运用运动学估计，通过运动学模型，对传感器信号运用直接积分法估计。Hac等[6]使用卡尔曼滤波器优化汽车运动学估计模型的参数。以上结果均有一定精度，但运动学模型对参数变化缺乏实时性，对设备精度依赖程度高。因此近年来对汽车状态估计大多采用动力学模型[7-8]。

动力学估计方法是以车辆动力学模型为基础，结合不同观测器技术进行车辆运动状态估计。因此选用合理的车辆模型和观测滤波技术对估计结果的准确程度至关重要。柳惠[9]基于二自由度车辆模型，以扩张观测理论为基础设计了一套状态观测器，二自由度模型不能准确表明汽车动力学关系。宗长富等[10]基于三自由度车辆模型，结合扩展卡尔曼滤波算法，对包含质心侧偏角和横摆角速度在内的状态进行估计。

扩展卡尔曼滤波（EKF）因利用泰勒展开并忽略高阶项，从而产生截断误差。目前针对减小车辆状态扩展卡尔曼滤波截断误差的改进研究还未见报道。本文首次针对车辆行驶状态估计问题设计其迭代扩展卡尔曼滤波（IEKF）算法，减小一般扩展卡尔曼器由于存在截断误差而引起的估计误差，从而提高状态精度。基于侧向、横摆和侧倾三自由度车辆模型，使用IEKF过滤噪声，通过IEKF和EKF仿真结果进行对比，验证在不同行驶条件下IEKF对噪声的过滤效果。

3 仿真验证

3.1 仿真模型

在不同条件下通过Simulink进行仿真，参数见表1，仿真模型如图2所示，设定仿真时间为30 s，采样时间为0.001 s，Carsim输入模块、状态噪声和量测噪声模块、三自由度车辆模型模块、IEKF模块、EKF模块、仿真对比图模块和量化分析模块各部分已在图2上标注，其中Carsim模块用于生成双移线、蛇形工况的前轮转角δω，以输出给三自由度车辆模型。

3.2 仿真结果

由图3可知：当汽车行驶条件最为简单时，EKF和IEKF与实际值相差可忽略，EKF和IEKF均能有效追踪实际值并过滤噪声。为了进一步探究，将行驶工况改为蛇形工况，仿真图如图4所示。

由图4可知，蛇形工况噪声比双移线工况大，EKF和IEKF均可以过滤绝大多数噪声，但IEKF对实际值追踪效果优于EKF，质心侧偏角的幅值差约为0.005 rad，横摆角速度幅值差约为0.03 rad。接下来通过提高车速进一步研究，仿真图如图5所示。

从图5中可得：高速行驶下质心侧偏角和横摆角速度变大，噪声也相应增加，IEKF噪声过滤效果优于EKF。EKF对质心侧偏角估计与实际值幅值差约为0.3 rad，横摆角速度幅值约为0.5 rad，估计精度降低。最后将路面附着系数改为低附着0.4，仿真图如图6所示。

路面附着系数降低致使前轮易打滑，前轮转角增大。由图6可看出，低附着路面下车辆质心侧偏角和横摆角速度大幅提高，IEKF依然能对实际值较好地追踪，图6明显看出EKF估计值在质心侧偏角和横摆角速度最大值处与测量值接近，EKF已经不能达到对实际值追踪的目的。

3.3 估计结果的量化分析

均方根误差（RMSE）可以进一步量化仿真结果。RMSE为估计值与实际值差值平方和估计次数n比值的平方根。均方根误差越小，估计精度越高。因此用RMSE表示EKF和IEKF对实际值的追踪能力。

表2和表3分别为各行驶条件下质心侧偏角和横摆角速度测量值、EKF和IEKF的RMSE。表中条件①—④分别为低速高附着简单、低速高附着复杂和高速高附着复杂、高速低附着复杂工况。

由表2和表3可知：汽车行驶条件相对简单时，EKF和IEKF均能对实际值有效追踪，随着行驶条件越来越复杂，噪声增加，EKF对于非线性较强时滤波性能下降的问题越来越严重，IEKF利用最大后验估计更新状态有效解决这一问题，在设定的行驶条件最为复杂的情况下针对质心侧偏角和横摆角速度过滤掉83.2%和73.29%的噪声。

对下一步实车试验可在不同工况、车速和路面进行，由于实际横摆角速度、质心侧偏角真值无法得到，在实验用车上利用高精度陀螺仪、差分GPS，并获取横摆角速度、质心侧偏角值，由于其精度高，因此近似作为真实值;通过一般精度的IMU惯性测量陀螺仪获取噪声较大的测量值，之后比较EKF和IEKF的仿真效果。

4 结论

本文针对车辆运动状态估计这一问题，建立了包含侧向、横摆和侧倾运动的非线性三自由度模型，对车辆模型线性化并设计迭代扩展卡尔曼滤波器，通过Simulink仿真验证，得到不同行驶条件下IEKF和EKF对噪声过滤和实际值的追踪情况。仿真结果表明：迭代扩展卡尔曼滤波观测器在复杂的行驶条件下能大幅过滤噪声并有效追踪实际值。

【参 考 文 献】

[1]赵福全，吴成明，潘之杰，等.中国汽车安全技术的现状与展望[J].汽车安全与节能学报，2011，2（2）：111-121.

ZHAO F Q， WU C M， PAN Z J， et al. Status and prospect of automotive safety technology in China[J]. Journal of Automotive Safety and Energy， 2011， 2（2）：111-121.

[2]郭洪艳，陈虹，赵海艳，等.汽车行驶状态参数估计研究进展与展望[J].控制理论与应用，2013，30（6）：661-672.

GUO H Y， CHEN H， ZHAO H Y， et al. State and parameter estimation for running vehicle： recent developments and perspective[J]. Control Theory & Applications， 2013， 30（6）：661-672.

[3]陳慧，高博麟，徐帆.车辆质心侧偏角估计综述[J].机械工程学报，2013，49（24）：76-94.

CHEN H， GAO B L， XU F. Review on vehicle sideslip angle estimation[J]. Journal of Mechanical Engineering， 2013， 49（24）：76-94.

[4]林棻，黄超.汽车质心侧偏角估计的研究现状及发展[J].中国机械工程，2013，24（1）：135-141.

LIN F， HUANG C. State-of-the-art of vehicle slip angle estimation[J]. China Mechanical Engineering， 2013， 24（1）： 135-141.

[5]KOIBUCHI K， YAMAMOTO M， FUKUDA Y， et al. Vehicle stability control in limit cornering by active brake[R]. SAE， 960487， 1996.

[6]HAC A， SIMPSON M. Estimation of vehicle side slip angle and yaw rate[R]. SAE， 2000-01-0696， 2000.

[7]刘飞，熊璐，邬肖鹏，等.车辆质心侧偏角估计算法设计与对比分析[J].同济大学学报（自然科学版），2015，43（3）：448-455.

LIU F， XIONG L，WU X P， et al. Vehicle sideslip angle estimation and contrastive analysis[J]. Journal of Tongji University （Natural Science）， 2015， 43（3）： 448-455.

[8]李亮，贾钢，宋健，等.汽车动力学稳定性控制研究进展[J].机械工程学报，2013，49（24）：95-107.

LI L， JIA G， SONG J， et al. Progress on vehicle dynamics stability control system[J]. Journal of Mechanical Engineering， 2013， 49（24）：95-107.

[9]柳惠.基于模型的车辆质心侧偏角估计方法研究[J].重庆理工大学学报（自然科学版），2016，30（11）：25-28.

LIU H. Slip angle of the center of mass of automobile estimation research based on the model[J]. Journal of Chongqing University of Technology （Natural Science）， 2016， 30（11）： 25-28.

[10]宗长富，胡丹，杨肖，等.基于扩展Kalman滤波的汽车行驶状态估计[J].吉林大学学报（工学版），2009，39（1）：7-11.

ZONG C F， HU D， YANG X， et al. Vehicle driving state estimation based on extended Kalman filter[J]. Journal of Jilin University （Engineering and Technology Edition）， 2009， 39（1）： 7-11.

[11]刘丽.车辆三自由度平面运动稳定性的非线性分析及控制策略评价[D].长春：吉林大学，2010.

LIU L. Nonlinear analysis and control strategy evaluation on the stability of vehicle 3-DOF planer motion[D]. Changchun： Jilin University， 2010.

[12]李程祎，左曙光，段向雷，等.考慮纵向加速度的三自由度车辆单轨非线性模型的建立与分析[J].制造业自动化，2012，34（22）：117-121.

LI C Y， ZUO S G， DUAN X L， et al. Modeling and analysis of a three-DOF single-track nonlinear vehicle model based on considering longitudinal acceleration[J]. Manufacturing Automation， 2012， 34（22）：117-121.

[13]陈学文，周越，张进国.考虑侧倾影响的汽车横摆角速度与质心侧偏角滤波估计[J].汽车技术，2017，48（11）：40-43

CHEN X W， ZHOU Y， ZHANG J G. A filtering estimation on YAW rate and slip angle considering roll motion of vehicle body[J]. Automobile Technology， 2017， 48 （11）：40-43.

[14]张满生，张学庄.卡尔曼滤波器及其工程应用[J].计算技术与自动化，2008，27（1）：136-139.

ZHANG M S， ZHANG X Z. Kalman filter and its engineering application[J]. Computing Technology and Automation， 2008， 27（1）： 136-139.

[15]杨宏，李亚安，李国辉.一种改进扩展卡尔曼滤波新方法[J].计算机工程与应用，2010，46（19）：18-20.

YANG H， LI Y A， LI G H. New method of improved extended Kalman filter[J]. Computer Engineering and Applications， 2010， 46（19）：18-20.

[16]李良群，姬红兵，罗军辉.迭代扩展卡尔曼粒子滤波器[J].西安电子科技大学学报（自然科学版），2007，34（2）：233-238.

LI L Q， JI H B， LUO J H. Iterated extended Kalman particle filtering[J]. Journal of Xidian University （Natural Science）， 2007， 34（2）： 233-238.

[17]常国宾，许江宁，李安，等.基于组合牛顿迭代法的改进IEKF及其在UNGM中的应用[J].海军工程大学学报，2012，24（1）：15-19.

CHANG G B， XU J N， LI A， et al. Modified iterated extended Kalman filter based on Gauss-Newton iteration and its application in UNGM[J]. Naval University of Engineering， 2012， 24（1）：15-19.