基于改进粒子滤波的半挂汽车列车状态估计

State Estimation of Tractor Semi-trailer Based on Improved Particle Filter

摘要

针对半挂汽车列车行驶过程中部分关键动力学状态不可测以及传感器测量值受发动机振动噪声等随机因素干扰的问题，提出了一种改进的粒子滤波方法对半挂汽车列车行驶过程中的动力学状态进行实时估计.首先建立了半挂汽车列车的17自由度动力学模型，通过将粒子滤波原理和自适应遗传算法结合增强粒子多样性，设计了分段提议分布函数，采用系统重采样方法抑制粒子贫化现象，实现对半挂汽车列车的纵向速度、侧向速度、横摆角速度等动力学状态的实时精确估计.搭建硬件在环仿真试验平台对算法进行不同工况下的试验验证.试验结果表明：与无迹粒子滤波算法相比，提出的改进粒子滤波算法在理想环境和随机噪声环境下均能够实现整车的状态估计，具有较高的估计精度.

Abstract

Aiming at the problem that some key dynamical states of tractor semi-trailer cannot be measured and the values of sensors are interfered by random factors such as engine vibration noise， an improved particle filter is proposed to estimate the dynamical states of the driving tractor semi-trailer in real-time. This paper establishes a 17 degrees of freedom dynamical model of tractor semi-trailer first. By combining the particle filter principle and the adaptive genetic algorithm to enhance the particle diversity， the piecewise proposal distribution function is designed， and the systematic resampling method is used to suppress the particle regression. The in-time and accurate estimation of longitudinal speed， lateral speed， yaw rate， and other states of tractor semi-trailer was realized. A hardware-in-the-loop （HIL） simulation test platform was built to verify the algorithm under different conditions. The testing results show that compared with the unscented particle filter algorithm， the improved particle filter algorithm proposed in this paper can realize the state estimation of the whole vehicle under both ideal and random noise environments， and has higher estimation accuracy.

Graphical abstract

关键词

车辆工程 / 状态估计 / 粒子滤波 / 半挂汽车列车

半挂汽车列车由于能够在运输驿站快速更换挂车， 避免了货物装卸的等待时间， 已成为道路运输的主力车型. 相比于四轮车辆， 半挂汽车列车的车身更长， 装载后重心位置更高， 因此存在更多的失稳形式及更大的失稳可能性^［1-2］. 半挂汽车列车的整车稳定性控制已成为目前车辆领域的研究热点， 而行驶过程中的动力学状态实时获取是影响稳定性控制效果的关键因素. 鉴于某些关键状态难以通过现有的车载传感器进行测量， 研究人员已经开始采用光学传感器来应对这一挑战， 例如利用摄像机来测量半挂汽车列车的铰接角度等^［3］. 然而， 该类传感器显著提升了整车的制造成本^［4］， 更实用的解决方案是通过可测量得到的部分状态信息结合数字滤波技术对不可测状态进行估计.

目前估计方法主要分为试验法和模型法^［5］. 试验法包括参数回归方法、神经网络、基于规则的方法等. 人工神经网络已被应用于估算车辆的侧倾角度^［6-7］. 非线性最小二乘法作为参数回归方法已被用于半挂汽车列车质量估计^［8-9］. 试验法虽然在实现手段上较为简便， 但是需要进行大量的预试验，获得足够的数据集后方能保证估计精度. 此外，大量的数据集对于整车控制器的存储空间也提出了较高要求^［10］. 与试验法相比， 模型法无须预先标定，对整车控制器的存储空间需求也较低. 在众多试验法中， 基于卡尔曼滤波原理的估计方法应用最为广泛. 标准卡尔曼滤波要求系统状态方程是线性的， 无法直接应用于车辆系统.扩展卡尔曼滤波理论通过雅可比矩阵的形式将非线性系统线性化来实现对非线性系统的估计，但是雅可比矩阵的复杂程度由系统状态空间维度决定，当系统状态空间维度较高时， 其较为复杂，且线性化形式的系统与原始非线性系统误差较大，这对于估计的精度和速度有一定的不良影响^［11-12］.无迹卡尔曼滤波（unscented Kalman filter， UKF）和容积卡尔曼滤波器（cubature Kalman filter， CKF）能够应对高维非线性系统的状态估计问题. UKF基于无迹变换生成样本点， 以此对非线性函数的概率密度分布进行近似^［13-15］. CKF则利用球面径向规则求解后验期望的积分近似^［16-17］. 两种方法均运用在车辆状态估计问题中. 但是UKF和CKF要求系统满足高斯假设， 而半挂汽车列车行驶过程中的发动机振动、车身振动等因素在传感器信号中引发的噪声属于随机噪声， 无法通过UKF或CKF滤除. 针对此问题， 粒子滤波（particle filter， PF）将蒙特卡罗方法与贝叶斯估计相结合， 通过粒子群逼近被估计状态的后验分布， 对于受随机噪声影响的非线性系统具有更好的估计效果^［18］. 标准粒子滤波算法在若干次迭代后会出现大量粒子权重过低， 只有少量粒子具有非零权重的问题， 研究人员称之为粒子贫化现象. 无迹粒子滤波（unscented particle filter， UPF）通过UKF得到建议密度函数来逼近后验分布， 以达到降低粒子退化的目的， 但是建议密度函数的准确性仍然受到系统噪声不确定性的影响， 当UPF应用于车辆时，随机噪声的影响导致其估计效果不一定优于传统PF^［19-21］.

