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

本发明属于系统状态估计领域，尤其涉及一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法。
背景技术：
：随着车辆主动安全技术的深入发展，车辆自身行驶状态的准确获取越来越重要。由于传感器技术与车辆成本的限制，基于模型预测的车辆行驶状态估计已成为研究的热点之一。目前基于模型预测的车辆状态估计方法主要包括扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)、无迹卡尔曼滤波(UnscentedKalmanFilter，UKF)、容积卡尔曼滤波(CubatureKalmanFilters，CKF)、粒子滤波(ParticleFilter，PF)等，其中扩展卡尔曼滤波通过泰勒级数展开将非线性系统近似转化为线性系统，在系统非线性较强时误差较大；无迹卡尔曼滤波通过Sigma点的选取，进行UT变换，避免了对非线性系统的近似，精度较高，但计算量较大；容积卡尔曼滤波的精度与无迹卡尔曼滤波相当，但是所需采样点较少；粒子滤波精度较高，但存在粒子样本枯竭与实时性差等问题。求积卡尔曼滤波(QuadratureFilter，QF)是一类非线性滤波器，其估计精度高于EKF、UKF、CKF，但是实时性较差，因为其计算量会随着状态维数的增加呈指数上升。近年来，稀疏网格理论得到了快速的发展，采用该方法可有效减少求积卡尔曼滤波的计算量，进而运用于车辆状态估计领域。技术实现要素：本发明提供了一种基于稀疏网格求积卡尔曼滤波(SparseGridQuadratureFilter，SGQF)的车辆行驶状态估计方法，旨在提高车辆状态估计的精度，使求积卡尔曼滤波理论有效地运用于车辆状态估计领域。本发明的技术方案为：一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，包括以下步骤：步骤(1)，建立七自由度车辆动力学模型；假设路面条件变化缓慢，考虑车辆纵向运动、侧向运动、横摆运动及四个轮胎的回转运动，建立如下七自由度车辆模型：其中，vx为纵向车速，vy为侧向车速，ax为纵向加速度，ay为侧向加速度，r为横摆角速度，a为质心距前轴的距离，b为质心距后轴的距离，δ为前轮转角，m为车辆的质量，lF为前轮轮距，lR为后轮轮距，Fx1、Fx2、Fx3、Fx4分别为左前、右前、左后、右后轮胎的纵向力，Fy1、Fy2、Fy3、Fy4分别为左前、右前、左后、右后轮胎的侧向力，Iz为车辆绕z轴的转动惯量，Mz为绕z轴的转矩。步骤(2)，基于稀疏网格求积卡尔曼滤波理论，选取多维积分点、初始化、进行时间更新与量测更新，估计车辆的纵向车速、侧向车速、质心侧偏角。多维积分点的选取：以纵向速度vx、侧向速度vy、横摆角速度r、纵向加速度ax、侧向加速度ay及车辆绕z轴转矩Mz为状态量x＝[vx,vy,r,ax,ay,Mz]T，状态量维数n＝6，精度水平L＝2，则多维积分点ξi及其权重wi具体如下所示：初始化过程：以前轮转角、四轮轮速为输入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；时间更新的过程为：①对误差协方差矩阵分解Pk-1|k-1＝SST，其中S为正交分解矩阵，k为当前时刻；②计算采样点其中ζi为本方法所采用的积分点，为当前时刻状态量矩阵；③状态预测：a.轮胎力预测F～f(Ok-1|k-1)式中，分别表示矩阵γi的第一行第一列元素、第二行第一列元素、第三行第一列元素、第四行第一列元素、第五行第一列元素；b.状态量预测将所得采样点代入七自由度车辆模型得状态转移矩阵；积分点转换为：式中，T为采样时间；则状态更新为：④状态预测误差协方差式中Q为系统噪声协方差矩阵，为状态更新矩阵；量测更新的过程为：①重采样Pk|k-1＝SST；②观测预测为观测预估矩阵；以纵向加速度、侧向加速度及横摆角速度为观测量，其观测方程为：③预测观测误差协方差式中R为观测噪声协方差矩阵；④计算互协方差⑤计算卡尔曼增益⑥状态更新⑦状态协方差更新本发明的有益效果：本发明提出了一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，将结合了稀疏网格理论的求积卡尔曼滤波方法应用于车辆状态参数估计，在提高状态估计精度的同时可以有效地改善状态估计的实时性，为车辆主动安全技术的发展提供了有益的支持。附图说明图1为本发明提出的一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法的流程图；图2为七自由度车辆动力学模型；图3为车辆纵向速度估计值比较图；图4为车辆侧向速度估计值比较图；图5为车辆质心侧偏角估计值比较图。具体实施方式下面结合附图和具体实施例对本发明作进一步说明。如图1所示，基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法，包括以下步骤：1.建立车辆动力学模型本实施例考虑车辆纵向运动、侧向运动、横摆运动及四个轮胎的回转运动(假设路面条件已知)，建立七自由度车辆动力学模型，如图2所示。图中，x轴为车辆纵向行驶方向，y轴为车辆侧向行驶方向，vx为纵向车速，vy为侧向车速，r为横摆角速度，β为质心侧偏角，a为质心距前轴的距离，b为质心距后轴的距离，δ为前轮转角，lF为前轮轮距，lR为后轮轮距，Fx1、Fx2、Fx3、Fx4分别为左前、右前、左后、右后轮胎的纵向力，Fy1、Fy2、Fy3、Fy4分别为左前、右前、左后、右后轮胎的侧向力，Mz1、Mz2、Mz3、Mz4分别为左前、右前、左后、右后轮胎绕z轴的转矩。式中，Iz为车辆绕z轴的转动惯量，m为车辆的质量，Mz为绕z轴的转矩。上式中的轮胎力通过Dugoff轮胎模型获得，Dugoff轮胎模型及其参数计算公式如下：式中，u为路面摩擦系数，Fz为轮胎所受垂直载荷，cx、cy为轮胎纵向刚度、侧偏刚度，α为轮胎侧偏角，λ为轮胎纵向滑移率，i＝1、2、3、4，分别代表左前轮、右前轮、左后轮、右后轮，ε为速度影响因子；式中，Re为轮胎有效半径，ω为轮胎转速，vw为车轮中心延其切面方向的速度；式中，h为车辆质心高度，g为重力加速度，FSZ为静态轮胎垂直载荷；基于Dugoff轮胎模型及其参数计算公式，所述轮胎纵向、侧向合力简写为：F～f(O)，O＝[vx,xy,r,ax,ay,u,δ,ω1,ω2,ω3,ω4]T。2.基于稀疏网格求积卡尔曼滤波理论实现车辆状态估计(1)选取多维积分点①确定单维积分点集及其权重本实施例选取Gauss-Hermite准则配置单维积分点及其权重，其单变量积分点集如下表1所示。表1Gauss-Hermite单维积分点及其权重②确定单维积分精度水平集合单维积分精度水平集合为：式中，q为辅助变量，满足L-n≤q≤L-1，L为精度水平；n为状态量维数；ik为单维积分精度水平；ik∈Ξ，本实施例以纵向速度、侧向速度、横摆角速度、纵向加速度、侧向加速度及车辆绕z轴转矩为状态向量x＝[vx,vy,r,ax,ay,Mz]T，状态量维数n＝6，选取精度水平L＝2；依据稀疏网格多维积分点配置理论，得单维积分精度水平集合③确定各积分点及其权重本实施例中多维积分点为六个单维积分点的张量积，若该积分点为一新积分点，其权重为：式中，w为积分点的权重；若该积分点已存在，其权重更新为：以q＝0时，取六个单维积分精度水平(1,1,1,1,1,1)为例说明：当单维积分精度水平集为时，其单维积分点集的张量积为可生成一个六维积分点ξ1＝[0,0,0,0,0,0]T；其权重为1，若后续所得积分点仍为[0,0,0,0,0,0]T，则更新权重。本实施例多维积分点ξi及其权重wi具体如下所示：式中，当i＝2,…,7时，-1在ξi的第一列至第六列变化，如i＝2时，ξ2＝[-1,0,0,0,0,0]T，i＝3时，ξ3＝[0,-1,0,0,0,0]T。(2)初始化本实施例以前轮转角、四轮轮轮速为输入u＝[δ,ω1,ω2,ω3,ω4,]T，以纵向加速度、侧向加速度及横摆角速度为观测量z＝[ax,ay,r]T，纵向速度、侧向速度、横摆角速度、纵向加速度、侧向加速度及车辆绕z轴转矩为状态向量x＝[vx,vy,r,ax,ay,Mz]T；设系统的状态初值x0＝[vx0,0,0,0,0,0]T，误差协方差矩阵初值为P0。(3)时间更新①对误差协方差矩阵分解Pk-1|k-1＝SST(12)其中，S为正交分解矩阵，k为当前时刻；②计算采样点式中，ξi为本实施例所采用的积分点；③状态预测a.轮胎力预测式中，分别表示矩阵γi的第一行第一列元素、第二行第一列元素、第三行第一列元素、第四行第一列元素、第五行第一列元素；b.状态量预测将所得采样点代入七自由度车辆模型得状态转移矩阵：积分点转换为：式中，T为采样时间；则状态更新为：④状态预测误差协方差式中，Q为系统噪声协方差矩阵；(4)量测更新①重采样②观测预测本实施例以纵向加速度、侧向加速度及横摆角速度为观测量，其观测方程为：③预测观测误差协方差式中，R为观测噪声协方差矩阵；④计算互协方差⑤计算卡尔曼增益⑥状态更新⑦状态协方差更新基于七自由车辆动力学模型，采用稀疏网格求积卡尔曼滤波估计方法，可有效地估计车辆的纵向车速、侧向速度和车辆质心侧偏角。3.仿真验证：本实施例通过Carsim/Matlab联合仿真验证所提状态估计方法，采用的车辆参数如表2所示。表2整车参数车辆模型参数数值整车质量m/kg1416质心至前轴距离a/m1.016质心至后轴距离b/m1.562质心高度h/m0.54横摆转动惯量Iz/kg·m21523前轮轮距lF/m1.539后轮轮距lR/m1.539轮胎有效半径Re/m0.31为验证本实施例所提方法的有效性，进行双移线工况仿真实验，双移线工况仿真参数设置：车速恒定为45km/h，路面附着系数μ＝0.85，采样时间为0.01s。状态量初值x0＝[45/3.6,0,0,0,0,0]T，Q＝100×I8×8，R＝I3×3。所得车辆纵向速度、侧向速度、质心侧偏角估计值如图3、图4、图5所示，由仿真结果可明显看出，本实施例所采用方法相对于扩展卡尔曼滤波方法，其精度得到较大的提高，即SGQF估计值与Carsim输出值更加接近。尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。当前第1页1 2 3