基于改进粒子滤波算法的车速估计

高彦 傅春耘 杨忠 杨官龙

收稿日期：2021-12-27

网络出版日期：2022-04-24

基金项目：重庆市自然科学基金资助项目（cstc2020jcyj-msxmX0664）；中央高校基本科研业务费项目（2020CDJ-LHZZ-043）。

作者简介：高彦（1994—），男，硕士研究生，主要从事智能汽车底盘控制研究，（E-mail）gaoyan_cqu@cqu.edu.cn。

通信作者：傅春耘，男，副教授，（E-mail）fuchunyun@cqu.edu.cn。

摘要：针对基于粒子滤波算法设计的车速估计器因提议分布与实际分布不一致导致粒子退化使估计误差变大的问题，提出了一种通过修正提议分布减弱粒子退化影响的改进粒子滤波车速估计器。首先，基于车辆运动学模型和传感器特性建立系统的状态转移方程和观测方程。然后，利用传感器测量值与粒子状态值的差值设计提议分布修正项对状态转移方程进行修正，并对过程噪声做自适应处理。最后，利用CarSim-Simulink联合仿真平台在双移线工况和正弦转角输入工况下进行仿真验证。与自适应粒子滤波器相比，双移线工况下改进粒子滤波估计器产生的纵向速度估计值和侧向速度估计值的平均绝对误差分别减小了40.25%和55.71%；正弦转角输入工况下，改进粒子滤波估计器产生的纵向速度估计值和侧向速度估计值的平均绝对误差分别减小了47.00%和41.21%。

关键词：车速估计；粒子滤波；提议分布

中图分类号：U461          文献标志码：A             文章編号：1000-582X（2024）03-044-09

随着现代汽车电子控制技术的快速发展，车载控制系统的数量越来越多。许多控制系统在工作时需要车辆的状态信息作为控制系统的输入，例如自适应巡航控制（adaptive cruise control，ACC）、电子稳定控制（electronic stability control，ESC）、防抱死制动系统（anti-lock braking system，ABS）。因此，获取准确的车辆状态信息是车辆控制系统正常、有效工作的前提^[1]。车辆状态信息中存在的误差将使车辆控制系统的性能下降甚至完全丧失。

在车辆的诸多状态中，车速（包括纵向速度和侧向速度）是最重要的车辆状态之一，许多重要的车载控制系统均需要准确的车速信息才能正常工作。由于通过传感器直接获取准确车速的成本很高，因此目前车速信息主要通过状态估计来间接获取。常用的状态估计方法包括卡尔曼滤波算法、龙贝格观测器、鲁棒观测器、滑模观测器和非线性观测器等，其中卡尔曼滤波算法是应用最广泛的状态估计算法^[2]。卡尔曼滤波算法又衍生出多种变体，例如扩展卡尔曼滤波（extended Kalman filter，EKF）、无迹卡尔曼滤波（unscented Kalman filter，UKF）、容积卡尔曼滤波（cubature Kalman filter，CKF）等。文献[3-5]中采用经典卡尔曼滤波算法估计车速，该方法以线性微分方程为基础，所以只适用于线性系统。但是，车辆的运行工况复杂多变，当车辆运行在极限工况下时，车辆系统表现出强非线性。因此，文献[6-9]中使用扩展卡尔曼滤波算法进行车速估计，但是该方法只适用于弱非线性系统，而且需要计算复杂的雅可比矩阵。文献[1，10-12]中采用无迹卡尔曼滤波算法估计车速，该方法不需要计算雅可比矩阵，能处理不可导的非线性函数，与扩展卡尔曼滤波算法相比更适用于非线性系统。文献[13-14]中采用容积卡尔曼滤波算法估计车速，该方法不需要计算复杂的雅可比矩阵，且计算多维函数积分时效率较高，在非线性滤波方面较其他种类的卡尔曼滤波器具有更大的优势^[15]。

上述各种基于卡尔曼滤波的车速估计算法都在处理强非线性系统方面存在缺陷。粒子滤波算法最早出现在20世纪50年代，是一种基于蒙特卡洛法和递归贝叶斯估计的统计滤波方法，具有很强的非线性处理能力^[16]。文献[16-19]中采用粒子滤波算法来估计车速，结果表明文献中的几种粒子滤波估计器均能够在强非线性工况下较准确地估计车速。

