[发明专利]一种基于卡尔曼滤波算法的坡度估算方法

权利要求书

1.一种基于卡尔曼滤波算法的坡度估算方法，其特征在于，包括如下步骤：

1)离线标定卡尔曼滤波参数中的测量噪声协方差矩阵R和过程噪声协方差矩阵Q；

1.1)由加速度传感器测得加速度信号；由变速箱输出轴转速信号计算得到车速信号；

1.2)采集车辆加速度信号和车速信号，利用MATLAB分别计算车速信号标准差和加速度信号的标准差；

1.3)利用加速度信号标准差和车速信号标准差得到测量噪声协方差矩阵R；

1.4)过程噪声协方差矩阵Q取三阶单位矩阵；

2)获取坡度估算方程式

2.1)利用步骤1.3)得到的测量噪声协方差矩阵R和步骤1.4)得到的过程噪声协方差矩阵Q计算估计误差的协方差P_k的稳态值和卡尔曼增益K_k的稳态值；

2.2)将P_k和K_k的稳态值以常数形式写入卡尔曼滤波方程，结合系统状态变量和系统状态方程，计算得到坡度估算方程式；

3)建立坡度估算模型

利用MATLAB/Simulink，同时基于步骤2.3)得到的坡度估算方程式获取坡度估算模型；坡度估算模型以加速度传感器信号和变速箱输出轴转速信号为输入，以实时估算的实时路面坡度正弦值、车辆行驶加速度及速度为输出。

2.根据权利要求1所述的基于卡尔曼滤波算法的坡度估算方法，其特征在于，步骤1.3)中，所述测量噪声协方差矩阵R为：

其中，V₁与V₂分别为车速信号的标准差和加速度信号的标准差。

3.根据权利要求2所述的基于卡尔曼滤波算法的坡度估算方法，其特征在于，步骤2.2)中，计算得到坡度估算方程式的具体过程包括：

2.2.1)定义系统状态变量和系统状态方程：

系统状态变量为：x(k)＝[v(k) a_senk(k) sinα(k)]^T，

系统状态方程为：

其中，x(k)为卡尔曼滤波过程的状态变量估计值；v(k)为车速；a_senk(k)为卡尔曼滤波后的加速度；sinα(k)为道路坡度的正弦值；g为重力加速度；w₁为车速的过程噪声；w₂为加速度的过程噪声；w₃为道路坡度的正弦值的过程噪声；k为k时刻；k-1为k时刻的上一时刻；△t为k时刻与k-1时刻的时间差；

2.2.2)将步骤2.2.1)得到的系统状态方程转化为矩阵形式：

2.2.3)根据所述车速信号与加速度信号得到所述系统观测方程为：

z(k)＝v(k)+a_senk(k)+μ

则系统观测方程的矩阵形式为：

其中，w和μ分别为过程噪声和测量噪声信号，假设w和μ均为常数；

2.2.4)卡尔曼滤波算法中用于表示离散时间过程的差分方程为：

x(k)＝Ax(k-1)+Bu(k-1)+w(k-1)

相应的系统观测量表示为：

z(k)＝Hx(k)+μ(k)

其中，A为过程矩阵；B为控制矩阵；H为观测矩阵；u(k-1)表示系统输入变量在k-1时刻状态；

则将步骤2.2.3)得到的系统观测方程的矩阵形式和步骤2.2.4)得到的系统观测量，对应到卡尔曼滤波算法中，得到所述系统的过程矩阵A、控制矩阵B和观测矩阵H：

由于所述系统没有输入量，则B＝0；

2.2.5)将卡尔曼增益矩阵K_k的每个元素、步骤2.2.3)得到的系统观测方程，以及步骤2.2.4)得到的A、B与H均带入原始的卡尔曼滤波算法中，结合步骤2.2.1)定义的系统状态变量和系统状态方程，得到如下坡度估算方程式：

在时间更新下，

测量更新下，

其中，v(k)的初值为初始时刻的车速；a_senk(k)的初值为加速度传感器初始时刻测量值；sinα(k)的初值为0；K₁₁、K₁₂、K₂₁、K₂₂、K₃₁、K₃₂为简化后的卡尔曼增益，即原卡尔曼增益K_k三行两列矩阵的各矩阵元素；acc为k时刻加速度传感器测量值。