CN110588657B - 一种针对车辆运动状态及道路坡度的联合估计方法 - Google Patents

Abstract

本发明公开了一种针对车辆运动状态及道路坡度的联合估计方法，构造坡道行车平台，运用平方根变分贝叶斯自适应容积卡尔曼滤波算法，估计车辆运动状态，形成车辆运动状态估计模块；搭建道路坡度单元，基于岭回归算法，求解道路坡度值，形成道路坡度估计模块；两个模块之间采用交互迭代的方式，实现车辆运动状态及道路坡度的联合估计。本发明的平方根变分贝叶斯自适应容积卡尔曼滤波算法能够对状态协方差及量测噪声协方差的平方根进行自适应，相比于同类噪声自适应算法具有计算量小，实时性好，精度高的特点；车辆运动状态及道路坡度的联合估计，考虑了两者之间的联系，能够提高估计结果的准确性。

Description

一种针对车辆运动状态及道路坡度的联合估计方法

技术领域

本发明属于车辆运动状态估计领域，尤其涉及一种针对车辆运动状态及道路坡度的联合估计方法。

背景技术

自20世纪以来，世界汽车保有量持续增长，与此同时，与汽车相关的交通事故数量也逐年增多。在汽车上配置主动安全系统，能够提升汽车的行车稳定性，减少交通事故的发生。汽车主动安全系统通过搜集车辆运动状态信息及环境信息做出决策控制，提升汽车行驶的安全性。可以说，实时且准确的估计出车辆运动状态信息及环境信息是汽车主动安全系统能够及时响应的关键因素。

目前，针对行驶于水平路面上的车辆运动状态估计问题已取得一定成果。而针对行驶于坡道(上坡或下坡)的车辆运动状态估计问题，研究较少。实际行车道路大部分具有一定坡度，且在不同路段坡度会发生变化。现有估计道路坡度的方法，通常考虑道路坡度为定值，不符合实际情况，应用价值不高。道路坡度的变化会降低车辆行车稳定性，进而导致估计车辆运动状态时，过程噪声和量测噪声的变化加剧。为解决这一问题，现有技术提出了一种变分贝叶斯自适应容积卡尔曼滤波算法，但该算法在每一次估计状态时，都需要完成数次迭代自适应计算，大大增加了算法的计算量，导致实时性变差，无法满足实际需求。此外，道路坡度信息也是车辆运动状态估计的基础；而准确的车辆运动状态信息同样能够提高道路坡度估计的精度。因此，研究针对车辆运动状态及道路坡度的联合估计策略具有重要意义。

发明内容

针对现有技术中存在的问题，本发明提供了一种针对车辆运动状态及道路坡度的联合估计方法，以同时估计车辆运动状态和道路坡度的值。

本发明是通过以下技术方案实现上述技术目的的。

一种针对车辆运动状态及道路坡度的联合估计方法，构造坡道行车平台，运用平方根变分贝叶斯自适应容积卡尔曼滤波(Square-root Variational Bayesian AdaptiveCubature Kalman Filter，SVBACKF)算法估计车辆运动状态，建立车辆运动状态估计模块；搭建道路坡度单元，利用岭回归算法求解道路坡度值，建立道路坡度估计模块；两个模块之间交互迭代，实现车辆运动状态及道路坡度的联合估计。

进一步，所述坡道行车平台为：

其中，v_x为车辆纵向车速，v_y为车辆侧向车速，θ为道路坡度，g为重力加速度，φ、

分别为车辆的侧倾角、侧倾角速度、侧倾角加速度，r、

分别为车辆的横摆角速度、横摆角加速度，F_x、F_y分别为轮胎的纵向力、侧向力，下标1、2、3、4分别表示左前轮、右前轮、左后轮、右后轮，δ为前轮转角，I_z为车辆绕z轴的转动惯量，l_F、l_R分别为质心到前轴、后轴的距离，b_F为前轴宽度，b_R为后轴宽度，I_sx为簧上质量部分绕x轴的转动惯量，h_s为簧上质量部分的质心距侧倾中心的高度，a_y为侧向加速度，κ_φ为侧倾刚度，β_φ为侧倾阻尼，m为车辆质量，m_s为车辆簧上质量。

进一步，所述平方根变分贝叶斯自适应容积卡尔曼滤波算法包括初始预估和变分迭代，所述变分迭代，具体为：

步骤①，赋迭代初值；

步骤②，状态协方差平方根的变分贝叶斯自适应；

步骤③，量测噪声协方差平方根的变分贝叶斯自适应；

