摘要
针对基于粒子滤波算法设计的车速估计器因提议分布与实际分布不一致导致粒子退化使估计误差变大的问题,提出了一种通过修正提议分布减弱粒子退化影响的改进粒子滤波车速估计器。首先,基于车辆运动学模型和传感器特性建立系统的状态转移方程和观测方程。然后,利用传感器测量值与粒子状态值的差值设计提议分布修正项对状态转移方程进行修正,并对过程噪声做自适应处理。最后,利用CarSim-Simulink联合仿真平台在双移线工况和正弦转角输入工况下进行仿真验证。与自适应粒子滤波器相比,双移线工况下改进粒子滤波估计器产生的纵向速度估计值和侧向速度估计值的平均绝对误差分别减小了40.25%和55.71%;正弦转角输入工况下,改进粒子滤波估计器产生的纵向速度估计值和侧向速度估计值的平均绝对误差分别减小了47.00%和41.21%。
随着现代汽车电子控制技术的快速发展,车载控制系统的数量越来越多。许多控制系统在工作时需要车辆的状态信息作为控制系统的输入,例如自适应巡航控制(adaptive cruise control,ACC)、电子稳定控制(electronic stability control,ESC)、防抱死制动系统(anti-lock braking system,ABS)。因此,获取准确的车辆状态信息是车辆控制系统正常、有效工作的前
在车辆的诸多状态中,车速(包括纵向速度和侧向速度)是最重要的车辆状态之一,许多重要的车载控制系统均需要准确的车速信息才能正常工作。由于通过传感器直接获取准确车速的成本很高,因此目前车速信息主要通过状态估计来间接获取。常用的状态估计方法包括卡尔曼滤波算法、龙贝格观测器、鲁棒观测器、滑模观测器和非线性观测器等,其中卡尔曼滤波算法是应用最广泛的状态估计算
上述各种基于卡尔曼滤波的车速估计算法都在处理强非线性系统方面存在缺陷。粒子滤波算法最早出现在20世纪50年代,是一种基于蒙特卡洛法和递归贝叶斯估计的统计滤波方法,具有很强的非线性处理能
虽然上述粒子滤波算法具有很强的非线性处理能力,并且通过重采样方法在一定程度上减弱了粒子退化对估计精度的影响,但是仍然存在粒子退化导致估计精度变差的问题。具体来讲,随着粒子滤波算法迭代次数增加,只有少部分粒子具有较大的权重,大部分粒子的权重很小,造成粒子多样性匮乏,从而影响估计精
粒子滤波适用于强非线性系统,因此非常适用于处理车辆状态估计问
与卡尔曼滤波一样,粒子滤波算法利用状态空间模型对系统进行描述,该模型包括状态转移方程和观测方程。
状态转移方程:
(1) |
观测方程:
。 | (2) |
式中:表示系统时刻的状态;表示系统时刻的状态;表示系统时刻的过程噪声;表示传感器时刻的测量结果;表示传感器时刻的测量噪声;f为状态转移函数;h为观测函数。
粒子滤波的均值思想是利用粒子集合的均值作为状态的估计值。如果粒子集合的分布不能很好地“覆盖”真实值,粒子滤波器就会出现滤波发散现象。粒子集合的有效性通过粒子的权重计算和粒子集合的重采样2个步骤来保证。粒子滤波算法中粒子集合的迭代过程如
1)将表示系统时刻状态()的每一个粒子代入
2)将一步预测状态集合中的每一个粒子带入
3)假设在当前时刻(即时刻),传感器采集到唯一的观测值,则可以根据该观测值来计算每个粒子的权重:
, | (3) |
。 | (4) |
式中:表示观测值的预测值与当前时刻观测值的差值;g为粒子的权重计算函数;表示粒子的权重。需要指出的是g必须体现“观测值的预测值与传感器真实观测值的绝对偏差越小,权重越大,反之权重越小”这一规律。高斯函数是其中一种比较具有代表性的权重计算函数。
4)对粒子集合进行重采样。重采样过程体现的是粒子滤波过程中的“优胜劣汰”,即:复制权重大的粒子,淘汰权重小的粒子,并保证重采样前后的粒子数量一致。重采样原理如

图1 重采样原理
Fig. 1 Principle of re-sampling
5)将重采样后的粒子集合表示的状态记为,代入到下一个迭代循环中。
车辆的运动学模型如
, | (5) |
。 | (6) |
式中:和分别表示汽车的纵向和侧向速度;和分别表示汽车的纵向和侧向加速度;表示汽车的横摆角速度。

