CN112577513A - 一种状态量误差确定方法及车载终端 - 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ⁿ×]为将Vⁿ变换为的反对称矩阵，所述δφ为姿态误差，所述δVⁿ为速度误差，所述δε为陀螺零偏误差。

可选的，所述车轮轮速的数量为多个，每个车轮轮速为所述车辆中不同车轮的轮速；所述根据所述车轮轮速和所述设备角速度，确定车辆中预设点在所述第一时刻的第一类速度的步骤，包括：

根据各个车轮轮速和所述设备角速度，分别确定车辆中预设点在所述第一时刻的各个第一类速度；

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

将各个第一类速度构成的矩阵作为状态量测量，将所述第二类速度作为状态预测量，根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

或者，

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

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

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

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

第一获取模块，用于获取轮速计和运动检测设备在第一时刻分别采集的车轮轮速和设备角速度；

第一确定模块，用于根据所述车轮轮速和所述设备角速度，确定车辆中预设点在所述第一时刻的第一类速度；

第二获取模块，用于获取根据定位设备采集的数据确定的所述预设点在所述第一时刻的第二类速度；

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

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

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

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

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

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

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

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

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

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

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

根据

对所述第二时刻的置信度Σ_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ⁿ×]为将Vⁿ变换为的反对称矩阵，所述δφ为姿态误差，所述δVⁿ为速度误差，所述δε为陀螺零偏误差。

可选的，所述车轮轮速的数量为多个，每个车轮轮速为所述车辆中不同车轮的轮速；所述第一确定模块，根据所述车轮轮速和所述设备角速度，确定车辆中预设点在所述第一时刻的第一类速度的步骤，包括：

根据各个车轮轮速和所述设备角速度，分别确定车辆中预设点在所述第一时刻的各个第一类速度；

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

将各个第一类速度构成的矩阵作为状态量测量，将所述第二类速度作为状态预测量，根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

或者，

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

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

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

由上述内容可知，本发明实施例提供的状态量误差确定方法及车载终端，可以根据车轮轮速和设备角速度确定预设点的第一类速度，根据定位设备采集的数据确定预设点的第二类速度，这两类速度的真实值应相等，因此根据第一类速度和第二类速度之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，能够确定预设点的状态量误差。在确定状态量误差之后，该状态量误差可以用于对状态量进行修正，得到更准确的状态量。因此，本发明实施例能够确定车辆中预设点的状态量误差，对状态量数据进行矫正。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。

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

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

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

3、当车轮轮速的数量为多个时，可以采用至少两种方式确定状态量误差，实现对多个车轮轮速以及运动检测设备之间的数据融合，能够提高状态量误差的准确性。

附图说明

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

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

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

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

具体实施方式

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

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

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

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

S110：获取轮速计和运动检测设备在第一时刻分别采集的车轮轮速和设备角速度。

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

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

S120：根据车轮轮速和设备角速度，确定车辆中预设点在第一时刻的第一类速度。

其中，预设点可以为运动检测设备的中心点。状态量误差可以包括以下中的至少一项：位置误差、速度误差、姿态误差、陀螺零偏误差、加速度计零偏误差。例如，预设点可以为IMU的中心点，状态量误差可以包括IMU的位置误差、IMU的速度误差、IMU的姿态误差、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采用向量表示，ω^v为设备角速度在车体坐标系下的分解，l为车轮轮速对应的车轮与中心点之间的位置差向量。

步骤1b，根据测量值等于真实值与扰动误差之和的关系，以及将第一类速度和第二类速度作为测量值，对相等关系进行扰动分解，得到状态量偏差与状态量误差之间的约束关系的步骤，包括：

根据测量值等于真实值与扰动误差之和的关系，以及将第一类速度和第二类速度作为测量值，对相等关系进行扰动分解，得到以下状态量偏差与状态量误差之间的约束关系：

其中，Vⁿ为中心点在世界坐标系中的第二类速度，

为运动检测设备所在的设备坐标系与车体坐标系之间的旋转矩阵，

为设备坐标系与世界坐标系之间的旋转矩阵，

为车体坐标系与世界坐标系之间的旋转矩阵，[Vⁿ×]为将Vⁿ变换为的反对称矩阵，δφ为姿态误差，δVⁿ为速度误差，δε为陀螺零偏误差。

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