步骤④，系统状态及状态协方差平方根的迭代更新；

步骤⑤，判断是否达到迭代次数：若未达到迭代次数则返回步骤②，若已达到迭代次数则锁定变分迭代终值。

更进一步，所述步骤②具体为：利用变分贝叶斯法对联合概率密度函数做近似，取联合概率密度函数与近似联合概率密度函数的KL散度，并将KL散度最小化，由逆威沙特(Inverse Wishart，IW)分布性质，更新状态协方差平方根：

其中，上标i表示迭代次数，a_k为IW分布的参数，

为k时刻的车辆运动状态预估值，

为k-1时刻的车辆运动状态，

为k时刻的车辆运动状态协方差平方根预估值，S_k为k时刻的车辆运动状态协方差平方根，A_k为α维的参数方阵，

分别为a_k、A_k的预估值。

更进一步，所述步骤③具体为：

其中，b_k为IW分布的参数，B_k为β维的参数方阵，

分别为b_k、B_k的预估值，

是由量测方程更新的量测值，z_k为实际量测值，协方差平方根

为由容积点配置的量测矩阵。

更进一步，所述步骤④具体为：由自适应获得的状态协方差平方根及量测噪声协方差平方根，更新车辆运动状态及车辆运动状态协方差平方根。

进一步，所述道路坡度单元为：

其中，T_s为变速器输出转矩，ρ为空气密度，R_v为车轮半径，C_d为空气阻力系数，A为迎风面积，f为滚动阻力系数，ρ_air为空气密度。

进一步，所述交互迭代具体为：车辆行驶过程中，传感器获取驾驶员输入信号及量测输出，将输入信号输入坡道行车平台，将量测输出输入道路坡度单元和变分迭代；车辆运动状态估计模块与道路坡度估计模块同步分别估计当前时刻车辆运动状态和道路坡度的值，并将估计值反馈给另一模块，为下一时刻的联合估计做准备。如此交互迭代，实现每一时刻车辆运动状态与道路坡度的联合估计。

本发明的有益效果为：

1.本发明针对行驶于坡道的车辆，构造了车辆坡道行车平台，考虑了坡道对车辆运动状态估计的影响，提高估计精度。

2.本发明采用SVBACKF算法估计车辆运动状态，能够对状态协方差和量测噪声协方差平方根进行自适应，提高算法估计精度。同时，算法加入平方根思想，可以减少计算量，提升实时性。

3.本发明道路坡度估计模块中岭回归算法的运用在满足实时性的同时，能够减小道路坡度估计值的均方差误差，提高结果精度。

4.本发明提供的车辆运动状态及道路坡度的联合估计策略，确保进行每一时刻估计时，输入数据均实时准确，提高估计结果的准确性。从而为汽车主动安全系统提供实时精确的车辆运动状态及道路坡度信息。

附图说明

图1是联合估计方法流程图；

图2是坡道行车平台示意图；

图3是坡道行车平台侧倾运动示意图。

具体实施方式

下面将结合附图对本发明的具体方案作进一步的说明。

如图1所示，一种针对车辆运动状态及道路坡度的联合估计方法，包括以下步骤：

步骤一，建立坡道行车平台

如图2所示，假设车辆行驶于坡度为θ的道路上，研究车辆运动情况。定义车辆质心处为车辆坐标系坐标原点，车辆前进方向为x轴方向，车辆前进方向的左侧为y轴方向，垂直车辆向上为z轴方向。坡道行车平台涉及车辆的纵向、侧向、横摆及侧倾方向的运动，并做如下假设：①忽略空气阻力及侧向风影响；②忽略各零部件之间的间隙和摩擦；③简化车辆簧上质量为一长方体，车辆总质量集中于质心处。

纵向、侧向运动：考虑到车辆重力mg沿y轴方向分量较小(可忽略不计，如图2所示)，将车辆重力沿z轴负方向和x轴负方向分解为mgcosθ和mgsinθ：

横摆运动：道路坡度的影响导致车辆轮胎力大小发生变化，在车辆坐标系下分析横摆运动有：

侧倾运动：簧上质量m_s沿z轴负方向的分量为m_sgcosθ(如图3所示)：

综上，坡道行车平台的运动表示为：

其中，v_x为车辆纵向车速，v_y为车辆侧向车速，θ为道路坡度，g为重力加速度，φ、

分别为车辆的侧倾角、侧倾角速度、侧倾角加速度，r、

