CN112660144A - 横摆角速度滤波方法、控制终端、车辆及存储介质 - Google Patents

Abstract

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

Description

横摆角速度滤波方法、控制终端、车辆及存储介质

技术领域

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

背景技术

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第一系统预测误差协方差矩阵计算公式为P_pk＝A*P_k-1*A^T+Q_k，P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵；

根据当前采样时刻的系统预测误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的卡尔曼增益计算公式，获得当前采样时刻的卡尔曼增益，其中卡尔曼增益计算公式为K_k＝P_pk*H^T/(H*P_pk*H^T+R_k)，K_k为当前采样时刻的卡尔曼增益、H为预设观测矩阵以及R_k为当前采样时刻的传感器噪声协方差矩阵；

根据当前采样时刻的横摆角速度预测值、横摆角速度测量值和卡尔曼增益和当前采样时刻的卡尔曼滤波模型中的横摆角速度滤波值计算公式，获得当前采样时刻的横摆角速度滤波值，其中横摆角速度滤波值计算公式为X_k＝X_pk+K_k(Z_k-H*X_pk)，其中X_k为当前采样时刻的横摆角速度滤波值，Z_k为当前采样时刻的横摆角速度测量值；

根据当前采样时刻的卡尔曼增益、系统预测误差协方差矩阵和卡尔曼滤波模型中的系统误差协方差矩阵计算公式，获得当前采样时刻的系统误差协方差矩阵，其中系统误差协方差矩阵计算公式为P_k＝(1-K_k*H)*P_pk，P_k为当前采样时刻的系统误差协方差矩阵。

可选地，所述根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵的步骤包括：

计算当前采样时刻横摆角速度预测值与横摆角速度滤波值间的第一差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度预测值与横摆角速度滤波值间的第一差值；

计算第一差值的样本方差，并作为下一采样时刻的环境噪声协方差矩阵；

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

将当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵更新为下一采样时刻的环境噪声协方差矩阵，获得更新后的卡尔曼滤波模型；

将更新后的卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型。

可选地，所述根据下一采样时刻的环境噪声协方差矩阵更新当前采样时刻的卡尔曼滤波模型，获得下一采样时刻的卡尔曼滤波模型的步骤之前还包括：

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

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

将当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵和传感器噪声协方差矩阵对应更新为下一采样时刻的环境噪声协方差矩阵和传感器噪声协方差矩阵，获得更新后的卡尔曼滤波模型；

将更新后的卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型。

可选地，所述根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值，获得下一采样时刻的传感器噪声协方差矩阵的步骤包括：

计算当前采样时刻横摆角速度测量值与横摆角速度滤波值间的第二差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值间的第二差值；

计算第二差值的样本方差，并作为下一采样时刻的传感器噪声协方差矩阵。

可选地，所述当系统上电后，根据预设采样频率采集横摆角速度测量值的步骤包括：

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

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

根据当前采样时刻的横向加速度以及多个预设横向加速度区间的数值范围，确定当前采样时刻对应的预设横向加速度区间；

根据当前采样时刻对应的预设横向加速度区间以及预设横向加速度区间与存储队列的对应关系，确定当前采样时刻对应的存储队列，其中，存储队列的最大存储数据量为预设数量；

根据当前采样时刻对应的存储队列以及存储队列与传感器噪声协方差矩阵的对应关系，确定当前采样时刻对应的传感器噪声协方差矩阵；

将当前采样时刻的卡尔曼滤波模型中的传感器噪声协方差矩阵替换为当前采样时刻对应的传感器噪声协方差矩阵，获得当前采样时刻的新卡尔曼滤波模型；

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

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

计算当前采样时刻的横摆角速度测量值和横摆角速度滤波值间的第二差值，并将当前采样时刻的第二差值按照先进先出方式存储至当前采样时刻对应的存储队列；

判断当前采样时刻对应的存储队列是否已满；

当当前采样时刻对应的存储队列已满时，根据该存储队列中已存储的第二差值，计算第二差值的样本方差，作为该存储队列对应的新传感器噪声协方差矩阵；

根据该存储队列对应的新传感器噪声协方差矩阵，更新存储队列与传感器噪声协方差矩阵对应关系。

可选地，所述根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵的步骤之前还包括：

根据预设差值平方计算公式、预设观测矩阵、当前采样时刻的横摆角速度预测值和横摆角速度测量值，计算获得当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方，其中，所述预设差值平方计算公式为D＝(X_pk-Z_k*H^-1)²；

计算当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值；

根据当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值以及多个预设比值区间的数值范围，确定当前采样时刻对应的预设比值区间；

根据当前采样时刻对应的预设比值区间以及预设比值区间与校正系数的对应关系，确定当前采样时刻对应的校正系数；

所述根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵的步骤包括：

根据当前采样时刻对应的校正系数、前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第二系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第二系统预测误差协方差矩阵计算公式为P_pk＝A*P_k-1*A^T+α_kQ_k，P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵，α_k为当前采样时刻对应的校正系数。