目前已有通过智能优化算法的手段避免粒子贫化现象的先例^［22］，本文提出一种IPF（improved particle filter）状态估计算法，使用自适应遗传算法对粒子进行自适应交叉、变异、遗传操作， 以保障粒子多样性，通过设计分段提议分布函数加强粒子权值区分，并采用系统重采样方法避免对权重较低粒子的盲目丢弃，以此形成改进的粒子滤波算法， 并将其应用于解决半挂汽车列车的状态实时估计问题中. 本文搭建硬件在环仿真试验台架对算法进行验证， 与作对照的UPF算法相比， 理想环境及随机噪声环境下的改进粒子滤波算法均能实现对半挂汽车列车行驶过程中的状态实时估计， 并保证一定的估计精度.

1 半挂汽车列车动力学建模

针对三轴牵引车与三轴挂车铰接的六轴半挂汽车列车， 在综合考虑模型精度与计算量的基础上， 本文搭建如图1所示的17自由度整车动力学模型， 17个自由度分别为牵引车的纵向、侧向、横摆自由度， 挂车的横摆自由度，牵引车前轮的转向自由度以及12个轮胎各自的转动自由度.图1中：O₁、O₂分别为牵引车及挂车质心位置，车辆坐标系分别为X₁O₁Y₁、X₂O₂Y₂， F_xi 和F_yi 为各轮胎受到的纵向力与侧向力， 轮胎下标i为1，2，…，12，在图1中标注出了轮胎标号及各物理量的方向.

1.1 牵引车动力学方程

牵引车的纵向、侧向、横摆运动可由如下公式描述：

\begin{array}{l}{m}_{\mathrm{1}}\left({\dot{v}}_{x\mathrm{1}}-{\gamma }_{\mathrm{1}}{v}_{y\mathrm{1}}\right)=\left(F_{x\mathrm{1}}+F_{x\mathrm{2}}\right)\mathrm{c}\mathrm{o}\mathrm{s}\delta +{\displaystyle \sum _{i=\mathrm{3}}^{\mathrm{6}}}{F}_{xi}-\\ \left(F_{y\mathrm{1}}+F_{y\mathrm{2}}\right)\mathrm{s}\mathrm{i}\mathrm{n}\delta +F_{\mathrm{j}x}\end{array}（1）

\begin{array}{l}{m}_{\mathrm{1}}\left({\dot{v}}_{y\mathrm{1}}+{\gamma }_{\mathrm{1}}{v}_{x\mathrm{1}}\right)=\left(F_{y\mathrm{1}}+F_{y\mathrm{2}}\right)\mathrm{c}\mathrm{o}\mathrm{s}\delta +{\displaystyle \sum _{i=\mathrm{3}}^{\mathrm{6}}}{F}_{yi}+\\ \left(F_{x\mathrm{1}}+F_{x\mathrm{2}}\right)\mathrm{s}\mathrm{i}\mathrm{n}\delta +F_{\mathrm{j}y}\end{array}（2）

\begin{array}{l}{I}_{\mathrm{z}\mathrm{1}}{\dot{\gamma }}_{\mathrm{1}}=\left[\left(F_{x\mathrm{1}}+F_{x\mathrm{2}}\right)\mathrm{s}\mathrm{i}\mathrm{n}\delta +\left(F_{y\mathrm{1}}+F_{y\mathrm{2}}\right)\mathrm{c}\mathrm{o}\mathrm{s}\delta \right]a_{\mathrm{1}}+\\ \left[\left(F_{x\mathrm{2}}-F_{x\mathrm{1}}\right)\mathrm{c}\mathrm{o}\mathrm{s}\delta +\left(F_{y\mathrm{1}}-F_{y\mathrm{2}}\right)\mathrm{s}\mathrm{i}\mathrm{n}\delta \right]\frac{t_{\mathrm{f}\mathrm{1}}}{\mathrm{2}}+\\ \left(F_{x\mathrm{4}}-F_{x\mathrm{3}}+F_{x\mathrm{6}}-F_{x\mathrm{5}}\right)\frac{t_{\mathrm{r}\mathrm{1}}}{\mathrm{2}}-F_{\mathrm{j}y}{l}_{\mathrm{p}}-\\ \left(F_{y\mathrm{3}}+F_{y\mathrm{4}}\right)b_{\mathrm{2}}-\left(F_{y\mathrm{5}}+F_{y\mathrm{6}}\right)b_{\mathrm{3}}\end{array}（3）