分别为车辆的横摆角速度、横摆角加速度，F_x、F_y分别为轮胎的纵向力、侧向力，下标1、2、3、4分别表示左前轮、右前轮、左后轮、右后轮，δ为前轮转角，I_z为车辆绕z轴的转动惯量，l_F、l_R分别为质心到前轴、后轴的距离，b_F为前轴宽度，b_R为后轴宽度，I_sx为簧上质量部分绕x轴的转动惯量，h_s为簧上质量部分的质心距侧倾中心的高度，a_y为侧向加速度，κ_φ为侧倾刚度，β_φ为侧倾阻尼。

纵向加速度a_x、侧向加速度a_y与纵向速度v_x、侧向速度v_y的关系为：

本实施例中，车辆前轮转角δ与方向盘转角δ₀的关系为：δ＝δ₀K，其中，K为角传动比。轮胎的纵向力F_x、侧向力F_y通过Dugoff轮胎模型获得：

其中，C_σ和C_α分别表示轮胎的纵向刚度和侧偏刚度，且有：

其中，μ为道路摩擦系数，α_t为轮胎侧偏角，λ为轮胎模型参数，轮胎纵向滑移率σ为：

其中，ω为轮胎转速，R_v为车轮半径，v_w为轮胎中心速度，i可取1、2、3、4，分别表示左前轮、右前轮、左后轮、右后轮。

由于道路坡度的影响，坡道行车平台的前后车轮之间存在载荷转移，各轮的垂直动载荷F_z为：

其中，h为车辆质心高度，轮胎静载荷F_sz为：

步骤二，车辆运动状态及状态协方差平方根的初始预估

结合步骤一中的坡道行车平台，选取车辆运动的状态量为

输入量为u＝[δ、F_xi、F_yi]，量测量为

i＝1，2，3，4；再选取式(4)、(5)分别为状态方程f(·)及量测方程h(·)，采样时间为ΔT。

本实施例假设过程噪声w和观测噪声v均为高斯白噪声，相应的协方差矩阵Q、R分别为Q＝Q₀Ι，

常数Q₀＝10000，I为单位矩阵，

分别为a_x、a_y、r、

的量测协方差。

由k-1时刻的车辆运动状态

预估k时刻的车辆运动状态

及k时刻的车辆运动状态协方差平方根

其中，ξ_j为容积点，采用三阶球面容积规则进行选取；

是k时刻由容积点配置的状态矩阵的预估值，u_k-1为k-1时刻的输入量，

过程噪声协方差平方根S_Q，k-1可由过程噪声协方差的乔列斯基分解获取：S_Q，k-1＝Chol(Q_k-1)，ω_j为容积权重矩阵，X_j,k-1是k-1时刻由容积点配置的状态矩阵，S_k-1为k-1时刻的状态协方差平方根。

步骤三，车辆运动状态的变分迭代

①赋迭代初值

将步骤二得到的预估值设为车辆运动状态和状态协方差平方根的迭代初值，即：

然后对量测噪声协方差平方根赋初值。

选取IW分布作为车辆运动状态协方差和量测噪声协方差的先验分布：

其中，

分别为α维和β维的参数方阵，a_k、b_k为IW分布的参数，z_1:k-1为量测值；由IW分布的性质，可得

进而，得到

R_k平方根

S_R,k的值。令

可得

其中调整参数τ≥0，常数α取4，

分别为a_k、A_k的预估值。

考虑到量测噪声变化缓慢，在处理噪声传递时引入遗忘因子ρ∈[0,1]；于是，k时刻的量测噪声参数迭代初值为：

其中，b_k-1、B_k-1分别为k-1时刻量测噪声IW分布的相关参数，

分别为b_k、B_k的预估值，常数β取4。

②状态协方差平方根的变分贝叶斯自适应

首先利用变分贝叶斯方法对联合概率密度函数做如下近似：

其中，

表示近似后验概率密度函数。

对上式两边求KL散度(Kullback-Leibler)可以得到：

最小化KL散度，得到如下方程组：

其中，X可分别表示

R_k，上式中符号

表示函数g(·)关于

的期望，

是关于

的常数。

通过定点迭代的方法求出

三者的次优近似解，得到车辆运动状态协方差

的自适应结果，进而，更新车辆运动状态协方差平方根

其中，上标i表示迭代次数；

③量测噪声协方差平方根的变分贝叶斯自适应

基于更新的车辆运动状态协方差平方根，获取新的容积点，结合车辆传感器实时数据计算量测值的协方差平方根γ。

其中，

为由容积点配置的量测矩阵。

同样，基于变分贝叶斯方法对量测噪声协方差平方根进行自适应：

④系统状态及状态协方差平方根的迭代更新

