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

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