CN112577512A - 一种基于轮速融合的状态量误差确定方法及车载终端 - Google Patents

Abstract

本发明实施例公开一种基于轮速融合的状态量误差确定方法及车载终端。该方法包括：获取轮速计采集的多个车轮轮速以及运动检测设备采集的设备角速度；根据多个车轮轮速分别和设备角速度的结合，确定车辆中预设点的多个预设点速度；从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量；根据状态量测量与状态预测量之间的状态量偏差以及状态量偏差与状态量误差之间的约束关系，确定预设点的状态量误差；约束关系基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。应用本发明实施例提供的方案，能够确定车辆中预设点的状态量误差，对状态量数据进行矫正。

Description

一种基于轮速融合的状态量误差确定方法及车载终端

技术领域

本发明涉及智能驾驶技术领域，具体而言，涉及一种基于轮速融合的状态量误差确定方法及车载终端。

背景技术

在车辆定位系统中，智能车辆通常安装有惯性测量单元(Inertial MeasurementUnit，IMU)等传感器。车载定位系统可以根据IMU输出的数据，并结合其他传感器的数据，对车辆的位置和状态进行测量，得到车辆的速度、位置、姿态、加速度、角速度等状态量。IMU的特点是，采集数据的频率在200Hz以上，且对速度、姿态的变化更为敏感。但是，如果长时间没有外部辅助信息的矫正，根据IMU采集的数据得到的速度、姿态都存在快速发散的趋势。

因此，如何能实时地对车辆的状态量数据进行校正，是亟需解决的一个问题。

发明内容

本发明提供了一种基于轮速融合的状态量误差确定方法及车载终端，以确定车辆中预设点的状态量误差，对状态量数据进行矫正。具体的技术方案如下。

第一方面，本发明实施例公开了一种基于轮速融合的状态量误差确定方法，包括：

获取轮速计在第一时刻采集的多个车轮的车轮轮速，以及运动检测设备在所述第一时刻采集的设备角速度；

针对每个车轮轮速，根据该车轮轮速和所述设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度；

从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量；

根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

其中，所述状态量偏差为状态量测量与状态预测量之间的偏差，所述约束关系为：基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。

可选的，在确定所述预设点在所述第一时刻的状态量误差之后，还包括：

根据所述预设点在所述第一时刻的状态量误差，对所述预设点在所述第一时刻的状态量进行修正，得到所述预设点在所述第一时刻的修正状态量；其中，所述预设点在所述第一时刻的状态量为：经过卡尔曼滤波后得到的估计状态量。

可选的，所述根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差的步骤，包括：

获取所述预设点在第二时刻的状态量误差的置信度；其中，所述第二时刻为所述第一时刻的前一时刻；

根据确定的状态转移矩阵，对所述第二时刻的置信度进行时间更新，得到所述第一时刻的预测置信度；

将所述预设点在所述第一时刻的状态量代入预先建立的约束关系中状态量误差的系数矩阵，得到所述第一时刻的量测矩阵；其中，所述约束关系为：状态量偏差与状态量误差之间的约束关系；

根据所述第一时刻的量测矩阵和所述第一时刻的预测置信度，确定第一时刻的卡尔曼增益；

根据所述第一时刻的卡尔曼增益、所述状态量测量与所述状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差。

可选的，所述根据所述第一时刻的状态转移矩阵，对所述第二时刻的置信度进行时间更新，得到所述第一时刻的预测置信度的步骤，包括：

根据

对所述第二时刻的置信度∑_t-1进行时间更新，得到所述第一时刻的预测置信度

其中，所述A_t为所述第一时刻的状态转移矩阵，所述R为状态方程置信度，所述t和t-1分别为所述第一时刻和所述第二时刻，所述T为转置符号；

所述根据所述第一时刻的量测矩阵和所述第一时刻的预测置信度，确定第一时刻的卡尔曼增益的步骤，包括：

根据

以及所述第一时刻的量测矩阵H_t和所述第一时刻的预测置信度

确定第一时刻的卡尔曼增益K_t；其中，所述Q为量测噪声；

所述根据所述第一时刻的卡尔曼增益、所述状态量测量与所述状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差的步骤，包括：

根据

并且将所述状态量测量与所述状态预测量之间的状态量偏差作为

进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差μ_t；其中，

所述Z_t预为所述状态预测量，所述Z_t量为所述状态量测量，

所述K_t为所述第一时刻的卡尔曼增益。

可选的，采用以下方式确定状态量偏差与状态量误差之间的约束关系：

将车辆视为刚体，以所述预设点视为质点，建立所述预设点的两个不同预设点速度真实值之间的相等关系；

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的约束关系。

可选的，所述预设点为所述运动检测设备的中心点；所述状态量误差包括：陀螺零偏误差。

可选的，所述针对每个车轮轮速，根据该车轮轮速和所述设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度的步骤，包括：

