[发明专利]横摆角速度滤波方法、控制终端、车辆及存储介质

说明书

本发明公开了一种横摆角速度滤波方法、控制终端、车辆及存储介质，所述方法包括：系统上电后，以预设采样频率采集横摆角速度测量值；将第一采样时刻采集的测量值和初始卡尔曼滤波模型作为当前横摆角速度滤波值和下一时刻卡尔曼滤波模型；当非第一采样时刻，采用当前的卡尔曼滤波模型计算当前滤波值与预测值；并当累积采集的测量值数量小于预设数量，将初始卡尔曼滤波模型作为下一时刻卡尔曼滤波模型；当累积采集的测量值数量大于或等于预设数量，根据当前和当前之前预设数量采集时刻的滤波值与预测值，获得下一采样时刻卡尔曼滤波模型的Q矩阵。本发明解决了现有基于固定Q的卡尔曼滤波法计算获得的横摆角速度滤波值的准确度低的问题。

技术领域

本发明涉及智能驾驶领域，尤其涉及一种横摆角速度滤波方法、控制终端、车辆及计算机可读存储介质。

背景技术

在主动安全系统控制车辆过程中，需要判断车辆的横摆角速度来进行决策计算。车辆底盘ESP系统所带的横摆角速度传感器测量出来的值，由于噪声原因，量测值会有较大波动，不能直接用来做控制，需要对其进行卡尔曼滤波法滤波。现有卡尔曼滤波法中将R和Q矩阵标定为固定值，这会导致滤波结果不能动态跟踪真实值，横摆角速度滤波值的准确度较低。

发明内容

本发明的主要目的在于提出一种横摆角速度滤波方法、控制终端、车辆及计算机可读存储介质，旨在解决现有基于固定Q的卡尔曼滤波法计算获得的横摆角速度滤波值的准确度低的问题。

为实现上述目的，本发明提供一种横摆角速度滤波方法，包括步骤：

当系统上电后，根据预设采样频率采集横摆角速度测量值；

每采集到一个横摆角速度测量值时，判断当前采样时刻是否为系统上电后的第一个采样时刻；

若当前采样时刻为系统上电后的第一个采样时刻，则将当前采样时刻的横摆角速度测量值和预设的系统误差协方差矩阵分别作为当前采样时刻的横摆角速度滤波值和系统误差协方差矩阵以及将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；

若当前采样时刻不为系统上电后的第一个采样时刻，则根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵；

判断当前采样时刻前累积采集的横摆角速度测量值的数量是否大于或等于预设数量；

若否，则将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；

若是，则根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵；

根据下一采样时刻的环境噪声协方差矩阵更新当前采样时刻的卡尔曼滤波模型，获得下一采样时刻的卡尔曼滤波模型。

可选地，所述根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵的步骤包括：

根据前一采样时刻的车辆横摆角速度滤波值和当前采样时刻的卡尔曼滤波模型中的预测值计算公式，获得当前采样时刻的横摆角速度预测值，其中预测值计算公式为X_pk＝A*X_k-1，X_pk为当前采样时刻的横摆角速度预测值、X_k-1为前一采样时刻的车辆横摆角速度滤波值和A为预设状态转移矩阵；