虽然上述粒子滤波算法具有很强的非线性处理能力，并且通过重采样方法在一定程度上减弱了粒子退化对估计精度的影响，但是仍然存在粒子退化导致估计精度变差的问题。具体来讲，随着粒子滤波算法迭代次数增加，只有少部分粒子具有较大的权重，大部分粒子的权重很小，造成粒子多样性匮乏，从而影响估计精度^[20]。针对这一问题，文献[17]中采用高斯分布作为提议分布，设计了自适应标准差参数，在每一个采样时刻修正提议分布的标准差，提升了粒子滤波估计器的估计精度和适应能力。但是该方法仍然存在因提议分布与实际分布不一致导致的粒子退化问题。笔者在文献[17]的基础上基于车辆运动学模型设计了一种改进粒子滤波估计器，改进提议分布使其更符合实际分布以进一步减弱粒子退化对估计精度的影响，从而有效估计车辆的纵向速度和侧向速度。仿真结果表明，与文献[17]中的自适应粒子滤波估计器相比，改进粒子滤波估计器在双移线和正弦转角输入工况下均使纵向速度和侧向速度估计值的平均绝对误差大幅减小。

1粒子滤波算法基本原理

粒子滤波适用于强非线性系统，因此非常适用于处理车辆状态估计问题^[21]。粒子滤波的核心思想是用一系列离散随机采样点来近似系统随机变量的概率密度，以样本均值代替积分运算，从而获得状态的最小方差估计^[22]。

与卡尔曼滤波一样，粒子滤波算法利用状态空间模型对系统进行描述，该模型包括状态转移方程和观测方程。

式中：表示系统时刻的状态；表示系统时刻的状态；表示系统时刻的过程噪声；表示传感器时刻的测量结果；表示传感器时刻的测量噪声；f为状态转移函数；h为观测函数。

粒子滤波的均值思想是利用粒子集合的均值作为状态的估计值。如果粒子集合的分布不能很好地“覆盖”真实值，粒子滤波器就会出现滤波发散现象。粒子集合的有效性通过粒子的权重计算和粒子集合的重采样2个步骤来保证。粒子滤波算法中粒子集合的迭代过程如下^[23]。

1）将表示系统时刻状态的每一个粒子代入式（1），得到系统状态的一步预测值。其中表示第个粒子，表示粒子集合的粒子数。

2）将一步预测状态集合中的每一个粒子带入式（2），計算观测值的预测值。

3）假设在当前时刻（即时刻），传感器采集到唯一的观测值，则可以根据该观测值来计算每个粒子的权重：

式中：表示观测值的预测值与当前时刻观测值的差值；g为粒子的权重计算函数；表示粒子的权重。需要指出的是g必须体现“观测值的预测值与传感器真实观测值的绝对偏差越小，权重越大，反之权重越小”这一规律。高斯函数是其中一种比较具有代表性的权重计算函数。

4）对粒子集合进行重采样。重采样过程体现的是粒子滤波过程中的“优胜劣汰”，即：复制权重大的粒子，淘汰权重小的粒子，并保证重采样前后的粒子数量一致。重采样原理如图1所示。通过重采样机制，可在一定程度抑制粒子退化，保证粒子滤波的状态估计效果。

5）将重采样后的粒子集合表示的状态记为，代入到下一个迭代循环中。

2基于改进粒子滤波的车速估计

2.1车辆运动学模型

车辆的运动学模型如图2所示。在汽车运动过程中，如下运动学关系始终成立：

式中：和分别表示汽车的纵向和侧向速度；和分别表示汽车的纵向和侧向加速度；表示汽车的横摆角速度。

2.2状态转移方程和观测方程

根据式（5）和式（6）得到汽车纵向速度、侧向速度的离散表达式如下：

2.3提议分布修正项

粒子滤波算法效果的好坏取决于粒子集合能否准确地“覆盖”真实值，如果不能很好地“覆盖”真实值则会出现粒子退化问题，从而导致粒子滤波算法的状态估计效果变差。为了减小粒子退化的影响，通常采取以下3种措施^[25]：

1）增加粒子数；