针对每个车轮轮速，根据v₁＝V+ω^v×l，确定车辆中所述运动检测设备的中心点的一个预设点速度；

其中，所述v₁为车体坐标系下所述中心点的预设点速度，所述V为车体坐标系下的一个车轮轮速，所述ω^v为所述设备角速度在车体坐标系下的分解，所述l为所述车轮轮速对应的车轮与所述中心点之间的位置差向量；

所述根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的约束关系的步骤，包括：

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的以下约束关系：

其中，V₀和V₁分别为车体坐标系下的两个不同的车轮轮速，所述l₀₁为两个车轮之间的位置差向量，所述δ_ε为陀螺零偏误差。

可选的，所述从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量的步骤，包括：

从各个预设点速度中依次选择两个速度分别作为状态量测量和状态预测量，得到多组状态量测量和状态预测量；

所述根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差的步骤，包括：

按照次序从多组状态量测量和状态预测量中选择状态量测量和状态预测量的目标组，按照以下方式确定所述目标组对应的修正后状态量误差，当所述目标组为各个状态量测量和状态预测量组中的最后一个组时，将所述目标组对应的修正后状态量误差作为所述预设点在所述第一时刻的状态量误差：

根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差；

其中，当所述目标组不为第一个组时，所述待修正状态量误差为：所述目标组的上一个组对应的修正后状态量误差；当所述目标组为第一个组时，所述待修正状态量误差为零。

第二方面，本发明实施例公开了一种车载终端，包括：处理器、轮速计和运动检测设备；其中，处理器包括：

获取模块，用于获取轮速计在第一时刻采集的多个车轮的车轮轮速，以及运动检测设备在所述第一时刻采集的设备角速度；

第一确定模块，用于针对每个车轮轮速，根据该车轮轮速和所述设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度；

选择模块，用于从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量；

第二确定模块，用于根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

其中，所述状态量偏差为状态量测量与状态预测量之间的偏差，所述约束关系为：基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。

可选的，所述处理器还包括：

修正模块，用于在确定所述预设点在所述第一时刻的状态量误差之后，根据所述预设点在所述第一时刻的状态量误差，对所述预设点在所述第一时刻的状态量进行修正，得到所述预设点在所述第一时刻的修正状态量；其中，所述预设点在所述第一时刻的状态量为：经过卡尔曼滤波后得到的估计状态量。

可选的，所述第二确定模块，具体用于：

获取所述预设点在第二时刻的状态量误差的置信度；其中，所述第二时刻为所述第一时刻的前一时刻；

根据确定的状态转移矩阵，对所述第二时刻的置信度进行时间更新，得到所述第一时刻的预测置信度；

将所述预设点在所述第一时刻的状态量代入预先建立的约束关系中状态量误差的系数矩阵，得到所述第一时刻的量测矩阵；其中，所述约束关系为：状态量偏差与状态量误差之间的约束关系；

根据所述第一时刻的量测矩阵和所述第一时刻的预测置信度，确定第一时刻的卡尔曼增益；

根据所述第一时刻的卡尔曼增益、所述状态量测量与所述状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差。

可选的，所述第二确定模块，根据所述第一时刻的状态转移矩阵，对所述第二时刻的置信度进行时间更新，得到所述第一时刻的预测置信度时，包括：

根据

对所述第二时刻的置信度∑_t-1进行时间更新，得到所述第一时刻的预测置信度

其中，所述A_t为所述第一时刻的状态转移矩阵，所述R为状态方程置信度，所述t和t-1分别为所述第一时刻和所述第二时刻，所述T为转置符号；

所述第二确定模块，根据所述第一时刻的量测矩阵和所述第一时刻的预测置信度，确定第一时刻的卡尔曼增益时，包括：

根据

以及所述第一时刻的量测矩阵H_t和所述第一时刻的预测置信度

确定第一时刻的卡尔曼增益K_t；其中，所述Q为量测噪声；

所述第二确定模块，根据所述第一时刻的卡尔曼增益、所述状态量测量与所述状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差时，包括：

根据

并且将所述状态量测量与所述状态预测量之间的状态量偏差作为

进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差μ_t；其中，

所述Z_t预为所述状态预测量，所述Z_t量为所述状态量测量，

所述K_t为所述第一时刻的卡尔曼增益。

可选的，所述处理器还包括：第三确定模块，用于采用以下操作确定所述状态量偏差与状态量误差之间的约束关系：

将车辆视为刚体，以所述预设点视为质点，建立所述预设点的两个不同预设点速度真实值之间的相等关系；

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的约束关系。

可选的，所述预设点为所述运动检测设备的中心点；所述状态量误差包括：陀螺零偏误差。

可选的，所述第一确定模块，具体用于：

针对每个车轮轮速，根据v₁＝V+ω^v×l，确定车辆中所述运动检测设备的中心点的一个预设点速度；