为实现上述目的，本发明还提供一种控制终端，所述控制终端包括存储器、处理器和存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如上所述的横摆角速度滤波方法的步骤。

为实现上述目的，本发明还提供一种车辆，所述车辆包括如前述的控制终端。

为实现上述目的，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的横摆角速度滤波方法的步骤。

本发明提出的一种横摆角速度滤波方法、控制终端、车辆及计算机可读存储介质，通过当系统上电后，根据预设采样频率采集横摆角速度测量值；每采集到一个横摆角速度测量值时，判断当前采样时刻是否为系统上电后的第一个采样时刻；若当前采样时刻为系统上电后的第一个采样时刻，则将当前采样时刻的横摆角速度测量值和预设的系统误差协方差矩阵分别作为当前采样时刻的横摆角速度滤波值和系统误差协方差矩阵以及将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；若当前采样时刻不为系统上电后的第一个采样时刻，则根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵；判断当前采样时刻前累积采集的横摆角速度测量值的数量是否大于或等于预设数量；若否，则将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；若是，则根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵；根据下一采样时刻的环境噪声协方差矩阵更新当前采样时刻的卡尔曼滤波模型，获得下一采样时刻的卡尔曼滤波模型。在达到一定采样数量后，在每个采样时刻使用卡尔曼滤波过程中利用已经获得的多个采样时刻的预测值与滤波值来动态更新环境噪声协方差矩阵Q，从而实现了自适应的效果，有利于提高滤波后的横摆角速度的准确度。

附图说明

图1是本发明实施例方案涉及的硬件运行环境的结构示意图；

图2为本发明横摆角速度滤波方法第一实施例的流程示意图；

图3为本发明横摆角速度滤波方法第一实施例中步骤S40的细化流程示意图；

图4为本发明横摆角速度滤波方法第一实施例中步骤S70的细化流程示意图；

图5为本发明横摆角速度滤波方法第二实施例的流程示意图；

图6为本发明横摆角速度滤波方法第二实施例的流程示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请参照图1，图1为本发明各个实施例中所提供的控制终端的硬件结构示意图。所述控制终端包括通信模块01、存储器02及处理器03等部件。本领域技术人员可以理解，图1中所示出的控制终端还可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。其中，所述处理器03分别与所述存储器02和所述通信模块01连接，所述存储器02上存储有计算机程序，所述计算机程序同时被处理器03执行。

通信模块01，可通过网络与外部设备连接。通信模块01可以接收外部设备发出的数据，还可发送数据、指令及信息至所述外部设备，所述外部设备可以是手机、平板电脑、笔记本电脑和台式电脑等电子设备。

存储器02，可用于存储软件程序以及各种数据。存储器02可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵)等；存储数据区可存储根据控制终端的使用所创建的数据或信息等。此外，存储器02可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

处理器03，是控制终端的控制中心，利用各种接口和线路连接整个控制终端的各个部分，通过运行或执行存储在存储器02内的软件程序和/或模块，以及调用存储在存储器02内的数据，执行控制终端的各种功能和处理数据，从而对控制终端进行整体监控。处理器03可包括一个或多个处理单元；优选的，处理器03可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、用户界面和应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器03中。

尽管图1未示出，但上述控制终端还可以包括电路控制模块，电路控制模块用于与市电连接，实现电源控制，保证其他部件的正常工作。

本领域技术人员可以理解，图1中示出的控制终端结构并不构成对控制终端的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。

根据上述硬件结构，提出本发明方法各个实施例。

参照图2，在本发明横摆角速度滤波方法的第一实施例中，所述横摆角速度滤波方法包括步骤：

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

步骤S20，每采集到一个横摆角速度测量值时，判断当前采样时刻是否为系统上电后的第一个采样时刻；若当前采样时刻为系统上电后的第一个采样时刻，则执行步骤S30；若当前采样时刻不为系统上电后的第一个采样时刻，则执行步骤S40；

在本方案中，当车辆上的控制终端的系统上电后，会启动横摆角速度传感器测量，通过横摆角速度传感器根据预设采样频率测量车辆的横摆角速度，以采集车辆横摆角速度。每采集到一个横摆角速度测量值时，会先判断当前采样时刻是否为系统上电后的第一个采样时刻，即判断当前采集的横摆角速度测量值是否为本次系统上电后采集的第一个横摆角速度测量值。

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

当确定当前采样时刻为本次系统上电后的第一个采样时刻后，不会对第一个采样时刻采集的横摆角速度测量值进行卡尔曼滤波，而是直接将该测量值作为当前采样时刻的横摆角速度滤波值，并且将预设的系统误差协方差矩阵作为当前采样时刻的系统误差协方差矩阵，以及将预设的初始卡尔曼滤波模型作为下一时刻的卡尔曼滤波模型，即第二个采样时刻的卡尔曼滤波模型为初始卡尔曼滤波模型。

本实施例中，预设的系统误差协方差矩阵为

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

将前一采样时刻的车辆横摆加速度滤波值和系统误差协方差矩阵和当前采样时刻的车辆横摆角速度初始值输入值当前采样时刻的卡尔曼滤波模型，进行卡尔曼滤波处理，计算获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵。

