CN114088113B - 一种里程计轨迹对齐及精度测评方法 - Google Patents

Abstract

本发明公开了一种里程计轨迹对齐及精度测评方法，使用里程计测量轨迹与真实轨迹计算得到里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系，从而对齐真实轨迹及测量轨迹，再计算出测量值的真实偏差。通过本发明的技术方案，可以计算出里程计测量系统坐标系与真值测量系统坐标系之间的变换关系，可对齐里程计测量轨迹与真实轨迹，且保证轨迹起始点重合，能够更合理地对里程计精度进行评测；本发明的方法主要用于测评定位系统的精度及可靠性，可以被广泛应用于无人机、无人驾驶、机器人设计等领域。

Description

一种里程计轨迹对齐及精度测评方法

技术领域

本发明属于智能机器人技术领域，尤其涉及一种里程计轨迹对齐及精度测评方法。

背景技术

随着人工智能高速发展与应用，智能机器人，无人驾驶等产业迎来巨大的需求及市场，同时也迎来技术上的挑战。其中，自主导航规划技术，成为智能化机器人产业迫切且广泛的需要。机器人自主导航规划是指机器人能够独立自主完成指定运动任务的能力。机器人自主导航规划的前提是具备自主定位的能力，即具备里程计的功能。

由于机器人等智能设备的轨迹真实值测量均是用自身以外的设备获取，如使用GPS、RTK技术，以及使用运动捕捉系统Optitrack等，而机器人的自身里程计的实现，多基于自身携带的传感器，如轮式里程计、视觉传感器及激光传感器等等。因此，里程计测量系统坐标系与真值测量系统坐标系通常并不一致，不能直接通过简单的求差来评测里程计的精度。里程计的精度评测也成为一种技术手段。

要测评里程计的精度，首先要对齐真实轨迹与里程计输出轨迹，即首先对齐真实轨迹所在的坐标系以及里程计所在的坐标系，通常的方法有：

手动标定获得：在里程计初始化之前，使用手动测量的方法，计算得到真值与自身传感器之间的坐标变换关系，但是该方法在每一次里程计初始化时均需要操作一次，此外，自身传感器的坐标系一般很难测量准确，因此这种方法的采用度并不高。

通过轨迹计算获得：通过得到的真值轨迹与里程计测量轨迹，使用算法处理计算得到里程计测量系统坐标系与真值测量系统坐标系变换关系，从而对齐轨迹。此类方法有良好的灵活性，是目前轨迹测评普遍采用的方法。Umeyama算法是目前主流的轨迹对齐算法，相对迭代下降求解算法，该算法计算量小且精度较高。但是，此类方法由于刻意对齐了真值轨迹与里程计测量轨迹，影响里程计的精度评测结果。

发明内容

为了解决里程计轨迹对齐及精度测评问题，本发明提出一种里程计轨迹对齐及精度测评方法，对齐轨迹的同时，对齐真值轨迹与测量轨迹的起始点，更真实并且直观地反映里程计测量结果的真实情况。本发明的具体技术方案如下：

一种里程计轨迹对齐及精度测评方法，设真实轨迹的位移序列为p^g，里程计输出轨迹对应的位移序列为p^e，里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系为(R,t)，通过p^g和p^e计算得(R,t)，并根据p^g，p^e和(R,t)计算该里程计的绝对误差；所述测评方法包括以下步骤：

S1：将p^g与p^e通过时间戳及位姿插值法处理，得到时间戳对齐的两组轨迹匹配序列

和

设N_p为匹配的位移序列数，即

和

包含N_p对时间戳匹配的位移序列，建立求解问题函数：

其中，i为位移序列索引值，取值从1至N_p，

为

中第i组位移序列，

为

中第i组位移序列，

与

为一组时间戳一致的位移序列对，最终求解结果记为(R^*,t^*)；

S2：里程计轨迹序列

减去

结果记为

里程计轨迹序列

减去

结果记为

则：

其中，

为

中第i组位移序列，

为

中第i组位移序列；

S3：利用SVD分解的方法，求得R^*；

S4：计算里程计测量轨迹与真值轨迹的初始化运动偏移量，得到t^*：

S5：真实轨迹序列

累积上(R^*,t^*)，结果记为p^*：

S6：计算里程计测量值与真值之间的绝对误差平均值，记为e；

其中，j为表示坐标系三轴索引值，

表示p^*第i组位移序列j轴上的位移值，

则表示

第i组位移序列j轴上的位移值；

得到的绝对误差平均值即能反映出里程计轨迹对齐及精度。

进一步地，所述步骤S3的具体过程为：

令：

