基于双自适应无迹卡尔曼滤波的半挂车状态估计

周兵 李涛 吴晓建 雷富强

摘要：针对半挂车辆状态估计过程中测量噪声不确定、累计误差影响严重、初值敏感等问题，提出一种适用于半挂车铰接角、车速等多个状态量估计的双自适应无迹卡尔曼滤波算法（FFUKF）.基于搭建的半挂汽车12自由度非线性动力学模型和轮胎模型，通过测量的轮速与车辆加速度等信息，首先利用模糊控制自适应调整滑移率容差，综合判断每个车轮的稳定状态，通过轮速估算出一种车速;与此同时，模糊控制自適应调整测量噪声，利用无迹卡尔曼算法，依据动力学估计出铰接角和另一种车速;然后通过卡尔曼滤波算法融合两种方法估计的结果，实现车辆的纵向、侧向速度、横摆角速度和挂车与牵引车铰接角的实时估计.最后在Simulink/TruckSim联合仿真环境下进行多工况仿真试验，验证所提出的双自适应无迹卡尔曼估计算法（FFUKF）有较强的适应性、稳定性和鲁棒性，相比普通模糊自适应无迹卡尔曼（FUKF）有更高的估计精度，能有效克服累计误差，即便在估计初始值不准和有ABS控制输入的情况，仍可以较精确地对车速和铰接角进行实时估计.

关键词：半挂车状态估计;铰接角;模糊控制;双自适应无迹卡尔曼

中图分类号：U469.6

文献标志码：A

车辆状态信息对车辆的稳定性控制、主动安全控制，乃至无人驾驶控制有十分重要的意义[1].实际应用中，轮速和加速度等车辆状态参量，可通过车载传感器直接测量，但诸如质心侧偏角和铰接角等状态量，或难以直接测量，或需昂贵传感器才能测量[2-3].为解决上述问题，利用经济性好的车载传感器、结合车辆动力学和运动学原理、运用智能算法估计出所需状态量的“软测量”技术得到重视[4].

车辆状态估计常用的算法主要有卡尔曼滤波（KF）[5-6]、扩展卡尔曼滤波（EKF）[7-9]、无迹卡尔曼滤波（UKF）[10-11]、神经网络[12-14]和深度学习[15]等.通过分析上述状态估计算法，经典卡尔曼滤波算法（KF）在车辆表现出复杂非线性时的估计精度低，具有局限性;扩展卡尔曼滤波算法（EKF）可针对性改善非线性系统的估计问题，但仍只是近似处理，其雅可比矩阵和协方差矩阵计算复杂;无迹卡尔曼滤波（UKF）能很好地适用于非线性系统，但传统的UKF算法，其过程噪声和测量噪声的统计特性预先设为定值，与实际不符，且定值化处理会降低估计精度，甚至导致滤波发散.针对过程噪声和测量噪声统计特性的问题，周聪[16]利用基于虚拟噪声补偿技术的非线性自适应滤波算法对汽车的行驶状态进行估计.文献[17]考虑在不同路面激励水平情况下，定义了不同的噪声协方差，提出了自适应无迹卡尔曼滤波状态参数估计算法，该算法能够适应不同路面的变化，具有较强的稳定性.张凤娇等[18]引入蚁群优化算法对汽车状态进行估计，降低噪声的干扰，提高了估计精度.周卫琪等[19]结合UKF算法与遗传算法对汽车状态参数进行估计，实现了噪声的自适应调节，有效提高了估计精度.张一西等[20]提出一种基于蚁狮算法的无迹卡尔曼滤波状态参数估计器，采用蚁狮优化算法（ALO）对噪声协方差矩阵进行寻优，提高估计的精度.需要说明的是，目前对于带有挂车的车辆进行状态估计的资料较少，张不扬等[21]在传统UKF基础上，加入了具有自适应性作用的遗忘因子，对重型半挂车横摆角速度和铰接角进行了估计.

以上文献对车辆状态估计做了积极的研究工作，但在以下几方面有待继续深入：