其中，所述v₁为车体坐标系下所述中心点的预设点速度，所述V为车体坐标系下的一个车轮轮速，所述ω^v为所述设备角速度在车体坐标系下的分解，所述l为所述车轮轮速对应的车轮与所述中心点之间的位置差向量；

所述第三确定模块，根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的约束关系时，包括：

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的以下约束关系：

其中，V₀和V₁分别为车体坐标系下的两个不同的车轮轮速，所述l₀₁为两个车轮之间的位置差向量，所述δ_ε为陀螺零偏误差。

可选的，所述选择模块，具体用于：

从各个预设点速度中依次选择两个速度分别作为状态量测量和状态预测量，得到多组状态量测量和状态预测量；

所述第二确定模块，具体用于：

按照次序从多组状态量测量和状态预测量中选择状态量测量和状态预测量的目标组，按照以下操作确定所述目标组对应的修正后状态量误差，当所述目标组为各个状态量测量和状态预测量组中的最后一个组时，将所述目标组对应的修正后状态量误差作为所述预设点在所述第一时刻的状态量误差：

根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差；

其中，当所述目标组不为第一个组时，所述待修正状态量误差为：所述目标组的上一个组对应的修正后状态量误差；当所述目标组为第一个组时，所述待修正状态量误差为零。

由上述内容可知，本发明实施例提供的基于轮速融合的状态量误差确定方法及车载终端，可以根据多个车轮轮速分别与设备角速度的结合，确定车辆中预设点的多个预设点速度，从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量，根据状态量偏差和预先建立的约束关系，确定预设点的状态量误差。因此，本发明实施例能够确定车辆中预设点的状态量误差，对状态量数据进行矫正。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。

本发明实施例的创新点包括：

1、通过不同车轮的车轮轮速计算的同一质点的真实速度应相同，每个预设点速度均可以视为真实值与扰动误差的和值，根据以不同轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系，可以建立两预设点速度之间的约束关系，并根据该约束关系和状态量偏差，确定预设点的状态量误差。

2、采用修改后的卡尔曼滤波算法确定预设点在第一时刻的状态量误差，能够更准确地确定状态量误差。

3、从多个预设点速度中依次选择两个速度作为状态量测量和状态预测量，得到多组状态量测量和状态预测量，在根据上一组数据确定的状态量误差基础上，依次采用不同组进行状态量误差的计算，实现对多个车轮轮速以及运动检测设备之间的数据融合，能够提高状态量误差的准确性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例。对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的基于轮速融合的状态量误差确定方法的一种流程示意图；

图2为本发明实施例提供的车辆运动模型示意图；

图3为本发明实施例提供的车载终端的一种结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。例如包含的一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。

本发明实施例公开了一种基于轮速融合的状态量误差确定方法及车载终端，能够确定车辆中预设点的状态量误差，对状态量数据进行矫正。下面对本发明实施例进行详细说明。

图1为本发明实施例提供的基于轮速融合的状态量误差确定方法的一种流程示意图。该方法应用于电子设备。该电子设备可以为普通计算机、服务器或者智能终端设备等，也可以为车载电脑或车载工业控制计算机(Industrial Personal Computer，IPC)等车载终端。本实施例中的车辆是指智能车辆。该车辆中可以安装有车载终端。该车载终端可以包括处理器和多个传感器。传感器可以包括轮速计、运动检测设备和定位设备等。运动检测设备可以包括但不限于惯性量测单元(Inertial Measurement Unit，IMU)。该方法具体包括以下步骤。

S110：获取轮速计在第一时刻采集的多个车轮的车轮轮速，以及运动检测设备在第一时刻采集的设备角速度。

本实施例中，可以获取大于一个或者小于四个车轮的车轮轮速。每个车轮对应一个车轮轮速。

轮速计和运动检测设备均可以按照对应的指定频率周期性地采集数据。轮速计可以输出车辆4个轮子的轮速，其采集数据的频率可以在10～100Hz不等。运动检测设备可以采集加速度、角速度等数据，其采集数据的频率在200Hz以上。本实施例中所涉及的传感器均设置于同一车辆中。

本实施例可以按照预设频率周期性执行。在获取第一时刻的多个车轮的车轮轮速时，可以从保存的各个时刻的多个车轮的车轮轮速中获取。在获取第一时刻的设备角速度时，可以从保存的各个时刻的设备角速度中获取。

S120：针对每个车轮轮速，根据该车轮轮速和设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度。

其中，预设点可以为运动检测设备的中心点。状态量误差可以包括以下中的至少一项：陀螺零偏误差。例如，预设点可以为IMU的中心点，状态量误差可以包括IMU的陀螺零偏误差，陀螺零偏为状态量。陀螺零偏为陀螺仪精度的重要指标，用于矫正角速度。

车辆中的预设点，还可以为车辆中的其他点，本实施例对此不作限定。预设点的位置可以预先测定。预设点与各个车轮、车体坐标系原点或传感器等之间的相对位置也可以预先测定。