将H进行奇异分解即SVD分解，设U∑V^T为H的奇异分解结果，即：

H＝U∑V^T

其中，UU^T＝VV^T＝I，∑＝diag(λ_i)，

解算得：

R^*＝USV^T

其中：

本发明的有益效果在于：

1.对比当前的轨迹对齐算法，本发明的方法可以计算出里程计测量系统坐标系与真值测量系统坐标系之间的变换关系，可对齐里程计测量轨迹与真实轨迹，且保证轨迹起始点重合，能够更合理地对里程计精度进行评测。

2.本发明的方法主要用于测评定位系统的精度及可靠性，可以被广泛应用于无人机、无人驾驶、机器人设计等领域。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：

图1是Umeyama算法的概要流程图；

图2是本发明方法的概要流程图；

图3是轨迹对齐结果对比图，其中，(a)为Umeyama对齐轨迹1(b)为本发明的方法对齐轨迹1，(c)为Umeyama对齐轨迹2，(d)为本发明的方法对齐轨迹2。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

主流的轨迹对齐方法基于Umeyama算法，以最大程度对齐轨迹为目标，计算出里程计测量系统坐标系与真值测量系统坐标系变换关系。

设真实轨迹的位移序列为p^g，里程计输出轨迹对应的位移序列为p^e，假设两组轨迹位移序列的时间戳已对齐。通过p^g和p^e计算出里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系(R,t)，Umeyama算法的解过程如图1所示，约束方程如公式(1)：

其中，p^g和p^e均包含三轴运动偏移量，N_p为位移序列数，即轨迹包含N_p个位移序列，i为位移序列索引值，取值从1至N_p，

为p^e中第i组位移序列，

为p^g中的第i组位移序列，最终求解结果记为(R^*,t^*)；

由求解约束方程(1)得到(R^*,t^*)，计算得到对齐真值坐标系的里程计估计轨迹，对齐后的里程计轨迹序列为p^*：

p^*＝R^*p^g+t^*                     (2)

里程计的绝对误差平均值e为：

其中，j表示坐标系三轴索引值，

表示p^*第i组位移序列j轴上的位移值，

则表示p^e第i组位移序列j轴上的位移值。

基于公式可知，以最小化方程(1)为目标，则会很大程度影响误差公式(3)的计算结果。

因此本发明提出一种里程计轨迹对齐及精度测评方法，加入起点约束，如公式(4)所示，并在求解思路上进行调整，求解流程如图2所示。本发明的算法经过验证进一步提高里程计精度测评的可信度，并且通过轨迹绘制能够更加直观的反映出里程计性能优劣情况。

本发明的方法使用里程计测量轨迹与真实轨迹计算得到里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系，从而对齐真实轨迹及测量轨迹，再而计算出测量值的真实偏差。

具体地，一种里程计轨迹对齐及精度测评方法，设真实轨迹的位移序列为p^g，里程计输出轨迹对应的位移序列为p^e，里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系为(R,t)，通过p^g和p^e计算得(R,t)，并根据p^g，p^e和(R,t)计算该里程计的绝对误差；具体测评方法包括以下步骤：

S1：将p^g与p^e通过时间戳及位姿插值法处理，得到时间戳对齐的两组轨迹匹配序列，分别记为

和

设N_p为匹配的位移序列数，即

和

包含N_p对时间戳匹配的位移序列，建立求解问题函数：

其中，i为位移序列索引值，取值从1至N_p，

为

中第i组位移序列，

为

中第i组位移序列，

与

为一组时间戳一致的位移序列对，最终求解结果记为(R^*,t^*)；

S2：里程计轨迹序列

减去

结果记为

里程计轨迹序列

减去

结果记为

则：

其中，

为

中第i组位移序列，

为

中第i组位移序列；

S3：利用SVD分解的方法，求得R^*；

令：

将H进行奇异分解即SVD分解，设U∑V^T为H的奇异分解结果，即：

H＝U∑V^T

其中，

解算得：

R^*＝USV^T

其中：

S4：计算里程计测量轨迹与真值轨迹的初始化运动偏移量，得到t^*：

S5：真实轨迹序列

累积上(R^*,t^*)，结果记为p^*：

S6：计算里程计测量值与真值之间的绝对误差平均值，记为e：

其中，j为表示坐标系三轴索引值，

表示p^*第i组位移序列j轴上的位移值，

则表示

第i组位移序列j轴上的位移值；

得到的绝对误差平均值即能反映出里程计轨迹对齐及精度。

为了方便理解本发明的上述技术方案，以下通过具体实施例对本发明的上述技术方案进行详细说明。

实施例1