具体地的，请参照图3，图3为本申请一实施例中步骤S40的流程细化示意图，基于上述实施例，步骤S40包括：

步骤S41，根据前一采样时刻的车辆横摆角速度滤波值和当前采样时刻的卡尔曼滤波模型中的预测值计算公式，获得当前采样时刻的横摆角速度预测值，其中预测值计算公式为：

X_pk＝A*X_k-1，

X_pk为当前采样时刻的横摆角速度预测值、X_k-1为前一采样时刻的车辆横摆角速度滤波值和A为预设状态转移矩阵；

将前一采样时刻的车辆横摆角速度滤波值输入至当前采样时刻的卡尔曼滤波模型中的预测值计算公式中，计算获得当前采样时刻的横摆角速度预测值，其中预测值计算公式为：

X_pk＝A*X_k-1，

X_pk为当前采样时刻的横摆角速度预测值、X_k-1为前一采样时刻的车辆横摆角速度滤波值和A为预设状态转移矩阵，

dt为0.01～0.1范围内任一值，本实施例中优选dt＝0.02。

步骤S42，根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第一系统预测误差协方差矩阵计算公式为：

P_pk＝A*P_k-1*A^T+Q_k，

P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵；

将前一采样时刻的系统误差协方差矩阵输入至当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式中，计算获得当前采样时刻的系统预测误差协方差矩阵，其中第一系统预测误差协方差矩阵计算公式为：

P_pk＝A*P_k-1*A^T+Q_k，

P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵。

需要说明的是，步骤S41和步骤S42可以同时执行，也可以先执行步骤S41，还可以先执行步骤S42，在此不对步骤S41和步骤S42之间的执行顺序进行限定。

步骤S43，根据当前采样时刻的系统预测误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的卡尔曼增益计算公式，获得当前采样时刻的卡尔曼增益，其中卡尔曼增益计算公式为：

K_k＝P_pk*H^T/(H*P_pk*H^T+R_k)，

K_k为当前采样时刻的卡尔曼增益、H为预设观测矩阵以及R_k为当前采样时刻的传感器噪声协方差矩阵；

在计算获得当前采样时刻的系统预测误差协方差矩阵之后，会将当前采样时刻的系统预测误差协方差矩阵输入至当前采样时刻的卡尔曼滤波模型中的系统预测误差协方差矩阵计算公式中，计算获得当前采样时刻的卡尔曼增益，其中系统预测误差协方差矩阵计算公式为：

K_k＝P_pk*H^T/(H*P_pk*H^T+R_k)，

K_k为当前采样时刻的卡尔曼增益、H为预设观测矩阵以及R_k为当前采样时刻的传感器噪声协方差矩阵，

步骤S44，根据当前采样时刻的横摆角速度预测值、横摆角速度测量值和卡尔曼增益和当前采样时刻的卡尔曼滤波模型中的横摆角速度滤波值计算公式，获得当前采样时刻的横摆角速度滤波值，其中横摆角速度滤波值计算公式为：

X_k＝X_pk+K_k(Z_k-H*X_pk)，

其中X_k为当前采样时刻的横摆角速度滤波值，Z_k为当前采样时刻的横摆角速度测量值；

在计算获得当前采样时刻的卡尔曼增益之后，会将前采样时刻的横摆角速度预测值、横摆角速度测量值和卡尔曼增益输入至当前采样时刻的卡尔曼滤波模型中的横摆角速度滤波值计算公式中，计算获得获得当前采样时刻的横摆角速度滤波值，其中横摆角速度滤波值计算公式为：

X_k＝X_pk+K_k(Z_k-H*X_pk)，

其中X_k为当前采样时刻的横摆角速度滤波值，Z_k为当前采样时刻的横摆角速度测量值。

步骤S45，根据当前采样时刻的卡尔曼增益、系统预测误差协方差矩阵和卡尔曼滤波模型中的系统误差协方差矩阵计算公式，获得当前采样时刻的系统误差协方差矩阵，其中系统误差协方差矩阵计算公式为：

P_k＝(1-K_k*H)*P_pk，

P_k为当前采样时刻的系统误差协方差矩阵。

在计算获得当前采样时刻的卡尔曼增益之后，还会将当前采样时刻的卡尔曼增益和系统预测误差协方差矩阵输入至卡尔曼滤波模型中的系统误差协方差矩阵计算公式中，计算获得当前采样时刻的系统误差协方差矩阵，其中系统误差协方差矩阵计算公式为：

P_k＝(1-K_k*H)*P_pk，P_k为当前采样时刻的系统误差协方差矩阵。

需要说明的是，步骤S44和步骤S45可以同时执行，也可以先执行步骤S44，还可以先执行步骤S45，在此不对步骤S44和步骤S45之间的执行顺序进行限定。

步骤S50，判断当前采样时刻前累积采集的横摆角速度测量值的数量是否大于或等于预设数量；若否，则执行步骤S60；若是，则执行步骤S70；