图2 车辆运动学模型
Fig. 2 Vehicle planar kinematics model
根据
, | (7) |
。 | (8) |
式中,表示采样时间间隔(即采样周期)。
定义系统状态矢量为,则根据
。 | (9) |
其中,
, | (10) |
, | (11) |
。 | (12) |
式中:、和分别表示3个相互独立的高斯噪声;矩阵表示5个状态变量对应的过程噪声之间的关系,在
定义传感器测量矢量为,直接得到如下观测方程:
。 | (13) |
式中:、和分别表示纵向加速度、侧向加速度和横摆角速度3个状态变量的传感器测量噪声。
粒子滤波算法效果的好坏取决于粒子集合能否准确地“覆盖”真实值,如果不能很好地“覆盖”真实值则会出现粒子退化问题,从而导致粒子滤波算法的状态估计效果变差。为了减小粒子退化的影响,通常采取以下3种措
1)增加粒子数;
2)采用能更有效抑制粒子退化的重采样技术;
3)选择合理的生成粒子的提议分布。
本研究中通过在状态转移方程中加入修正项,实现实时修正提议分布,使粒子集合更准确地“覆盖”真实值,从而减弱粒子退化问题带来的影响,提升状态估计精度。基于时刻传感器对纵向加速度、侧向加速度及横摆角速度的测量值(即、和rz)与时刻每一个粒子的纵向加速度、侧向加速度及横摆角速度值、、()的差值对提议分布进行修正。提议分布修正项如下:
。 | (14) |
。 | (15) |
对于固定参数的状态估计器,在实际应用中,不可能针对每一种工况都分别调试出适合的参数,而只能兼顾各种常用工况来调试出一组折中的参数。因此,在工况变化时,固定参数估计器往往无法达到最佳的性能。为了保证估计器能够在大多数工况下有效工作,提升估计器的鲁棒性,本研究中对估计器的状态转移方程进行自适应设计以提高其工况适应能
。 | (16) |
其中,
, | (17) |
。 | (18) |
式中:和分别为3个自定义常数,和分别为3个相互独立的标准高斯噪声。将
。 | (19) |
估计器的观测方程仍为
本研究中基于运动学模型设计了改进粒子滤波车速估计器,为了排除不同模型体系对车速估计精度的影响,选用文献[
将CarSim软件直接输出的车速数据作为真实值,下文中简称实际值;将文献[
在本工况中,设定车辆以120 km/h的车速匀速行驶,方向盘转角输入选择CarSim中的“跟随驾驶路径-双移线”选项(

图4 双移线工况方向盘输入(预瞄路径)
Fig. 4 Steering input for the double lane change maneuver

图5 双移线工况仿真结果
Fig. 5 Simulation results for the double lane change maneuver
进行50次蒙特卡洛仿真试验后,自适应粒子滤波估计器和改进粒子滤波估计器的估计误差结果如
算法 | 纵向速度 | 侧向速度 | ||||
---|---|---|---|---|---|---|
平均绝对误差 | 最大绝对误差 | 均方根误差 | 平均绝对误差 | 最大绝对误差 | 均方根误差 | |
自适应粒子滤波 | 0.007 7 | 0.019 5 | 0.752 5 | 0.021 0 | 0.053 1 | 2.114 6 |
改进粒子滤波 | 0.004 6 | 0.009 9 | 0.559 6 | 0.009 3 | 0.024 3 | 0.903 8 |
在本工况中,设定车辆以80 km/h的车速匀速行驶,方向盘转角输入为正弦波,如

图6 正弦转角输入工况方向盘转角输入
Fig. 6 Steering input for the sine-wave maneuver

图7 正弦转角输入工况仿真结果
Fig. 7 Simulation results under the sine-wave maneuver
用自适应粒子滤波估计器和改进粒子滤波估计器进行50次蒙特卡洛仿真的估计误差结果如
算法 | 纵向速度 | 侧向速度 | ||||
---|---|---|---|---|---|---|
平均绝对误差 | 最大绝对误差 | 均方根误差 | 平均绝对误差 | 最大绝对误差 | 均方根误差 | |
自适应粒子滤波 | 0.010 0 | 0.026 2 | 0.908 2 | 0.014 8 | 0.033 6 | 1.310 6 |
改进粒子滤波 | 0.005 3 | 0.015 2 | 0.509 9 | 0.008 7 | 0.019 9 | 0.617 9 |
本研究中通过修正提议分布来减弱粒子滤波算法粒子退化问题对车速估计精度的影响。仿真结果表明,加入提议分布修正项后的粒子滤波估计器能够在不同工况下准确、稳定地估计车辆的纵向速度和侧向速度。与没有修正提议分布的自适应粒子滤波估计器相比,改进后的粒子滤波估计器产生的平均绝对误差、最大绝对误差和均方根误差均较小,车速估计精度得到较大的提升。
参考文献
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. [百度学术]
余卓平, 高晓杰. 车辆行驶过程中的状态估计问题综述[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) [百度学术]
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. [百度学术]
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. [百度学术]
王仁广, 刘昭度, 齐志权, 等. 基于自适应卡尔曼滤波算法确定汽车参考车速[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) [百度学术]
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. [百度学术]
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. [百度学术]
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. [百度学术]
张志勇, 张淑芝, 黄彩霞, 等. 基于自适应扩展卡尔曼滤波的分布式驱动电动汽车状态估计[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) [百度学术]
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. [百度学术]
金贤建, 杨俊朋, 殷国栋, 等. 分布式驱动电动汽车双无迹卡尔曼滤波状态参数联合观测[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) [百度学术]
张志达, 郑玲, 吴行, 等. 基于鲁棒自适应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) [百度学术]
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. [百度学术]
李刚, 王野, 宗长富. 四轮轮毂电机电动汽车行驶状态估计[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) [百度学术]
樊东升, 李刚. 分布式驱动电动汽车行驶状态估计综述[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) [百度学术]
包瑞新, 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) [百度学术]
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. [百度学术]
姬鹏, 徐硕硕, 赵一凡, 等. 改进粒子滤波算法的车辆状态估计研究[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) [百度学术]
靳彪. 轮毂电机驱动电动汽车状态参数观测及转矩分配策略研究[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) [百度学术]
李天成, 范红旗, 孙树栋. 粒子滤波理论、方法及其在多目标跟踪中的应用[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) [百度学术]
胡士强, 敬忠良. 粒子滤波算法综述[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) [百度学术]
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. [百度学术]
黄小平, 王岩, 缪鹏程. 粒子滤波原理及应用: 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) [百度学术]
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. [百度学术]
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. [百度学术]