2）采用能更有效抑制粒子退化的重采样技术；

3）选择合理的生成粒子的提议分布。

本研究中通过在状态转移方程中加入修正项，实现实时修正提议分布，使粒子集合更准确地“覆盖”真实值，从而减弱粒子退化问题带来的影响，提升状态估计精度。基于时刻传感器对纵向加速度、侧向加速度及横摆角速度的测量值（即和r_z）与时刻每一个粒子的纵向加速度、侧向加速度及横摆角速度值的差值对提议分布进行修正。提议分布修正项如下：

2.4改进粒子滤波估计器设计

对于固定参数的状态估计器，在实际应用中，不可能针对每一种工况都分别调试出适合的参数，而只能兼顾各种常用工况来调试出一组折中的参数。因此，在工况变化时，固定参数估计器往往无法达到最佳的性能。为了保证估计器能够在大多数工况下有效工作，提升估计器的鲁棒性，本研究中对估计器的状态转移方程进行自适应设计以提高其工况适应能力^[17]：

估计器的观测方程仍为式（13）。所提出的改进粒子滤波估计器完整算法以伪代码形式表示如图3所示。

算法1. 改进粒子滤波车速估计算法伪代码

初始化滤波器：确定粒子数，仿真步长（采样周期）和仿真时间

3仿真验证分析

本研究中基于运动学模型设计了改进粒子滤波车速估计器，为了排除不同模型体系对车速估计精度的影响，选用文献[17]中提出的基于运动学模型的粒子滤波估计器作为对照组，基于Matlab/Simulink和CarSim联合仿真平台，在双移线工况和正弦转角输入工况下对设计的基于改进粒子滤波算法的车速估计器性能进行仿真验证。

将CarSim软件直接输出的车速数据作为真实值，下文中简称实际值；将文献[17]提出的自适应粒子滤波估计器产生的车速估计值称为自适应粒子滤波估计值；将改进粒子滤波估计器产生的车速估计值称为改进粒子滤波估计值。

3.1双移线工况

在本工况中，设定车辆以120 km/h的车速匀速行驶，方向盘转角输入选择CarSim中的“跟随驾驶路径-双移线”选项（图4），得到的仿真结果如图5所示。由图5可见，与自适应粒子滤波估计值相比，改进粒子滤波估计值更接近实际值。

进行50次蒙特卡洛仿真试验后，自适应粒子滤波估计器和改进粒子滤波估计器的估计误差结果如表1所示。可见，改进粒子滤波估计器产生的纵向速度估计值的平均绝对误差、最大绝对误差和均方根误差比自适应粒子滤波估计器产生的相应误差大幅降低，分别减小了40.25%、49.23%和25.63%。类似地，改进粒子滤波估计器产生的侧向速度估计值的平均绝对误差、最大绝对误差和均方根误差比自适应粒子滤波估计器产生的相应误差分别减小了55.71%、54.23%和57.25%。在双移线工况下，与自适应粒子滤波估计器相比，改进粒子滤波估计器的车速估计性能大幅提升。

3.2正弦转角输入工况

在本工况中，设定车辆以80 km/h的车速匀速行驶，方向盘转角输入为正弦波，如图6所示。仿真时间设定为10 s，得到仿真结果如图7所示。由图7可见，与自适应粒子滤波估计值相比，改进粒子滤波估计值更接近实际值。

用自适应粒子滤波估计器和改进粒子滤波估计器进行50次蒙特卡洛仿真的估计误差结果如表2所示。可见，改进粒子滤波估计器产生的纵向速度估计值的平均绝对误差、最大绝对误差和均方根误差比自适应粒子滤波估计器产生的相应误差分别减小了47.00%、41.98%和43.85%。类似地，改进粒子滤波估计器产生的侧向速度估计值的平均绝对误差、最大绝对误差和均方根误差比自适应粒子滤波估计器产生的相应误差分别减小了41.21%、37.43%和52.85%。与双移线工况类似，在正弦转角输入工况下，与自适应粒子滤波估计器相比，改进粒子滤波估计器的车速估计性能大幅提升。

4结束语