1）针对带有挂车这类复杂铰接车辆的状态估计，现有研究较少聚焦，尤其是考虑牵引-半挂车高维自由度、强非线性特性的状态估计.

2）上述估计方法都以UKF为基础，属于无限增长记忆型滤波，在进行k时刻的最优估计时，要用到k时刻之前的所有数据，因此随着时间的推移，容易造成误差累积，可能导致滤波发散[22].

3）上述方法对估计初值要求较高，当初值不准确时，可能导致估计效果不佳，而在断电再恢复的情形，这种情况更加明显.

4）许多状态估计是在标准工况下进行，当出现ABS控制制动和大转角等特殊的情形，由于轮速变化剧烈，对应的轮速传感器所测值误差大，此时很难实现准确的状态估计.

本文以半挂车辆为研究对象，针对状态估计过程中存在的上述累计误差、初值敏感、ABS介入等问题，提出一种双自适应无迹卡尔曼滤（FFUKF）估计算法，可对强非线性动力学系统同时实时估计出其纵向/侧向速度、横摆角速度和铰接角，且比普通模糊自适应无迹卡尔曼（FUKF）有更高的精度.

1车辆动力学模型建立

1.1整车模型

因本文关注半挂车纵向/侧向速度、横摆角速度以及铰接角等状态量的估计，且使之适用于ABS控制工况，故分别建立牵引车和挂车的纵向、侧向和横摆自由度，以及6个车轮的旋转自由度.根据上述情况，建立了挂车和牵引车的12自由度车辆模型，车辆动力学分析俯视示意图如图1所示.

车辆运动微分方程如下：

（1）牵引车模型

纵向运动方程：

侧向运动方程：

横摆运动方程：

（2）半挂车模型

纵向运动方程：

侧向运动方程：

横摆运动方程：

（3）铰接点受力关系

牵引车和半挂车通过鞍座铰接，铰接点所受到的纵向力与横向力的关系如图2所示.

牵引点的纵向力和侧向力的坐标变换关系如下：

上列各式中，下标1、2分别代表牵引车和挂车;fl、fr、rl、rr、l2、r2分别代表6个车轮.各符号含义见表1.

1.2轮胎模型

魔术公式轮胎模型具有较高的拟合精度、良好的鲁棒性和极限状况下较高的置信度，并以一套形式相同的公式就可以描述等优点，被广泛使用[23].本文采取“魔术公式”轮胎模型来求取轮胎力，模型对应关系如图3所示，其表达式为：

ï式中：X为输入变量;Y为输出变量;B为刚度系数;C为曲线形状系数，控制了正弦函数的范围，故决定了曲线的形状;D为曲线峰值;E控制了曲线峰值的曲率.具体各系数的值与轮胎载荷、车轮外倾角及路面附着条件等因素有关.

各轮滑移率通过下式得到：

侧偏角通过下式得到：

式中，K为各车轮的滑移率;α）为各车轮的侧偏角;w）为各轮心速度.

2双自适应UKF设计

本文的估计量为挂车与牵引车的纵向速度、侧向速度、横摆角速度以及挂车与牵引车的铰接角.为精确估计，选取纵向、侧向加速度和横摆角加速度为测量量，以方向盘转角和车轮输入力矩为控制输入.依据状态估计的方法，将上述整车动力学模型状态和测量方程写成标准形式为：

式中：x（k）和z（k）分别为系统状态向量和测量向量;u（k）为输入向量;W（k）和V（k）分别为过程噪声和测量噪声;f和h分别为非线性状态函数和观测函数;Tfl、Tfr、Trl、Trr、Tl2、Tr2对应6个车轮的转矩输入，具体通过下式获得：

2.1模糊自适应UKF估计算法

2.1.1UKF算法

无迹卡尔曼（Unscented Kalman Filter，UKF）在卡尔曼线性滤波框架的基础上，采用无迹变换处理均值和协方差的非线性传递问题，在估计点附近确定采样点，用这些样本点来逼近状态的后验概率密度，这样是对非線性函数的概率密度分布进行近似，所得到的均值和协方差至少具有泰勒展开式的2阶精度[24]，具体无迹卡尔曼滤波方法见图4.