计算车辆运动状态的量测协方差平方根、状态与量测的互协方差：

其中，

确定卡尔曼增益、更新车辆运动状态和车辆运动状态协方差平方根：

⑤判断是否达到迭代次数

本实施例设置迭代次数N＝4，若未达最大迭代次数，则令i＝i+1，并跳至步骤②继续迭代；若已达最大迭代次数，则锁定变分迭代终值为：

步骤四，建立道路坡度单元

假设：①由变速器主轴到车轮之间的传动无损耗，即传动效率为1；②车辆沿道路坡度正向行驶。

结合车辆驱动力-行驶阻力平衡方程，可以确定道路坡度单元：

其中，f为滚动阻力系数，T_s为变速器输出转矩，C_d为空气阻力系数，A为迎风面积，ρ_air为空气密度。

步骤五，求解坡度值

考虑实际车辆由于乘客数量或后备箱物品的影响导致车辆质量m发生变化，因此假设m与θ同为未知量。将道路坡度单元表示为如下形式：ψ＝tΣ+Ω，其中ψ＝a_x，

Ω为残差矩阵。行车过程中车辆各参数存在扰动，产生病态矩阵。通过岭回归算法估计线性方程中的参数Σ，具体如下：

Σ＝[t^Tt+lI]^-1t^Tψ (22)

其中，l≥0为岭回归参数，可通过岭迹法确定；I为单位矩阵。

本实施例中，取k时刻及其之前9个采样时刻(总共10个时刻)的数据作为样本，用于岭回归算法。进一步，根据Σ的值求解道路坡度θ。

步骤六，车辆运动状态及道路坡度的联合估计

驾驶员正常驾驶车辆时，通过车辆传感器获得驾驶员输入信号u′(包含δ₀,ω_i)及量测输出z′(包含T_s,a_x,a_y,r,

)的值。将输入信号u′(δ₀,ω_i)输入坡道行车平台，将量测输出z′(a_x,T_s)输入道路坡度单元，将量测输出z′(a_x,a_y,r,

)输入变分迭代。

步骤一-三组成车辆运动状态估计模块，而道路坡度估计模块包括步骤四-五；上述两个模块之间采用交互迭代的方式，进行联合估计。

1时刻：预设联合估计策略中车辆运动状态和道路坡度的初值均为0。车辆运动状态估计模块接收道路坡度初值，结合坡道行车平台及SVBACKF算法，估计出此时的车辆运动状态值并反馈给道路坡度估计模块；同时，道路坡度估计模块接收车辆运动状态初值，结合道路坡度单元及岭回归算法，计算出此时的道路坡度值并反馈给车辆运动状态估计模块。

2时刻：车辆运动状态估计模块接收道路坡度估计模块反馈的道路坡度值，更新这一时刻的车辆运动状态并反馈到道路坡度估计模块；同时，道路坡度估计模块接收车辆运动状态估计模块反馈的车辆运动状态估计值，更新此时的道路坡度值并反馈到车辆运动状态估计模块。

3时刻及以后任意k时刻：车辆运动状态估计模块及道路坡度估计模块继续更新各自状态并反馈，直到结束。

通过这种方式获得每一时刻的估计值，实现车辆运动状态及道路坡度的联合估计。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化和修改，并不影响本发明的实质内容。在不冲突的情况下，本申请实施例中的特征可以任意相互组合。

Claims (4)

1.一种针对车辆运动状态及道路坡度的联合估计方法，其特征在于，构造坡道行车平台，运用平方根变分贝叶斯自适应容积卡尔曼滤波算法估计车辆运动状态，建立车辆运动状态估计模块；搭建道路坡度单元，利用岭回归算法求解道路坡度值，建立道路坡度估计模块；两个模块之间交互迭代，实现车辆运动状态及道路坡度的联合估计；

假设车辆行驶于坡度为θ的道路上，定义车辆质心处为车辆坐标系坐标原点，车辆前进方向为x轴方向，车辆前进方向的左侧为y轴方向，垂直车辆向上为z轴方向；坡道行车平台涉及车辆的纵向、侧向、横摆及侧倾方向的运动，搭建如下所述的坡道行车平台，具体为：

其中，v_x为车辆纵向车速，v_y为车辆侧向车速，θ为道路坡度，g为重力加速度，φ、

分别为车辆的侧倾角、侧倾角速度、侧倾角加速度，r、