针对每个车轮轮速，在确定车辆中预设点在一个预设点速度时，可以根据该运动检测设备与预设点之间的相对距离以及设备角速度，确定车辆转动带来的预设点的转动速度，并根据车轮与预设点之间的相对距离以及车轮轮速，确定车辆平动带来的预设点的平动速度，转动速度和平动速度的合成速度作为预设点速度。

每个车轮轮速均分别与设备角速度结合，可以确定该预设点的多个预设点速度。

S130：从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量。

在选择速度时，可以任意选择两个车轮轮速，也可以按照相邻的规则选择两个相邻的车轮轮速。

S140：根据上述状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定预设点在第一时刻的状态量误差。

其中，状态量偏差为状态量测量与状态预测量之间的偏差，约束关系为：基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。

具体的，测量值可以等于真实值加上扰动误差，将两个预设点速度均视为测量值，两个预设点速度包含的真实值应相等。根据以上关系，可以得到状态量偏差与状态量误差之间的约束关系。

由上述内容可知，本实施例可以根据多个车轮轮速分别与设备角速度的结合，确定车辆中预设点的多个预设点速度，从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量，根据状态量偏差和预先建立的约束关系，确定预设点的状态量误差。因此，本实施例能够确定车辆中预设点的状态量误差，对状态量数据进行矫正。

虽然轮速计可以直接输出车辆的速度信息，但是因为不同类型车辆的轮速输出频率和死区以及分辨率的不同，轮速计直接输出的速度信息往往不足以满足无人驾驶中的需求。

本实施例将轮速计、运动检测设备以及其他传感器的数据进行融合，能够确定更准确的状态量误差。当采用该状态量误差对预设点的状态量进行修正之后，能够得到更准确的状态量信息。

在本发明的另一实施例中，基于图1所示实施例，在确定预设点在所述第一时刻的状态量误差之后，还包括：

根据预设点在第一时刻的状态量误差，对预设点在第一时刻的状态量进行修正，得到预设点在第一时刻的修正状态量。

其中，预设点在第一时刻的状态量为：经过卡尔曼滤波后得到的估计状态量。

虽然直接对状态量进行卡尔曼滤波后能够提高状态量的准确性，但是由于状态量中包含数量级非常大的数值，例如世界坐标系中的位置数据，也包括数据量非常小的数值，在这种情况下采用卡尔曼滤波对状态量进行求逆运算时，其会存在抖动误差。

本实施例中，针对经过卡尔曼滤波后得到的估计状态量，使用状态量误差对其进行修正，能够得到更准确的状态量。

在本发明的另一实施例中，基于图1所示实施例，步骤S140，根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在第一时刻的状态量误差的步骤，可以包括以下步骤1a～步骤5a。

步骤1a：获取预设点在第二时刻的状态量误差的置信度。

其中，第二时刻为第一时刻的前一时刻。确定状态量误差的操作可以为按照预设频率周期性进行的，前一时刻可以理解为上一次确定状态量误差时的时刻。

由于上一次确定的状态量误差已经修正至上一时刻的状态量中，因此认为修正后的状态量不存在误差，即认为第二时刻的状态量误差为零。对第二时刻的状态量误差进行时间更新后得到的结果仍然为零。

步骤2a：根据确定的状态转移矩阵，对第二时刻的置信度进行时间更新，得到第一时刻的预测置信度。

本步骤中，在确定状态转移矩阵时，可以根据预设点在第一时刻的状态量对预设的状态转移矩阵进行更新，得到第一时刻的状态转移矩阵。在更新预设的状态转移矩阵时，可以将第一时刻的状态量代入该预设的状态转移矩阵。

步骤3a：将预设点在第一时刻的状态量代入预先建立的约束关系中状态量误差的系数矩阵，得到第一时刻的量测矩阵。

其中，约束关系为：状态量偏差与状态量误差之间的约束关系。

步骤4a：根据第一时刻的量测矩阵和第一时刻的预测置信度，确定第一时刻的卡尔曼增益。

步骤5a：根据第一时刻的卡尔曼增益、状态量测量与状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差。

本实施例中，还可以根据以下步骤确定第一时刻的状态量误差的置信度：

根据第一时刻的卡尔曼增益、第一时刻的量测矩阵和第一时刻的预测置信度进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差的置信度。

综上，本实施例中提供了具体的根据状态量偏差和约束关系确定状态量误差的实施方式。这种实施方式为采用修改后的卡尔曼滤波算法确定预设点的状态量误差的过程。修改后的卡尔曼滤波能够利用线性的状态转移矩阵，通过输入的观测数据，对系统的状态量进行最优估计。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。本实施例中，系统是指车辆和各个传感器组成的系统。基于修改后的卡尔曼滤波算法以及状态量偏差和约束关系，能够更准确地确定状态量误差。