轨迹真实值(Groundtruth)由运动捕捉系统(Optitrack)获得，轨迹如图3(a)和图3(b)中的实线所示。

估计轨迹(Estimate)由视觉惯性里程计获得，轨迹如图3(a)和图3(b)中的虚线所示。

分别使用Umeyama及本发明的方法对估计轨迹与真实轨迹进行对齐，并测算该里程计的绝对误差。

图3(a)为使用Umeyama算法对齐的估计轨迹与真值的对比图，绝对误差平均值约为0.797m，图3(b)为使用本发明的方法对齐的估计轨迹与真值的对比图，绝对误差平均值约为0.883m。

从图3(a)和图3(b)的对比，可以看出本发明的方法能够对齐真值与估计值的起始点(图中▲)，图3(b)也更能清晰反应里程计在运行过程中产生的累积误差情况。同时，从测算的绝对误差值可以看出，单纯以公式(1)作为优化约束，里程计的评估会一定程度上被高估。

实施例2

轨迹真实值(Groundtruth)以高精度视觉惯性里程计获得，里程计估计值分别采用其他两种不同的视觉里程计算法获得，轨迹测算结果分为为Trajectory1与Trajectory2，分别对应图3(c)和图3(d)中的虚线(..)及点画线(-.)。

分别使用Umeyama及本发明的方法分别对两组轨迹进行对齐，并测算该里程计的绝对误差平均值。结果如下表所示：

表1绝对误差平均值表(单位：m)

当进行多个轨迹进行同时测评时，使用Umeyama对齐的方法对齐轨迹，如图3(c)所示，很难直观看出两个里程计的累积误差及性能优劣情况。使用本发明的方法对齐轨迹，约束轨迹的起始点(图中▲)，如图3(d)所示，Trajectory1与Trajectory2均出现不同程度的累积漂移。由于Umeyama的对齐最小化原理，使得Trajectory2过分对齐真实轨迹，导致最后求得的绝对误差平均值(0.89m)可信度不高，里程计2的性能被高估。

综合以上实施例，可以看出，主流的Umeyama以最大程度对齐轨迹为优化条件，若以此得到的对齐轨迹计算里程计的精度，会一定程度上影响对里程计的性能评估结果。本发明的方法对齐轨迹起始点，以计算出里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系更接近真实值为约束，而非以最大程度减少估计轨迹与真实轨迹的差值作为目标。因此，本发明的方法能够保证对齐后轨迹的真实性，此外，如果对多个里程计的结果轨迹进行同时对比，使用本发明的方法也会更加直观且合理地反映出各个里程计的性能。

在本发明中，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，不能理解为指示或暗示相对重要性。术语“多个”指两个或两个以上，除非另有明确的限定。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种里程计轨迹对齐及精度测评方法，其特征在于，设真实轨迹的位移序列为p^g，里程计输出轨迹对应的位移序列为p^e，里程计测量系统坐标系与真值测量系统坐标系之间的坐标系变换关系为(R,t)，通过p^g和p^e计算得(R,t)，并根据p^g，p^e和(R,t)计算该里程计的绝对误差；所述测评方法包括以下步骤：

S1：将p^g与p^e通过时间戳及位姿插值法处理，得到时间戳对齐的两组轨迹匹配序列

和

设N_p为匹配的位移序列数，即

和

包含N_p对时间戳匹配的位移序列，建立求解问题函数：

其中，i为位移序列索引值，取值从1至N_p，

为

中第i组位移序列，

为

中第i组位移序列，

与

为一组时间戳一致的位移序列对，最终求解结果记为(R^*,t^*)；

S2：里程计轨迹序列

减去

结果记为

里程计轨迹序列

减去

结果记为

则：

其中，

为

中第i组位移序列，

为

中第i组位移序列；

S3：利用SVD分解的方法，求得R^*；

S4：计算里程计测量轨迹与真值轨迹的初始化运动偏移量，得到t^*：

S5：真实轨迹序列

累积上(R^*,t^*)，结果记为p^*：

S6：计算里程计测量值与真值之间的绝对误差平均值，记为e：

其中，j为表示坐标系三轴索引值，

表示p^*第i组位移序列j轴上的位移值，

则表示

第i组位移序列j轴上的位移值；

得到的绝对误差平均值即能反映出里程计轨迹对齐及精度。

2.根据权利要求1所述的一种里程计轨迹对齐及精度测评方法，其特征在于，所述步骤S3的具体过程为：

令：

将H进行奇异分解即SVD分解，设U∑V^T为H的奇异分解结果，即：

H＝U∑V^T

其中，UU^T＝VV^T＝I，∑＝diag(λ_i)，

解算得：

R^*＝USV^T

其中：

Images