2.1.2噪声自适应模糊算法

实际车辆在行驶过程中，外界情况复杂且多变，测量噪声协方差阵R是变量，需要动态调整[25].可通过模糊控制动态调整R，使状态估计结果更为准确.

计算卡∑尔曼滤波协方差的理论值：

计算实际残差的协∑方差：

式中：M为平滑窗口.

确定模糊控制输入输出：

式中：t[r]为矩阵的迹.

定义输入输出模糊集：

建立模糊规则如表2所示

当无迹卡尔曼协方差越准确，残差实际值与理论值之差ek越接近0，相反，其绝对值越大.故将ek和其变化率eck定义为模糊控制输入，定义ε为模糊控制输出，再通过下式得到自适应后的测量噪声协方差阵Rf.

通过动态调整测量噪声协方差阵Rf，使UKF中系统预测的协方差动态调整，进而完成自适应滤波功能，达到使状态估计更准确和稳定的目的.

2.2车轮等效车速估计算法

车辆在正常行驶中，车轮做纯滚动的情况占居较大的比例，且目前车轮转速的测量较准确.故这里通过模糊控制自适应判断稳定单轮，以车轮转速为输入，利用稳定轮转速，等效计算出整车车速.

2.2.1等效车速计算

当车轮处于稳定纯滚动状态时，车轮轮速与车速存在运动学关系.分别利用各纯滚动轮的轮速，计算出相应的车辆纵向速度，再利用最小二乘法计算得到等效车辆纵向速度，计算如下：

式中：入mn为最小残差平方和;为等效车辆纵向速度;为各车轮处速度;R为各车轮滚动半径.

2.2.2车轮稳定性判断

上述为车轮纯滚动计算得到的车辆速度，实际上在紧急制动和加速等工况车轮并非纯滚动，需对其进行稳定性判断，筛选出稳定的车轮再用2.2.1计算等效车速.这里采用基于车轮速度和加速度两种方法，综合判断各个车轮的稳定状态，同时引入模糊控制自适应调节，提高估计器的适应性.

（1）基于速度判断

以测量的车轮转速为输入，通过汽车动力学关系求得轮胎的滑移率，依据各车轮的滑移率是否小于滑移率容差来判断车轮是否做纯滚动运动.

式中，v（ij）为车辆轮心速度;κvf为滑移率容差.

（2）基于加速度判断

以测量的车轮转速和车辆加速度为输入，通过车轮转速算出车辆的等效加速度，与（1）类似，该值与传感器测得的车辆加速度求容差，判断各车轮是否做纯滚动运动.

式中：ax（ij）为等效的车辆加速度;acx为测量的车辆加速度;κaf为加速度容差.

2.2.3容差自适应模糊算法

同样由于实际外界情况复杂，不同附着路面、急剧的转向、驱动与制动等对（1）（2）影响巨大[26]，故通过模糊控制原理动态调整容差，以适应在ABS等复杂工况，提高状态估计的准确性.

（1）确定模糊控制输入输出这里以车辆转角δ及其斜率δc和车轮转矩T（ij）及

其斜率Tc（ij）为模糊控制输入，以车轮滑移率容差κvf和车辆加速度容差κaf为模糊控制输出.

（2）定义输入输出模糊集

输入：δ={NB，NS，Z，PS，PB}

δc={NB，NS，Z，PS，PB}

T（ij）={NB，NS，Z，PS，PB}

Tc（ij）={NB，NS，Z，PS，PB}

输出：kvf={NB，NM，NS，Z，PS，PM，PB}kaf={NB，NM，NS，Z，PS，PM，PB}

（3）建立模糊规则，如表3所示.

在急剧的转角和驱动与制动的情况下，适当的减小容差，以提高判断车轮稳定的准确性，提高估计器的精度和适应性.本估计器后续可扩展估计出路面附着系数，该值亦可做为模糊控制的输入，进一步改善估计效果.