在本发明的另一实施例中，基于上述实施例，步骤2a，根据第一时刻的状态转移矩阵，对第二时刻的置信度进行时间更新，得到第一时刻的预测置信度的步骤，可以包括：

根据

对所述第二时刻的置信度∑_t-1进行时间更新，得到所述第一时刻的预测置信度

其中，A_t为第一时刻的状态转移矩阵，R为状态方程置信度，t和t-1分别为第一时刻和所述第二时刻，T为转置符号。状态转移矩阵置信度可以为预设值，也可以为根据预设点在第一时刻的状态量以及对应的预设公式计算得到。

本实施例中，置信度也可以称为协方差，用于表示参量的可信程度。

步骤4a，根据第一时刻的量测矩阵和第一时刻的预测置信度，确定第一时刻的卡尔曼增益的步骤，可以包括：

根据

以及第一时刻的量测矩阵H_t和所述第一时刻的预测置信度

确定第一时刻的卡尔曼增益K_t。

其中，Q为量测噪声。量测噪声可以通过预设点在第一时刻的状态量以及对应的预设公式计算的得到。

步骤5a，根据第一时刻的卡尔曼增益、上述状态量测量与状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差的步骤，包括：

根据

并且将状态量测量与状态预测量之间的状态量偏差作为

进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差μ_t。

其中，

Z_t预为第一时刻的状态预测量，Z_t量为第一时刻的状态量测量，

K_t为第一时刻的卡尔曼增益。

在本实施例中，根据第一时刻的卡尔曼增益、第一时刻的量测矩阵和第一时刻的预测置信度进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差的置信度的步骤，可以包括：

根据

进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差的置信度∑_t。

其中，K_t为第一时刻的卡尔曼增益，H_t为第一时刻的量测矩阵，

为第一时刻的预测置信度，I为单位矩阵。

综上，本实施例提供了采用修改后的卡尔曼滤波算法公式确定预设点在第一时刻的状态量误差的具体公式表示。

在本发明的另一实施例中，基于上述实施例，可以采用采用以下步骤1b和2b确定状态量偏差与状态量误差之间的约束关系。

步骤1b：将车辆视为刚体，以预设点视为质点，建立预设点的两个不同预设点速度真实值之间的相等关系。

本实施例中，两个不同的预设点速度均可以等于真实值和对应的扰动误差之和，且这两个不同的预设点速度均可以视为测量值，是带有误差的值。

步骤2b：根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对上述相等关系进行扰动分解，得到状态量偏差与状态量误差之间的约束关系。

其中，测量值等于真实值与扰动误差之和的关系可以采用下式表示：

为测量值，x_真为真实值，Δx为扰动误差。

对相等关系进行扰动分解时，将状态量项移至等式一边，将误差量项移至等式另一边，得到状态量偏差与状态量误差之间的约束关系。

下面以具体实例再对上述实施例中的步骤进行具体说明。

在本实施例中，预设点为运动检测设备的中心点，状态量误差包括陀螺零偏误差。

步骤S120，即针对每个车轮轮速，根据该车轮轮速和设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度的步骤，可以包括：

针对每个车轮轮速，根据v₁＝V+ω^v×l，确定车辆中运动检测设备的中心点的一个预设点速度。

其中，v₁为车体坐标系下该运动检测设备中心点的预设点速度，V为车体坐标系下的一个车轮轮速，ω^v为设备角速度在车体坐标系下的分解，l为车轮轮速对应的车轮与中心点之间的位置差向量。

步骤1b，即根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对相等关系进行扰动分解，得到状态量偏差与状态量误差之间的约束关系的步骤，包括：

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对上述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的以下约束关系：

其中，V₀和V₁分别为车体坐标系下的两个不同的车轮轮速，l₀₁为两个车轮之间的位置差向量，δ_ε为陀螺零偏误差。本实施例可以视为轮速差速模型。

以驱动轮为例，当车辆直线形式时，驱动轮的速度是一致的，两个车轮轮速的差值为0，并且车辆的方位角信息是保持恒定的。当车辆转弯时，两个车轮的速度差发生变化，相应的，车辆的方位角也发生变化。因此可以得到轮速计的轮速差值与车辆的方位角变化信息之间直接相关。本实施例的轮速差速模型仅与陀螺零偏误差相关，对陀螺零偏的修正更为直接。

下面结合图2所示的车辆运动模型示意图对上述约束关系的建立过程进行说明。其中，以车辆绕r点向左转弯为例，r为旋转中心。车辆的各个车轮轮速分别采用V₀₀、V₀₁、V₀₂和V₀₃表示，方向如图2所示。r₀、r₁、r₂和r₃分别为四个车轮中心点到旋转中心的距离，也称为转弯半径。l₀、l₁、l₂和l₃采用分别表示四个车轮中心点至IMU中心点的杆臂。xoy为车体坐标系，其坐标原点为后轮连续的中心点。且前后轮距为H，左右轮距为L。