式中：v_x1、v_y1和γ₁分别为牵引车纵向、侧向速度和横摆角速度；δ为前轮转角；F_jy、F_jx 分别为挂车对牵引车铰接力在侧向、纵向的分力；m₁为牵引车总质量；a₁为牵引车质心到前轴的距离； b₂为牵引车质心到牵引车2轴的距离；b₃为牵引车质心到牵引车3轴的距离；t_f1为牵引车前轴轮距； t_r1为牵引车后轴轮距； I_z1为牵引车总转动惯量；l_p为牵引车质心到铰接点距离.

1.2 挂车动力学方程

挂车的纵向、侧向、横摆运动可由如下公式描述：

m_{\mathrm{2}}\left({\dot{v}}_{x\mathrm{2}}-{\gamma }_{\mathrm{2}}{v}_{y\mathrm{2}}\right)={\displaystyle \sum _{i=\mathrm{7}}^{\mathrm{12}}}{F}_{xi}-F_{\mathrm{j}x}^{\mathrm{\text{'}}}（4）

m_{\mathrm{2}}\left({\dot{v}}_{y\mathrm{2}}+{\gamma }_{\mathrm{2}}{v}_{x\mathrm{2}}\right)={\displaystyle \sum _{i=\mathrm{7}}^{\mathrm{12}}}{F}_{yi}-F_{\mathrm{j}y}^{\mathrm{\text{'}}}（5）

\begin{array}{l}{I}_{\mathrm{z}\mathrm{2}}{\dot{\gamma }}_{\mathrm{2}}=\left(F_{\mathrm{j}y}\mathrm{c}\mathrm{o}\mathrm{s}\delta +F_{\mathrm{j}x}\mathrm{s}\mathrm{i}\mathrm{n}\delta \right)a_{\mathrm{2}}-\left(F_{y\mathrm{7}}+F_{y\mathrm{8}}\right)b_{\mathrm{4}}-\\ \left(F_{y\mathrm{9}}+F_{y\mathrm{10}}\right)b_{\mathrm{5}}-\left(F_{y\mathrm{11}}+F_{y\mathrm{12}}\right)b_{\mathrm{6}}+\\ \left(F_{x\mathrm{8}}-F_{x\mathrm{7}}+F_{x\mathrm{10}}-F_{x\mathrm{9}}+F_{x\mathrm{12}}-F_{x\mathrm{11}}\right)\frac{t_{\mathrm{r}\mathrm{2}}}{\mathrm{2}}\end{array}（6）

式中：v_x2、v_y2和γ₂分别为挂车纵向、侧向速度和横摆角速度；F_{\mathrm{j}x}^{\text{'}}、F_{\mathrm{j}y}^{\text{'}}分别为牵引车对牵引车铰接力在侧向、纵向的分力；a₂为挂车质心到铰接点的距离；b₄为挂车质心到挂车1轴的距离；b₅为挂车质心到挂车2轴的距离；b₆为挂车质心到挂车3轴的距离； t_r2为挂车后轴轮距；m₂为挂车总质量；I_z2为挂车总转动惯量.

1.3 铰接点动力学方程

牵引车与挂车之间通过鞍座形成铰接， 两者的纵向运动、侧向运动、横摆运动在铰接点处存在一定的约束关系， 约束方程表达如下：

v_{x\mathrm{2}}=v_{x\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta -\left(v_{y\mathrm{1}}-l_{\mathrm{p}}{\gamma }_{\mathrm{1}}\right)\mathrm{s}\mathrm{i}\mathrm{n}\theta（7）

v_{y\mathrm{2}}=v_{x\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta +\left(v_{y\mathrm{1}}-l_{\mathrm{p}}{\gamma }_{\mathrm{1}}\right)\mathrm{c}\mathrm{o}\mathrm{s}\theta -a_{\mathrm{2}}{\gamma }_{\mathrm{2}}（8）

\dot{\theta }={\gamma }_{\mathrm{1}}-{\gamma }_{\mathrm{2}}（9）

\left\{\begin{array}{c}{F}_{\mathrm{j}x}=F_{\mathrm{j}x}^{\text{'}}\mathrm{c}\mathrm{o}\mathrm{s}\theta +F_{\mathrm{j}y}^{\text{'}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ F_{\mathrm{j}y}=F_{\mathrm{j}y}^{\text{'}}\mathrm{c}\mathrm{o}\mathrm{s}\theta -F_{\mathrm{j}x}^{\text{'}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \end{array}\right.（10）

式中：θ为铰接角.

对式（7）、式（8）求导可得挂车纵侧向加速度：

\begin{array}{l}{\dot{v}}_{x\mathrm{2}}={\dot{v}}_{x\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta -v_{x\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \dot{\theta }-{\dot{v}}_{y\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta -\\ v_{y\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta \dot{\theta }+l_{\mathrm{p}}{\dot{\gamma }}_{\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta +l_{\mathrm{p}}{\gamma }_{\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta \dot{\theta }\end{array}（11）

\begin{array}{l}{\dot{v}}_{y\mathrm{2}}={\dot{v}}_{x\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta +v_{x\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta \dot{\theta }+{\dot{v}}_{y\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta -\\ v_{y\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \dot{\theta }-l_{\mathrm{p}}{\dot{\gamma }}_{\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}\theta +l_{\mathrm{p}}{\gamma }_{\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \dot{\theta }-a_{\mathrm{2}}{\dot{\gamma }}_{\mathrm{2}}\end{array}（12）

1.4 轮胎动力学方程

轮胎侧向力与侧偏角的关系存在线性与非线性区域， 侧偏角小于5°时为线性， 大于5°时为非线性. 魔术公式对不同侧偏角范围内轮胎侧向力的拟合精度较好^［23］. 本文使用魔术公式对轮胎的纵向力与侧向力进行计算， 非联合滑移工况下的轮胎纵向力与侧向力如下：

\left\{\begin{array}{c}\begin{array}{l}{F}_{x\mathrm{0}i}=D_{\mathrm{z}i}\mathrm{s}\mathrm{i}\mathrm{n}\left\{C_i\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left[\left(\mathrm{1}-E_i\right)B_i{s}_i+\right.\right.\\ \left.\left.E_i\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(B_i{s}_i\right)\right]\right\}\end{array}\\ \begin{array}{l}{F}_{y\mathrm{0}i}=D_{\mathrm{z}i}\mathrm{s}\mathrm{i}\mathrm{n}\left\{C_i\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left[\left(\mathrm{1}-E_i\right)B_i{\alpha }_i+\right.\right.\\ \left.\left.E_i\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(B_i{\alpha }_i\right)\right]\right\}\end{array}\end{array}\right.（13）

式中：i为各轮胎对应编号；B_i 为各轮胎刚度因子；C_i 为各轮胎的形状因子；D_zi 为各轮胎的峰值轮胎力因子；E_i 为各轮胎的曲率因子，上述因子通过试验数据拟合魔术公式即可得出； s_i 为轮胎滑移率；α_i 为轮胎侧偏角. 12轮的滑移率依次定义为：

s_i=\left\{\begin{array}{c}\frac{R_i{\omega }_i-v_{x\mathrm{1}}\pm {\gamma }_{\mathrm{1}}{t}_{\mathrm{f}\mathrm{1}}/\mathrm{2}}{\mathrm{m}\mathrm{a}\mathrm{x}\left(R_i{\omega }_i,v_{x\mathrm{1}}\mp {\gamma }_{\mathrm{1}}{t}_{\mathrm{f}\mathrm{1}}/\mathrm{2}\right)},i=\mathrm{1,2}\\ \frac{R_i{\omega }_i-v_{x\mathrm{1}}\pm {\gamma }_{\mathrm{1}}{t}_{\mathrm{r}\mathrm{1}}/\mathrm{2}}{\mathrm{m}\mathrm{a}\mathrm{x}\left(R_i{\omega }_i,v_{x\mathrm{1}}\mp {\gamma }_{\mathrm{1}}{t}_{\mathrm{r}\mathrm{1}}/\mathrm{2}\right)},i=\mathrm{3,4},\mathrm{5,6}\\ \frac{R_i{\omega }_i-v_{x\mathrm{2}}\pm {\gamma }_{\mathrm{2}}{t}_{\mathrm{r}\mathrm{2}}/\mathrm{2}}{\mathrm{m}\mathrm{a}\mathrm{x}\left(R_i{\omega }_i,v_{x\mathrm{2}}\mp {\gamma }_{\mathrm{2}}{t}_{\mathrm{r}\mathrm{2}}/\mathrm{2}\right)},i=\mathrm{7,8},\cdots ,\mathrm{12}\end{array}\right.（14）

12轮的侧偏角依次定义为：

{\alpha }_i=\left\{\begin{array}{l}-\delta +\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v_{y\mathrm{1}}+{\gamma }_{\mathrm{1}}{a}_{\mathrm{1}}}{v_{x\mathrm{1}}\mp {\gamma }_{\mathrm{1}}{t}_{\mathrm{f}\mathrm{1}}/\mathrm{2}}\right),i=\mathrm{1,2}\\ \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v_{y\mathrm{1}}-{\gamma }_{\mathrm{1}}{b}_{\mathrm{2}}}{v_{x\mathrm{1}}\mp {\gamma }_{\mathrm{1}}{t}_{\mathrm{r}\mathrm{1}}/\mathrm{2}}\right),i=\mathrm{3,4}\\ \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v_{y\mathrm{1}}-{\gamma }_{\mathrm{1}}{b}_{\mathrm{3}}}{v_{x\mathrm{1}}\mp {\gamma }_{\mathrm{1}}{t}_{\mathrm{r}\mathrm{1}}/\mathrm{2}}\right),i=\mathrm{5,6}\\ \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v_{y\mathrm{2}}-{\gamma }_{\mathrm{2}}{b}_{\mathrm{4}}}{v_{x\mathrm{2}}\mp {\gamma }_{\mathrm{2}}{t}_{\mathrm{r}\mathrm{2}}/\mathrm{2}}\right),i=\mathrm{7,8}\\ \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v_{y\mathrm{2}}-{\gamma }_{\mathrm{2}}{b}_{\mathrm{5}}}{v_{x\mathrm{2}}\mp {\gamma }_{\mathrm{2}}{t}_{\mathrm{r}\mathrm{2}}/\mathrm{2}}\right),i=\mathrm{9,10}\\ \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v_{y\mathrm{2}}-{\gamma }_{\mathrm{2}}{b}_{\mathrm{6}}}{v_{x\mathrm{2}}\mp {\gamma }_{\mathrm{2}}{t}_{\mathrm{r}\mathrm{2}}/\mathrm{2}}\right),i=\mathrm{11,12}\end{array}\right.（15）

轮胎的动力学方程为：

I_i{\dot{\omega }}_i=T_{\mathrm{f}i}-T_{\mathrm{b}i}-F_{xi}{R}_i,i=\mathrm{1,2},\cdots ,\mathrm{12}（16）

式中： I_i、R_i、ω_i、T_fi、T_bi 分别为对应下标车轮i的转动惯量、滚动半径、角速度、驱动力矩和制动力矩. 在联合滑移工况下， 轮胎的纵向力和侧向力方程为：

\left\{\begin{array}{l}{F}_{xi}=\frac{\left|s_i\right|}{\sqrt[]{s_{\mathrm{i}}^{\mathrm{2}}+{\left(\mathrm{t}\mathrm{a}\mathrm{n}{\alpha }_i\right)}^{\mathrm{2}}}}{F}_{x\mathrm{0}i}\\ F_{yi}=\frac{\left|\mathrm{t}\mathrm{a}\mathrm{n}{\alpha }_i\right|}{\sqrt[]{s_{\mathrm{i}}^{\mathrm{2}}+{\left(\mathrm{t}\mathrm{a}\mathrm{n}{\alpha }_i\right)}^{\mathrm{2}}}}{F}_{y\mathrm{0}i}\end{array}\right.,i=\mathrm{1,2},\cdots ,\mathrm{12}（17）

2 IPF状态估计算法

基于上一章建立的17自由度动力学模型的半挂汽车列车状态估计算法原理如图2所示. 半挂汽车列车目前通用的传感器包括轮速传感器和分别安装在牵引车和挂车上的惯性导航传感器，用于测量各轮胎转速、牵引车和挂车的纵向加速度、侧向加速度、横摆角速度， 结合上述传感器测量值对整车行驶状态进行实时估计.

标准粒子滤波算法根据重要性概率密度函数对上一时刻估计的系统状态进行随机抽样， 在得到附带对应权值的随机样本后， 根据状态方程对系统观测变量进行预测，然后根据当前时刻的实际观测结果对权值进行调节，使粒子状态获得更新并逼近真实状态的后验分布，最终将粒子的加权均值作为估计结果进行输出.与其他的估计方法相比， 粒子滤波估计不要求状态转移函数及观测函数是线性的， 也不需要噪声满足正态分布，但是其迭代过程中存在着粒子退化的问题.本文使用自适应遗传算法对粒子群进行多样化操作，以提升粒子多样性， 抑制粒子退化趋势，此外，采用系统重采样方法进一步增加粒子多样性.最终得到的改进粒子滤波算法流程如图3所示.

具体的实施方法如下：

1）初始化和粒子采样， 在初始采样时刻依据先验分布选择粒子.

{\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},\mathrm{0}}={\mathit{x}}_{\mathrm{s},\mathrm{0}}+\mathit{\zeta }\times \left(\frac{i-\mathrm{1}}{N}-\mathrm{0.5}\right),i=\mathrm{1,2},\cdots ,N+\mathrm{1}（18）

式中：{\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},\mathrm{0}}为车辆状态估计过程中的初始状态粒子群；{\mathit{x}}_{\mathrm{s},\mathrm{0}}为状态初始向量；N为粒子数； ζ 为根据经验设定的各个状态变量的粒子散布范围.

2）权重更新. 执行更新过程重要性评价， 通过状态方程对车辆状态进行预测：

{\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},k}=f\left({\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},k-\mathrm{1}},{\mathit{u}}_{k-\mathrm{1}}\right)（19）

式中：f为半挂汽车列车的状态函数；{\mathit{u}}_{k-\mathrm{1}}为上一采样时刻输入. 分别更新各个粒子的均值和标准差：

\left\{\begin{array}{l}{\overline{\mathit{x}}}_{\mathrm{s}}\left(i\right)={\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}\times {\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},k-\mathrm{1}}^{\mathrm{T}}\\ {\sigma }_{\mathrm{s}}\left(i\right)=\sqrt[]{\frac{\mathrm{1}}{p}{\left[{\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}-{\overline{\mathit{x}}}_{\mathrm{s}}\left(i\right)\right]}^{\mathrm{2}}\times {\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},k-\mathrm{1}}^{\mathrm{T}}}\end{array}\right.（20）

式中：{\overline{\mathit{x}}}_{\mathrm{s}}(i)和{\sigma }_{\mathrm{s}}(i)分别为第i个粒子群的加权均值和标准差；{\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},k-\mathrm{1}}^{}为上一采样时刻粒子群权重. 已有文献通过设计提议分布函数的方法来提升粒子滤波性能^［24］，借鉴该思想，本文通过分段提议分布函数区分不同权重粒子之间的差距以进一步提升滤波效果，如图4所示，粒子越接近粒子群期望值时， 其权重越大.

基于此， 对粒子的更新过程权重及其归一化权重计算如下：

（21）

分段提议分布函数中， 参数k₁=0.4，k₂=2.5，k₃=3，w₁=0.7，w₂=0.5，w₃=0.35，w₄=0.05.

更新权重过程中， 如图3所示， 阈值设定为初始粒子散布范围的k_s倍， 该阈值过大会导致粒子过快贫化， 而过小则会导致算法无法收敛， 本文阈值取0.02. 当粒子群的标准差低于设定阈值时， 在粒子群期望不变的前提下通过自适应遗传算法对其进行选择、交叉、变异操作，三种操作概率分别为p_sel， p_co， p_mu. 选择操作如下：

{\tilde{\mathit{x}}}_{\mathrm{s}\mathrm{e}\mathrm{l}}={\mathit{x}}_{\mathrm{s}\mathrm{e}\mathrm{l}}+k_{\mathrm{s}\mathrm{e}\mathrm{l}}\mathit{\zeta }（22）

交叉操作如下：

\left\{\begin{array}{l}{\tilde{\mathit{x}}}_{\mathrm{m}}=a{\mathit{x}}_{\mathrm{m}}+b{\mathit{x}}_{\mathrm{f}}+k_{\mathrm{c}\mathrm{o}}\mathit{\zeta }\\ {\tilde{\mathit{x}}}_{\mathrm{f}}=a{\mathit{x}}_{\mathrm{f}}+b{\mathit{x}}_{\mathrm{m}}+k_{\mathrm{c}\mathrm{o}}\mathit{\zeta }\\ b=\mathrm{1}-a\end{array}\right.（23）

式中： x_f及 x_m分别为交叉公粒子及交叉母粒子， 利用交叉操作后的粒子计算后验概率分布， 若大于p（z_k | x_m，k ）及p（z_k | x_f，k ）则选择该生成粒子， 否则选择后验概率分布较大的原始粒子.

变异操作如下：

{\tilde{\mathit{x}}}_{\mathrm{m}\mathrm{u}}={\mathit{x}}_{\mathrm{m}\mathrm{u}}+k_{\mathrm{m}\mathrm{u}}\mathit{\zeta }（24）

式中： x_mu为变异粒子， 利用变异操作后的粒子计算后验概率分布， 若大于p（z_k | x_mu）则选择该生成粒子， 否则选择原始粒子. 上述过程中， k_sel=0.5， k_co=0.5， k_mu=1.5. 自适应遗传算法通过粒子的权重调节交叉操作和遗传操作的概率：

（25）

（26）

式中：w_co，1 及w_co，2分别为参与交叉操作的粒子权重；w_mu为参与遗传操作的粒子权重；w_max、 w_min及w_avg分别为最大权值、最小权值及权值均值.

然后进行观测过程重要性评价，将粒子代入系统观测方程后求得观测粒子群，并以传感器信号作为参考对观测粒子群进行重要性评价，求解观测过程的粒子群权重{\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},\mathrm{o}\mathrm{b}\mathrm{s}}，最终对粒子群权重进行归一化， 如下所示：

{\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}=\frac{{\mathit{w}}_{\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}{\mathit{w}}_{\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},\mathrm{o}\mathrm{b}\mathrm{s},i}}{\sum {\mathit{w}}_{\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}{\mathit{w}}_{\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},\mathrm{o}\mathrm{b}\mathrm{s},i}}（27）

式中：{\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}为粒子群归一化后的权重.

3）输出和重采样.

得到权重后输出滤波结果：

{\mathit{x}}_{\mathrm{s},\mathrm{o}\mathrm{u}\mathrm{t}\mathrm{p}\mathrm{u}\mathrm{t}}={\displaystyle \sum _{i=\mathrm{1}}^p}{\mathit{x}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}{\mathit{w}}_{\mathrm{s},\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}（28）

通过粒子群有效粒子数判断其是否已退化到需要进行重采样， 有效粒子数计算方法如下：

N_{\mathrm{e}\mathrm{f}\mathrm{f}}=\frac{\mathrm{1}}{{\displaystyle \sum _{i=\mathrm{1}}^N}{\left({\mathit{w}}_{\mathrm{s}\mathrm{w}\mathrm{a}\mathrm{r}\mathrm{m},i}\right)}^{\mathrm{2}}}（29）

当N_eff<2N/3时，认为粒子群发生退化，使用系统重采样方法，其原理如图5所示，其伪代码如图6所示.

系统重采样方法在按照权重划分的区间中使用随机数进行分层后采样. 其能够确保采样覆盖所有权重区以及大权重区的更高采样频率.

3 硬件在环仿真试验验证

为验证本文所提出IPF估计方法的有效性， 本文搭建了半挂汽车列车硬件在环试验平台， 如图7所示， 在LabVIEW环境下编写IPF估计算法并植入快速控制原型NI cRIO-9038中. NI cRIO-9038通过CAN信号方式采集NI PXI中牵引车及挂车的加速度等信息，并通过计算机实时显示控制器前面板以及采集得到的信号.NI PXI中运行Trucksim半挂汽车列车模型，整车关键参数如表1所示.

基于所搭建台架仿真试验， 对本文所提出的IPF估计效果进行验证， 硬件在环仿真试验的算法采样时间为0.01 s， 设置UPF作为对照， 采用均方根误差（root mean square error， RMSE）对IPF和UPF的估计精度进行表征.

3.1 理想环境下估计效果验证

验证IPF在理想行驶环境下对半挂汽车列车进行实时状态估计的有效性. 双移线滑行工况下， 半挂汽车列车初始速度为72 km/h， 附着系数为0.8. 两种工况的仿真结果如图8所示.

理想条件仿真的RMSE如表2所示， 综合各个状态估计曲线可看出， 在无噪声的理想环境下， 设计的IPF、UPF均能够完成状态估计.

3.2 随机噪声干扰条件下估计效果验证

验证IPF在随机噪声环境下的估计效果， 由于轮速传感器和前轮转角传感器在工程实践中往往已在传感电路中采用滤波手段以保证其信号可用性， 因此本文只在牵引车惯性导航器及挂车惯性导航器中加入随机噪声信号对车辆的实际行驶环境进行模拟，以此进行硬件在环仿真试验.具体地，在横纵向加速度信号中加入1 Hz幅值0.5 m/s²的车辆低频振动模拟噪声、50 Hz幅值0.25 m/s²的车辆中频振动模拟噪声、1 000 Hz幅值0.02 m/s²的高频环境模拟噪声；在横摆角速度信号中加入1 Hz幅值0.4 rad/s的车辆低频振动模拟噪声、50 Hz幅值0.3 rad/s的车辆中频振动模拟噪声、1 000 Hz幅值0.002 rad/s的高频环境模拟噪声.双移线滑行工况下的仿真结果如图9所示，估计的RMSE如表3所示.

由图9及表3可看出， IPF能够保持状态及参数估计结果收敛较好. UPF的状态估计结果发散， 而IPF估计的整体精度较高. 分析产生误差的原因是UPF通过无迹卡尔曼滤波获得状态的后验分布， 当传感器信号包含较大的随机噪声时， 由于不满足高斯分布， 其无法发挥原有的滤波性能. 随机噪声环境下的试验结果验证了IPF在随机噪声环境下的鲁棒性和有效性.

4 结 论

1）基于半挂汽车列车及魔术公式轮胎模型搭建了半挂汽车列车的17自由度动力学模型，采用目前通用的车载传感器参数作为测量值，实现对牵引车纵向车速、侧向车速、横摆角、横摆角速度、铰接角、铰接角速度的估计， 具有成本低、易于实施的特点， 为解决半挂汽车列车的状态估计问题提供一种思路.

2）基于自适应遗传算法， 结合粒子滤波提升粒子多样性， 通过设计分段提议分布函数提升粒子权值差距， 使用系统重采样进一步抑制粒子贫化趋势， 综合上述手段对标准粒子滤波进行改进并搭建硬件在环仿真试验平台进行验证. 硬件在环仿真试验表明了基于IPF算法的半挂汽车列车状态估计在理想环境和环境随机噪声环境下均能保持较高的估计精度.

参考文献

[1] DING N G， SHI X B， ZHANG Y P，et al ．Analysis of bifurcation and stability for a tractor semi-trailer in planar motion［J］．Vehicle System Dynamics， 2014， 52（12）： 1729-1751．

[2] MENG F Y， SHI S M， BAI M H，et al ．Dissipation of energy analysis approach for vehicle plane motion stability［J］．Vehicle System Dynamics， 2022， 60（12）： 4035-4058．

[3] DE SAXE C， CEBON D ．Camera-based articulation angle sensing for heavy goods vehicles［J］. IEEE Transactions on Vehicular Technology， 2021， 70（8）： 7522-7535．

[4] 邢德鑫， 魏民祥， 赵万忠， 等 ．基于自适应容积粒子滤波的车辆状态估计［J］．南京航空航天大学学报， 2020，52（3）：445-453．

[5] XING D X， WEI M X， ZHAO W Z，et al ．Vehicle state estimation based on adaptive cubature particle filtering［J］．Journal of Nanjing University of Aeronautics & Astronautics，2020，52（3）：445-453．（in Chinese）

[6] HABIBNEJAD KORAYEM A， KHAJEPOUR A， FIDAN B. A review on vehicle-trailer state and parameter estimation［J］．IEEE Transactions on Intelligent Transportation Systems，2022， 23（7）： 5993-6010．

[7] GARCÍA GUZMÁN J， PRIETO GONZÁLEZ L， PAJARES REDONDO J，et al ．Real-time vehicle roll angle estimation based on neural networks in IoT low-cost devices［J］．Sensors，2018， 18（7）： 2188．

[8] CHU Z Z， ZHU D Q， YANG S X ．Observer-based adaptive neural network trajectory tracking control for remotely operated vehicle［J］．IEEE Transactions on Neural Networks and Learning Systems， 2017， 28（7）： 1633-1645．

[9] KOBER W， HIRSCHBERG W. On-board payload identification for commercial vehicles［C］//2006 IEEE International Conference on Mechatronics. July 3-5，2006，Budapest，Hungary. IEEE，2006： 144-149．

[10] KORAYEM A H， KHAJEPOUR A， FIDAN B ．Trailer mass estimation using system model-based and machine learning approaches［J］．IEEE Transactions on Vehicular Technology，2020， 69（11）： 12536-12546．

[11] PETSCHNIGG C， PILZ J ．Uncertainty estimation in deep neural networks for point cloud segmentation in factory planning［J］．Modelling， 2021， 2（1）： 1-17．

[12] AHMADI JEYED H， GHAFFARI A ．Nonlinear estimator design based on extended Kalman filter approach for state estimation of articulated heavy vehicle［J］．Proceedings of the Institution of Mechanical Engineers，Part K： Journal of Multi-body Dynamics，2019， 233（2）： 254-265．

[13] PÀMIES-VILÀ R， FONT-LLAGUNES J M， LUGRÍS U，et al ．Parameter identification method for a three-dimensional foot-ground contact model［J］. Mechanism and Machine Theory， 2014，75： 107-116．

[14] MORRISON G， CEBON D. Sideslip estimation for articulated heavy vehicles at the limits of adhesion［J］. Vehicle System Dynamics， 2016， 54（11）： 1601-1628．

[15] 周兵， 李涛， 吴晓建， 等. 基于双自适应无迹卡尔曼滤波的 半挂车状态估计 ［J］. 湖南大学学报（自然科学版）， 2022， 49（2）： 63-73.

[16] ZHOU B， LI T， WU X J， et al. Semi-trailer state estimation based on double adaptive unscented kalman filter ［J］. Journal of Hunan University（Natural Sciences）， 2022， 49（2）： 63-73. （in Chinese）

[17] 周兵， 田晨， 宋义彤， 等 ．基于路面附着系数估计的AFS控制策略研究［J］．湖南大学学报（自然科学版）， 2017， 44（4）：16-22．

[18] ZHOU B， TIAN C， SONG Y T，et al ．Control strategy of AFS based on estimation of tire-road friction coefficient［J］．Journal of Hunan University （Natural Sciences），2017，44（4）：16-22．（in Chinese）

[19] YE J， ZHANG Z， JIN J， et al. Estimation of tire-road friction coefficient with adaptive tire stiffness based on RC-SCKF ［J］. Nonlinear Dynamics， 2023， 112： 945-960.

[20] LIU Y M， ZHANG R Y， SHI P C， et al. Distributed electric vehicle state parameter estimation based on the ASO-SRGHCKF algorithm［J］. IEEE Sensors Journal，2022，22（19）：18780-18792．

[21] SUN S X， ZHANG N， WALKER P，et al ．Intelligent estimation for electric vehicle mass with unknown uncertainties based on particle filter［J］. IET Intelligent Transport Systems，2020，14（5）：463-467．

[22] LI T C， CORCHADO J M， BAJO J，et al ．Effectiveness of Bayesian filters：an information fusion perspective［J］. Information Sciences， 2016， 329： 670-689．

[23] 陈昱衡， 吴鸿云， 边有钢. 基于AUPF算法的水下履带车动力学参数估计 ［J］. 湖南大学学报（自然科学版）， 2022， 49（8）： 29-35.

[24] CHEN Y H， WU H Y， BIAN Y G. Estimation of motion parameters of a underwater track mining vehicle based on adaptive unscented particle filter algorithm［J］. Journal of Hunan University（Natural Sciences）， 2022， 49（8）： 29-35. （in Chinese）

[25] 龙云泽， 韦韬， 封进， 等. 四轮驱动EV自适应抗差无迹粒子滤波状态估计 ［J］. 湖南大学学报（自然科学版）， 2022， 49（2）： 31-37.

[26] LONG Y Z， WEI T， FENG J， et al. Estimation of adaptive robust unscented particle filter state for four-wheel driving EV［J］. Journal of Hunan University（Natural Sciences）， 2022， 49（2）： 31-37. （in Chinese）

[27] 韩锟， 张赫 ．基于果蝇优化算法改进的粒子滤波及其在目标跟踪中的应用［J］．湖南大学学报（自然科学版）， 2018， 45（10）： 130-138．

[28] HAN K， ZHANG H ．Improved particle filter based on fruit fly optimization and its application in target tracking［J］．Journal of Hunan University （Natural Sciences），2018，45（10）：130-138．（in Chinese）

[29] BAKKER E， PACEJKA H B， LIDNER L ．A new tire model with an application in vehicle dynamics studies［C］//SAE Technical Paper Series．SAE International， 1989： 101-110．

[30] ZHU J J， WANG Z P， ZHANG L，et al ．State and parameter estimation based on a modified particle filter for an in-wheel-motor-drive electric vehicle［J］. Mechanism and Machine Theory，2019， 133： 606-624．

基金资助

国家自然科学基金资助项目（51905293）