本研究中通过修正提议分布来减弱粒子滤波算法粒子退化问题对车速估计精度的影响。仿真结果表明，加入提议分布修正项后的粒子滤波估计器能够在不同工况下准确、稳定地估计车辆的纵向速度和侧向速度。与没有修正提议分布的自适应粒子滤波估计器相比，改进后的粒子滤波估计器产生的平均绝对误差、最大绝对误差和均方根误差均较小，车速估计精度得到较大的提升。

参考文献

［1］  Zhao Z G， Chen H J， Yang J， et al. Estimation of the vehicle speed in the driving mode for a hybrid electric car based on an unscented Kalman filter[J]. Proceedings of the Institution of Mechanical Engineers， Part D： Journal of Automobile Engineering， 2015， 229（4）： 437-456.

［2］  余卓平， 高晓杰. 车辆行驶过程中的状态估计问题综述[J]. 机械工程学报， 2009， 45（5）： 20-33.

Yu Z P， Gao X J. Review of vehicle state estimation problem under driving situation[J]. Journal of Mechanical Engineering， 2009， 45（5）： 20-33. （in Chinese）

［3］  Daiss A， Kiencke U. Estimation of vehicle speed fuzzy-estimation in comparison with Kalman-filtering[C]//Proceedings of International Conference on Control Applications， September 28-29， 1995， Albany， NY， USA. IEEE， 1995： 281-284.

［4］  Moaveni B， Khosravi Roqaye Abad M， Nasiri S. Vehicle longitudinal velocity estimation during the braking process using unknown input Kalman filter[J]. Vehicle System Dynamics， 2015， 53（10）： 1373-1392.

［5］  王仁廣， 刘昭度， 齐志权， 等. 基于自适应卡尔曼滤波算法确定汽车参考车速[J]. 农业机械学报， 2006， 37（4）： 9-11， 41.

Wang R G， Liu Z D， Qi Z Q， et al. Vehicle reference speed determination using adaptive Kalman filter algorithm[J]. Transactions of the Chinese Society for Agricultural Machinery， 2006， 37（4）： 9-11， 41. （in Chinese）

［6］  Guo H Y， Chen H， Xu F， et al. Implementation of EKF for vehicle velocities estimation on FPGA[J]. IEEE Transactions on Industrial Electronics， 2013， 60（9）： 3823-3835.

［7］  Park G， Choi S B， Hyun D， et al. Integrated observer approach using in-vehicle sensors and GPS for vehicle state estimation[J]. Mechatronics， 2018， 50： 134-147.

［8］  Wenzel T A， Burnham K J， Blundell M V， et al. Dual extended Kalman filter for vehicle state and parameter estimation[J]. Vehicle System Dynamics， 2006， 44（2）： 153-171.

［9］  张志勇， 张淑芝， 黄彩霞， 等. 基于自适应扩展卡尔曼滤波的分布式驱动电动汽车状态估计[J]. 机械工程学报， 2019， 55（6）： 156-165.

Zhang Z Y， Zhang S Z， Huang C X， et al. State estimation of distributed drive electric vehicle based on adaptive extended Kalman filter[J]. Journal of Mechanical Engineering， 2019， 55（6）： 156-165. （in Chinese）

［10］  Chu L， Zhang Y S， Shi Y R， et al. Vehicle lateral and longitudinal velocity estimation based on unscented Kalman filter[C]//2010 2nd International Conference on Education Technology and Computer， June 22-24， 2010， Shanghai， China. IEEE， 2010： V3-427.

［11］  金贤建， 杨俊朋， 殷国栋， 等. 分布式驱动电动汽车双无迹卡尔曼滤波状态参数联合观测[J]. 机械工程学报， 2019， 55（22）： 93-102.

Jin X J， Yang J P， Yin G D， et al. Combined state and parameter observation of distributed drive electric vehicle via dual unscented Kalman filter[J]. Journal of Mechanical Engineering， 2019， 55（22）： 93-102. （in Chinese）

［12］  张志达， 郑玲， 吴行， 等. 基于鲁棒自适应UKF的分布式电动汽车状态估计[J]. 中国科学： 技术科学， 2020， 50（11）： 1461-1473.

Zhang Z D， Zheng L， Wu H， et al. State estimation of distributed electric vehicle based on robust adaptive UKF[J]. Scientia Sinica （Technologica）， 2020， 50（11）： 1461-1473. （in Chinese）