当计算出当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵后，会继续判断当前采样时刻之前累积采集的横摆角速度测量值的数量是否大于或等于预设数量，该预设数量可为大于或等于3的任一整数值，优选为50。

步骤S60，将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；

当确定当前采样时刻不为本次系统上电后的第一采样时刻，并且本次系统上电后当前采样时刻之前累积采样的横摆角速度测量值的数量小于预设数量时，会将预设的初始卡尔曼滤波模型作为下一个采样时刻的卡尔曼滤波模型。

本实施例中，预设的初始卡尔曼滤波模型中的环境噪声协方差矩阵为

传感器噪声协方差矩阵为

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

当确定当前采样时刻不为系统上电后的第一采样时刻，并且当前采样时刻之前累积采样的横摆角速度测量值的数量大于或等于预设数量时，会根据当前采样时刻的的横摆角速度滤波值与横摆角速度预测值以及当前采样时刻之前的前预设数量采样时刻分别对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵，例如预设数量为10，当前采样时刻为系统上电后第21个采样时刻，会根据第11个采样时刻至第21个采样时刻总共11个采样时刻的横摆角速度滤波值与横摆角速度预测值，获得第22个采样时刻的环境噪声协方差矩阵。

具体地的，请参照图4，图4为本申请一实施例中步骤S70的流程细化示意图，基于上述实施例，步骤S70包括：

步骤S701，计算当前采样时刻横摆角速度预测值与横摆角速度滤波值间的第一差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度预测值与横摆角速度滤波值间的第一差值；

步骤S702，计算第一差值的样本方差，并作为下一采样时刻的环境噪声协方差矩阵。

当确定当前采样时刻不为系统上电后的第一采样时刻，并且当前采样时刻之前累积采样的横摆角速度测量值的数量大于或等于预设数量时，会计算当前采样时刻横摆角速度预测值与横摆角速度滤波值间的第一差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度预测值与横摆角速度滤波值间的第一差值，然后根据这些第一差值，计算第一差值的样本方差，作为下一采样时刻的环境噪声协方差矩阵，具体计算第一差值的样本方差的公式为：

其中，

为第一差值的样本方差，N为预设数量，y_i为当前采样时刻和当前采样时刻的前预设数量采样时刻中一采样时刻对应的横摆角速度滤波值与横摆角速度预测值间的差值，

为当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值间差值的均值。

具体地，可以在控制终端中预先设置一个第一差值存储队列，该第一差值存储队列的存储数量为预设数量，第一差值存储队列采用先进先出原则，在每一个采样时刻完成卡尔曼滤波处理后，都会将该采样时刻获得的横摆角速度滤波值与横摆角速度预测值间的差值存储至第一差值存储队列中，计算下个时刻的环境噪声协方差矩阵可以直接使用存储队列中存储的各第一差值作为样本，计算该采样时刻的样本方差。

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

通过将下一采样时刻的环境噪声协方差矩阵替换当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵以更新当前采样时刻的卡尔曼滤波模型，将更新后的卡尔曼滤波模型作为下一个采样时刻的卡尔曼滤波模型。然后继续下一个采样时刻采集横摆角速度测量值，直至系统下电。

需要说明的是，每次系统上电后，第二采样时刻都会采用预设初始卡尔曼滤波模型。

本实例通过当系统上电后，根据预设采样频率采集横摆角速度测量值；每采集到一个横摆角速度测量值时，判断当前采样时刻是否为系统上电后的第一个采样时刻；若当前采样时刻为系统上电后的第一个采样时刻，则将当前采样时刻的横摆角速度测量值和预设的系统误差协方差矩阵分别作为当前采样时刻的横摆角速度滤波值和系统误差协方差矩阵以及将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；若当前采样时刻不为系统上电后的第一个采样时刻，则根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵；判断当前采样时刻前累积采集的横摆角速度测量值的数量是否大于或等于预设数量；若否，则将预设的初始卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型；若是，则根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵；根据下一采样时刻的环境噪声协方差矩阵更新当前采样时刻的卡尔曼滤波模型，获得下一采样时刻的卡尔曼滤波模型。在达到一定采样数量后，在每个采样时刻使用卡尔曼滤波过程中利用已经获得的多个采样时刻的预测值与滤波值来动态更新环境噪声协方差矩阵Q，从而实现了自适应的效果，有利于提高滤波后的横摆角速度的准确度。

进一步地，请参照图5，图5为根据本申请横摆角速度滤波方法的第一实施例提出本申请横摆角速度滤波方法的第二实施例，在本实施例中，步骤S80之前还包括：

步骤S71，根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值，获得下一采样时刻的传感器噪声协方差矩阵；

计算当前采样时刻横摆角速度测量值与横摆角速度滤波值间的第二差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值间的第二差值，然后根据这些第二差值，计算第二差值的样本方差，作为下一采样时刻的传感器噪声协方差矩阵，具体计算第二差值的样本方差的公式为：

其中，

为第二差值的样本方差，N为预设数量，w_i为当前采样时刻和当前采样时刻的前预设数量采样时刻中一采样时刻对应的横摆角速度测量值与横摆角速度滤波值间的差值，

