CN112287289A - 一种面向云控智能底盘的车辆非线性状态融合估计方法 - Google Patents

Abstract

本发明公开了一种面向云控智能底盘的车辆非线性状态融合估计方法，包括：建立包含质心纵向、侧向、横摆及侧倾运动的车辆四自由度运动微分方程；根据车辆四自由度运动微分方程建立车辆非线性状态方程和观测方程；将车辆非线性状态方程和观测方程进行近似线性化处理得到车辆状态参数的状态方程和观测方程；将线性化后的运动状态参数迭代至局部强跟踪滤波器，获得车辆非线性状态的局部无偏估计值，并将得到的局部无偏估计值进行线性组合最优融合，得到车辆非线性状态融合估计值，实现对车辆底盘子系统的闭环控制。本发明在有效解决传统扩展卡尔曼滤波状态估计方法带来的滤波发散问题基础上，克服因个别传感器误差过大造成的估计误差偏大的问题。

Description

一种面向云控智能底盘的车辆非线性状态融合估计方法

技术领域

本发明属于智能驾驶环境感知领域，具体涉及一种面向云控智能底盘的车辆非线性状态融合估计方法。

背景技术

目前的车辆底盘控制系统正在朝功能多元化、系统集成化、体积微型化和通信网络化等方向发展，控制系统已变得愈来愈复杂。在车辆底盘主动控制系统中，表征车辆自身运行状态的横摆角速度、纵横向速度、车身侧偏角等关键状态变量，既是车辆底盘集成控制系统中的主要控制变量，也是实时辨识车辆行驶状态及制定底盘子系统协调控制规则的重要依据。这些状态变量通常通过陀螺仪、加速度传感器、车轮角速度传感器等车载传感器来直接测量，但由于汽车动力学控制过程的复杂性及车载传感器的测试水平和测试成本等多方面的影响，很多关键状态变量无法直接、准确或低成本的测量。

现有的汽车行驶状态估计方法(如授权公布号CN106250591B)主要是先建立汽车质心运动、横摆运动及侧倾运动等具有非线性特征的运动微分方程，再用扩展Kalman滤波进行间接的车辆状态参数估计。但在工程应用中，由于动态系统建模的不确定性及系统本身元器件的不稳定性因素的影响，给系统噪声和观测噪声统计特性的准确描述带来了很大的困难，因此，如果采用预先建立噪声模型的常规Kalman或扩展Kalman滤波器，将会出现状态估计不准，甚至发散等现象，导致无法实现中央控制系统对车辆底盘系统的精准控制。

发明内容

针对于上述现有技术的不足，本发明的目的在于提供一种面向云控智能底盘的车辆非线性状态融合估计方法，本发明通过以车辆非线性动力学模型为基础，将强跟踪滤波器理论及多传感器信息融合技术引入到车辆非线性状态估计中，在有效解决传统扩展卡尔曼滤波状态估计方法带来的滤波发散问题基础上，进一步克服因个别传感器误差过大造成的估计误差偏大的问题，并改善车辆状态信息量测的不完全性，从而实现用于判断车辆行驶状态的关键变量乃至云控智能底盘主动控制关键变量的实时融合估计。在当前车辆非线性状态估计方法的基础上，进一步提升了车辆非线性状态估计的精准度。

为达到上述目的，本发明采用的技术方案如下：

本发明的一种面向云控智能底盘的车辆非线性状态融合估计方法，步骤如下：

1)建立包含质心纵向、侧向、横摆及侧倾运动的车辆四自由度运动微分方程；

2)根据所述车辆四自由度运动微分方程建立车辆非线性状态方程和观测方程；

3)将所述车辆非线性状态方程和观测方程进行近似线性化处理得到关于车辆状态参数的状态方程和观测方程；