由刚体的运动模型可得：

该等式左边表示根据定位设备采集的数据确定的预设点在车体坐标系下的速度，等式右边表示根据车轮轮速和设备角速度确定的预设点在车体系下的速度。转向轮和驱动轮对于姿态、速度、陀螺零偏均有修正作用，因此V可以为V₀、V₁、V₂和V₃中的任意一个。

将每个参量的测量值表示成真实值与扰动误差的和，即Vⁿ＝f(Vⁿ _真，δφ，δVⁿ)，ω^v＝f(ω^v _真，δε)，并结合

经过整理后得到

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

Vⁿ、l、

等参量有关。

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

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

其中，

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

为正，倒车时为负。

在上述实施例中，车轮轮速可以为四个轮速中的任意一个。当车辆直行时，四个轮子的轮速均可以采用上述V₂的形式表示。

在另一实施例中，基于图1所示实施例，车轮轮速的数量为多个，每个车轮轮速为所述车辆中不同车轮的轮速，例如，车轮轮速可以为四个。步骤S120，根据车轮轮速和设备角速度，确定车辆中预设点在第一时刻的第一类速度的步骤，包括：

根据各个车轮轮速和设备角速度，分别确定车辆中预设点在第一时刻的各个第一类速度。

本步骤中，针对每个车轮轮速，根据该车轮轮速和设备角速度，可以确定车辆中预设点在第一时刻的一个第一类速度，共得到多个第一类速度。例如，当车轮轮速为四个时，可以计算得到预设点在第一时刻的四个第一类速度。

步骤S140，将第一类速度和第二类速度作为状态量测量和状态预测量，根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定预设点在所述第一时刻的状态量误差的步骤，至少包括以下两种实施方式。

方式一，将各个第一类速度构成的矩阵作为状态量测量，将第二类速度作为状态预测量，根据状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定预设点在第一时刻的状态量误差。

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

将第二类速度作为状态预测量，根据状态量测量与状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，对待修正状态量误差进行修正，得到修正后状态量误差。

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

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

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

以

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

在本实施方式中，采用迭代的方式，以前一个车轮轮速对应的状态量误差作为基础，通过卡尔曼滤波的量测更新得到新的状态量误差，直至计算到最后一个车轮轮速。

综上，本实施例中，当车轮轮速的数量为多个时，可以采用至少两种方式确定状态量误差，实现对多个车轮轮速以及运动检测设备之间的数据融合，能够提高状态量误差的准确性。

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

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

第一确定模块，用于根据车轮轮速和设备角速度，确定车辆中预设点在第一时刻的第一类速度；

第二获取模块，用于获取根据定位设备340采集的数据确定的预设点在第一时刻的第二类速度；

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

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

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

在本发明的另一实施例中，基于图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ⁿ×]为将Vⁿ变换为的反对称矩阵，δφ为姿态误差，δVⁿ为速度误差，δε为陀螺零偏误差。

在本发明的另一实施例中，基于图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ⁿ×]为将Vⁿ变换为的反对称矩阵，所述δφ为姿态误差，所述δVⁿ为速度误差，所述δε为陀螺零偏误差。

8.如权利要求1所述的方法，其特征在于，所述车轮轮速的数量为多个，每个车轮轮速为所述车辆中不同车轮的轮速；所述根据所述车轮轮速和所述设备角速度，确定车辆中预设点在所述第一时刻的第一类速度的步骤，包括：

根据各个车轮轮速和所述设备角速度，分别确定车辆中预设点在所述第一时刻的各个第一类速度；

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

将各个第一类速度构成的矩阵作为状态量测量，将所述第二类速度作为状态预测量，根据所述状态量测量与所述状态预测量之间的状态量偏差，以及预先建立的状态量偏差与状态量误差之间的约束关系，确定所述预设点在所述第一时刻的状态量误差；

或者，

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

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

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

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

第一获取模块，用于获取轮速计和运动检测设备在第一时刻分别采集的车轮轮速和设备角速度；

第一确定模块，用于根据所述车轮轮速和所述设备角速度，确定车辆中预设点在所述第一时刻的第一类速度；

第二获取模块，用于获取根据定位设备采集的数据确定的所述预设点在所述第一时刻的第二类速度；

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

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

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

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

Images