为当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值间差值的均值。

步骤S80包括：

步骤S81，将当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵和传感器噪声协方差矩阵对应更新为下一采样时刻的环境噪声协方差矩阵和传感器噪声协方差矩阵，获得更新后的卡尔曼滤波模型；

步骤S82，将更新后的卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型。

通过将下一采样时刻的环境噪声协方差矩阵和传感器噪声协方差矩阵对应替换当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵和传感器噪声协方差矩阵以更新当前采样时刻的卡尔曼滤波模型，将更新后的卡尔曼滤波模型作为当前采样时刻的卡尔曼滤波模型

需要说明的是，可以同时执行步骤S70和步骤S71，也可以先执行步骤S70，还可以先执行步骤S71，在此不对步骤S70和步骤S71之间的执行顺序进行限定。

在本实施例中，在达到一定采样数量后，除了利用已经获得的多个采样时刻的预测值与滤波值更新卡尔曼滤波模型的环境噪声协方差矩阵，还会同时利用已经获得的多个采样时刻的滤波值与测量值更新卡尔曼滤波模型的传感器噪声协方差矩阵，从而实现了自适应更新卡尔曼滤波模型的Q和R，进一步提高滤波后的横摆角速度的准确度。

进一步地，请参照图6，图6为根据本申请横摆角速度滤波方法的第一实施例和第二实施例提出本申请横摆角速度滤波方法的第三实施例，在本实施例中，步骤S10还包括：

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

本实施例中，当系统上电后，除了会启动横摆角速度传感器测量，通过横摆角速度传感器根据预设采样频率测量车辆的横摆角速度，以采集车辆横摆角速度，还会启动横向加速度传感器，通过横向加速度传感器根据预设采样频率测量车辆的横向加速度，采集的横向加速度带有正负符号，车辆左转，横向加速度为负值，车辆右转，车辆的横向加速度为正值，也可以是车辆左转，横向加速度为正值，车辆右转，车辆的横向加速度为负值。

步骤S40还包括：

步骤S401，根据当前采样时刻的横向加速度以及多个预设横向加速度区间的数值范围，确定当前采样时刻对应的预设横向加速度区间；

步骤S402，根据当前采样时刻对应的预设横向加速度区间以及预设横向加速度区间与存储队列的对应关系，确定当前采样时刻对应的存储队列，其中，存储队列的最大存储数据量为预设数量；

步骤S403，根据当前采样时刻对应的存储队列以及存储队列与传感器噪声协方差矩阵的对应关系，确定当前采样时刻对应的传感器噪声协方差矩阵；

步骤S404，将当前采样时刻的卡尔曼滤波模型中的传感器噪声协方差矩阵替换为当前采样时刻对应的传感器噪声协方差矩阵，获得当前采样时刻的新卡尔曼滤波模型；

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

本实施例中，控制终端中预设了多个预设横向加速度区间以及与预设横向加速度区间数量相同的多个存储队列，该存储队列用于存储横摆角速度测量值和横摆角速度滤波值间的第二差值，每个存储队列中的最大存储数据量均为预设数量，即每个存储队列都可以存储预设数量个第二差值，每个存储队列采用先进先出存储原则，还预先构建了预设横向加速度区间与存储队列间的对应关系，还构建了存储队列与传感器噪声协方差矩阵的对应关系，该存储队列与传感器噪声协方差矩阵的对应关系是动态关系，会被更新，在初始构建时，每个存储队列对应的传感器噪声协方差矩阵均是预设的初始传感器噪声协方差矩阵，优选为预设的初始卡尔曼滤波模型中的传感器噪声协方差矩阵。

在执行步骤S40过程中，会将当前采样时刻的横摆加速度与预设横向加速度区间的数值范围进行比较，确定当前采样时刻的横摆加速度对应的预设横向加速度区间，再根据确定的预设横向加速度区间以及预设横向加速度区间与存储队列的对应关系，确定当前采样时刻对应的存储队列，然后根据存储队列与传感器噪声协方差矩阵的对应关系，确定当前采样时刻对应的传感器噪声协方差矩阵。最后将当前采样时刻对应的传感器噪声协方差矩阵替换掉前一采样时刻时确定的当前采样时刻的卡尔曼滤波模型中的传感器噪声协方差矩阵，获得当前采样时刻的新卡尔曼滤波模型，将前一采样时刻的车辆横摆加速度滤波值和系统误差协方差矩阵和当前采样时刻的车辆横摆角速度初始值输入值当前采样时刻的新卡尔曼滤波模型，进行卡尔曼滤波处理，计算获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵，该具体过程如步骤S41至步骤S45，在此不再赘述。

步骤S40之后还包括：

步骤S46，计算当前采样时刻的横摆角速度测量值和横摆角速度滤波值间的第二差值，并将当前采样时刻的第二差值按照先进先出方式存储至当前采样时刻对应的存储队列；

步骤S47，判断当前采样时刻对应的存储队列是否已满；