将每个预设点速度表示成真实值与扰动误差的和，即ω^v＝f(ω^v _真，δ_ε)，经过整理后得到

上式中等式左边表示两个预设点速度的偏差，即状态预测量与状态量测量之间的状态量偏差，等式右边表示状态量误差。其中，等式右边可以表示成H_tμ，μ为状态量误差对应的矩阵；H_t为量测矩阵，也是状态量误差的系数矩阵。H_t是与l₀₁、

等参量有关。

当轮速计采集的各个车轮的车轮轮速为速度的大小V₀′、V₁′、V₂′和V₃′时，可以根据以下公式确定驱动轮的速度：

在车辆转弯时，考虑车体的旋转角速度，计算转弯时转向轮的速度如下：

其中，

表示车辆的行驶方向。在车体坐标系下，正向行驶时

为正，倒车时为负。

当车辆直行时，四个轮子的轮速均可以采用上述V₀₂的形式表示。

在本发明的另一实施例中，基于上述实施例，步骤S130，从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量的步骤，可以包括：

从各个预设点速度中依次选择两个速度分别作为状态量测量和状态预测量，得到多组状态量测量和状态预测量。

例如，针对四个车轮0、1、2、3分别对应的四个预设点速度v₀、v₁、v₂、v₃，分别将以下两个速度作为一组状态量测量和状态预测量：

v₀和v₁，v₁和v₂，v₂和v₃，v₃和v₀。

步骤S140，根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定预设点在第一时刻的状态量误差的步骤，可以包括：

按照次序从多组状态量测量和状态预测量中选择状态量测量和状态预测量的目标组，按照以下方式确定目标组对应的修正后状态量误差，当目标组为各个状态量测量和状态预测量组中的最后一个组时，将目标组对应的修正后状态量误差作为预设点在第一时刻的状态量误差：

根据上述状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差。

其中，当目标组不为第一个组时，待修正状态量误差为：目标组的上一个组对应的修正后状态量误差；当目标组为第一个组时，待修正状态量误差为零。

根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差时，可以采用

公式，将状态量测量与状态预测量之间的状态量偏差作为

以

为待修正状态量误差，确定修正后状态量误差μ_t。

在本实施方式中，从多个预设点速度中依次选择两个速度作为状态量测量和状态预测量，得到多组状态量测量和状态预测量，采用迭代的方式，在根据上一组数据确定的状态量误差基础上，依次采用不同组进行状态量误差的计算，实现对多个车轮轮速以及运动检测设备之间的数据融合，能够提高状态量误差的准确性。

图3为本发明实施例提供的车载终端的一种结构示意图。该实施例与图1所示实施例相对应。该车载终端包括：处理器310、轮速计320和运动检测设备330；其中，处理器310包括：获取模块、第一确定模块、选择模块和第二确定模块(图中未示出)。

获取模块，用于获取轮速计320在第一时刻采集的多个车轮的车轮轮速，以及运动检测设备330在第一时刻采集的设备角速度；

第一确定模块，用于针对每个车轮轮速，根据该车轮轮速和设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度；

选择模块，用于从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量；

第二确定模块，用于根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定预设点在第一时刻的状态量误差；

其中，状态量偏差为状态量测量与状态预测量之间的偏差，约束关系为：基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。

轮速计320用于采集各个车轮的车轮轮速，运动检测设备330用于采集至少包含设备角速度的数据。

在本发明的另一实施例中，基于图3所示实施例，处理器310还包括：

修正模块(图中未示出)，用于在确定预设点在第一时刻的状态量误差之后，根据预设点在第一时刻的状态量误差，对预设点在第一时刻的状态量进行修正，得到预设点在第一时刻的修正状态量；其中，预设点在第一时刻的状态量为：经过卡尔曼滤波后得到的估计状态量。

在本发明的另一实施例中，基于图3所示实施例，第二确定模块，具体用于：

获取预设点在第二时刻的状态量误差的置信度；其中，第二时刻为第一时刻的前一时刻；

根据确定的状态转移矩阵，对第二时刻的置信度进行时间更新，得到第一时刻的预测置信度；

将预设点在第一时刻的状态量代入预先建立的约束关系中状态量误差的系数矩阵，得到第一时刻的量测矩阵；其中，约束关系为：状态量偏差与状态量误差之间的约束关系；

根据第一时刻的量测矩阵和第一时刻的预测置信度，确定第一时刻的卡尔曼增益；

根据第一时刻的卡尔曼增益、状态量测量与状态预测量之间的状态量偏差以及约束关系，进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差。

在本发明的另一实施例中，基于图3所示实施例，第二确定模块，根据第一时刻的状态转移矩阵，对第二时刻的置信度进行时间更新，得到第一时刻的预测置信度时，包括：

根据

对第二时刻的置信度∑_t-1进行时间更新，得到第一时刻的预测置信度