［13］  Xin X S， Chen J X， Zou J X. Vehicle state estimation using cubature Kalman filter[C]//2014 IEEE 17th International Conference on Computational Science and Engineering， December 19-21， 2014， Chengdu， China. IEEE， 2014： 44-48.

［14］  李剛， 王野， 宗长富. 四轮轮毂电机电动汽车行驶状态估计[J]. 汽车工程， 2018， 40（2）： 150-155.

Li G， Wang Y， Zong C F. Driving state estimation of electric vehicle with four-wheel-hub-motors[J]. Automotive Engineering， 2018， 40（2）： 150-155. （in Chinese）

［15］  樊东升， 李刚. 分布式驱动电动汽车行驶状态估计综述[J]. 汽车实用技术， 2019（16）： 10-12.

Fan D S， Li G. Review on driving state estimation of distributed drive electric vehicles[J]. Automobile Applied Technology， 2019（16）： 10-12. （in Chinese）

［16］  包瑞新， Edoardo S. 基于辅助粒子滤波算法的车辆行驶状态和参数联合估计方法研究[J]. 机械设计与制造， 2015（10）： 26-30.

Bao R X， Edoardo S. The research of state and parameter estimation under driving situation based on auxiliary particle filter method[J]. Machinery Design & Manufacture， 2015（10）： 26-30. （in Chinese）

［17］  Huang F H， Gao Y， Fu C Y， et al. Vehicle state estimation based on adaptive state transition model[C]//2020 4th CAA International Conference on Vehicular Control and Intelligence （CVCI）， December 18-20， 2020， Hangzhou， China. IEEE， 2020： 92-96.

［18］  姬鹏， 徐硕硕， 赵一凡， 等. 改进粒子滤波算法的车辆状态估计研究[J]. 机械设计与制造， 2020（2）： 43-46， 50.

Ji P， Xu S S， Zhao Y F， et al. Research on vehicle state estimation based on improved particle filter algorithm[J]. Machinery Design & Manufacture， 2020（2）： 43-46， 50. （in Chinese）

［19］  靳彪. 轮毂电机驱动电动汽车状态参数观测及转矩分配策略研究[D]. 北京： 北京交通大学， 2016.

Jin B. Research on state parameter observation and torque distribution strategy of electric vehicle driven by hub motor[D] . Beijing： Beijing Jiaotong University， 2016. （in Chinese）

［20］  李天成， 范红旗， 孙树栋. 粒子滤波理论、方法及其在多目标跟踪中的应用[J]. 自动化学报， 2015， 41（12）： 1981-2002.

Li T C， Fan H Q， Sun S D. Particle filtering： theory， approach， and application for multitarget tracking[J]. Acta Automatica Sinica， 2015， 41（12）： 1981-2002. （in Chinese）

［21］  胡士強， 敬忠良. 粒子滤波算法综述[J]. 控制与决策， 2005， 20（4）： 361-365， 371.

Hu S Q， Jing Z L. Overview of particle filter algorithm[J]. Control and Decision， 2005， 20（4）： 361-365， 371. （in Chinese）

［22］  Xu Y， Chen B Y， Chi C. Estimation of road friction coefficient and vehicle states by 3-DOF dynamic model and HSRI model based on information fusion[J]. Asian Journal of Control， 2018， 20（3）： 1067-1076.

［23］  黄小平， 王岩， 缪鹏程. 粒子滤波原理及应用： MATLAB仿真[M]. 北京： 电子工业出版社， 2017.

Huang X P， Wang Y， Miao P C. Particle filter principle and application： MATLAB simulation[M]. Beijing： Publishing House of Electronics industry， 2017. （in Chinese）

［24］  Panicker S， Gostar A K， Bab-Haidashar A， et al. Sensor control for selective object tracking using labeled multi-bernoulli filter[C]//2018 21st International Conference on Information Fusion （FUSION）， July 10-13， 2018， Cambridge， UK. IEEE， 2018： 2218-2224.

［25］  Li X R， Bar-Shalom Y. Design of an interacting multiple model algorithm for air traffic control tracking[J]. IEEE Transactions on Control Systems Technology， 1993， 1（3）： 186-194.

（编辑  罗敏）