位姿估计方法、装置、设备及介质与流程

1.本公开涉及信息技术领域，尤其涉及一种位姿估计方法、装置、设备及介质。


背景技术：

2.在无人驾驶场景中，若要实现无人车等车辆的高精度定位及导航，除了借助地图信息，还需要能够对无人车的位姿进行精确估计。
3.现有的位姿估计方案中大多需要无人车配置高品质imu(inertial measurement unit，惯性测量单元)，但是高品质imu的造价昂贵，所需成本高昂，难以普及适用于诸如低端车型等车辆，普适性较差，难以广泛应用。


技术实现要素：

4.为了解决上述技术问题或者至少部分地解决上述技术问题，本公开提供了一种位姿估计方法、装置、设备及介质。
5.第一方面，本公开实施例提供了一种位姿估计方法，获取车辆在t时刻和t+1时刻之间的目标观测数据；其中，所述目标观测数据包括轮速计测量得到的轮速以及陀螺仪测量得到的角速度；所述t时刻和所述t+1时刻为指定传感器在进行数据观测时的两个相邻时刻，且所述指定传感器为所述车辆上设置的用于观测车辆位姿的传感器；根据预设的轮速计模型和车辆运动学模型，获取所述目标观测数据表征的车辆名义状态的时间导数；对所述目标观测数据表征的车辆名义状态的时间导数进行数值积分，基于积分结果获取所述车辆在所述t时刻和所述t+1时刻之间的名义状态增量；所述名义状态增量包括基于所述目标观测数据得到的车辆位姿增量和陀螺仪偏置增量；根据误差传播算法、所述轮速计模型、所述车辆运动学模型以及所述车辆名义状态的时间导数，得到所述t时刻和所述t+1时刻之间对应的目标误差置信度；其中，所述目标误差置信度是所述t时刻和所述t+1时刻之间真实状态增量与所述名义状态增量的误差的置信度；根据所述指定传感器在所述t时刻的观测数据和所述t+1时刻的观测数据、所述目标观测数据对应的名义状态增量以及所述目标误差置信度，对所述车辆在所述t时刻和所述t+1时刻的真实位姿进行估计。
6.第二方面，本公开实施例还提供了一种位姿估计装置，包括：数据获取模块，用于获取车辆在t时刻和t+1时刻之间的目标观测数据；其中，所述目标观测数据包括轮速计测量得到的轮速以及陀螺仪测量得到的角速度；所述t时刻和所述t+1时刻为指定传感器在进行数据观测时的两个相邻时刻，且所述指定传感器为所述车辆上设置的用于观测车辆位姿的传感器；状态获取模块，用于根据预设的轮速计模型和车辆运动学模型，获取所述目标观测数据表征的车辆名义状态的时间导数；积分模块，用于对所述目标观测数据表征的车辆名义状态的时间导数进行数值积分，基于积分结果获取所述车辆在所述t时刻和所述t+1时刻之间的名义状态增量；所述名义状态增量包括基于所述目标观测数据得到的车辆位姿增量和陀螺仪偏置增量；置信度获取模块，用于根据误差传播算法、所述轮速计模型、所述车辆运动学模型以及所述车辆名义状态的时间导数，得到所述t时刻和所述t+1时刻之间对应
的目标误差置信度；其中，所述目标误差置信度是所述t时刻和所述t+1时刻之间真实状态增量与所述名义状态增量的误差的置信度；位姿估计模块，用于根据所述指定传感器在所述t时刻的观测数据和所述t+1时刻的观测数据、所述目标观测数据对应的名义状态增量以及所述目标误差置信度，对所述车辆在所述t时刻和所述t+1时刻的真实位姿进行估计。
7.第三方面，本公开实施例还提供了一种电子设备，所述电子设备包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现如本公开实施例提供的位姿估计方法。
8.第四方面，本公开实施例还提供了一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序用于执行如本公开实施例提供的位姿估计方法。
9.本公开实施例提供的上述技术方案，根据车辆在t时刻和t+1时刻之间的目标观测数据(轮速计测量得到的轮速以及陀螺仪测量得到的角速度)、轮速计模型和车辆运动学模型确定轮速计观测数据表征的车辆名义状态的时间导数，并进一步通过对目标观测数据表征的车辆名义状态的时间导数进行数值积分以得到车辆在t时刻和t+1时刻之间的名义状态增量，以及根据误差传播算法、轮速计模型、车辆运动学模型以及车辆名义状态的时间导数，得到目标误差置信度，最后根据指定传感器在t时刻的观测数据和t+1时刻的观测数据、轮速计观测数据对应的名义状态增量以及目标误差置信度，对车辆在t时刻和t+1时刻的真实位姿进行估计，上述方式无需高品质imu传感器，仅基于低成本的陀螺仪和轮速计进行多传感器耦合即可实现，通过结合低成本的陀螺仪与轮速计中精确度通常较高的测量数据(也即轮速计测量得到的轮速以及陀螺仪测量得到的角速度)、以及其他用于观测车辆位姿的传感器便可进行位姿估计，普适性较强，便于广泛应用。
10.应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
11.此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。
12.为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
13.图1为本公开实施例提供的一种位姿估计方法的流程示意图；
14.图2为本公开实施例提供的一种位姿估计装置的结构示意图；
15.图3为本公开实施例提供的一种电子设备的结构示意图。
具体实施方式
16.为了能够更清楚地理解本公开的上述目的、特征和优点，下面将对本公开的方案进行进一步描述。需要说明的是，在不冲突的情况下，本公开的实施例及实施例中的特征可以相互组合。
17.在下面的描述中阐述了很多具体细节以便于充分理解本公开，但本公开还可以采用其他不同于在此描述的方式来实施；显然，说明书中的实施例只是本公开的一部分实施
例，而不是全部的实施例。
18.建图与定位技术是自动驾驶领域的核心技术之一，对于无人驾驶场景而言，通常首先需要对环境进行建图，然后在建好的地图中实现定位功能。但是提前建立好的地图会随着环境变化和时间推移而可能过时，逐渐无法为车辆定位提供足够的信息，因此若要实现高精度定位，除了借助地图的信息，还需要鲁棒的里程计，也即需要基于车辆上的传感器对车辆位姿进行估计，位姿估计的精度会直接影响车辆定位。
19.目前的位姿估计方案中大多需要采用高精度的imu观测数据与诸如激光雷达/视觉传感器等车辆上配置的其它传感器的观测数据进行紧耦合(融合处理)，但是这种方案对imu精度要求较高，需要无人车配置高品质的imu，但高品质的imu非常昂贵，因此无人驾驶的大多低端车型难以采用上述紧耦合方案。相比之下，大多车辆都配置有轮速计以及低成本imu，本公开实施例可以充分结合低成本imu与轮速计中精确度通常较高的测量数据(也即轮速计测量得到的轮速以及陀螺仪测量得到的角速度)以及其他传感器的观测数据进行耦合，也即，仅采用低成本imu中测量精确度通常较高的陀螺仪角速度以及轮速计中测量精确度通常较高的轮速，而低成本imu所得的加速度噪声大，测量精确度较差，轮速计所得的偏转角噪声大，测量精确度差，因此本公开实施例不采用低成本imu所得的加速度及轮速计所得的偏转角。又由于轮速计的轮速以及陀螺仪角速度(低成本imu的陀螺仪即可满足)都非常容易获取，因此发明人研究了将轮速计测量得到的轮速、陀螺仪测量得到的角速度和诸如相机、雷达等其它传感器的观测数据进行位姿估计的方案，该方案也可称为基于轮速计及低成本imu的多传感器融合的紧耦合方案，为便于理解，以下进行详细阐述说明。
20.图1为本公开实施例提供的一种位姿估计方法的流程示意图，该方法可以由位姿估计装置执行，其中该装置可以采用软件和/或硬件实现，一般可集成在电子设备中。如图1所示，该方法主要包括如下步骤s102～步骤s110：
21.步骤s102，获取车辆在t时刻和t+1时刻之间的目标观测数据；其中，目标观测数据包括轮速计测量得到的轮速以及陀螺仪测量得到的角速度；t时刻和t+1时刻为指定传感器在进行数据观测时的两个相邻时刻，且指定传感器为车辆上设置的用于观测车辆位姿的传感器。
22.在一些实施示例中，t时刻和t+1时刻可以为预设时段中的任意两个相邻时刻，实际应用中可以设置一个时间窗口(也即对应前述预设时段)，该时间窗口中可以包含有指定传感器进行数据观测的x个时刻，t时刻和t+1时刻即为x个时刻中的任意两个相邻时刻。
23.在本公开实施例中，主要获取t时刻和t+1时刻之间的轮速和角速度，且该轮速是通过轮速计测量所得，而该角速度是通过陀螺仪测量所得，且该陀螺仪采用低成本imu即可满足。由于大多车辆都会配置有低成本的imu及轮速计，因此轮速计测量得到的轮速以及陀螺仪测量得到的角速度非常容易获得，且这两个数据分别是轮速计和陀螺仪的观测数据中精确度最高的数据，有助于进一步保障位姿估计精度。
24.上述指定传感器的数量可以为一个，也可以为多个，而且本公开对指定传感器的种类不进行限定，任何可以用于估计位姿的传感器均可，诸如可以为激光雷达、超声波传感器、视觉传感器等。假设一共有m个指定传感器，m个指定传感器可以包括种类相同或不同的传感器，诸如指定传感器的数量为2个(也即m＝2)，第一个指定传感器为激光雷达，第二个指定传感器为视觉传感器；又诸如，指定传感器的数量为4个，第一个指定传感器和第二个
指定传感器均为超声波传感器(设置在车辆的不同位置)，第三个指定传感器为视觉传感器，第四个指定传感器为激光雷达。
25.步骤s104，根据预设的轮速计模型和车辆运动学模型，获取目标观测数据表征的车辆名义状态的时间导数。其中，车辆名义状态包括车辆名义位姿以及名义陀螺仪偏置。车辆名义位姿即为名义的车辆位置及姿态，也可理解为名义的车辆位移及朝向。
26.本公开实施例对轮速计模型不进行限制，诸如可以为差分轮速计、ackermann模型、单车模型等，具体可根据实际需求而预设所需采用的轮速计模型。车辆运动学模型是用采集的车辆相关数据计算车辆位置和姿态随时间变化的公式，本公开实施例可直接采用相关技术中的车辆运动学模型实现。
27.在轮速计模型和车辆运动学模型的基础上，即可采用目标观测数据来表征车辆名义状态的时间导数，具体而言，基于轮速计模型表示的目标观测数据代入至车辆运动学模型，以此来表征车辆名义状态的时间导数。应当注意的是，在实际情况中无论多么先进的轮速计都会携带有测量噪声，只是携带的噪声大小有差异，无法达到完全没有噪声的理想状态，因此轮速计观测数据也通常是携带有噪声的测量值，因此用其表征的车辆状态也仅为车辆名义状态，也即采用名义上的观测数据所得的车辆状态。
28.步骤s106，对目标观测数据表征的车辆名义状态的时间导数进行数值积分，基于积分结果获取车辆在t时刻和t+1时刻之间的名义状态增量；名义状态增量包括基于目标观测数据得到的车辆位姿增量和陀螺仪偏置增量。
29.在本公开实施例中，通过对目标观测数据表征的车辆名义状态的时间导数进行数值积分，可以得到车辆名义状态的转移方程，然后根据车辆名义状态的转移方程可以进一步获取基于目标观测数据得到的名义状态增量，该名义状态增量包括车辆位姿增量(也即名义位姿增量)和陀螺仪偏置增量(也即名义陀螺仪偏置增量)，示例性地，基于车辆名义状态的转移方程进行递推处理，从而得到名义状态增量。本公开实施例对数值积分的具体方式不进行限制，诸如数值积分可以为欧拉积分、中值积分或者高阶龙格库塔积分等，具体可根据实际情况灵活选用所需的数值积分方式。
30.步骤s108，根据误差传播算法、轮速计模型、车辆运动学模型以及车辆名义状态的时间导数，得到t时刻和t+1时刻之间对应的目标误差置信度；其中，目标误差置信度是t时刻和t+1时刻之间真实状态增量与名义状态增量的误差的置信度。
31.在误差传播算法中，假设理想状况下可以得到车辆真实状态(true state)，也就是没有任何误差的状态；而名义状态到真实状态之间的误差就是误差状态(error state)。在任何时候都可以把误差状态的均值加入到名义状态(nominal state)中计算出当前对真实状态的估计。本公开实施例在具体实现时，可以首先基于误差传播算法、轮速计模型、车辆运动学模型以及车辆名义状态的时间导数，得到t时刻和t+1时刻之间对应的目标误差置信度，置信度越高，状态估计的可信度越高。在一种具体实施示例中，可以首先基于轮速计模型和车辆运动学模型获取车辆真实状态的时间导数；然后根据误差传播算法，采用车辆真实状态的时间导数结合车辆名义状态的时间导数，确定t时刻和t+1时刻之间真实状态增量与名义状态增量的误差的时间导数。具体而言，误差传播算法表征真实状态减去名义状态等于误差状态，根据误差传播算法可知，令车辆真实状态的时间导数减去车辆名义状态的时间导数，便可得到误差状态的时间导数，也即可得到上述t时刻和t+1时刻之间真实状
态增量与名义状态增量的误差的时间导数。之后对误差的时间导数进行数值积分，获得误差的转移方程；基于误差的转移方程递推得到t时刻和t+1时刻之间真实状态增量与名义状态增量之间对应的目标误差置信度。
32.在一些具体的实施示例中，基于误差的转移方程递推得到t时刻和t+1时刻之间真实状态增量与名义状态增量之间的误差的协方差，并采用误差的协方差表征t时刻和t+1时刻之间对应的目标误差置信度。假设误差的协方差为p，则轮速计误差置信度可以为p-1，协方差越大，置信度越小。在已知目标观测数据对应的名义状态增量以及目标误差置信度的基础上，再与其它指定传感器的观测数据进行耦合。
33.步骤s110，根据指定传感器在t时刻的观测数据和t+1时刻的观测数据、目标观测数据对应的名义状态增量以及目标误差置信度，对车辆在t时刻和t+1时刻的真实位姿进行估计。
34.在一些具体的实施示例中，可以根据指定传感器在t时刻的观测数据和t+1时刻的观测数据，获取车辆在t时刻的第一名义位姿和第一传感器误差置信度，以及车辆在t+1时刻的第二名义位姿和第二传感器误差置信度。上述获取指定传感器在每个时刻对应的名义位姿及相应的误差置信度的方式，均可参照相关技术中实现，在此不进行限制。
35.然后根据轮速计观测数据对应的名义状态增量和目标误差置信度，以及指定传感器对应的第一名义位姿、第一传感器误差置信度、第二名义位姿和第二传感器误差置信度，对车辆在t时刻和t+1时刻的真实位姿进行估计。
36.在本公开实施例中，不仅可以通过目标观测数据计算出车辆的状态，还会通过诸如激光雷达、视觉传感器等其它指定传感器计算车辆的状态，每个传感器都对应一个误差置信度，最后在将目标观测数据和其它指定传感器的观测数据紧耦合进行位姿估计时，会考虑包括轮速计在内的所有传感器的置信度，传感器的置信度越高，真实状态的估计结果就会越接近该传感器对应的估计结果。在实际应用中，把多种传感器的估计结果做加权平均，以得到真实状态的估计结果，而加权平均所采用的权重基于误差置信度确定。通过上述方式，可以得到相对可靠的真实状态估计结果。
37.综上，本公开实施例提供的上述方式无需高品质imu传感器，仅基于低成本的陀螺仪和轮速计进行多传感器耦合即可实现，通过结合低成本的陀螺仪与轮速计中精确度通常较高的测量数据(也即轮速计测量得到的轮速以及陀螺仪测量得到的角速度)、以及其他用于观测车辆位姿的传感器便可进行位姿估计，普适性较强，便于广泛应用。
38.为便于进一步理解前述位姿估计方法，本公开实施例以预设的轮速计模型是单车模型为例，给出了上述位姿估计方法的具体实施示例，可以参照如下核心步骤实现：
39.步骤一：获取车辆在t时刻和t+1时刻之间的目标观测数据(轮速计测量得到的轮速以及陀螺仪测量得到的角速度)，并根据单车模型和车辆运动学模型，获取目标观测数据表征的车辆名义状态的时间导数。
40.在单车模型中，自动驾驶车辆的两个前轮被简化建模为单车模型的前轮，两个后轮被简化为单车模型的后轮。单车模型的前轮通过转向控制运动方向，前轮或者后轮为车辆提供驱动力。在此以前轮驱动为例进行说明。在该模型中，轮速计观测数据涉及2个参数，参数1为时间戳，表示数据采集的时间t；参数2为驱动轮的轮速u，在此为前轮速，轮速为正表示车辆向前行驶，为负表示车辆向后倒退；为方便描述，可以假设轮速单位为米/秒，同时
假设车辆的前后轴距离为，，另外假设陀螺仪测量得到的角速度为其中i表示imu坐标系。假设车辆坐标系到imu坐标系的外参是已知的，并且外参的旋转部分为r
vi
，v表示车辆坐标系，在此假设车辆坐标系以车后轴中心为原点，车头朝向为y轴正方向，沿着车后轴向右为x轴正方向。基于此，就可以直接通过坐标系转换得到车辆的角速度：
[0041][0042]
在本公开实施例提供的位姿估计方法中，所需估计的车辆状态包括车辆的位置向量、姿态以及陀螺仪偏置，基于单车模型和车辆运动学模型，获取目标观测数据表征的车辆名义状态的时间导数可以表示为：
[0043][0044][0045]
其中，表示名义位置向量；表示名义车辆速度向量；vy，表示t时刻车辆坐标系y轴方向的名义车辆速度；为t时刻车辆坐标系y轴方向的单位向量；为t时刻车辆坐标系z轴方向的单位向量；r{q}表示四元数q的旋转矩阵；四元数q表征车辆名义姿态；u表示车辆驱动轮的轮速；φ表示车轮偏转角；um表示轮速测量值；r
vi
表示imu坐标系变换至车辆坐标系的旋转矩阵；表示陀螺仪测量得到的名义角速度向量；表示陀螺仪偏置向量；表示车辆坐标系下的名义角速度向量；表示imu坐标系下的名义角速度向量；表示陀螺仪偏置的高斯白噪声；l表示车辆前轮轴与后轮轴的轴距。
[0046]
应当说明的是，ucosφ为通过前轮速计算车体的后轮速，如果车为后轮驱动，则不需要该步。另外，应当注意的是，在本公开实施例中的车体的偏转角不是通过轮速计读取到的，而是通过陀螺仪得到车的角速度并乘以车的轴距得到。
[0047]
步骤二：基于目标观测数据进行数值积分(也即，预积分)，基于积分结果获取车辆在t时刻和t+1时刻之间的名义状态增量。
[0048]
预积分的最终目的是为了将目标观测数据与其它指定传感器的观测数据进行融合，从而进行真实位姿估计。应当注意的是，本公开实施例所提及的t和t+1时刻是指采集两帧连续的其它传感器数据的时间，比如视觉传感器采集两帧图像的两个相邻时刻。一般两帧图像之间会有多帧轮速计及陀螺仪对应的目标观测数据，也即轮速计和陀螺仪在t时刻和t+1时刻之间的n个时刻分别采集有目标观测数据，t为n个时刻中除最后一个时刻的其它任意时刻。预计分指的是基于目标观测数据进行积分，具体而言，将这些目标观测数据表征的车辆名义状态的时间导数进行积分，得到t时刻到t+1时刻基于轮速计轮速和陀螺仪角速度所得的车辆位姿增量(也即车辆位姿变化量)和陀螺仪偏置增量(也即陀螺仪偏置变化量)，也就是通过积分可获知车辆发生了多少位移、多少旋转以及多少陀螺仪偏置。在实际应用中，也可采用预积分计算与位姿变化相关的量，但在本公开实施例中主要是计算位置、姿态和陀螺仪偏置的变化量。
[0049]
在一些具体的实施方式中，可以对目标观测数据表征的车辆名义状态的时间导数进行数值积分，然后基于积分结果获取车辆在t时刻和t+1时刻之间的名义状态增量；其中，
名义状态增量包括基于目标观测数据得到的车辆位姿增量和陀螺仪偏置增量。可以理解的是，车辆状态中的位置和姿态一开始是未知的，在得到车辆名义状态的时间导数后，可以通过t时刻车辆的状态和观测递推得到t+1时刻车辆的状态，该过程需要对车辆(车体)的速度和角速度进行数值积分，可以选择欧拉积分、中值积分或者高阶龙格库塔积分，本公开实施例对数值积分的形式不进行限制。本公开实施例以中值积分为例进行说明，
[0050]
通过对基于轮速计测量所得的速度和基于陀螺仪测量所得的角速度进行中值积分，得到的积分结果表示为：
[0051][0052][0053][0054]
其中，在所述t时刻和所述t+1时刻之间的n个时刻分别对应有目标观测数据，t为n个时刻中除最后一个时刻的其它任意时刻；δt是第t个时刻和第t+1时刻的间隔；为t时刻车辆坐标系y轴方向的单位向量；为t时刻车辆坐标系z轴方向的单位向量；表示四元数的旋转矩阵；r
vi
表示imu坐标系变换至车辆坐标系的旋转矩阵；e
mid
表示t时刻和t+1时刻的中间时刻的状态量e，换言之，e
t
表示t时刻的状态量e，e
t+1
表示t+1时刻的状态量e；状态量e为um、q
θ
、q、θ或其中，um表示轮速测量值；表示陀螺仪测量得到的名义角速度；表示陀螺仪偏置；q表示四元数，θ表示偏航角，四元数q和偏航角θ均用于表征车辆名义姿态；q
θ
表示偏航角θ对应的旋转矩阵对应的四元数；表示名义位置向量；l表示车辆前轮轴与后轮轴的轴距。
[0055]
在上述公式中，上角标加mid表示对应的状态量在t和t+1之间的中间时刻的值，上角标加t表示对应的状态量在t时刻的值，加t+1表示对应的状态量在t+1时刻的值。应当说明的是，矩阵的上角标加t表示数学运算中对矩阵的转置操作，诸如，表示的转置矩阵。
[0056]
另外，上述公式中的表示四元数中的角度是真实的旋转角度的一半。
[0057]
在本公开实施例提供的上述公式中，陀螺仪偏置使用了t时刻的值，这是因为一般可以认为陀螺仪偏置在短时间内不会发生太大的变化。在计算车后轮速的过程中，可以首先令陀螺仪的测量值减去当时的偏置，并通过外参矩阵转换到车体坐标系，然后只取第3维的角速度，也即yaw角(偏航角θ)的角速度。
[0058]
应当注意的是，上述驱动轮均以前轮为例。分别为车辆坐标系x、y、z轴方向的单位向量，具体来说就是以车轮后轴中心为原点，向右，向前，向上的三个单位向量。以
上公式中所有的向量、四元数、四元数表示的旋转矩阵都是以车辆在t时刻的位姿为参考系，也就是t时刻路面切平面所在的坐标系，后续公式也如此，之后不再赘述。
[0059]
在对所述目标观测数据表征的车辆名义状态的时间导数进行数值积分，得到的上述积分结果即为车辆名义状态的转移方程，基于车辆名义状态的转移方程进行递推，即可获得车辆在t时刻和t+1时刻之间的名义状态增量，该名义状态增量包括基于目标观测数据得到的车辆位姿增量和陀螺仪偏置增量。
[0060]
步骤三：根据误差传播算法、轮速计模型、车辆运动学模型以及车辆名义状态的时间导数，得到t时刻和t+1时刻之间真实状态增量与名义状态增量的误差的协方差。该协方差可用于表征目标误差置信度。
[0061]
在误差传播算法中涉及名义状态、真实状态和误差状态，各状态之间的关系可参见表1所示：
[0062][0063]
表1
[0064]
其中，pos表示位置，speed表示车辆驱动轮的轮速，angularvelocity表示角速度，gyroscopebias表示陀螺仪偏置；vel表示车辆速度，quat表示四元数，rot表示旋转矩阵，anglevector表示角矢量，truestate表示真实状态，nominalstate表示名义状态，error state表示误差状态，composition表示结合方式，具体为真实状态、名义状态及误差状态之间的关系，measure表示测量值，noise表示噪声值。
[0065]
在具体实施时，可以首先基于轮速计模型和车辆运动学模型获取车辆真实状态的时间导数；然后根据误差传播算法，采用车辆真实状态的时间导数结合车辆名义状态的时间导数，确定t时刻和t+1时刻之间真实状态增量与名义状态增量的误差的时间导数；之后对误差的时间导数进行数值积分，获得误差的转移方程；最后基于误差的转移方程递推得到t时刻和t+1时刻之间真实状态增量与名义状态增量之间的误差的协方差，并采用误差的协方差表征t时刻和t+1时刻之间对应的目标误差置信度。也即，目标误差置信度通过t时刻和t+1时刻之间真实位姿增量与名义位姿增量之间的误差的协方差表征。
[0066]
由上可知，误差的协方差的转移方程基于误差的转移方程确定，且误差的转移方程基于误差的时间导数确定。因此，首先需要计算误差的时间导数。其中，误差状态的时间导数等于真实状态的时间导数减去名义状态的时间导数。具体的：
[0067]
名义状态的时间导数为：
[0068]
[0069][0070][0071][0072]
真实状态的时间导数为：
[0073][0074][0075][0076]
误差状态的时间导数为：
[0077][0078][0079]
这里令：
[0080][0081][0082][0083][0084]
应当说明的是，上述a和b都是标量，表达的物理含义都是通过前轮速和陀螺仪得到的车辆后轮速，只是b中没有包含噪声和误差状态等小量。为了简化公式，以下进一步约定c为a的平方。
[0085]
将a在(un，δωb，ωn)均为0的位置按照泰勒公式展开则上述公式简化为：
[0086][0087]
由于a0为a在(un，δωb，ωn)均为0的值，所以有：a0＝b
[0088]
带入到上式展开可得：
[0089][0090]
其中，为标量，为标量，为误差或噪声等小量的乘积可以近似为0。
[0091]
但是这里需要考虑到车辆不会在车辆坐标系的z轴方向产生运动，因此在相邻的两帧图像或者雷达观测之间(即为在t时刻和t+1时刻之间)车辆实际上是在平面内运动，因此产生的位移也只在x-o-y平面内。因此，区别于传统的陀螺仪和轮速计的融合算法，本公开实施例只对平面内车辆的运动进行建模，更加符合车辆在路面运动的实际情况。
[0092]
因此，本公开实施例进一步改进，令误差的位移参数基于六自由度模型中的t时刻车辆坐标系对应的x轴方向位移和y轴方向位移确定。也即，从车辆在xyz这3自由度的运动中只提取2自由度的平面运动，由此可得到位移状态量的名义状态和真实状态：
[0093][0094][0095]
上述公式中，r
θ
表示只包含yaw角(也即偏航角θ)旋转分量的3x3的旋转矩阵，表示在两帧观测之间(也即t时刻和t+1时刻之间)车辆做平面运动时发生的偏航旋转，由于r
θ
只表示平面内的旋转，因此其误差状态只与3自由度的旋转误差状态中的yaw角分量相关，因此r
θ
名义状态、真实状态和误差状态之间的关系为：
[0096][0097]
通过上面平面内位移的名义状态和真实状态的定义可以得到误差状态：
[0098][0099]
其中，[]
x
的运算符表示向量对应的叉乘矩阵。上述等式加号后面的部分可以继续展开得到：
[0100]
[0101]
在此应当注意的是r
θ
的左上角矩阵为偏航角对应的矩阵，r
θ
不包含翻滚角或者俯仰角的分量，也即：
[0102][0103]
因此虽然是3维向量，但第3维是0，所以直接将向量的前2维抽取出来，就是所求平面内位移的误差状态：
[0104][0105]
仍然采用上面的约定：
[0106][0107][0108]
则上述平面内位移误差状态表达式中的各偏导数分别为：
[0109][0110][0111][0112]
则各偏导数在t时刻和t+1时刻中间时刻都中值为：
[0113][0114][0115][0116][0117]
其中：
[0118][0119][0120][0121][0122]
另外旋转和角速度偏置的误差状态方程分别为：
[0123][0124][0125]
将误差状态微分方程组写成矩阵形式(也即，误差的时间导数为)：
[0126][0127]
各子矩阵分别为：
[0128][0129][0130][0131][0132][0133][0134]
以上即为误差状态方程，下面使用中值积分求解误差状态的转移方程：首先从开始：
[0135][0136][0137]
然后是陀螺仪偏置：
[0138][0139]
最后是平面内的位移：
[0140][0141][0142][0143][0144][0145]
将基于中值积分得到的状态转移方程写成矩阵形式即可得(以下即为误差的转移方程的表达式)：
[0146][0147]
各子矩阵分别为：
[0148][0149][0150][0151][0152]f12
＝-r
vi
δt
[0153][0154][0155][0156]g24
＝δti3×3[0157]
其中：
[0158][0159][0160][0161][0162][0163][0164][0165][0166][0167][0168][0169][0170][0171]
通过上述误差的转移方程即可从t时刻至t+1时刻进行迭代，并最后得到t+1时刻都误差状态量，也即：
[0172][0173]
上述向量中的三个子向量分别为2自由度的位移误差状态量，3自由度的旋转误差状态量，和3自由度的陀螺仪偏置误差状态量。
[0174]
并可以得到t+1时刻都误差状态的协方差矩阵。
[0175]
具体的，误差的协方差的转移方程的表达式为：
[0176]
p
t+1
＝(fδt+i)p
t
(fδt+i)
t
+gδtq(gδt)
t
[0177]
其中，q为轮速计噪声，定义为如下对角矩阵：
[0178][0179]
以下对上述公式中涉及到的字母释义进行示例性说明：
[0180]
在所述t时刻和所述t+1时刻之间的n个时刻分别对应有目标观测数据，t为n个时刻中除最后一个时刻的其它任意时刻；δt是第t个时刻和第t+1时刻的间隔；为t时刻车辆坐标系x轴方向的单位向量；为t时刻车辆坐标系y轴方向的单位向量；为t时刻车辆坐标系z轴方向的单位向量；r
θ
为只包含偏航角θ旋转分量的旋转矩阵，r
vi
表示imu坐标系变换
至车辆坐标系的旋转矩阵；um表示轮速测量值；un表示轮速噪声；表示陀螺仪测量得到的名义角速度；表示陀螺仪偏置；表示陀螺仪偏置的高斯白噪声；表示陀螺仪角速度噪声；表示车辆绕t时刻车辆坐标系的z轴旋转的角速度；q表示四元数，θ表示偏航角，四元数q和偏航角θ均用于表征车辆名义姿态；q
θ
表示偏航角θ对应的旋转矩阵对应的四元数；表示名义位置向量；l表示车辆前轮轴与后轮轴的轴距；为t时刻车辆坐标系中基于六自由度模型中的x轴位移和y轴位移得到的名义位置向量；为t时刻车辆坐标系中基于六自由度模型中的x轴位移和y轴位移得到的位置误差增量；为基于六自由度模型得到的姿态误差增量，为基于六自由度模型得到的陀螺仪偏置误差增量；e
mid
表示t时刻和t+1时刻的中间时刻的状态量e，e
t
表示t时刻的状态量e，e
t+1
表示t+1时刻的状态量e；状态量e为um、q
θ
、q、θ、r
θ
、或为偏航角θ对应的四元数q
mid
的旋转矩阵；为轮速噪声方差，分别为陀螺仪角速度在t时刻车辆坐标系x轴、y轴、z轴方向上的噪声方差；分别为陀螺仪偏置随机游走在t时刻车辆坐标系x轴、y轴、z轴方向上的噪声方差。
[0181]
步骤四：基于误差的协方差融合轮速计、陀螺仪和其他的各个传感器
[0182]
通过前述步骤一至步骤三，可以得到t时刻到t+1时刻之间通过轮速计测量所得的轮速以及陀螺仪测量所得的角速度递推得到的名义状态增量，以及真实状态增量与所述名义状态增量之间的误差的协方差矩阵(可用于表征上述目标误差置信度)另外，还可根据指定传感器在t时刻的观测数据和t+1时刻的观测数据，获取车辆在t时刻的第一名义位姿和第一传感器误差置信度，以及车辆在t+1时刻的第二名义位姿和第二传感器误差置信度。
[0183]
在本公开实施例中，可以根据目标观测数据对应的名义状态增量和目标误差置信度，以及指定传感器对应的第一名义位姿、第一传感器误差置信度、第二名义位姿和第二传感器误差置信度，对车辆在t时刻和t+1时刻的真实位姿进行估计。在一些具体的实施示例中，可以按照以下公式对车辆在所述t时刻和所述t+1时刻的真实位姿进行估计：
[0184][0185]
其中，为车辆的真实状态，其中为车辆在世界坐标系中的真实位姿，为陀螺仪偏置，是所述车辆在t时刻的真实状态，是所述车辆在t+1时刻的真实状态，δξ是所述目标观测数据对应的t时刻到t+1时刻之间的名义状态增量，p-1
为所述目标误差置信度，是所述车辆在t时刻的真实位姿，是所述车辆在t+1时刻的真实位姿，m为所述指定传感器的数量，m的取值范围为[1，m]，是第m个指定传感器在t时刻对应的第一名义位姿，是第m个指定传感器在t+1时刻对应的第二名义位姿，是第m个指定传感
器在t时刻对应的第一传感器误差置信度，第m个指定传感器在t+1时刻对应的第二传感器误差置信度。
[0186]
在具体实现时，参照如下公式表达
[0187][0188]
其中，表示车辆在t+1时刻在世界坐标系中的位移向量；表示车辆在t时刻在世界坐标系中的位移向量；表示通过预积分得到的车辆从t时刻到t+1时刻在t时刻车辆坐标系中的名义位移增量；表示车辆在t时刻在世界坐标系中的姿态角，采用四元数表示；表示车辆在t+1时刻在世界坐标系中的姿态角，采用四元数表示；四元数对应的旋转矩阵，[]
xy
表示提取车辆在t时刻车辆坐标系xy平面内运动的位移；表示通过预积分得到的车辆从t时刻到t+1时刻的旋转变化量；[]
xyz
表示提取四元数的向量部分；表示t时刻到t+1时刻陀螺仪偏置的变化量与通过预积分得到的名义变化量的差值。其中，上述名义变化量等于0。
[0189]
在上述公式中，角标v表示车辆坐标系，角标w表示世界坐标系，因为本公开实施例假设车辆在t时刻到t+1时刻之间在局部平面内运动，因此通过[]
xy
只把局部平面内的位移提取出来，另外，通过[]
xyz
把四元数的向量部分提取出来。
[0190]
另外，其它传感器(也即，指定传感器)在t时刻真实位姿与名义位姿的差为：
[0191][0192]
其它传感器在t+1时刻真实位姿与名义位姿的差和上述t时刻真实位姿与名义位姿的差的表达形式类似，只是将t改为t+1即可，也可参照相关技术实现，在此不再赘述。
[0193]
上述方程可以通过比如g2o，ceres等非线性最小二乘求解器高效求解，从而得到最优的车辆位姿估计结果。
[0194]
综上所述，本公开实施例提供的上述位姿估计方法，具有如下特点：
[0195]
1)提出来基于低成本的陀螺仪及轮速计中质量最高的测量数据(陀螺仪测量得到的角速度以及轮速计测量得到的轮速)，从而可以最大程度发挥两种传感器各自的优势，达到最优的位姿估计效果。
[0196]
2)提出了基于单车模型的预积分模型，进而基于陀螺仪测量得到的角速度以及轮速计测量得到的轮速得到误差状态的协方差矩阵(可用于表征t时刻和t+1时刻之间车辆的真实状态增量与名义状态增量的误差的置信度)，从而可以更好地通过概率论的方式将陀
螺仪和轮速计与其它传感器的数据进行耦合，也即，能够更好地通过对轮速计和轮速计与其它传感器的紧耦合优化以实现最优位姿估计的具体方式。
[0197]
3)使用帧间局部的平面约束代替全局平面约束，从而使得模型能够适用于存在上下坡或者路面倾斜的场景，具体的，将车辆的3维位移简化为2维的平面位移，无需额外的平面约束。一方面从模型设计上就满足了帧间的局部平面约束假设，又去除了额外使用平面约束的必要性，从而提升了模型的效率。相比之下，相关技术虽然考虑车辆在实际路面上运动是平面运动，因此在6自由度的位姿上额外增加了全局的平面约束：即约束车辆在整个运动过程中，在z方向的平移，以及roll和pitch(翻滚角和俯仰角)为0，但却存在全局平面的强假设，也就是认为车辆运动的整个场景是一个平面或者接近一个平面。但是在实际的场景中不仅存在各种上下坡，而且路面也可能会存在侧向倾斜，全局平面的假设是难以成立的。此外，额外的平面假设意味着需要加入额外的约束，从而增加了计算量，降低了计算的效率。而本公开实施例提供的上述使用帧间局部的平面约束代替全局平面约束的方式，可以有效解决相关技术中全局平面假设带来的应用场景受限和计算效率低下的问题。
[0198]
对应于前述位姿估计方法，图2为本公开实施例提供的一种位姿估计装置的结构示意图，该装置可由软件和/或硬件实现，一般可集成在电子设备中，如图2所示，位姿估计装置包括如下模块：
[0199]
数据获取模块202，用于获取车辆在t时刻和t+1时刻之间的目标观测数据；其中，目标观测数据包括轮速计测量得到的轮速以及陀螺仪测量得到的角速度；t时刻和t+1时刻为指定传感器在进行数据观测时的两个相邻时刻，且指定传感器为车辆上设置的用于观测车辆位姿的传感器；
[0200]
状态获取模块204，用于根据预设的轮速计模型和车辆运动学模型，获取目标观测数据表征的车辆名义状态的时间导数；
[0201]
积分模块206，用于对目标观测数据表征的车辆名义状态的时间导数进行数值积分，基于积分结果获取车辆在t时刻和t+1时刻之间的名义状态增量；名义状态增量包括基于目标观测数据得到的车辆位姿增量和陀螺仪偏置增量；
[0202]
置信度获取模块208，用于根据误差传播算法、轮速计模型、车辆运动学模型以及车辆名义状态的时间导数，得到t时刻和t+1时刻之间对应的目标误差置信度；其中，目标误差置信度是t时刻和t+1时刻之间真实状态增量与名义状态增量的误差的置信度；
[0203]
位姿估计模块210，用于根据指定传感器在t时刻的观测数据和t+1时刻的观测数据、目标观测数据对应的名义状态增量以及目标误差置信度，对车辆在t时刻和t+1时刻的真实位姿进行估计。
[0204]
综上，本公开实施例提供的上述装置无需高品质imu传感器，仅基于低成本的陀螺仪和轮速计进行多传感器耦合即可实现，通过结合低成本的陀螺仪与轮速计中精确度通常较高的测量数据(也即轮速计测量得到的轮速以及陀螺仪测量得到的角速度)、以及其他用于观测车辆位姿的传感器便可进行位姿估计，普适性较强，便于广泛应用。
[0205]
在一些实施方式中，轮速计模型包括单车模型，积分模块206具体用于：对目标观测数据表征的车辆名义状态的时间导数进行中值积分。
[0206]
在一些实施方式中，目标误差置信度通过t时刻和t+1时刻之间真实位姿增量与名义位姿增量之间的误差的协方差表征。
[0207]
在一些实施方式中，误差的位移参数基于六自由度模型中的t时刻车辆坐标系对应的x轴方向位移和y轴方向位移确定。
[0208]
在一些实施方式中，置信度获取模块208具体用于：基于轮速计模型和车辆运动学模型获取车辆真实状态的时间导数；根据误差传播算法，采用车辆真实状态的时间导数结合车辆名义状态的时间导数，确定t时刻和t+1时刻之间真实状态增量与名义状态增量的误差的时间导数；对误差的时间导数进行数值积分，获得误差的转移方程；基于误差的转移方程递推得到t时刻和t+1时刻之间真实状态增量与名义状态增量之间的误差的协方差，并采用误差的协方差表征t时刻和t+1时刻之间对应的目标误差置信度。
[0209]
在一些实施方式中，位姿估计模块210具体用于：根据指定传感器在t时刻的观测数据和t+1时刻的观测数据，获取车辆在t时刻的第一名义位姿和第一传感器误差置信度，以及车辆在t+1时刻的第二名义位姿和第二传感器误差置信度；根据目标观测数据对应的名义状态增量和目标误差置信度，以及指定传感器对应的第一名义位姿、第一传感器误差置信度、第二名义位姿和第二传感器误差置信度，对车辆在t时刻和t+1时刻的真实位姿进行估计。
[0210]
在一些实施方式中，位姿估计模块210具体用于：按照以下公式对车辆在t时刻和t+1时刻的真实位姿进行估计：
[0211][0212]
其中，为车辆的真实状态，其中为车辆在世界坐标系中的真实位姿，为陀螺仪偏置，是所述车辆在t时刻的真实状态，是所述车辆在t+1时刻的真实状态，δξ是所述目标观测数据对应的t时刻到t+1时刻之间的名义状态增量，p-1
为所述目标误差置信度，是所述车辆在t时刻的真实位姿，是所述车辆在t+1时刻的真实位姿，m为所述指定传感器的数量，m的取值范围为[1，m]，是第m个指定传感器在t时刻对应的第一名义位姿，是第m个指定传感器在t+1时刻对应的第二名义位姿，是第m个指定传感器在t时刻对应的第一传感器误差置信度，第m个指定传感器在t+1时刻对应的第二传感器误差置信度。
[0213]
本公开实施例所提供的位姿估计装置可执行本公开任意实施例所提供的位姿估计方法，具备执行方法相应的功能模块和有益效果。位姿估计装置中涉及的公式均可参照位姿估计方法实施例中的内容，在此不再赘述。
[0214]
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置实施例的具体工作过程，可以参考方法实施例中的对应过程，在此不再赘述。
[0215]
本公开实施例还提供了一种电子设备，电子设备包括：处理器；用于存储处理器可执行指令的存储器；处理器，用于从存储器中读取可执行指令，并执行指令以实现上述位姿估计方法。
[0216]
图3为本公开实施例提供的一种电子设备的结构示意图。如图3所示，电子设备300包括一个或多个处理器301和存储器302。
[0217]
处理器301可以是中央处理单元(cpu)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备300中的其他组件以执行期望的功能。
[0218]
存储器302可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(ram)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(rom)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器301可以运行所述程序指令，以实现上文所述的本公开的实施例的位姿估计方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。
[0219]
在一个示例中，电子设备300还可以包括：输入装置303和输出装置304，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。
[0220]
此外，该输入装置303还可以包括例如键盘、鼠标等等。
[0221]
该输出装置304可以向外部输出各种信息，包括确定出的距离信息、方向信息等。该输出装置304可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
[0222]
当然，为了简化，图3中仅示出了该电子设备300中与本公开有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备300还可以包括任何其他适当的组件。
[0223]
除了上述方法和设备以外，本公开的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本公开实施例所提供的位姿估计方法。
[0224]
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本公开实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如java、c++等，还包括常规的过程式程序设计语言，诸如“c”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
[0225]
此外，本公开的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本公开实施例所提供的位姿估计方法。
[0226]
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。
[0227]
本公开实施例还提供了一种计算机程序产品，包括计算机程序/指令，该计算机程
序/指令被处理器执行时实现本公开实施例中的位姿估计方法。
[0228]
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0229]
以上所述仅是本公开的具体实施方式，使本领域技术人员能够理解或实现本公开。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本公开的精神或范围的情况下，在其它实施例中实现。因此，本公开将不会被限制于本文所述的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。