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

周兵，李涛，吴晓建，雷富强

（1.湖南大学汽车车身先进设计制造国家重点实验室，湖南长沙 410082；2.南昌大学机电工程学院，江西南昌 330031）

车辆状态信息对车辆的稳定性控制、主动安全控制，乃至无人驾驶控制有十分重要的意义［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 12自由度半挂车动力学模型Fig.1 Twelve-degree-of-freedom dynamic model of semi-trailer

车辆运动微分方程如下：

（1）牵引车模型

纵向运动方程：

侧向运动方程：

横摆运动方程：

（2）半挂车模型

纵向运动方程：

侧向运动方程：

横摆运动方程：

（3）铰接点受力关系

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

图2 铰接点受力图Fig.2 Force diagram of hinge point

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

（4）6个车轮的力矩平衡方程

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

表1 符号含义Tab.1 Meaning of symbols

1.2 轮胎模型

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

图3 魔术轮胎模型Fig.3 Magic tire model

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

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

侧偏角通过下式得到：

式中，κ(ij)为各车轮的滑移率；α(ij)为各车轮的侧偏角；v(ij)为各轮心速度.

2 双自适应UKF设计

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

状态向量：x(k)=[vx，vy，γ，vx2，vy2，γ2，θ]T

测量向量：

z(k)=[ax，ay，γ，ax2，ay2，γ2，ωfl，ωfr，ωrl，ωrr，ωl2，ωr2]T

控制输入：u(k)=[δ，Tfl，Tfr，Trl，Trr，Tl2，Tr2]T

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

2.1 模糊自适应UKF估计算法

2.1.1 UKF算法

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

图4 UKF流程图Fig.4 The flow chart of UKF

2.1.2 噪声自适应模糊算法

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

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

计算实际残差的协方差：

式中：M为平滑窗口.

确定模糊控制输入输出：

式中：tr［］为矩阵的迹.

定义输入输出模糊集：

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

ec={NB，NS，Z，PS，PB}

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

建立模糊规则如表2所示

表2 测量噪声模糊规则Tab.2 Fuzzy rules of measuring noise

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

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

2.2 车轮等效车速估计算法

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

2.2.1 等效车速计算

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

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

2.2.2 车轮稳定性判断

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

（1）基于速度判断

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

式中，v(ij)为车辆轮心速度；为滑移率容差.

（2）基于加速度判断

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

式中：ax(ij)为等效的车辆加速度；为测量的车辆加速度；为加速度容差.

2.2.3 容差自适应模糊算法

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

（1）确定模糊控制输入输出

这里以车辆转角δ及其斜率δc和车轮转矩T(ij)及其斜率Tc(ij)为模糊控制输入，以车轮滑移率容差和车辆加速度容差为模糊控制输出.

（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}

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

表3 容差模糊规则Tab.3 Fuzzy rules of tolerance

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

2.3 双自适应UKF算法

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

图5 估计器流程图Fig.5 Flow chart of estimator

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

3 仿真验证

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

表4 整车参数表Tab.4 Parameters of complete vehicle

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

3.1 角阶跃工况

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

图6 方向盘转角和制动泵输入Fig.6 Steering wheel angle and brake pump input

图7 牵引车后轮轮速（左）Fig.7 Wheel speed of rear wheel of tractor（left）

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

图8 牵引车纵向速度Fig.8 Longitudinal speed of tractor

通过仿真结果对比可知：

图9 挂车纵向速度Fig.9 Longitudinal speed of trailer

图10 牵引车侧向速度Fig.10 Lateral speed of tractor

图11 挂车侧向速度Fig.11 Lateral speed of trailer

图12 牵引车横摆角速度Fig.12 Yaw rate of tractor

图13 挂车横摆角速度Fig.13 Yaw rate of trailer

图14 牵引车与挂车铰接角Fig.14 Articulation angle between tractor and trailer

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

3.2 双移线工况

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

图15 双移线运动轨迹Fig.15 Trajectory of double shift line

图16 方向盘转角Fig.16 Turning angle of steering wheel

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

图17 牵引车纵向速度Fig.17 Longitudinal speed of tractor

图18 挂车纵向速度Fig.18 Longitudinal speed of trailer

图19 牵引车侧向速度Fig.19 Lateral speed of tractor

图20 挂车侧向速度Fig.20 Lateral speed of trailer

图21 牵引车横摆角速度Fig.21 Yaw rate of tractor

图22 挂车横摆角速度Fig.22 Yaw rate of trailer

图23 牵引车与挂车铰接角Fig.23 Articulation angle between tractor and trailer

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

4 结论与展望

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

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

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