其中，A_t为第一时刻的状态转移矩阵，R为状态方程置信度，t和t-1分别为第一时刻和第二时刻，T为转置符号；

第二确定模块，根据第一时刻的量测矩阵和第一时刻的预测置信度，确定第一时刻的卡尔曼增益时，包括：

根据

以及第一时刻的量测矩阵H_t和第一时刻的预测置信度

确定第一时刻的卡尔曼增益K_t；其中，Q为量测噪声；

第二确定模块，根据第一时刻的卡尔曼增益、状态量测量与状态预测量之间的状态量偏差以及约束关系，进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差时，包括：

根据

并且将状态量测量与状态预测量之间的状态量偏差作为

进行卡尔曼滤波的量测更新，得到预设点在第一时刻的状态量误差μ_t；其中，

Z_t预为状态预测量，Z_t量为状态量测量，

K_t为第一时刻的卡尔曼增益。

在本发明的另一实施例中，基于图3所示实施例，处理器310还包括：第三确定模块(图中未示出)，用于采用以下操作确定状态量偏差与状态量误差之间的约束关系：

将车辆视为刚体，以预设点视为质点，建立预设点的两个不同预设点速度真实值之间的相等关系；

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对相等关系进行扰动分解，得到状态量偏差与状态量误差之间的约束关系。

在本发明的另一实施例中，基于图3所示实施例，预设点为运动检测设备的中心点；状态量误差包括：陀螺零偏误差。

在本发明的另一实施例中，基于图3所示实施例，第一确定模块具体用于：

针对每个车轮轮速，根据v₁＝V+ω^v×l，确定车辆中运动检测设备的中心点的一个预设点速度；

其中，v₁为车体坐标系下中心点的预设点速度，V为车体坐标系下的一个车轮轮速，ω^v为设备角速度在车体坐标系下的分解，l为车轮轮速对应的车轮与中心点之间的位置差向量；

第三确定模块，根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对相等关系进行扰动分解，得到状态量偏差与状态量误差之间的约束关系时，包括：

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对相等关系进行扰动分解，得到状态量偏差与状态量误差之间的以下约束关系：

其中，V₀和V₁分别为车体坐标系下的两个不同的车轮轮速，l₀₁为两个车轮之间的位置差向量，δ_ε为陀螺零偏误差。

在本发明的另一实施例中，基于图3所示实施例，选择模块具体用于：

从各个预设点速度中依次选择两个速度分别作为状态量测量和状态预测量，得到多组状态量测量和状态预测量；

第二确定模块，具体用于：

按照次序从多组状态量测量和状态预测量中选择状态量测量和状态预测量的目标组，按照以下方式确定目标组对应的修正后状态量误差，当目标组为各个状态量测量和状态预测量组中的最后一个组时，将目标组对应的修正后状态量误差作为预设点在第一时刻的状态量误差：

根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差；

其中，当目标组不为第一个组时，待修正状态量误差为：目标组的上一个组对应的修正后状态量误差；当目标组为第一个组时，待修正状态量误差为零。

该终端实施例与图1所示方法实施例是基于同一发明构思得到的实施例，相关之处可以相互参照。上述终端实施例与方法实施例相对应，与该方法实施例具有同样的技术效果，具体说明参见方法实施例。

本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。

本领域普通技术人员可以理解：实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。

Claims (10)

1.一种基于轮速融合的状态量误差确定方法，其特征在于，包括：

获取轮速计在第一时刻采集的多个车轮的车轮轮速，以及运动检测设备在所述第一时刻采集的设备角速度；

针对每个车轮轮速，根据该车轮轮速和所述设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度；

从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量；

根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

其中，所述状态量偏差为状态量测量与状态预测量之间的偏差，所述约束关系为：基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。

2.如权利要求1所述的方法，其特征在于，在确定所述预设点在所述第一时刻的状态量误差之后，还包括：

根据所述预设点在所述第一时刻的状态量误差，对所述预设点在所述第一时刻的状态量进行修正，得到所述预设点在所述第一时刻的修正状态量；其中，所述预设点在所述第一时刻的状态量为：经过卡尔曼滤波后得到的估计状态量。

3.如权利要求1或2所述的方法，其特征在于，所述根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差的步骤，包括：

获取所述预设点在第二时刻的状态量误差的置信度；其中，所述第二时刻为所述第一时刻的前一时刻；

根据确定的状态转移矩阵，对所述第二时刻的置信度进行时间更新，得到所述第一时刻的预测置信度；

将所述预设点在所述第一时刻的状态量代入预先建立的约束关系中状态量误差的系数矩阵，得到所述第一时刻的量测矩阵；其中，所述约束关系为：状态量偏差与状态量误差之间的约束关系；

根据所述第一时刻的量测矩阵和所述第一时刻的预测置信度，确定第一时刻的卡尔曼增益；