4)将所述线性化后的运动状态参数迭代至局部强跟踪滤波器，获得车辆非线性状态的局部无偏估计值，并将得到的局部无偏估计值进行线性组合最优融合，得到车辆非线性状态融合估计值，从而实现对车辆底盘子系统的闭环控制。

进一步地，所述步骤1)中的运动微分方程为：

式中，m为整车质量，u为纵向速度，v为侧向速度，φ、r分别为侧倾、横摆角速度；h₀为质心到侧倾轴距离；h_r为侧倾中心高度；h_rf、h_rr分别为前后轴到侧倾轴距离；ε为侧倾轴与纵轴之间的夹角；δ为前轮转角；I_b,x为绕X轴转动惯量，I_b,z为绕Z轴转动惯量，I_b,xz为绕X、Z轴的惯量积；t_f为前轮距，t_r为后轮距；k_f为前轮胎侧偏刚度，k_r为后轮胎侧偏刚度；a为质心至前轴距离，b为质心至后轴距离；c_f为前悬架侧倾角阻尼，c_r为后悬架侧倾角阻尼；F_y1为左前轮侧向力，F_y2为右前轮侧向力，F_y3为左后轮侧向力，F_y4为右后轮侧向力；F_x1为左前轮纵向力，F_x2为右前轮纵向力，F_x3为左后轮纵向力，F_x4为右后轮纵向力。

进一步地，所述步骤2)中的车辆非线性状态方程和观测方程为：

式中，状态变量x(k)＝[u,v,r,p,φ]^T，观测矢量y(k)＝[z_y1,z_y2,…,z_yn]^T，二者与传感器的种类及其数量有关，输入变量u(k)＝[δ]，w(k)、η_i(k)分别为过程噪声和测量噪声，两者为相互独立、均值为零的高斯白噪声。

进一步地，所述步骤3)中的关于车辆状态参数的状态方程为：

F_x(k)为非线性函数f(x(k),u(k),k)对状态x(k)的一阶偏导，计算公式如下：

进一步地，所述步骤3)中的关于车辆状态参数的观测方程为：

H_xix(k)为非线性函数h_i(x(k),k)对状态x(k)的一阶偏导，计算公式如下：

进一步地，所述步骤4)中的局部强跟踪滤波器包括状态预测过程：

状态预测方程为：

误差协方差预测方程：

其中，

Λ(k+1)＝diag{λ₁(k+1),λ₂(k+1),…,λ_n(k+1)}

式中，α_i≥1，i＝1,2,…,n，n为传感器的个数，β≥1为一个预先选定的弱化因子，取值为1.3；tr[·]为求迹算子；V₀(k+1)为输出序列的均方误差阵，γ(k+1)为残差序列；0＜ρ≤1为遗忘因子，取ρ为0.96。

进一步地，所述残差序列：

进一步地，所述局部强跟踪滤波器还包括状态更新(校正)过程：

状态更新估计方程：

误差协方差更新方程：

其中，增益矩阵K(k+1)的计算公式如下：

进一步地，所述局部无偏估计的估计误差为：

局部无偏估计的误差协方差矩阵为：

进一步地，所述线性组合最优融合估计为：

线性组合最优融合估计误差协方差矩阵为：

其中，最优融合加权系数a_i计算公式如下：

本发明的有益效果：

本发明通过以车辆非线性动力学模型为基础，将强跟踪滤波器技术及多传感器信息融合技术引入到车辆非线性状态估计中，在有效解决传统扩展卡尔曼滤波状态估计方法带来的滤波发散问题基础上，进一步克服因个别传感器误差过大造成的估计误差偏大的问题，并改善车辆状态信息量测的不完全性，从而实现用于判断车辆行驶状态的关键变量乃至云控智能底盘主动控制关键变量的实时融合估计。在当前车辆非线性状态估计方法的基础上，进一步提升了车辆非线性状态估计的精准度。

附图说明

图1为本发明方法的流程图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

参照图1所示，本发明的一种面向云控智能底盘的车辆非线性状态融合估计方法，包括以下步骤：

