车辆状态确定方法、装置、设备、介质及产品与流程

1.本技术属于自动驾驶技术领域，尤其涉及一种车辆状态确定方法、装置、设备、介质及产品。


背景技术：

2.随着自动驾驶技术的快速发展，自动驾驶系统内部的功能模块越来越丰富，如控制模块、感知模块、定位模块等，充分利用各模块传感器的信息可提高系统的可靠性和通用性。定位模块的最小系统为惯性导航系统(inertial measurement unit，imu)和轮速组合的航迹推算模块，其可以保证车辆速度的稳定输出。
3.目前，在传统的imu与轮速融合定位技术中，由于无法确定车辆的三维速度，因此，只能假设车辆的侧向速度和垂向速度为0，仅基于车辆的轮速，也即纵向速度，进行定位。但是，实际上车辆的侧向速度并不一定为0，例如当车辆转弯时侧向速度并不为零。因此传统的imu与轮速融合定位技术，会造成假设与实际不一致，从而导致定位不准确的问题。


技术实现要素：

4.本技术实施例提供一种车辆状态确定方法、装置、设备、介质及产品，可以准确地得到目标车辆的纵向速度和侧向速度，从而提高定位的准确性。
5.第一方面，本技术实施例提供一种车辆状态确定方法，该方法包括：
6.获取目标车辆的行驶数据，行驶数据包括纵向速度、前轮转向角和惯性导航系统imu数据；
7.通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度；
8.通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据确定目标车辆的三维速度。
9.第二方面，本技术实施例提供了一种车辆状态确定装置，该装置包括：
10.第一获取模块，用于获取目标车辆的行驶数据，行驶数据包括纵向速度、前轮转向角和惯性导航系统imu数据；
11.第一确定模块，用于通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度；
12.第二确定模块，用于通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据确定目标车辆的三维速度。
13.第三方面，本技术实施例提供了一种电子设备，设备包括：处理器以及存储有计算机程序指令的存储器；
14.处理器执行计算机程序指令时实现如第一方面的任一项实施例中所示的车辆状态确定方法。
15.第四方面，本技术实施例提供了一种计算机存储介质，计算机存储介质上存储有计算机程序指令，计算机程序指令被处理器执行时实现第一方面的任一项实施例中所示的
车辆状态确定方法。
16.第五方面，本技术实施例提供了一种计算机程序产品，计算机程序产品中的指令由电子设备的处理器执行时，使得电子设备执行第一方面的任一项实施例中所示的车辆状态确定方法。
17.本技术实施例的车辆状态确定方法、装置、设备、介质及产品，可以获取目标车辆的行驶数据，该行驶数据包括纵向速度、前轮转向角和imu数据，然后通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度，再通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据便可以确定目标车辆的三维速度，这样，就可以准确地得到目标车辆的三维速度中包括的纵向速度和侧向速度，而无需将侧向速度假设为0，基于准确的纵向速度和侧向速度进行定位，便可以提高定位的准确性。
附图说明
18.为了更清楚地说明本技术实施例的技术方案，下面将对本技术实施例中所需要使用的附图作简单的介绍，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
19.图1是本技术一个实施例提供的一种车辆状态确定方法的流程图；
20.图2是是本技术一个实施例提供的一种单车模型的示意图；
21.图3是本技术一个实施例提供的一种全局坐标系和载体坐标系的示意图；
22.图4是本技术一个实施例提供的一种车辆状态确定装置的结构示意图；
23.图5是本技术一个实施例提供的一种电子设备的结构示意图。
具体实施方式
24.下面将详细描述本技术的各个方面的特征和示例性实施例，为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施例，对本技术进行进一步详细描述。应理解，此处所描述的具体实施例仅意在解释本技术，而不是限定本技术。对于本领域技术人员来说，本技术可以在不需要这些具体细节中的一些细节的情况下实施。下面对实施例的描述仅仅是为了通过示出本技术的示例来提供对本技术更好的理解。
25.需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
26.如背景技术，随着自动驾驶行业的快速发展，自动驾驶系统内部的功能模块越来越丰富，如控制、感知、定位模块，充分利用各模块传感器的信息可提高系统的可靠性和通用性。其中，定位模块的最小系统为imu和轮速组合的航迹推算模块，保证了车辆速度的稳定输出。目前，传统imu与轮速融合定位中轮速只有一维纵向速度信息，另外两维认为是零速。该假设在车辆直线行驶时成立，而当车辆转弯时，侧向速度不为零，因此会造成假设与
实际不一致的问题，导致定位不准确。
27.本技术实施例提供了一种车辆状态确定方法、装置、设备、介质及产品，可以获取目标车辆的行驶数据，该行驶数据包括纵向速度、前轮转向角和imu数据，然后通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度，再通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据便可以确定目标车辆的三维速度，这样，就可以准确地得到目标车辆的三维速度中包括的纵向速度和侧向速度，而无需将侧向速度假设为0，基于准确的纵向速度和侧向速度进行定位，便可以提高定位的准确性。
28.图1示出了本技术一个实施例提供的一种车辆状态确定方法的流程示意图，需要说明的是，该车辆状态确定方法的执行主体可以是车辆状态确定装置，如图1所示，该车辆状态确定方法可以包括如下步骤：
29.s110，获取目标车辆的行驶数据；
30.s120，通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度；
31.s130，通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据确定目标车辆的三维速度。
32.由此，可以获取目标车辆的行驶数据，该行驶数据包括纵向速度、前轮转向角和imu数据，然后通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度，再通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据便可以确定目标车辆的三维速度，这样，就可以准确地得到目标车辆的三维速度中包括的纵向速度和侧向速度，而无需将侧向速度假设为0，基于准确的纵向速度和侧向速度进行定位，便可以提高定位的准确性。
33.涉及s110，行驶数据可以包括纵向速度、前轮转向角和imu数据。imu数据可以包括三轴的加速度和三轴的角速度。
34.涉及s120，第一卡尔曼滤波器可以是基于动力学约束的卡尔曼滤波器。将纵向速度、前轮转向角和imu数据输入至第一卡尔曼滤波器，可以估计目标车辆的侧向速度。
35.示例性地，车辆状态确定装置初始化之后，可以将目标车辆的自动驾驶系统实时输出的纵向速度、前轮转向角和imu数据传入目标车辆的单车模型观测器，单车模型观测器完成参数初始化后，基于第一卡尔曼滤波器进行实时的侧向速度估计，得到目标车辆的侧向速度。
36.在一些实施方式中，为了更加准确地估计目标车辆的侧向速度，在s120之前，该方法还可以包括：
37.以目标车辆的质心处侧偏角、侧向速度和转向角速度为第一卡尔曼滤波器的系统状态，分别确定目标车辆的质心处侧偏角、侧向速度和转向角速度的微分方程；
38.根据目标车辆的质心处侧偏角、侧向速度和转向角速度的微分方程，构建第一卡尔曼滤波器的系统状态方程，并以纵向速度与质心处侧偏角的乘积和imu的z轴角速度为约束观测量，根据第一卡尔曼滤波器的系统状态，构建第一卡尔曼滤波器的观测方程，得到第一卡尔曼滤波器。
39.这里，可以如图2所示，第一卡尔曼滤波器的参数可以基于单车模型。以质心处侧偏角β、侧向速度vy、转向角速度r为第一卡尔曼滤波器的系统状态x(t)，分别确定质心处侧偏角β、侧向速度vy、转向角速度r的微分方程如下：
[0040][0041][0042][0043]
其中，β可以为质心处侧偏角，可以为质心处侧偏角的微分，vy可以为侧向速度，可以为侧向速度的微分，r可以为转向角速度，可以为转向角速度的微分，cf可以为前轮侧偏刚度，cr可以为后轮侧偏刚度，if可以为车辆质心到前轴的距离，ir可以为车辆质心到后轴的距离，m可以为整车质量，iz可以为车辆绕z轴的转动惯量，v
x
可以为纵向速度，δf可以为前轮转向角，w可以为前轮转向角的量化噪声。
[0044]
需要说明的是，cf、cr、if、ir、m和iz等参数可以由厂商提供，对于同一车辆，这些参数固定不变。
[0045]
此外，图2中还示出了前后轮距离l(l＝lf+lr)、前轮侧偏力f
yf
、后轮侧偏力f
yr
。侧偏力可以是使车辆产生侧向速度的原因，f
yr
＝cr*轮胎对地面的压力，侧偏力可以是第一卡尔曼滤波器系统状态方程推导时的中间量。
[0046]
然后，可以根据上述质心处侧偏角、侧向速度和转向角速度的微分方程，构建第一卡尔曼滤波器的系统状态方程。
[0047]
在一些实施方式中，第一卡尔曼滤波器的系统状态方程可以为：
[0048][0049]
其中，x(t)可以为第一卡尔曼滤波器的系统状态，可以为第一卡尔曼滤波器的系统状态的微分，u(t)可以为第一卡尔曼滤波器的系统输入，w(t)可以为第一卡尔曼滤波器的系统噪声，波器的系统噪声，可以为第一卡尔曼滤波器的系统状态矩阵，可以为第一卡尔曼滤波器的系统控制矩阵，可以为第一卡尔曼滤波器的系统噪声矩阵，cf可以为前轮侧偏刚度，cr可以为后轮侧偏刚度，if可以为车辆质心到前轴的距离，ir可以为车辆质心到后轴的距离，m可以为整车质量，iz可以为车辆绕z轴的转动惯量，v
x
可以为纵向速度。
[0050]
需要说明的是，u(t)可以等于前轮转向角δf。w(t)可以等于前轮转向角的量化噪声w。
[0051]
此外，由于车辆在行驶过程中，转向角速度r和imu的z轴角速度ωz保持一致，质心处侧偏角β较小的情况下，纵向速度v
x
与质心处侧偏角β的乘积v
x
β与侧向速度vy保持一致，因此，可以基于该关系构建约束观测量z(t)＝[v
x
βωz]
t
。并以z(t)＝[v
x
βωz]
t
为约束观测量，结合第一卡尔曼滤波器的系统状态x(t)，构建第一卡尔曼滤波器的观测方程。
[0052]
在一些实施方式中，第一卡尔曼滤波器的观测方程可以为：
[0053]
z(t)＝h1(t)x(t)+v1(t)
[0054]
其中，z(t)可以为第一卡尔曼滤波器的约束观测量，h1(t)＝[0 v
x 1]可以为第一卡尔曼滤波器的观测矩阵，v1(t)可以为第一卡尔曼滤波器的观测噪声。
[0055]
需要说明的是，v1(t)可以是预先统计确定的，具体确定方法属于本领域的常规技术手段，在此不做赘述。
[0056]
如此，通过上述过程可以设计出基于动力学约束的卡尔曼滤波器，也即第一卡尔曼滤波器，从而可以基于该第一卡尔曼滤波器更加准确地估计目标车辆的侧向速度。
[0057]
涉及s130，第二卡尔曼滤波器可以是基于速度信息辅助的完整约束卡尔曼滤波器。该速度信息可以包括纵向速度、侧向速度、垂向速度和imu数据，其中，纵向速度(也即，轮速)和imu数据可以是从自动驾驶系统获取的，侧向速度可以是通过第一卡尔曼滤波器估计得到的，垂向速度可以假设为0。将侧向速度、纵向速度和imu数据输入至第二卡尔曼滤波器，可以进一步估计目标车辆的三维速度，也即纵向速度、第一侧向速度和垂向速度。第一侧向速度可以比通过第一卡尔曼滤波器估计得到的侧向速度更准确。
[0058]
示例性地，通过单车模型观测器获得侧向速度之后，可以将纵向速度、侧向速度和imu数据传入误差运动学模型观测器，误差运动学模型观测器完成参数初始化后，可以基于第二卡尔曼滤波器进行实时的三维速度估计，得到目标车辆的三维速度。
[0059]
在一些实施方式中，为了更加准确地估计目标车辆的三维速度，在s130之前，该方法还可以包括：
[0060]
以目标车辆的多项误差为系统状态为第二卡尔曼滤波器的系统状态，分别确定多项误差的微分方程；
[0061]
根据多项误差的微分方程，构建第二卡尔曼滤波器的系统状态方程，并以目标车辆的三维速度误差为约束观测量，根据第二卡尔曼滤波器的系统状态，构建第二卡尔曼滤波器的观测方程，得到第二卡尔曼滤波器。
[0062]
这里，多项误差可以包括位置误差、速度误差、姿态误差、加速度计漂移和陀螺仪漂移。以位置误差δpn、速度误差δvn、姿态误差δθn、加速度计漂移δab、陀螺仪漂移δωb为第二卡尔曼滤波器的系统状态x(t)，分别确定位置误差δpn、速度误差δvn、姿态误差δθn、加速度计漂移δab和陀螺仪漂移δωb的微分方程如下：
[0063][0064][0065][0066][0067][0068]
其中，δpn可以为位置误差，δvn可以为速度误差，δθn可以为姿态误差，δab可以为加速度计漂移，δωb可以为陀螺仪漂移，可以为位置误差的微分，可以为速度误差的微分，可以为姿态误差的微分，可以为加速度计漂移的微分，可以为陀螺仪漂移的微分，r可以为载体坐标系与全局坐标系之间的姿态矩阵，am可以为加速度的测量值，ab可以为加速度的零偏，an可以为加速度的测量噪声，ωn可以为角速度的测量噪声，aw可以为加
速度的随机游走噪声，ωw可以为角速度的随机游走噪声。
[0069]
示例性地，载体坐标系(b系)与全局坐标系(n系)可以如图3所示。其中，载体坐标系(b系)可以以固连在目标车辆上的imu中心为原点，y轴沿车辆纵向向前，x轴垂直y轴指向车辆右侧，z轴垂直于x轴与y轴组成的平面向上(因角度原因，图中未示出z轴)。将目标车辆启动时的载体坐标系固定在世界坐标系中，该坐标系不随载体移动而变化，与地球相对静止，则该固定坐标系可以确定为全局坐标系(n系)。
[0070]
然后，可以根据上述位置误差、速度误差、姿态误差、加速度计漂移和陀螺仪漂移的微分方程，构建第二卡尔曼滤波器的系统状态方程。
[0071]
在一些实施方式中，第二卡尔曼滤波器的系统状态方程可以为：
[0072][0073]
其中，x(t)可以为第二卡尔曼滤波器的系统状态，可以为第二卡尔曼滤波器的系统状态的微分，可以为第二卡尔曼滤波器的系统状态矩阵，可以为第二卡尔曼滤波器的系统噪声矩阵，w(t)＝[a
n ω
n a
w ωw]
t
，i可以为单位矩阵，r可以为载体坐标系与全局坐标系之间的姿态矩阵，am可以为加速度的测量值，ab可以为加速度的零偏，an可以为加速度的测量噪声，ωn可以为角速度的测量噪声，aw可以为加速度的随机游走噪声，ωw可以为角速度的随机游走噪声。
[0074]
此外，三维速度误差可以包括纵向速度误差、侧向速度误差和垂向速度误差。可以假设目标车辆的垂向速度为0，根据纵向速度误差侧向速度误差及垂向速度误差构建完整约束观测量构建完整约束观测量并以为完整约束观测量，结合第二卡尔曼滤波器的系统状态x(t)，构建第二卡尔曼滤波器的观测方程。
[0075]
具体地，可以将全局坐标系中含有噪声影响的真实三维速度投影到载体坐标系，公式如下：
[0076][0077]
其中，可以为全局坐标系中含有噪声的真实三维速度在载体坐标系的投影，vb可以为载体坐标系中不含噪声的三维标称速度，δvb可以为载体坐标系中的三维速度误差，δvn可以为全局坐标系中的三维速度误差，vn可以为不含噪声的三维速度在全局坐标系的投影，δθn可以为姿态误差。
[0078]
然后，以δvb作为完整约束观测量并以为完整约束观测量，结合第二卡尔曼滤波器的系统状态x(t)，构建第二卡尔曼滤波器的观测方程。
[0079]
在一些实施方式中，第二卡尔曼滤波器的观测方程可以为：
[0080]
z(t)＝h2(t)x(t)+v2(t)
[0081]
其中，z(t)可以为第二卡尔曼滤波器的约束观测量，其中，z(t)可以为第二卡尔曼滤波器的约束观测量，可以为第二卡尔曼滤波器的观测矩阵，v2(t)可以为第二卡尔曼滤波器的观测噪声，vn可以
为不含噪声的三维速度在全局坐标系的投影。
[0082]
如此，通过上述过程可以设计出基于速度信息辅助的完整约束卡尔曼滤波器，也即第二卡尔曼滤波器，从而可以基于该第二卡尔曼滤波器更加准确地估计目标车辆的三维速度。
[0083]
在一些实施方式中，由于第二卡尔曼滤波器同时对位置、速度、姿态、陀螺仪和加速度计零偏进行了修正。但是，车辆运动以二维平面为主，航向角的能观性较弱，因此修正航向角误差会导致航向角的累计漂移，而航向角与横滚角、俯仰角线性相关，因此为了保持滤波器的一致性，获得准确的横滚角和俯仰角，并降低航向角漂移，在s120之后，该方法还可以包括：
[0084]
获取目标车辆的第一姿态信息；
[0085]
通过第二卡尔曼滤波器，基于侧向速度、纵向速度和第一姿态信息确定目标车辆的第二横滚角和第二俯仰角；
[0086]
输出目标车辆的目标姿态信息，目标姿态信息包括第一航向角、第二横滚角和第二俯仰角。
[0087]
这里，第一姿态信息可以是imu积分(也即，航迹推算)得到的目标车辆当前的姿态信息。第一姿态信息可以包括第一航向角、第一横滚角和第一俯仰角。目标姿态信息可以包括第一航向角、第二横滚角和第二俯仰角。
[0088]
具体地，可以在imu积分过程中，同步计算隔离陀螺仪z轴零偏的输出状态，此处用四元数表示姿态信息，记常规更新的姿态四元数为q，最终输出的姿态四元数为q
out
，q和q
out
的更新方式分别如下式所述：
[0089][0090][0091]
其中，qi可以为第i时刻常规更新的姿态信息，q
i+1
可以为第i+1时刻常规更新的姿态信息，(q
out
)i可以为第i时刻最终输出的姿态信息，(q
out
)
i+1
可以为第i+1时刻最终输出的姿态信息，ωm可以为三轴角速度的测量值，ωb可以为三轴角速度的零偏，ω
bz
可以为陀螺仪零偏的z轴分量，δt可以为预设周期。
[0092]
需要说明的是，ωb可以通过第二卡尔曼滤波器估计获得。
[0093]qout
的更新方式可以隔离第二卡尔曼滤波器对陀螺仪z轴零偏的修正。
[0094]
经过第二卡尔曼滤波器的滤波修正之后，q的横滚角、俯仰角和航行角都会得到修正，其中，横滚角和俯仰角的修正较为有效，而航向角与陀螺仪z轴零偏估计不准确，且两者都反馈进了q，导致q逐渐偏离真实航向；由于q
out
更新时隔离了不准确的陀螺仪z轴零偏，其航向角与真实航向角基本保持一致，因此，可以用q
out
对q的航向角进行覆盖，防止第二卡尔曼滤波器修正陀螺仪z轴零偏带来的航向漂移，具体修正方式如下式：
[0095]
[roll pitchyaw]＝f(q)
[0096]
[roll pitchyaw]
out
＝f(q
out
)
[0097]qout
＝f-1
([roll pitchyaw
out
])
[0098]
其中，f可以表示将姿态四元数转换为欧拉角的映射，f-1
以表示将姿态四元数转换
为欧拉角的逆映射。
[0099]
如此，保留了第二卡尔曼滤波器对横滚角和俯仰角的修正效果，并且防止了航向角的漂移，从而可以得到最优的姿态信息。
[0100]
本技术实施例提供的车辆状态确定方法，将动力学模型的侧向速度估计应用于误差运动学模型中，充分利用了自动驾驶系统丰富的数据，在未加入杆臂的情况下为误差运动学模型提供了准确的侧向速度约束，降低了误差运动学模型的维数，提高了车辆三维速度估计的精度和稳定性。此外，还提出了状态优化的姿态保持技术，在状态输出时隔离了陀螺仪z轴零偏和航向角的修正，同时保证了滤波器的一致性，降低了imu与轮速融合定位的累计漂移，提高了姿态估计的精度和稳定性，为控制模块和规划模块的工作打下了重要基础。
[0101]
此外，存在一些方案会估计车辆的侧向速度并用于运动学模型观测，最终输出车辆速度和姿态信息。但是，其采用的运动学模型为非线性模型，且未考虑imu的漂移等因素，因此该方案的精度和鲁棒性较低。而本技术实施例提供的车辆状态确定方法将估计出的侧向速度用于线性化的误差运动学模型观测，解决了仅通过纵向速度观测，信息不充分的问题，线性化的误差运动学模型和侧向速度观测提高了定位精度和稳定性，实现自动驾驶系统数据的充分利用，然后结合姿态稳定技术，降低imu与轮速融合的累计漂移误差，从而可以更好的进行融合定位和感知的开发工作。
[0102]
基于相同的发明构思，本技术实施例还提供了一种车辆状态确定装置。下面结合图4对本技术实施例提供的车辆状态确定装置进行详细说明。
[0103]
图4示出了本技术一个实施例提供的一种车辆状态确定装置的结构示意图。
[0104]
如图4所示，该车辆状态确定装置可以包括：
[0105]
第一获取模块401，用于获取目标车辆的行驶数据，行驶数据包括纵向速度、前轮转向角和惯性导航系统imu数据；
[0106]
第一确定模块402，用于通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度；
[0107]
第二确定模块403，用于通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据确定目标车辆的三维速度。
[0108]
由此，可以获取目标车辆的行驶数据，该行驶数据包括纵向速度、前轮转向角和imu数据，然后通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度，再通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据便可以确定目标车辆的三维速度，这样，就可以准确地得到目标车辆的三维速度中包括的纵向速度和侧向速度，而无需将侧向速度假设为0，基于准确的纵向速度和侧向速度进行定位，便可以提高定位的准确性。
[0109]
在一些实施方式中，由于第二卡尔曼滤波器同时对位置、速度、姿态、陀螺仪和加速度计零偏进行了修正。但是，车辆运动以二维平面为主，航向角的能观性较弱，因此修正航向角误差会导致航向角的累计漂移，而航向角与横滚角、俯仰角线性相关，因此为了保持滤波器的一致性，获得准确的横滚角和俯仰角，并降低航向角漂移，该装置还可以包括：
[0110]
第二获取模块，用于在通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度之后，获取目标车辆的第一姿态信息，第一姿态信息包括第一航向角、第一横滚角和第一俯仰角；
[0111]
第三确定模块，用于通过第二卡尔曼滤波器，基于侧向速度、纵向速度和第一姿态信息确定目标车辆的第二横滚角和第二俯仰角；
[0112]
输出模块，用于输出目标车辆的目标姿态信息，目标姿态信息包括第一航向角、第二横滚角和第二俯仰角。
[0113]
在一些实施方式中，为了更加准确地估计目标车辆的侧向速度，该装置还可以包括：
[0114]
第四确定模块，用于在通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度之前，以目标车辆的质心处侧偏角、侧向速度和转向角速度为第一卡尔曼滤波器的系统状态，分别确定目标车辆的质心处侧偏角、侧向速度和转向角速度的微分方程；
[0115]
第一构建模块，用于根据目标车辆的质心处侧偏角、侧向速度和转向角速度的微分方程，构建第一卡尔曼滤波器的系统状态方程，并以纵向速度与质心处侧偏角的乘积和imu的z轴角速度为约束观测量，根据第一卡尔曼滤波器的系统状态，构建第一卡尔曼滤波器的观测方程，得到第一卡尔曼滤波器。
[0116]
在一些实施方式中，第一卡尔曼滤波器的系统状态方程为：
[0117][0118]
其中，x(t)为第一卡尔曼滤波器的系统状态，为第一卡尔曼滤波器的系统状态的微分，u(t)为第一卡尔曼滤波器的系统输入，w(t)为第一卡尔曼滤波器的系统噪声，为第一卡尔曼滤波器的系统状态矩阵，为第一卡尔曼滤波器的系统控制矩阵，为第一卡尔曼滤波器的系统噪声矩阵，cf为前轮侧偏刚度，cr为后轮侧偏刚度，if为车辆质心到前轴的距离，ir为车辆质心到后轴的距离，m为整车质量，iz为车辆绕z轴的转动惯量，v
x
为纵向速度，
[0119]
第一卡尔曼滤波器的观测方程为：
[0120]
z(t)＝h1(t)x(t)+v1(t)
[0121]
其中，z(t)为第一卡尔曼滤波器的约束观测量，h1(t)＝[0v
x 1]为第一卡尔曼滤波器的观测矩阵，v1(t)为第一卡尔曼滤波器的观测噪声。
[0122]
在一些实施方式中，为了更加准确地估计目标车辆的三维速度，该装置还可以包括：
[0123]
第五确定模块，用于在通过第二卡尔曼滤波器，基于侧向速度、纵向速度和imu数据确定目标车辆的三维速度之前，以目标车辆的多项误差为系统状态为第二卡尔曼滤波器的系统状态，分别确定多项误差的微分方程；
[0124]
第二构建模块，用于根据多项误差的微分方程，构建第二卡尔曼滤波器的系统状态方程，并以目标车辆的三维速度误差为约束观测量，根据第二卡尔曼滤波器的系统状态，构建第二卡尔曼滤波器的观测方程，得到第二卡尔曼滤波器。
[0125]
在一些实施方式中，第二卡尔曼滤波器的系统状态方程为：
[0126][0127]
其中，x(t)为第二卡尔曼滤波器的系统状态，为第二卡尔曼滤波器的系统状态的微分，为第二卡尔曼滤波器的系统状态矩阵，为第二卡尔曼滤波器的系统噪声矩阵，w(t)＝[anω
n awωw]
t
，i为单位矩阵，r为载体坐标系与全局坐标系之间的姿态矩阵，am为加速度的测量值，ab为加速度的零偏，an为加速度的测量噪声，ωn为角速度的测量噪声，aw为加速度的随机游走噪声，ωw为角速度的随机游走噪声，
[0128]
第二卡尔曼滤波器的观测方程为：
[0129]
z(t)＝h2(t)x(t)+v2(t)
[0130]
其中，z(t)为第二卡尔曼滤波器的约束观测量，其中，z(t)为第二卡尔曼滤波器的约束观测量，为第二卡尔曼滤波器的观测矩阵，v2(t)为第二卡尔曼滤波器的观测噪声，vn为不含噪声的三维速度在全局坐标系的投影。
[0131]
图5示出了本技术一个实施例提供的一种电子设备的结构示意图。
[0132]
如图5所示，该电子设备5能够实现根据本技术实施例中的车辆状态确定方法以及车辆状态确定装置的电子设备的示例性硬件架构的结构图。
[0133]
该电子设备可以指代本技术实施例中的电子设备。
[0134]
该电子设备5可以包括处理器501以及存储有计算机程序指令的存储器502。
[0135]
具体地，上述处理器501可以包括中央处理器(cpu)，或者特定集成电路(application specific integrated circuit，asic)，或者可以被配置成实施本技术实施例的一个或多个集成电路。
[0136]
存储器502可以包括用于数据或指令的大容量存储器。举例来说而非限制，存储器502可包括硬盘驱动器(hard disk drive，hdd)、软盘驱动器、闪存、光盘、磁光盘、磁带或通用串行总线(universal serial bus，usb)驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器502可包括可移除或不可移除(或固定)的介质。在合适的情况下，存储器502可在综合网关容灾设备的内部或外部。在特定实施例中，存储器502是非易失性固态存储器。在特定实施例中，存储器502可包括只读存储器(rom)，随机存取存储器(ram)，磁盘存储介质设备，光存储介质设备，闪存设备，电气、光学或其他物理/有形的存储器存储设备。因此，通常，存储器502包括一个或多个编码有包括计算机可执行指令的软件的有形(非暂态)计算机可读存储介质(例如，存储器设备)，并且当该软件被执行(例如，由一个或多个处理器)时，其可操作来执行参考根据本技术的一方面的方法所描述的操作。
[0137]
处理器501通过读取并执行存储器502中存储的计算机程序指令，以实现上述实施例中的任意一种车辆状态确定方法。
[0138]
在一个示例中，该电子设备还可包括通信接口503和总线504。其中，如图5所示，处理器501、存储器502、通信接口503通过总线504连接并完成相互间的通信。
[0139]
通信接口503，主要用于实现本技术实施例中各模块、装置、单元和/或设备之间的通信。
[0140]
总线504包括硬件、软件或两者，将电子设备的部件彼此耦接在一起。举例来说而非限制，总线可包括加速图形端口(agp)或其他图形总线、增强工业标准架构(eisa)总线、
前端总线(fsb)、超传输(ht)互连、工业标准架构(isa)总线、无限带宽互连、低引脚数(lpc)总线、存储器总线、微信道架构(mca)总线、外围组件互连(pci)总线、pci-express(pci-x)总线、串行高级技术附件(sata)总线、视频电子标准协会局部(vlb)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线504可包括一个或多个总线。尽管本技术实施例描述和示出了特定的总线，但本技术考虑任何合适的总线或互连。
[0141]
该电子设备可以执行本技术实施例中的车辆状态确定方法，从而实现结合图1至图4描述的车辆状态确定方法和装置。
[0142]
另外，结合上述实施例中的车辆状态确定方法，本技术实施例可提供一种计算机存储介质来实现。该计算机存储介质上存储有计算机程序指令；该计算机程序指令被处理器执行时实现上述实施例中的任意一种车辆状态确定方法。
[0143]
需要明确的是，本技术并不局限于上文所描述并在图中示出的特定配置和处理。为了简明起见，这里省略了对已知方法的详细描述。在上述实施例中，描述和示出了若干具体的步骤作为示例。但是，本技术的方法过程并不限于所描述和示出的具体步骤，本领域的技术人员可以在领会本技术的精神后，作出各种改变、修改和添加，或者改变步骤之间的顺序。
[0144]
以上所述的结构框图中所示的功能块可以实现为硬件、软件、固件或者它们的组合。当以硬件方式实现时，其可以例如是电子电路、专用集成电路(asic)、适当的固件、插件、功能卡等等。当以软件方式实现时，本技术的元素是被用于执行所需任务的程序或者代码段。程序或者代码段可以存储在机器可读介质中，或者通过载波中携带的数据信号在传输介质或者通信链路上传送。“机器可读介质”可以包括能够存储或传输信息的任何介质。机器可读介质的例子包括电子电路、半导体存储器设备、rom、闪存、可擦除rom(erom)、软盘、cd-rom、光盘、硬盘、光纤介质、射频(rf)链路，等等。代码段可以经由诸如因特网、内联网等的计算机网络被下载。
[0145]
还需要说明的是，本技术中提及的示例性实施例，基于一系列的步骤或者装置描述一些方法或系统。但是，本技术不局限于上述步骤的顺序，也就是说，可以按照实施例中提及的顺序执行步骤，也可以不同于实施例中的顺序，或者若干步骤同时执行。
[0146]
上面参考根据本技术的实施例的方法、装置(系统)和计算机程序产品的流程图和/或框图描述了本技术的各方面。应当理解，流程图和/或框图中的每个方框以及流程图和/或框图中各方框的组合可以由计算机程序指令实现。这些计算机程序指令可被提供给通用计算机、专用计算机、或其它可编程数据处理装置的处理器，以产生一种机器，使得经由计算机或其它可编程数据处理装置的处理器执行的这些指令使能对流程图和/或框图的一个或多个方框中指定的功能/动作的实现。这种处理器可以是但不限于是通用处理器、专用处理器、特殊应用处理器或者现场可编程逻辑电路。还可理解，框图和/或流程图中的每个方框以及框图和/或流程图中的方框的组合，也可以由执行指定的功能或动作的专用硬件来实现，或可由专用硬件和计算机指令的组合来实现。
[0147]
以上所述，仅为本技术的具体实施方式，所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的系统、模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。应理解，本技术的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到各种等效的修改或替换，
这些修改或替换都应涵盖在本技术的保护范围之内。