步骤S48，当当前采样时刻对应的存储队列已满时，根据该存储队列中已存储的第二差值，计算第二差值的样本方差，作为该存储队列对应的新传感器噪声协方差矩阵；

步骤S49，根据该存储队列对应的新传感器噪声协方差矩阵，更新存储队列与传感器噪声协方差矩阵对应关系。

获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵后，会计算当前采样时刻的横摆角速度测量值和横摆角速度滤波值间的第二差值，并将当前采样时刻的第二差值按照先进先出方式存储至当前采样时刻确定的存储队列中。存储后，判断当前采样时刻对应的存储队列是否已满，即判断存储队列中已存储的第二差值数量是否达到预设数量。当确定当前采样时刻对应的存储队列已满时，会根据该存储队列中已存储的第二差值，计算第二差值的样本方差，作为该存储队列对应的新传感器噪声协方差矩阵，然后根据该存储队列对应的新传感器噪声协方差矩阵，更新存储队列与传感器噪声协方差矩阵对应关系。

例如在一方案中，确定了4个预设横向加速度区间分别为(-∞，a1)、(a1，0)、[0，a2)和[a2，+∞)，并为(-∞，a1)区间配置了对应的第一存储队列、(a1，0)区间配置了对应的第二存储队列、[0，a2)区间配置了对应的第三存储队列和[a2，+∞)区间配置了对应的第四存储队列，a1可以为-0.5～-0.1之间任一值，a2可以为0.1～0.5之间任一值，优选a1＝-0.3m/s²，a2＝0.3m/s²，每个存储队列中的最大存储第二差值数量为大于等于3的整数，优选为50；还为第一存储队列、第二存储队列、第三存储队列和第四存储队列分别配置了对应的传感器噪声协方差矩阵R1、R2、R3和R4，在系统上电时，R1、R2、R3和R4都是一个预设初始协方差矩阵，即在第二采样时刻，采用的是预设初始协方差矩阵，优选和预设初始卡尔曼滤波模型中的传感器噪声协方差矩阵相同。在某一采样时刻采集到横向加速度为大于a2，确定该采样时刻对应的存储队列为第四存储队列，该第四存储队列的传感器噪声协方差矩阵R4。

该采样时刻的

而前一个采样时刻确定的该采样时刻的卡尔曼滤波模型中的传感器噪声协方差矩阵为

会将该采样时刻的卡尔曼滤波模型中的传感器噪声协方差矩阵由

替换为

得到该采样时刻的新卡尔曼滤波模型。

然后根据新卡尔曼滤波模型计算该采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵。接着计算该采样时刻的横摆角速度测量值和横摆角速度滤波值间的第二差值，并将该采样时刻的第二差值按照先进先出方式存储至第四存储队列中。存储后，判断第四存储队列是否已满，当确定第四存储队列已满时，会根据第四存储队列中已存储的第二差值，计算第二差值的样本方差，作为第四存储队列对应的新传感器噪声协方差矩阵

然后根据第四存储队列的新传感器噪声协方差矩阵

更新存储队列与传感器噪声协方差矩阵对应关系。在下一个采样时刻时，第四存储队列对应的

需要说明的时，每次系统上电时或者每次接收都系统下单指令时，都会将存储队列与传感器噪声协方差矩阵的对应关系初始化，以及删除各存储队列中已存储的第二差值。可以同时执行步骤S46-步骤S49与步骤S80，也可以先执行步骤S46-步骤S49，还可以先执行步骤S80，在此不对步骤S46-步骤S49与步骤S80之间的执行顺序进行限定，当当前采样时刻的步骤S48和步骤S80均执行完成后，会继续下一个采样时刻采集横摆角速度测量值，直至系统下电。

本实施例中，由于横摆角速度传感器多采用转动惯量的原理测量，其传感器误差来源于转动过程的快慢区别，横摆角的产生来源于驾驶员转动方向盘，但是单纯根据方向盘转动快慢来更新传感器噪声协方差矩阵是不可靠的，因为还受到横向加速度的影响，横向加速度不同，对传感器误差的影响也不同，即滤波过程采用的传感器噪声协方差矩阵也不同。因此本发明划分为多个横向加速度区间，在不同的横向加速度，采用不同的传感器噪声协方差矩阵，滤波后的横摆角速度的准确度，并且利用同一区间对应已采集到多个采样时刻的测量值与滤波值自适应更新该区间对应的传感器噪声协方差矩阵，进一步提高滤波后的横摆角速度的准确度。

进一步地，根据本申请横摆角速度滤波方法的前述实施例提出本申请横摆角速度滤波方法的第四实施例，在本实施例中，步骤S42之前还包括：

步骤S90，根据预设差值平方计算公式、预设观测矩阵、当前采样时刻的横摆角速度预测值和横摆角速度测量值，计算获得当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方，其中，所述预设差值平方计算公式为D＝(X_pk-Z_k*H^-1)²；

步骤S91，计算当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值；

步骤S92，根据当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值以及多个预设比值区间的数值范围，确定当前采样时刻对应的预设比值区间；