为了较全面地反映车辆非线性状态，实时融合估计出云控智能底盘控制中必需而又难以直接获取的关键状态，基于非线性四自由度汽车模型，推导包含车辆纵向、横向、侧倾及横摆运动的微分方程如下：

X方向的受力平衡方程：

其中，u为纵向速度，v为侧向速度；φ、r分别为侧倾、横摆角速度；h₀为质心到侧倾轴距离；ε为侧倾轴与纵轴之间的夹角；δ为前轮转角；F_y1为左前轮侧向力，F_y2为右前轮侧向力；F_x1为左前轮纵向力，F_x2为右前轮纵向力，F_x3为左后轮纵向力，F_x4为右后轮纵向力；

Y方向的受力平衡方程：

式中，F_y3为左后轮侧向力，F_y4为右后轮侧向力；

绕Z轴的力矩方程：

式中，I_b,x为绕X轴转动惯量，I_b,z为绕Z轴转动惯量，I_b,xz为绕X、Z轴的惯量积；a为质心至前轴距离，b为质心至后轴距离；t_f为前轮距，t_r为后轮距；

绕X轴的力矩方程：

式中，h_r为侧倾中心高度；h_rf、h_rr分别为前后轴到侧倾轴距离；k_f为前轮胎侧偏刚度，k_r为后轮胎侧偏刚度；c_f为前悬架侧倾角阻尼，c_r为后悬架侧倾角阻尼。

根据估计对象建立状态方程与量测方程，对非线性模型进行线性化并赋初值进行递推估计，主要包括预测过程与校正过程，其具体过程如下：

步骤1)：建立车辆非线性状态方程与量测方程：

式中，状态变量x(k)＝[u,v,r,p,φ]^T，观测矢量y(k)＝[z_y1,z_y2,…,z_yn]^T，二者与传感器的种类及其数量有关，输入变量u(k)＝[δ]，w(k)、η_i(k)分别为过程噪声和测量噪声，两者为相互独立、均值为零的高斯白噪声；

步骤2)：假设车辆以恒定纵向车速u和驱动轮转速w运动，则状态变量即变为x(k)＝[v,r,p,φ]^T；

步骤3)：车辆非线性状态方程与量测方程进行线性化，得到新的状态方程与观测方程：

其中，

步骤4)：预估计，即状态预测方程为：

其中，

步骤5)：计算预测误差协方差：

其中，

为k时刻过程噪声的协方差矩阵；

Λ(k+1)＝diag{λ₁(k+1),λ₂(k+1),…,λ_n(k+1)}

式中，Λ(·)为次优渐消因子，引入次优渐消因子的目的是使残差序列在每一步相互正交，表明残差序列中的所有有用信息都已被提取出来，用作对当前时刻系统状态的估计；α_i≥1，i＝1,2,…,n，n为传感器的个数，β≥1为一个预先选定的弱化因子，取值为1.3，引入弱化因子的目的是使状态估计值更加平滑；tr[·]为求迹算子；V₀(k+1)为输出序列的均方误差阵，γ(k+1)为残差序列；0＜ρ≤1为遗忘因子，取ρ为0.96；

步骤6)：计算残差序列：

其中，

步骤7)：校正过程，更新方程为：

增益矩阵为：

状态更新估计方程为：

更新误差协方差为：

步骤8)：信息融合过程，对状态变量x(k)的n个无偏估计值

进行线性组合最优融合，即可得到更为精确的综合航迹；

对各局部强跟踪滤波器输出的无偏估计进行误差估计：

计算各无偏估计的误差协方差矩阵：

对

作无偏估计线性组合，即：

式中，a_i为权值系数，则将多传感器线性组合最优融合估计的主要问题即转化为准确求解

加权系数；由于

与

不相关，则有：

式中，P(k)为

的协方差矩阵。

建立极小化准则

