[发明专利]一种基于自适应容积粒子滤波的车辆状态估计方法

权利要求书

1.一种基于自适应容积粒子滤波的车辆状态估计方法，其特征在于，包括下述步骤：

步骤1根据待估计状态变量和通过车载传感器获得的信息建立车辆动力学模型，并转换成状态方程和量测方程的形式；

步骤2算法初始化：根据球面径向准则确定容积点和对应的权值，从初始状态概率密度分布函数抽取粒子和协方差矩阵；

步骤3采用自适应容积卡尔曼滤波设计粒子滤波重要性密度采样函数，导入传感器最新观测数据，产生预测粒子集合和对应的方差；

步骤4重新产生粒子、计算重要性权值并权值归一化；

步骤5根据权值归一化结果，对粒子集合进行重采样；

步骤6计算当前时刻粒子滤波的状态估计值和误差协方差矩阵；

步骤7判断状态估计是否结束，如果结束则输出估计结果并退出状态估计，否则输出估计结果，并将估计结果和最新观测数据输入到步骤3中，继续进行状态估计；

所述车载传感器包括各车轮轮速传感器、纵向加速度传感器、侧向加速度传感器、侧倾角速度传感器和横摆角速度传感器、方向盘转角传感器；

所述车辆动力学模型的状态方程和量测方程形式为：

其中，为待估计状态变量，u(t)＝[δ,w_fl,w_fr,w_rl,w_rr]'为输入变量，z(t)＝[a_x,a_y,p,w_r]'为观测变量，观测变量和输入变量通过车载传感器采集得到，w(t)、v(t)分别为过程噪声和量测噪声；

此外，状态方程f(x(t),u(t))为：

量测方程h(x(t),u(t))为：

式中，x₁＝V_x，x₂＝V_y，x₃＝w_r，x₄＝p，

∑M_z＝(F_xrr-F_xrl)d_r/2-b(F_yrl+F_yrr)+a(F_xflsinδ_fl+F_xfrsinδ_fr+F_yflcosδ_fl+F_yfrcosδ_fr)+(F_xfrcosδ_fr-F_xflcosδ_fl+F_yflsinδ_fl-F_yfrsinδ_fr)d_f/2

∑F_y＝F_xflsinδ_fl+F_xfrsinδ_fr+F_yflcosδ_fl+F_yfrcosδ_fr+F_yrl+F_yrr

∑F_x＝F_xflcosδ_fl+F_xfrcosδ_fr-F_yflsinδ_fl-F_yfrsinδ_fr+F_xrl+F_xrr

式中，m为车辆总质量，m_s为车辆簧载质量，V_x为车辆质心纵向速度，V_y为车辆质心侧向速度，w_r为质心横摆角速度，e为侧倾臂高度，为质心侧倾角，p为质心侧倾角速度，δ为方向盘转角，∑F_x为车辆轮胎纵向合力，∑F_y为车辆轮胎侧向合力，a_x为车辆质心纵向加速度，a_y为车辆质心侧向加速度；i＝f、r分别表示前轮和后轮，j＝l、r分别表示左轮和右轮，w_ij为车轮转速，δ_ij为轮胎侧偏角，F_xij为轮胎纵向力，F_yij为轮胎侧向力，∑M_x为车辆侧倾力矩，∑M_z为车辆横摆力矩，I_xxs为车辆簧载质量绕x轴的转动惯量，I_xzs为车辆簧载质量绕x、z轴的转动惯量积，I_zz为车辆横摆转动惯量，g为重力加速度，为车辆侧倾刚度，C_φi为车辆侧倾阻尼，a为车辆质心距前轴的距离，b为车辆质心距后轴的距离，d_f为车辆前轴轮距，d_r为车辆后轴轮距；

所述步骤2中，算法初始化过程具体包括：输入状态方程和量测方程中的固定参数信息、待估计量初值、过程噪声和量测噪声协方差矩阵、误差协方差矩阵、粒子数和滤波参数、采样时间；

而后根据球面径向准则产生2M个容积点和对应的权值，即：

其中，M为状态方程维数，ξ_n为第n个容积点，ω_cn表示第n个权值，[I]_n表示容积点集[I]第n列，采用三阶容积原则，容积点集[I]为：[I]＝[I¹，-I¹]，I¹为5×5单位矩阵；

从初始状态概率分布密度p(x₀)抽取粒子为粒子数，且：

式中，为初始粒子，为初始粒子均值，初始粒子误差协方差；

所述步骤3分为时间更新和量测更新过程，具体包括：

3.1时间更新：

对误差协方差矩阵进行分解：

计算Cubature点：

计算通过状态方程传导的Cubature点：

更新状态预测值和误差协方差预测值：

式中，时刻k＝1,2,3，···，为k-1时刻的状态估计值，u_k-1为k-1时刻由传感器获得的输入变量，w_k-1为k-1时刻过程噪声；

3.2量测更新：

对更新后的误差协方差矩阵进行分解：

计算Cubature点：

计算通过量测方程传导的Cubature点：

更新量测一步预测：

更新量测误差协方差预测值：

计算互协方差：

计算容积卡尔曼滤波增益矩阵：

计算容积卡尔曼滤波之后的状态估计值：

计算容积卡尔曼滤波之后的误差协方差估计值：

其中α_k为自适应因子，计算方程为：

式中，c₀∈(1.0～1.5)，c₁∈(3.0～8.0)。

2.根据权利要求1所述的一种基于自适应容积粒子滤波的车辆状态估计方法，其特征在于，所述步骤5中的重采样方法选取残差重采样。