技术特征：
1.一种车辆状态确定方法，其特征在于，包括：获取目标车辆的行驶数据，所述行驶数据包括纵向速度、前轮转向角和惯性导航系统imu数据；通过第一卡尔曼滤波器，基于所述行驶数据确定所述目标车辆的侧向速度；通过第二卡尔曼滤波器，基于所述侧向速度、所述纵向速度和所述imu数据确定所述目标车辆的三维速度。2.根据权利要求1所述的方法，其特征在于，在所述通过第一卡尔曼滤波器，基于所述行驶数据确定所述目标车辆的侧向速度之后，所述方法还包括：获取所述目标车辆的第一姿态信息，所述第一姿态信息包括第一航向角、第一横滚角和第一俯仰角；通过第二卡尔曼滤波器，基于所述侧向速度、所述纵向速度和所述第一姿态信息确定所述目标车辆的第二横滚角和第二俯仰角；输出所述目标车辆的目标姿态信息，所述目标姿态信息包括所述第一航向角、所述第二横滚角和所述第二俯仰角。3.根据权利要求1所述的方法，其特征在于，在所述通过第一卡尔曼滤波器，基于所述行驶数据确定所述目标车辆的侧向速度之前，所述方法还包括：以所述目标车辆的质心处侧偏角、侧向速度和转向角速度为所述第一卡尔曼滤波器的系统状态，分别确定所述目标车辆的质心处侧偏角、侧向速度和转向角速度的微分方程；根据所述目标车辆的质心处侧偏角、侧向速度和转向角速度的微分方程，构建所述第一卡尔曼滤波器的系统状态方程，并以所述纵向速度与所述质心处侧偏角的乘积和imu的z轴角速度为约束观测量，根据所述第一卡尔曼滤波器的系统状态，构建所述第一卡尔曼滤波器的观测方程，得到所述第一卡尔曼滤波器。4.根据权利要求3所述的方法，其特征在于，所述第一卡尔曼滤波器的系统状态方程为：其中，x(t)为所述第一卡尔曼滤波器的系统状态，为所述第一卡尔曼滤波器的系统状态的微分，u(t)为所述第一卡尔曼滤波器的系统输入，w(t)为所述第一卡尔曼滤波器的系统噪声，系统噪声，为所述第一卡尔曼滤波器的系统状态矩阵，为所述第一卡尔曼滤波器的系统控制矩阵，为所述第一卡尔曼滤波器的系统噪声矩阵，c
f
为前轮侧偏刚度，c
r
为后轮侧偏刚度，i
f
为车辆质心到前轴的距离，i
r
为车辆质心到后轴的距离，m为整车质量，i
z
为车辆绕z轴的转动惯量，v
x
为纵向速度，所述第一卡尔曼滤波器的观测方程为：z(t)＝h1(t)x(t)+v1(t)
其中，z(t)为所述第一卡尔曼滤波器的约束观测量，h1(t)＝[0v
x 1]为所述第一卡尔曼滤波器的观测矩阵，v1(t)为所述第一卡尔曼滤波器的观测噪声。5.根据权利要求1所述的方法，其特征在于，在所述通过第二卡尔曼滤波器，基于所述侧向速度、所述纵向速度和所述imu数据确定所述目标车辆的三维速度之前，所述方法还包括：以所述目标车辆的多项误差为系统状态为所述第二卡尔曼滤波器的系统状态，分别确定所述多项误差的微分方程；根据所述多项误差的微分方程，构建所述第二卡尔曼滤波器的系统状态方程，并以所述目标车辆的三维速度误差为约束观测量，根据所述第二卡尔曼滤波器的系统状态，构建所述第二卡尔曼滤波器的观测方程，得到所述第二卡尔曼滤波器。6.根据权利要求5所述的方法，其特征在于，所述第二卡尔曼滤波器的系统状态方程为：其中，x(t)为所述第二卡尔曼滤波器的系统状态，为所述第二卡尔曼滤波器的系统状态的微分，为所述第二卡尔曼滤波器的系统状态矩阵，为所述第二卡尔曼滤波器的系统噪声矩阵，w(t)＝[a
n
ω
n a
w
ω
w
]
t
，i为单位矩阵，r为载体坐标系与全局坐标系之间的姿态矩阵，a
m
为加速度的测量值，a
b
为加速度的零偏，a
n
为加速度的测量噪声，ω
n
为角速度的测量噪声，a
w
为加速度的随机游走噪声，ω
w
为角速度的随机游走噪声，所述第二卡尔曼滤波器的观测方程为：z(t)＝h2(t)x(t)+v2(t)其中，z(t)为所述第二卡尔曼滤波器的约束观测量，其中，z(t)为所述第二卡尔曼滤波器的约束观测量，为所述第二卡尔曼滤波器的观测矩阵，v2(t)为所述第二卡尔曼滤波器的观测噪声，v
n
为不含噪声的三维速度在全局坐标系的投影。7.一种车辆状态确定装置，其特征在于，所述装置包括：第一获取模块，用于获取目标车辆的行驶数据，所述行驶数据包括纵向速度、前轮转向角和惯性导航系统imu数据；第一确定模块，用于通过第一卡尔曼滤波器，基于所述行驶数据确定所述目标车辆的侧向速度；第二确定模块，用于通过第二卡尔曼滤波器，基于所述侧向速度、所述纵向速度和所述imu数据确定所述目标车辆的三维速度。8.一种电子设备，其特征在于，所述设备包括：处理器以及存储有计算机程序指令的存储器；所述处理器执行所述计算机程序指令时实现如权利要求1-6任意一项所述的车辆状态确定方法。9.一种计算机存储介质，其特征在于，所述计算机存储介质上存储有计算机程序指令，
所述计算机程序指令被处理器执行时实现如权利要求1-6任意一项所述的车辆状态确定方法。10.一种计算机程序产品，其特征在于，所述计算机程序产品中的指令由电子设备的处理器执行时，使得所述电子设备执行如权利要求1-6任意一项所述的车辆状态确定方法。

技术总结
本申请公开了一种车辆状态确定方法、装置、设备、介质及产品。该方法包括：获取目标车辆的行驶数据，行驶数据包括纵向速度、前轮转向角和惯性导航系统IMU数据；通过第一卡尔曼滤波器，基于行驶数据确定目标车辆的侧向速度；通过第二卡尔曼滤波器，基于侧向速度、纵向速度和IMU数据确定目标车辆的三维速度。这样，可以准确地得到目标车辆的纵向速度和侧向速度，从而提高定位的准确性。从而提高定位的准确性。从而提高定位的准确性。


技术研发人员：周盟孟 李瑞涵 高梁
受保护的技术使用者：长沙智能驾驶研究院有限公司
技术研发日：2023.02.20
技术公布日：2023/6/26