分别为车辆的横摆角速度、横摆角加速度，F_x、F_y分别为轮胎的纵向力、侧向力，下标1、2、3、4分别表示左前轮、右前轮、左后轮、右后轮，δ为前轮转角，I_z为车辆绕z轴的转动惯量，l_F、l_R分别为质心到前轴、后轴的距离，b_F为前轴宽度，b_R为后轴宽度，I_sx为簧上质量部分绕x轴的转动惯量，h_s为簧上质量部分的质心距侧倾中心的高度，a_y为侧向加速度，κ_φ为侧倾刚度，β_φ为侧倾阻尼，m为车辆质量，m_s为车辆簧上质量；

所述平方根变分贝叶斯自适应容积卡尔曼滤波算法估计车辆运动状态，包括初始预估和变分迭代；

所述初始预估，具体为：

由k-1时刻的车辆运动状态

预估k时刻的车辆运动状态

及k时刻的车辆运动状态协方差平方根

其中，ξ_j为容积点，采用三阶球面容积规则进行选取；

是k时刻由容积点配置的状态矩阵的预估值，u_k-1为k-1时刻的输入量，

过程噪声协方差平方根S_Q，k-1可由过程噪声协方差的乔列斯基分解获取：S_Q，k-1＝Chol(Q_k-1)，ω_j为容积权重矩阵，X_j,k-1是k-1时刻由容积点配置的状态矩阵，S_k-1为k-1时刻的状态协方差平方根；

所述变分迭代，具体为：

步骤①，赋迭代初值；

步骤②，状态协方差平方根的变分贝叶斯自适应；

步骤③，量测噪声协方差平方根的变分贝叶斯自适应；

步骤④，系统状态及状态协方差平方根的迭代更新；

步骤⑤，判断是否达到迭代次数：若未达到迭代次数则返回步骤②，若已达到迭代次数则锁定变分迭代终值；

由车辆驱动力-行驶阻力平衡方程，建立道路坡度单元为：

其中，T_s为变速器输出转矩，R_v为车轮半径，C_d为空气阻力系数，A为迎风面积，f为滚动阻力系数，ρ_air为空气密度；

将道路坡度单元表示为：ψ＝tΣ+Ω，其中ψ＝a_x，

Ω为残差矩阵；通过岭回归算法估计线性方程中的参数Σ＝[t^Tt+lI]^-1t^Tψ，其中，l≥0为岭回归参数，可通过岭迹法确定；I为单位矩阵；根据Σ的值求解道路坡度值θ；

所述交互迭代具体为：车辆行驶过程中，传感器获取驾驶员输入信号及量测输出，将输入信号输入坡道行车平台，将量测输出输入道路坡度单元和变分迭代；车辆运动状态估计模块与道路坡度估计模块同步分别估计当前时刻车辆运动状态和道路坡度的值，并将估计值反馈给另一模块，为下一时刻的联合估计做准备；如此交互迭代，实现每一时刻车辆运动状态与道路坡度的联合估计；

所述车辆运动状态估计模块的建立为：基于坡道行车平台预估k时刻的车辆运动状态

及k时刻的车辆运动状态协方差平方根

再进行变分迭代，包括状态协方差平方根的变分贝叶斯自适应、量测噪声协方差平方根的变分贝叶斯自适应以及系统状态及状态协方差平方根的迭代更新；

所述道路坡度估计模块的建立为：基于道路坡度单元和岭回归算法，求解道路坡度值θ。

2.根据权利要求1所述的针对车辆运动状态及道路坡度的联合估计方法，其特征在于，所述步骤②具体为：利用变分贝叶斯方法对联合概率密度函数做近似，取联合概率密度函数与近似联合概率密度函数的KL散度，并将KL散度最小化；结合逆威沙特分布的性质，更新状态协方差平方根：

其中，上标i表示迭代次数，a_k为逆威沙特分布的参数，

为k时刻的车辆运动状态预估值，

为k-1时刻的车辆运动状态，

为k时刻的车辆运动状态协方差平方根预估值，S_k为k时刻的车辆运动状态协方差平方根，A_k为α维的参数方阵，

分别为a_k、A_k的预估值。

3.根据权利要求2所述的针对车辆运动状态及道路坡度的联合估计方法，其特征在于，所述步骤③具体为：

其中，b_k为逆威沙特分布的参数，B_k为β维的参数方阵，

分别为b_k、B_k的预估值，

是由量测方程更新的量测值，z_k为实际量测值，协方差平方根

为由容积点配置的量测矩阵。

4.根据权利要求3所述的针对车辆运动状态及道路坡度的联合估计方法，其特征在于，所述步骤④具体为：由自适应获得的状态协方差平方根及量测噪声协方差平方根，更新车辆运动状态及状态协方差平方根。

Images