2.3双自适应UKF算法

在基于半挂汽车的12自由度非线性动力学模型的基础上，通过测量的轮速、车辆加速度和方向盘转角等信息，首先利用模糊控制自适应调整速度和加速度容差，综合判断筛选出稳定状态的车轮，结合动力学原理，通过轮速算出等效纵向车速，此为估计器1;然后模糊控制自适应调整测量噪声，利用无迹卡尔曼估计出铰接角和车速，此为估计器2;最后通过卡尔曼算法，以估计器1纵向车速为测量量，估计器2纵向速度为状态量，融合二者所估计的结果，实时估计出车辆的纵向/侧向速度、横摆角速度和挂车与牵引车的铰接角，具体流程如图5.

如果估计器1判断得到全部车轮均为非稳定状态，输出上状态车速;如存在稳定车轮，则会在估计器2的基础上融入估计器1的结果，由于引入了新的测量值，且为非积分估计，可以有效克服估计器2的累计误差，也因此对整体估计器的初值的准确性要求低，即使出现断电再恢复等特殊情况，依然可以较精确地重新估计出状态量.

3仿真验证

本文采用Simulink与TruckSim联合仿真平台，以TruckSim输出的值为参考值，验证本算法的有效性.在TruckSim模型中选用有ABS制动的半挂车辆，添加挂车后，将方向盘转角和制动力矩做为输入.TruckSim输出的车轮转角和车轮转矩为估计算法的输入;纵向、侧向加速度、横摆角速度以及车轮转速为估计器的测量值，表4为整车参数.

双自适应无迹卡尔曼估计器（FFUKF）的状态变量初始值为[50/3.6，0，0，0]T，过程噪声协方差矩阵初始值为Q=diag[0.01，0.01，0.01，0.1]，观测噪声协方差初始值为R=1，采样时间为0.001s.为进一步验证所提出算法的精确性，本文与普通模糊自适应无迹卡尔曼估计器（FUKF）估计的结果进行对比分析.下面分别进行两种工况仿真分析，在不同控制下对估计器进行验证.

3.1角阶跃工况

依据国标GB/T6323—2014设置方向盘转角阶跃输入仿真.初始车速为50km/h，直线行驶4.5s后方向盘转动90°并固定数秒，在8s时，给牵引车后轮制动泵以30MPa的压强，进行紧急制动，并保持2s，方向盘转角和制动设置如图6所示，牵引车后轮的轮速如图7所示.

方向盘转角阶跃输入仿真工况中，本文所提双自适应无迹卡尔曼算法（FFUKF）和普通模糊自适应无迹卡尔曼算法（FUKF）估计出的牵引车与挂车的纵向、侧向速度和横摆角速度以及二者的铰接角分别与TruckSim的输出值进行对比，如图8-14所示.

通过仿真结果对比可知：

纵向速度估计中，自适应无迹卡尔曼（FFUKF）和模糊自适应无迹卡尔曼（FUKF）算法在未有转角输入前（0～4.5s）对挂车和牵引车均有较高的估计精度，但是将曲线比例放大后，可以对比看出FUKF仍然有一定误差，FFUKF算法的估计精度更高，在有转角输入后（4.5～12s），FUKF算法对牵引车纵向速度的估计的误差逐渐增大，而FFUKF算法能一直保持较高的估计精度，其中牵引车在11s时由8.356m/s下降到8.217m/s，与参考值8.218m/s相比，误差由1.679%缩小到0.012%;侧向速度估计中，二者均有一定误差，但FUKF算法的估计误差相对较小，尤其牵引车的侧向速度估计，可明显观察出FFUKF估计误差在逐渐向参考值靠近，其中牵引车在11s时由0.209m/s下降到0.197m/s，与参考值0.188m/s相比，误差由11.170%缩小到4.787%;横摆角速度以及牵引车与挂车的铰接角估计中，自适应无迹卡尔曼（FFUKF）和FUKF算法对铰接角的估计均有较高的精度，放大后可看出，FFUKF算法的估计值更接近参考值，尤其是铰接角基本与参考值重合.

