一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法与流程

技术特征：

1.一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，包括以下步骤：

步骤(1)，建立七自由度车辆动力学模型；

步骤(2)，基于稀疏网格求积卡尔曼滤波理论，选取多维积分点、初始化、进行时间更新与量测更新，估计车辆的纵向车速、侧向车速、质心侧偏角。

2.根据权利要求1所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述步骤(1)中，七自由度车辆动力学模型的建立：

假设路面条件变化缓慢，考虑车辆纵向运动、侧向运动、横摆运动及四个轮胎的回转运动，建立如下七自由度车辆模型：

其中，vx为纵向车速，vy为侧向车速，ax为纵向加速度，ay为侧向加速度，r为横摆角速度，a为质心距前轴的距离，b为质心距后轴的距离，δ为前轮转角，m为车辆的质量，lF为前轮轮距，lR为后轮轮距，Fx1、Fx2、Fx3、Fx4分别为左前、右前、左后、右后轮胎的纵向力，Fy1、Fy2、Fy3、Fy4分别为左前、右前、左后、右后轮胎的侧向力，Iz为车辆绕z轴的转动惯量，Mz为绕z轴的转矩。

3.根据权利要求1所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述步骤(2)中，多维积分点的选取：

以纵向速度vx、侧向速度vy、横摆角速度r、纵向加速度ax、侧向加速度ay及车辆绕z轴转矩Mz为状态量x＝[vx,vy,r,ax,ay,Mz]^T，状态量维数n＝6，精度水平L＝2，则多维积分点ξi及其权重wi具体如下所示：

4.根据权利要求1所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述步骤(2)中，初始化过程：

以前轮转角、四轮轮速为输入u＝[δ,ω1,ω2,ω3,ω4,]^T，以纵向加速度ax、侧向加速度ay及横摆角速度r为观测量z＝[ax,ay,r]^T，纵向速度vx、侧向速度vy、横摆角速度r、纵向加速度ax、侧向加速度ay及车辆绕z轴转矩Mz为状态量x＝[vx,vy,r,ax,ay,Mz]^T；设系统的状态初值x0＝[vx0,0,0,0,0,0]^T，误差协方差矩阵初值为P0。

5.根据权利要求3或4所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述步骤(2)中，所述时间更新的过程为：

①对误差协方差矩阵分解Pk-1|k-1＝SS^T，其中S为正交分解矩阵，k为当前时刻；

②计算采样点其中ζi为本方法所采用的积分点，为当前时刻状态量矩阵；

③状态预测：轮胎力预测及状态量预测；

④状态预测误差协方差式中Q为系统噪声协方差矩阵，为状态更新矩阵。

6.根据权利要求5所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述状态预测的过程为：

a.轮胎力预测

F～f(Ok-1|k-1)

式中，分别表示矩阵γi的第一行第一列元素、第二行第一列元素、第三行第一列元素、第四行第一列元素、第五行第一列元素。

b.状态量预测

将所得采样点代入七自由度车辆模型得状态转移矩阵；

积分点转换为：

式中，T为采样时间；

则状态更新为：

7.根据权利要求5所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述步骤(2)中，所述量测更新的过程为：

①重采样

②观测预测为观测预估矩阵；

③预测观测误差协方差式中R为观测噪声协方差矩阵；

④计算互协方差

⑤计算卡尔曼增益

⑥状态更新

⑦状态协方差更新

8.根据权利要求7所述的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，其特征在于，所述观测预测的过程：

以纵向加速度、侧向加速度及横摆角速度为观测量，其观测方程为：