步骤S93，根据当前采样时刻对应的预设比值区间以及预设比值区间与校正系数的对应关系，确定当前采样时刻对应的校正系数；

步骤S42包括：

步骤S421，根据当前采样时刻对应的校正系数、前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第二系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第二系统预测误差协方差矩阵计算公式为P_pk＝A*P_k-1*A^T+α_kQ_k，P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵，α_k为当前采样时刻对应的校正系数。

本实施例中，在执行步骤S42之前，还会先根据当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方和传感器噪声协方差矩阵间的比值大小对当前采样时刻中的卡尔曼滤波模型的环境噪声协方差矩阵进行校正。

具体地，首先会将预设观测矩阵、当前采样时刻的横摆角速度预测值和横摆角速度测量值输入至预设差值平方计算公式中，计算获得当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方，预设差值平方计算公式为D＝(X_pk-Z_k*H)²。然后计算当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值。本实施例中预先设置了多个预设比值区间以及预设比值区间与校正系数的一一对应关系，在获得当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值后，将该比值与多个预设比值区间的数值范围进行比较，确定该比值对应的预设比值区间，最后根据该比值对应的预设比值区间以及预设比值区间与校正系数的对应关系，确定当前采样时刻对应的校正系数。例如在一方案中，采用拉依达准则确定了4个预设比值区间分别为[0，1)、[1，2)、[2，3)和[3，+∞)，并为[0，1)区间配置了对应的第一校正系数、[1，2)区间配置了对应的第二校正系数、[2，3)区间配置了对应的第三校正系数和[3，+∞)区间配置了对应的第四校正系数，并且第一校正系数为1，第四校正系数为0，第二校正系数和第三校正系数均大于0且小于1，并且第二校正系数大于第三校正系数，，该第三校正系数可为0.8～0.2中任一值，优选为0.4，该第三校正系数为0.1～0.02中任一值，优选为0.05。

然后当前采样时刻对应的校正系数和前一采样时刻的系统误差协方差矩阵输入至当前采样时刻的卡尔曼滤波模型中的第二系统预测误差协方差矩阵计算公式，计算获得当前采样时刻的系统预测误差协方差矩阵，其中第二系统预测误差协方差矩阵计算公式为：

P_pk＝A*P_k-1*A^T+α_kQ_k，

P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵，α_k为当前采样时刻对应的校正系数。

对于当某一个采样时刻的实际噪声较大的情况，若直接套用卡尔曼滤波公式计算，会把噪声因素带入滤波结果，导致滤波后的结果准确度较低。因此，本实施例根据当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方和传感器噪声协方差矩阵间的比值大小标定环境噪声协方差矩阵的校正系数，来降低环境噪声的影响，从而使得在实际噪声较大的情况下，也能提高滤波后结果的准确度。

本发明还提出一种车辆，该车辆包括执行本发明各个实施例所述方法的控制终端。

本发明还提出一种计算机可读存储介质，其上存储有计算机程序。所述计算机可读存储介质可以是图1的控制终端中的存储器02，也可以是如ROM(Read-Only Memory，只读存储器)/RAM(Random Access Memory，随机存取存储器)、磁碟、光盘中的至少一种，所述计算机可读存储介质包括若干信息用以使得控制终端执行本发明各个实施例所述的方法。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种横摆角速度滤波方法，其特征在于，包括步骤：

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

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

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

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

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

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

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

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

2.根据权利要求1所述的横摆角速度滤波方法，其特征在于，所述根据前一采样时刻的车辆横摆角速度滤波值和系统误差协方差矩阵以及当前采样时刻的横摆角度速度测量值和卡尔曼滤波模型，获得当前采样时刻的横摆角速度预测值、横摆角速度滤波值和系统误差协方差矩阵的步骤包括：

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

根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第一系统预测误差协方差矩阵计算公式为P_pk＝A*P_k-1*A^T+Q_k，P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵；

根据当前采样时刻的系统预测误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的卡尔曼增益计算公式，获得当前采样时刻的卡尔曼增益，其中卡尔曼增益计算公式为K_k＝P_pk*H^T/(H*P_pk*H^T+R_k)，K_k为当前采样时刻的卡尔曼增益、H为预设观测矩阵以及R_k为当前采样时刻的传感器噪声协方差矩阵；

根据当前采样时刻的横摆角速度预测值、横摆角速度测量值和卡尔曼增益和当前采样时刻的卡尔曼滤波模型中的横摆角速度滤波值计算公式，获得当前采样时刻的横摆角速度滤波值，其中横摆角速度滤波值计算公式为X_k＝X_pk+K_k(Z_k-H*X_pk)，其中X_k为当前采样时刻的横摆角速度滤波值，Z_k为当前采样时刻的横摆角速度测量值；

根据当前采样时刻的卡尔曼增益、系统预测误差协方差矩阵和卡尔曼滤波模型中的系统误差协方差矩阵计算公式，获得当前采样时刻的系统误差协方差矩阵，其中系统误差协方差矩阵计算公式为P_k＝(1-K_k*H)*P_pk，P_k为当前采样时刻的系统误差协方差矩阵。

