本专利属于汽车行驶状态估计方法技术领域，具体涉及一种基于加权融合算法的汽车行驶状态估计方法，包括下列步骤：S1、建立包含汽车纵向运动，侧向运动以及横摆运动的具有非线性特征的三自由度运动微分方程；S2、基于三自由度运动微分方程建立状态空间表达式；S3、将状态空间表达式离散化，获得实时刻汽车质心纵向速度、侧向速度以及横摆角速度的估计值；S4、根据所述汽车质心纵向速度、侧向速度求得汽车质心侧偏角的估计值；S5、采用加权融合算法，获得汽车质心侧偏角的最优估计值。本发明通过加权融合算法，求得最优加权因子，进一步可得最优状态估计值，精度得到了一定程度的提升。本发明用于汽车行驶状态的估计。
1.一种基于加权融合算法的汽车行驶状态估计方法，其特征在于：包括下列步骤：
S1、建立包含汽车纵向运动，侧向运动以及横摆运动的具有非线性特征的三自由度运动微分方程；
S2、基于三自由度运动微分方程建立状态空间表达式；
S3、将状态空间表达式离散化，建立关于汽车质心纵向速度、侧向速度以及横摆角速度的状态方程与量测方程，将所述汽车质心纵向速度、侧向速度以及横摆角速度迭代至粒子滤波估计器中，获得实时刻汽车质心纵向速度、侧向速度以及横摆角速度的估计值，所述粒子滤波估计器的算法为：利用离散化的非线性状态空间表达式，根据上一时刻的状态估计值求当前时刻的状态预测值，根据状态空间表达式中的量测方程，结合传感器的测量方差，求解各粒子的权值，并且进行粒子权值的归一化处理，进行粒子的重采样，所述粒子滤波估计器采用轮盘赌进行粒子的重采样，用N个粒子的样本序列，利用均值求出当前时刻状态的估计值，将各个时刻状态估计值输出，即可求得某一行驶工况下汽车行驶状态的估计；
S4、根据所述汽车质心纵向速度、侧向速度求得汽车质心侧偏角的估计值；
S5、采用加权融合算法，获得汽车质心侧偏角的最优估计值，所述加权融合算法为：设左前轮转矩传感器、右前轮转矩传感器、左后轮转矩传感器、右后轮转矩传感器的方差分别为σ12、σ22、σ32、σ42，针对单个车轮转矩，通过S3粒子滤波算法的状态估计值为：左前轮：X1、右前轮：X2、左后轮：X3、右后轮：X4，首先，将X1、X2、X3、X4进行融合，融合后状态估计值为此时其中，wi(i＝1,2,3,4)分别为左前轮，右前轮，左后轮，右后轮状态估计值的加权因子，此时，总方差σ2为：利用朗格朗日乘法，可得各车轮的最优加权因子为：
通过加权融合算法得到车辆行驶状态的最优估计值 为：
2.根据权利要求1所述的一种基于加权融合算法的汽车行驶状态估计方法，其特征在于：所述S1中三自由度运动微分方程的求法为：纵向运动：FX＝(FXfl+FXfr)cosδ-(FYfl+FYfr)sinδ+(FXrl+FXrr)侧向运动：FY＝(FYfl+FYfr)cosδ+(FXfl+FXfr)sinδ+(FYrl+FYrr)横摆运动：
FX为汽车受到的外力沿X方向的合力，FY为汽车受到的力沿Y方向的合力，MZ为绕质心处的力矩，FXfl、FXfr、FXrl、FXrr分别为地面对汽车的左前轮纵向力、右前轮纵向力、左后轮纵向力、右后轮纵向力；FYfl、FYfr、FYrl、FYrr分别为地面对汽车的左前轮侧向力、右前轮侧向力、左后轮侧向力、右后轮侧向力，δ为汽车的前轮转角，a为汽车前轴中心至汽车质心的距离，b为汽车后轴中心至汽车质心的距离，L为汽车轮距，M为整车质量，VX为汽车质心纵向速度，VY为汽车质心侧向速度，Wr为横摆角速度，IZ为汽车绕Z轴的转动惯量；
3.根据权利要求1所述的一种基于加权融合算法的汽车行驶状态估计方法，其特征在于：所述S2中状态空间表达式为：Z(t)＝C·X(t)+D·U(t)+Vk
其中X(t)为实际的状态变量；X(t)＝[VX(t) VY(t) Wr(t)]′，VX(t)、VY(t)、Wr(t)分别为t时刻汽车质心纵向速度、侧向速度、横摆角速度，U(t)＝[FX FY MZ]′，U为整个系统的输入变量Z(t)为待观测向量，C、D向量随着观测向量的不同而在改变，Wk为过程噪声，Vk为观测噪声，本粒子滤波估计器将各个车轮的转矩作为观测量，Z(t)＝[Wfl(t) Wfr(t) Wrl(t) Wrr(t)]′，Wfl(t)、Wfr(t)、Wrl(t)、Wrr(t)分别为左前轮，右前轮，左后轮，右后轮在t时刻的车轮转矩，IZ为汽车绕Z轴的转动惯量，M为整车质量，Wr为汽车质心的横摆角速度，δ为汽车的前轮转角，A为变向量A，B为常向量B；
其中，λ为车轮与地面之间的相对滑移率，γ为车轮的滚动半径，a为汽车前轴中心至汽车质心的距离，L为汽车轮距。
4.根据权利要求1所述的一种基于加权融合算法的汽车行驶状态估计方法，其特征在于：所述S3中将三自由度状态空间表达式离散化为：其中，X(kT)为k时刻状态变量的估计值，X[(k-1)T]为(k-1)时刻转态变量的估计值U(kT)为k时刻系统的输入变量，Z(kT)为k时刻的待观测向量，Wk为过程噪声，Vk为观测噪声，C、D向量随着观测向量的不同而在改变，G(T)＝eAT， G(T)、H(T)分别为两个中间变量，T为粒子滤波器的采样时间，B为常向量B。
5.根据权利要求1所述的一种基于加权融合算法的汽车行驶状态估计方法，其特征在于：所述S4中汽车质心侧偏角的估计值的求法为：汽车质心侧偏角的估计值为
6.根据权利要求1所述的一种基于加权融合算法的汽车行驶状态估计方法，其特征在于：所述S5中质心侧偏角的最优估计值的求法为在总方差最小条件下的最优汽车质心侧向速度与汽车质心纵向速度基础上，求最优质心侧偏角的估计值 所述 VX为汽车质心纵向速度，VY为汽车质心侧向速度。
一种基于加权融合算法的汽车行驶状态估计方法
技术领域
[0001] 本专利属于汽车行驶状态估计方法技术领域，具体涉及一种基于加权融合算法的汽车行驶状态估计方法。
背景技术
[0002] 车辆质心侧偏角是表征车辆稳定性和安全性的一个重要状态参数，但其无法直接测量，只能进行观测与估算。虽然横摆角速度能通过方向盘转角传感器测得，但其测量精度较差。因此对汽车质心侧偏角与横摆角速度的估计是一个重点及难点。对于汽车整个系统而言，由于其行驶过程是一个处于非线性非高斯分布的随机过程，而粒子滤波算法能够有效的解决非高斯非线性系统的状态观测。
[0003] 当前质心侧偏角与横摆角速度估计器大多建立在线性二自由度汽车运动模型上，尽管二自由度模型在车轮侧偏角小于5°时能够准确的进行汽车行驶状态的估计，但在汽车极限运动或高速行驶的情况下，由于轮胎处于非线性状态，此时估计效果较差。此外。基于非线性三自由度的粒子滤波估计器，由于观测量的不唯一，大部分采用均值思想进行行驶状态的估计，均值思想由于忽略了各传感器之间的测量精度，按统一测量精度处理观测数值，导致状态估计精度较差。
发明内容
[0004] 针对上述技术问题，提供了一种精度高的基于加权融合算法的汽车行驶状态估计方法。
[0005] 为了解决上述技术问题，本发明采用的技术方案为：
[0006] 一种基于加权融合算法的汽车行驶状态估计方法，包括下列步骤：
[0007] S1、建立包含汽车纵向运动，侧向运动以及横摆运动的具有非线性特征的三自由度运动微分方程；
[0008] S2、基于三自由度运动微分方程建立状态空间表达式；
[0009] S3、将状态空间表达式离散化，建立关于汽车质心纵向速度、侧向速度以及横摆角速度的状态方程与量测方程，将所述汽车质心纵向速度、侧向速度以及横摆角速度迭代至粒子滤波估计器中，获得实时刻汽车质心纵向速度、侧向速度以及横摆角速度的估计值；
[0010] S4、根据所述汽车质心纵向速度、侧向速度求得汽车质心侧偏角的估计值；
[0011] S5、采用加权融合算法，获得汽车质心侧偏角的最优估计值。
[0012] 所述S1中三自由度运动微分方程的求法为：
[0013] 纵向运动：FX＝(FXfl+FXfr)cosδ-(FYfl+FYfr)sinδ+(FXrl+FXrr)
[0014]
[0015] 侧向运动：FY＝(FYfl+FYfr)cosδ+(FXfl+FXfr)sinδ+(FYrl+FYrr)
[0016]
[0017] 横摆运动：
[0018]
[0019] FX为汽车受到的外力沿X方向的合力，FY为汽车受到的力沿Y方向的合力，MZ为绕质心处的力矩，FXfl、FXfr、FXrl、FXrr分别为地面对汽车的左前轮纵向力、右前轮纵向力、左后轮纵向力、右后轮纵向力；FYfl、FYfr、FYrl、FYrr分别为地面对汽车的左前轮侧向力、右前轮侧向力、左后轮侧向力、右后轮侧向力，δ为汽车的前轮转角，a为汽车前轴中心至汽车质心的距离，b为汽车后轴中心至汽车质心的距离，L为汽车轮距，M为整车质量，VX为汽车质心纵向速度，VY为汽车质心侧向速度，IZ为汽车绕Z轴的转动惯量。
[0020] 整理上述公式可得三自由度非线性运动微分方程为：
[0021]
[0022]
[0023]
[0024] 所述S2中状态空间表达式为：
[0025]
[0026] Z(t)＝C(t)·X(t)+D·U(t)+Vk
[0027] 其中X(t)为实际的状态变量；X(t)＝[VX(t)VY(t)Wr(t)]′，VX(t)、VY(t)、Wr(t)分别为t时刻汽车质心纵向速度、侧向速度、横摆角速度，U(t)＝[FX FY MZ]′，U为整个系统的输入变量
[0028]
[0029] Z(t)为待观测向量，C、D向量随着观测向量的不同而在改变，Wk为过程噪声，Vk为观测噪声，本粒子滤波估计器将各个车轮的转矩作为观测量，Z(t)＝[Wfl(t) Wfr(t) Wrl(t) Wrr(t)]′，Wfl(t)、Wfr(t)、Wrl(t)、Wrr(t)分别为左前轮，右前轮，左后轮，右后轮在t时刻的车轮转矩，
[0030]
[0031] 其中，λ为车轮与地面之间的相对滑移率，γ为车轮的滚动半径，a为汽车前轴中心至汽车质心的距离，L为汽车轮距。
[0032] 所述S3中将三自由度空间状态表达式离散化为：
[0033]
[0034] 其中，X(kT)为k时刻状态变量的估计值，X[(k-1)T]为(k-1)时刻转态变量的估计值U(kT)为k时刻系统的输入变量，Z(kT)为k时刻的待观测向量，Wk为过程噪声，Vk为观测噪声，C、D向量随着观测向量的不同而在改变，G(T)＝eAT， G(T)、H(T)分别为两个中间变量，T为粒子滤波器的采样时间，B为常向量B。
[0035] 所述S3中粒子滤波估计器的算法为：利用离散化的非线性空间状态表达式，根据上一时刻的状态估计值求当前时刻的状态预测值，根据状态空间表达式中的量测方程，结合传感器的测量方差，求解各粒子的权值，并且进行粒子权值的归一化处理，进行粒子的重采样，所述粒子滤波估计器采用轮盘赌进行粒子的重采样，用N个粒子的样本序列，利用均值求出当前时刻状态的估计值，将各个时刻状态估计值输出，即可求得某一行驶工况下汽车行驶状态的估计。
[0036] 所述S4中汽车质心侧偏角的估计值的求法为：汽车质心侧偏角的估计值为[0037] 所述S5中加权融合算法为：设左前轮转矩传感器、右前轮转矩传感器、左后轮转矩传感器、右后轮转矩传感器的方差分别为σ12、σ22、σ32、σ42针对单个车轮转矩，通过S3粒子滤波算法的状态估计值为：左前轮：X1、右前轮：X2、左后轮：X3、右后轮：X4，首先，将X1、X2、X3、X4进行融合，融合后状态估计值为 此时
[0038]
[0039]
[0040] 其中，wi(i＝1,2,3,4)分别为左前轮，右前轮，左后轮，右后轮状态估计值的加权因子，此时，总方差σ2为：
[0041]
[0042] 利用朗格朗日乘法(最优极值理论)，可得各车轮的最优加权因子为：
[0043]
[0044] 通过加权融合算法得到车辆行驶状态的最优估计值 为：
[0045] 所述S5中质心侧偏角的最优估计值的求法为在总方差最小条件下的最优汽车质心侧向速度与汽车质心纵向速度基础上，求最优质心侧偏角的估计值 所述
[0046] 本发明与现有技术相比，具有的有益效果是：
[0047] 1、本发明采用粒子滤波算法进行车辆状态的估计，能够有效的解决汽车行驶过程中非线性非高斯状态的估计，相比于其他估计器，更能降低噪声对观测精度的影响，提高整体状态估计的精度。
[0048] 2、通过加权融合算法，在总方差最小的条件下，根据各传感器的测量值以及传感器的测量方差，求得最优加权因子，进一步可得最优状态估计值，相比于标准粒子滤波估计器得到的状态值，精度得到了一定程度的提升。
附图说明
[0049] 图1为本发明所述的三自由度汽车模型简图；
[0050] 图2为粒子滤波算法流程图；
[0051] 图3为本发明所述的质心侧偏角对比图；
[0052] 图4为本发明所述的横摆角速度对比图。
具体实施方式
[0053] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0054] S1、如图1所示，考虑汽车的纵向运动，侧向运动以及横摆运动建立非线性三自由度汽车模型，推导汽车的纵向运动，侧向运动以及横摆运动的微分方程如下：
[0055] 纵向的受力平衡方程为：
[0056] FX＝MaX
[0057] 其中M为整车质量，FX为汽车受到的外力沿X方向的合力，aX为汽车在行驶过程中沿X方向的加速度。假设前左轮与前右轮转角相同，后左轮与后右轮转角为0，且不考虑车轮侧倾角的影响，则有：
[0058] FX＝(FXfl+FXfr)cosδ-(FYfl+FYfr)sinδ+(FXrl+FXrr)
[0059]
[0060] 其中，FXfl、FXfr、FXrl、FXrr分别为地面对汽车的左前轮纵向力、右前轮纵向力、左后轮纵向力、右后轮纵向力，aX为汽车纵向加速度，VX为汽车质心速度，VY为汽车横向加速度，Wr为横摆角速度。
[0061] 侧向的受力平衡方程为：
[0062] FY＝MaY
[0063] 其中FY为汽车受到的外力沿Y方向的合力，aY为汽车的侧向加速度，
[0064] FY＝(FYfl+FYfr)cosδ+(FXfl+FXfr)sinδ+(FYrl+FYrr)
[0065]
[0066] 绕Z轴的力矩方程为：
[0067]
[0068] 其中，MZ为横摆力矩，IZ为汽车绕Z轴的转动惯量
[0069]
[0070] FX为汽车受到的外力沿X方向的合力，FY为汽车受到的力沿Y方向的合力，MZ为绕质心处的力矩，FXfl、FXfr、FXrl、FXrr分别为地面对汽车的左前轮纵向力、右前轮纵向力、左后轮纵向力、右后轮纵向力；FYfl、FYfr、FYrl、FYrr分别为地面对汽车的左前轮侧向力、右前轮侧向力、左后轮侧向力、右后轮侧向力，δ为汽车的前轮转角，a为汽车前轴中心至汽车质心的距离，b为汽车后轴中心至汽车质心的距离，L为汽车轮距，M为整车质量，VX为汽车质心纵向速度，VY为汽车质心侧向速度，Wr为横摆角速度，IZ为汽车绕Z轴的转动惯量。
[0071] 整理上述公式可得三自由度非线性运动微分方程为：
[0072]
[0073]
[0074]
[0075] S2、将三自由度非线性运动微分方程转化为空间状态表达式，
[0076]
[0077] Z(t)＝C(t)·X(t)+D·U(t)+Vk
[0078] 其中X(t)为实际的状态变量；X(t)＝[VX(t) VY(t) Wr(t)]′，VX(t)、VY(t)、Wr(t)分别为t时刻汽车质心纵向速度、侧向速度、横摆角速度，U(t)＝[FX FY MZ]′，U为整个系统的输入变量
[0079]
[0080] Z(t)为待观测向量，C、D向量随着观测向量的不同而在改变，Wk为过程噪声，Vk为观测噪声，本粒子滤波估计器将各个车轮的转矩作为观测量，Z(t)＝[Wfl(t) Wfr(t) Wrl(t) Wrr(t)]′，Wfl(t)、Wfr(t)、Wrl(t)、Wrr(t)分别为左前轮，右前轮，左后轮，右后轮在t时刻的车轮转矩，
[0081]
[0082] 其中，λ为车轮与地面之间的相对滑移率，γ为车轮的滚动半径(将各个车轮与地面间的相对滑移率以及滚动半径近似的看作相等)，a为汽车前轴中心至汽车质心的距离，L为汽车轮距。
[0083] S3、将三自由度空间状态表达式离散化为：
[0084]
[0085] 其中，X(kT)为k时刻状态变量的估计值，X[(k-1)T]为(k-1)时刻转态变量的估计值U(kT)为k时刻系统的输入变量，Z(kT)为k时刻的待观测向量，Wk为过程噪声，Vk为观测噪声，C、D向量随着观测向量的不同而在改变，G(T)＝eAT， G(T)、H(T)分别为两个中间变量，T为粒子滤波器的采样时间，B为常向量B。
[0086] 利用粒子滤波算法求解离散化的空间状态表达式，如图2所示为粒子滤波算法流程图。
[0087] 首先，初始化各种参数设置，在k＝0时刻，随机生成N个粒子；
[0088] 在k＝1,2,……时刻，循环执行以下几个步骤：
[0089] S3.1、利用离散化的非线性空间状态表达式，根据上一时刻的状态估计值求当前时刻的状态预测值；
[0090] S3.2、根据状态空间表达式中的量测方程，结合传感器的测量方差，求解各粒子的权值，并且进行粒子权值的归一化处理；
[0091] S3.3、进行粒子的重采样，所述粒子滤波估计器采用轮盘赌进行粒子的重采样；
[0092] S3.4、用N个粒子的样本序列，利用均值求出当前时刻状态的估计值；
[0093] 最后，将各个时刻状态估计值输出，即可求得某一行驶工况下汽车行驶状态的估计。
[0094] S4、汽车质心侧偏角的估计值为汽车质心侧偏角的估计值为
[0095] S5、由于传感器制造误差的存在，各传感器的测量值存在一定程度的偏差。在S3的基础上，对通过粒子滤波算法估计的状态值进行融合算法，使得在总体方差最小的条件下，通过求解最优加权因子，使融合后的状态估计值最优。
[0096] S6、设左前轮转矩传感器、右前轮转矩传感器、左后轮转矩传感器、右后轮转矩传感器的方差分别为σ12、σ22、σ32、σ42针对单个车轮转矩，通过S3粒子滤波算法的状态估计值为：左前轮：X1、右前轮：X2、左后轮：X3、右后轮：X4，首先，将X1、X2、X3、X4进行融合，融合后状态估计值为 此时
[0097]
[0098]
[0099] 其中，wi(i＝1,2,3,4)分别为左前轮，右前轮，左后轮，右后轮状态估计值的加权因子，此时，总方差σ2为：
[0100]
[0101] 利用朗格朗日乘法(最优极值理论)，可得各车轮的最优加权因子为：
[0102]
[0103] 通过加权融合算法得到车辆行驶状态的最优估计值 为：
[0104] 在总方差最小条件下的最优汽车质心侧向速度与汽车质心纵向速度基础上，求最优质心侧偏角的估计值 所述
[0105] 实施例
[0106] 图3，图4为汽车在120km/h高速蛇形行驶工况下的仿真结果图，从图中可以清楚看出，粒子滤波算法可以有效的实现质心侧偏角与横摆角速度的状态，而相比于均值法，基于加权融合的粒子滤波算法估计精度更高，跟随效果更好。
[0107] 上面仅对本发明的较佳实施例作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化，各种变化均应包含在本发明的保护范围之内。