3.2双移线工况

设置双移线工况仿真，运动轨迹如图15所示，参考ISO3888-1—1999标准[27]，考虑半挂车车速略低乘用车，本文设置初始车速为50km/h，直线行驶50m后开始转向，方向盘转角如图16所示.

双移线仿真工况中，本文所提双自适应无迹卡尔曼算法（FFUKF）和模糊自适应无迹卡尔曼算法（FUKF）估计出的牵引车与挂车的纵向、侧向速度和横摆角速度以及二者的铰接角分别与TruckSim的输出值进行对比，如图17-23所示.

通过仿真结果对比可知，在纵向速度和横摆角速度估计中，双自适应无迹卡尔曼（FFUKF）算法的估计精度均高于模糊自适应无迹卡尔曼（FUKF）算法，尤其在牵引车纵向速度估计中，双自适应无迹卡尔曼（FFUKF）的估计精度优势最为突出，其中牵引车在9s时由13.72m/s上升到13.745m/s，与参考值13.75m/s相比，误差由0.218%缩小到0.036%.在挂车的侧向速度估计中，FUKF算法也具有较高精度，但将曲线比例放大后，看出本文所提算法仍然具有更好的估计精度.牵引车与挂车的铰接角估计中，在10s之后，明显对比出FFUKF算法的精度更高.

4结论与展望

1）本文以半挂车辆为研究对象，搭建车辆12自由度非线性动力学模型和轮胎模型，利用模糊控制自适应调整滑移率容差和测量噪声，在UKF算法的基础上，提出双自适应无迹卡尔曼估计算法（FFUKF），可以实现实时估计出车辆的纵向、侧向速度、横摆角速度和挂车与牵引车的铰接角的目的.

2）在Simulink/TruckSim联合环境下进行多工况实验，验证所提出的双自适应无迹卡尔曼估计算法（F℉UKF）具有良好的鲁棒性，降低估计对估计初值准确性的依赖性，且在有ABS控制输入的情况，仍可以更精确地对车速和铰接角进行实时估计.

3）本文所提算法只在纵向速度估计中引入新的测量量，有效克服了纵向速度的累计误差，减少了由于纵向速度的累計误差对侧向速度和横摆角速度的影响，但在侧向速度和横摆角速度的估计里面暂时未引入新的量，不能根本上消除侧向速度和横摆角速度的累计误差，后续需要进一步的研究扩展.其次本估计方法用到了模糊控制，其在实际应用需要根据具体情况进行调整.

参考文献

[1] TIN LEUNG K，WHIDBORNE J F，PURDY D，et al.A review of ground vehicle dynamic state estimations utilising GPS/INS[J]. Vehicle System Dynamics，2011，49（1/2）：29-58.

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

[3] WANG C，WANG Z P，ZHANG L，et al.A vehicle rollover evalu⁃ation system based on enabling state and parameter estimation[J]. IEEE Transactions on Industrial Informatics，2021，17（6）：4003- 4013.

[4] GUO H Y，CAO D P，CHEN H，et al.Vehicle dynamic state esti⁃mation：state of the art schemes and perspectives[J].IEEE/CAA Journal of Automatica Sinica，2018，5（2）：418-431.

[5] BERGMAN M J，DELLEUR J W.Kalman filter estimation and prediction of daily stream flows：ii. application to the Potomac river1[J].JAWRA Journal of the American Water Resources As⁃s o c i a t i o n ，1 9 8 5 ，2 1（ 5 ）：8 2 7 - 8 3 2 .

[6] LI Y，JIA H L，QI J T，et al.An acquisition method of agricultural equipment roll angle based on multi-source information fusion [ J ]. S e n s o r s ，2 0 2 0 ，2 0（ 7 ）：2 0 8 2 .

[J].Sensors，2020，20（7）：2082.