根据所述第一时刻的卡尔曼增益、所述状态量测量与所述状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差。

4.如权利要求2所述的方法，其特征在于，所述根据所述第一时刻的状态转移矩阵，对所述第二时刻的置信度进行时间更新，得到所述第一时刻的预测置信度的步骤，包括：

根据

对所述第二时刻的置信度∑_t-1进行时间更新，得到所述第一时刻的预测置信度

其中，所述A_t为所述第一时刻的状态转移矩阵，所述R为状态方程置信度，所述t和t-1分别为所述第一时刻和所述第二时刻，所述T为转置符号；

所述根据所述第一时刻的量测矩阵和所述第一时刻的预测置信度，确定第一时刻的卡尔曼增益的步骤，包括：

根据

以及所述第一时刻的量测矩阵H_t和所述第一时刻的预测置信度

确定第一时刻的卡尔曼增益K_t；其中，所述Q为量测噪声；

所述根据所述第一时刻的卡尔曼增益、所述状态量测量与所述状态预测量之间的状态量偏差以及所述约束关系，进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差的步骤，包括：

根据

并且将所述状态量测量与所述状态预测量之间的状态量偏差作为

进行卡尔曼滤波的量测更新，得到所述预设点在所述第一时刻的状态量误差μ_t；其中，

所述Z_t预为所述状态预测量，所述Z_t量为所述状态量测量，

所述K_t为所述第一时刻的卡尔曼增益。

5.如权利要求4所述的方法，其特征在于，采用以下方式确定所述状态量偏差与状态量误差之间的约束关系：

将车辆视为刚体，以所述预设点视为质点，建立所述预设点的两个不同预设点速度真实值之间的相等关系；

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的约束关系。

6.如权利要求5所述的方法，其特征在于，所述预设点为所述运动检测设备的中心点；所述状态量误差包括：陀螺零偏误差。

7.如权利要求6所述的方法，其特征在于，所述针对每个车轮轮速，根据该车轮轮速和所述设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度的步骤，包括：

针对每个车轮轮速，根据v₁＝V+ω^v×l，确定车辆中所述运动检测设备的中心点的一个预设点速度；

其中，所述v₁为车体坐标系下所述中心点的预设点速度，所述V为车体坐标系下的一个车轮轮速，所述ω^v为所述设备角速度在车体坐标系下的分解，所述l为所述车轮轮速对应的车轮与所述中心点之间的位置差向量；

所述根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的约束关系的步骤，包括：

根据测量值等于真实值与扰动误差之和的关系，以及将两个不同的预设点作为测量值，对所述相等关系进行扰动分解，得到所述状态量偏差与状态量误差之间的以下约束关系：

其中，V₀和V₁分别为车体坐标系下的两个不同的车轮轮速，所述l₀₁为两个车轮之间的位置差向量，所述δε为陀螺零偏误差。

8.如权利要求1所述的方法，其特征在于，所述从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量的步骤，包括：

从各个预设点速度中依次选择两个速度分别作为状态量测量和状态预测量，得到多组状态量测量和状态预测量；

所述根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差的步骤，包括：

按照次序从多组状态量测量和状态预测量中选择状态量测量和状态预测量的目标组，按照以下方式确定所述目标组对应的修正后状态量误差，当所述目标组为各个状态量测量和状态预测量组中的最后一个组时，将所述目标组对应的修正后状态量误差作为所述预设点在所述第一时刻的状态量误差：

根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差；

其中，当所述目标组不为第一个组时，所述待修正状态量误差为：所述目标组的上一个组对应的修正后状态量误差；当所述目标组为第一个组时，所述待修正状态量误差为零。

9.一种车载终端，其特征在于，包括：处理器、轮速计和运动检测设备；其中，处理器包括：

获取模块，用于获取轮速计在第一时刻采集的多个车轮的车轮轮速，以及运动检测设备在所述第一时刻采集的设备角速度；

第一确定模块，用于针对每个车轮轮速，根据该车轮轮速和所述设备角速度，确定车辆中预设点的一个预设点速度，针对多个车轮轮速得到多个预设点速度；

选择模块，用于从各个预设点速度中选择两个速度分别作为状态量测量和状态预测量；

第二确定模块，用于根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

其中，所述状态量偏差为状态量测量与状态预测量之间的偏差，所述约束关系为：基于同时刻不同车轮的车轮轮速计算的同一质点的真实速度相同的刚体模型以及测量值、真实值和扰动误差之间的关系得到。

10.如权利要求9所述的车载终端，其特征在于，所述处理器还包括：

修正模块，用于在确定所述预设点在所述第一时刻的状态量误差之后，根据所述预设点在所述第一时刻的状态量误差，对所述预设点在所述第一时刻的状态量进行修正，得到所述预设点在所述第一时刻的修正状态量；其中，所述预设点在所述第一时刻的状态量为：经过卡尔曼滤波后得到的估计状态量。

Images