3.根据权利要求2所述的横摆角速度滤波方法，其特征在于，所述根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度滤波值与横摆角速度预测值，获得下一采样时刻的环境噪声协方差矩阵的步骤包括：

计算当前采样时刻横摆角速度预测值与横摆角速度滤波值间的第一差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度预测值与横摆角速度滤波值间的第一差值；

计算第一差值的样本方差，并作为下一采样时刻的环境噪声协方差矩阵；

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

将当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵更新为下一采样时刻的环境噪声协方差矩阵，获得更新后的卡尔曼滤波模型；

将更新后的卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型。

4.根据权利要求3所述的横摆角速度滤波方法，其特征在于，所述根据下一采样时刻的环境噪声协方差矩阵更新当前采样时刻的卡尔曼滤波模型，获得下一采样时刻的卡尔曼滤波模型的步骤之前还包括：

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

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

将当前采样时刻的卡尔曼滤波模型中的环境噪声协方差矩阵和传感器噪声协方差矩阵对应更新为下一采样时刻的环境噪声协方差矩阵和传感器噪声协方差矩阵，获得更新后的卡尔曼滤波模型；

将更新后的卡尔曼滤波模型作为下一采样时刻的卡尔曼滤波模型。

5.根据权利要求4所述的横摆角速度滤波方法，其特征在于，所述根据当前采样时刻和当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值，获得下一采样时刻的传感器噪声协方差矩阵的步骤包括：

计算当前采样时刻横摆角速度测量值与横摆角速度滤波值间的第二差值以及当前采样时刻的前预设数量采样时刻对应的横摆角速度测量值与横摆角速度滤波值间的第二差值；

计算第二差值的样本方差，并作为下一采样时刻的传感器噪声协方差矩阵。

6.根据权利要求3所述的横摆角速度滤波方法，其特征在于，所述当系统上电后，根据预设采样频率采集横摆角速度测量值的步骤包括：

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

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

根据当前采样时刻的横向加速度以及多个预设横向加速度区间的数值范围，确定当前采样时刻对应的预设横向加速度区间；

根据当前采样时刻对应的预设横向加速度区间以及预设横向加速度区间与存储队列的对应关系，确定当前采样时刻对应的存储队列，其中，存储队列的最大存储数据量为预设数量；

根据当前采样时刻对应的存储队列以及存储队列与传感器噪声协方差矩阵的对应关系，确定当前采样时刻对应的传感器噪声协方差矩阵；

将当前采样时刻的卡尔曼滤波模型中的传感器噪声协方差矩阵替换为当前采样时刻对应的传感器噪声协方差矩阵，获得当前采样时刻的新卡尔曼滤波模型；

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

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

计算当前采样时刻的横摆角速度测量值和横摆角速度滤波值间的第二差值，并将当前采样时刻的第二差值按照先进先出方式存储至当前采样时刻对应的存储队列；

判断当前采样时刻对应的存储队列是否已满；

当当前采样时刻对应的存储队列已满时，根据该存储队列中已存储的第二差值，计算第二差值的样本方差，作为该存储队列对应的新传感器噪声协方差矩阵；

根据该存储队列对应的新传感器噪声协方差矩阵，更新存储队列与传感器噪声协方差矩阵对应关系。

7.根据权利要求2至6中任一项所述的横摆角速度滤波方法，其特征在于，所述根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵的步骤之前还包括：

根据预设差值平方计算公式、预设观测矩阵、当前采样时刻的横摆角速度预测值和横摆角速度测量值，计算获得当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方，其中，所述预设差值平方计算公式为D＝(X_pk-Z_k*H^-1)²；

计算当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值；

根据当前采样时刻的横摆角速度预测值和横摆角速度测量值间差值的平方与传感器噪声协方差矩阵的比值以及多个预设比值区间的数值范围，确定当前采样时刻对应的预设比值区间；

根据当前采样时刻对应的预设比值区间以及预设比值区间与校正系数的对应关系，确定当前采样时刻对应的校正系数；

所述根据前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第一系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵的步骤包括：

根据当前采样时刻对应的校正系数、前一采样时刻的系统误差协方差矩阵和当前采样时刻的卡尔曼滤波模型中的第二系统预测误差协方差矩阵计算公式，获得当前采样时刻的系统预测误差协方差矩阵，其中第二系统预测误差协方差矩阵计算公式为P_pk＝A*P_k-1*A^T+α_kQ_k，P_pk为当前采样时刻的系统预测误差协方差矩阵，P_k-1为前一采样时刻的系统误差协方差矩阵，Q_k为当前采样时刻的环境噪声协方差矩阵，α_k为当前采样时刻对应的校正系数。

8.一种控制终端，其特征在于，所述控制终端包括存储器、处理器和存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至7中任一项所述的横摆角速度滤波方法的步骤。

9.一种车辆，其特征在于，所述车辆包括如权利要求8所述的控制终端。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的横摆角速度滤波方法的步骤。

Images