[7]耿国庆，韦斌源，江浩斌，等.基于NA-EKF的分布式驱动电动汽车行驶状态估计研究[J].汽车工程，2018，40（7）：770-776.

[8]李旭，宋翔，张为公.基于扩展卡尔曼滤波的车辆状态可靠估计[J].东南大学学报（自然科学版），2014，44（4）：740-744.

[9]  KATRINIOK A，ABEL D.Adaptive EKF-based vehicle state es⁃timation with online assessment of local observability[J].IEEE Transactions on Control Systems Technology，2016，24（4）：1368- 1381.

[10]  CHEN J，SONG J，LI L，et al.UKF-based adaptive variable struc⁃ture observer for vehicle sideslip with dynamic correction[J]. IET Control Theory & Applications，2016，10（14）：1641-1652.

[11] LUO Z Y，FU Z H，XU Q W.An adaptive multi-dimensional ve⁃hicle driving state observer based on modified sage–husa UKF al⁃g o r i t h m[ J ]. S e n s o r s ，2 0 2 0 ，2 0（ 2 3 ）：6 8 8 9 .

[12]褚端峰，吴超仲，胡钊政，等.基于双线性观测器的汽车侧倾状态估计[J].中国公路学报，2016，29（1）：128-134.

[13]禄盛，连马俊，刘洋，等.基于非线性观测器和UKF的车辆状态估计对比研究[J].吉林大学学报（工学版），2020，50（04）：1288-1300.

[14]  VARGAS-MELENDEZ L，BOADA B，BOADA M，et al. Sensor fusion based on an integrated neural network and probability den⁃sity function （PDF） dual Kalman filter for on-line estimation of vehicle parameters and states[J].Sensors，2017，17（5）：987.

[15]张凤娇，汪䶮，赵万忠.基于深度学习的极限工况下车辆的状态估计[J].重庆理工大学学报（自然科学），2018，32（10）：64-70.

[16]周聪.基于非线性估计理论的线控转向汽车状态估计研究[D].成都：西南交通大学，2012.

[17]  WANG Z F，QIN Y C，GU L，et al.Vehicle system state estima⁃tion based on adaptive unscented Kalman filtering combing with road classification[J].IEEE Access，2017，5：27786-27799.

[18]张凤娇，魏民祥，赵万忠.基于蚁群优化UKF算法的汽车状态估计[J].中国机械工程，2015，26（22）：3046-3050.

[19]周卫琪，齐翔，陈龙，等.基于无迹卡尔曼滤波与遗传算法相结合的车辆状态估计[J].汽车工程，2019，41（2）：198-205.

[20]张一西，马建，赵轩，等.基于蚁狮算法的UKF车辆状态参数估计器[J].中国公路学报，2020，33（5）：165-177.

[21]张不扬，宗长富，王化吉，等.基于自适应卡尔曼算法的重型半挂车状态估计研究[J].汽车技术，2012（7）：1-5.

[22]孙威，付翔，唐科懿，等.四轮独立驱动车辆纵向车速与坡度的联合估计[J].武汉理工大学学报，2017，39（6）：28-33.

[23] SUN F，HUANG X H，RUDOLPH J，et al.Vehicle state estima⁃tion for anti-lock control with nonlinear observer[J].Control En⁃gineering Practice，2015，43：69-84.

[24] PEI X F，CHEN Z F，YANG B，et al.Estimation of states and pa?rameters of multi-axle distributed electric vehicle based on dual unscented Kalman filter[J].Science Progress，2020，103（1）： 003685041988008.

[25]方贤生，高秀琴，罗良铨，等.基于模糊自适应无迹Kalman滤波的车辆状态参数估计[J].现代制造工程，2020（7）：25-30.

[26]孙磊，刘平，杨明亮，等.四轮独立驱动电动汽车多模式自适应车速估计方法[J].汽车技术，2019（7）：36-41.

[27]王建锋，郭维.重型车辆运动状态估计仿真研究[J].计算技术与自动化，2018，37（1）：14-18.