采用拉格朗日法，引入辅助函数：

令

最终可得多传感器线性组合最优融合加权系数为：

至此可完成信息融合过程。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (10)

1.一种面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，步骤如下：

1)建立包含质心纵向、侧向、横摆及侧倾运动的车辆四自由度运动微分方程；

2)根据所述车辆四自由度运动微分方程建立车辆非线性状态方程和观测方程；

3)将所述车辆非线性状态方程和观测方程进行近似线性化处理得到关于车辆状态参数的状态方程和观测方程；

4)将所述线性化后的运动状态参数迭代至局部强跟踪滤波器，获得车辆非线性状态的局部无偏估计值，并将得到的局部无偏估计值进行线性组合最优融合，得到车辆非线性状态融合估计值，从而实现对车辆底盘子系统的闭环控制。

2.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述步骤1)中的运动微分方程为：

式中，m为整车质量，u为纵向速度，v为侧向速度，φ、r分别为侧倾、横摆角速度；h₀为质心到侧倾轴距离；h_r为侧倾中心高度；h_rf、h_rr分别为前后轴到侧倾轴距离；ε为侧倾轴与纵轴之间的夹角；δ为前轮转角；I_b,x为绕X轴转动惯量，I_b,z为绕Z轴转动惯量，I_b,xz为绕X、Z轴的惯量积；t_f为前轮距，t_r为后轮距；k_f为前轮胎侧偏刚度，k_r为后轮胎侧偏刚度；a为质心至前轴距离，b为质心至后轴距离；c_f为前悬架侧倾角阻尼，c_r为后悬架侧倾角阻尼；F_y1为左前轮侧向力，F_y2为右前轮侧向力，F_y3为左后轮侧向力，F_y4为右后轮侧向力；F_x1为左前轮纵向力，F_x2为右前轮纵向力，F_x3为左后轮纵向力，F_x4为右后轮纵向力。

3.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述步骤2)中的车辆非线性状态方程和观测方程为：

式中，状态变量x(k)＝[u,v,r,p,φ]^T，观测矢量y(k)＝[z_y1,z_y2,…,z_yn]^T，二者与传感器的种类及其数量有关，输入变量u(k)＝[δ]，w(k)、η_i(k)分别为过程噪声和测量噪声，两者为相互独立、均值为零的高斯白噪声。

4.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述步骤3)中的车辆状态参数的状态方程为：

F_x(k)为非线性函数f(x(k),u(k),k)对状态x(k)的一阶偏导，计算公式如下：

5.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述步骤3)中的车辆状态参数的观测方程为：

H_xix(k)为非线性函数h_i(x(k),k)对状态x(k)的一阶偏导，计算公式如下：

6.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述步骤4)中的局部强跟踪滤波器包括状态预测过程：

状态预测方程为：

误差协方差预测方程：

其中，

Λ(k+1)＝diag{λ₁(k+1),λ₂(k+1),…,λ_n(k+1)}

式中，Λ(·)为次优渐消因子，α_i≥1，i＝1,2,…,n，n为传感器的个数，β≥1为一个预先选定的弱化因子，取值为1.3；tr[·]为求迹算子；V₀(k+1)为输出序列的均方误差阵，γ(k+1)为残差序列；0＜ρ≤1为遗忘因子，取ρ为0.96。

7.根据权利要求6所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述残差序列：

8.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述局部强跟踪滤波器还包括状态更新过程：

状态更新估计方程：

误差协方差更新方程：

其中，增益矩阵K(k+1)的计算公式如下：

9.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述局部无偏估计的估计误差为：

局部无偏估计的误差协方差矩阵为：

10.根据权利要求1所述的面向云控智能底盘的车辆非线性状态融合估计方法，其特征在于，所述线性组合最优融合估计为：

线性组合最优融合估计误差协方差矩阵为：

最优融合加权系数a_i计算公式如下：

Images