CN115096288B - 移动机器人导航轨迹解算精度和能力测试方法 - Google Patents

Abstract

移动机器人导航轨迹解算精度和能力测试方法,解决了现有不便使用靶标轨迹评测机器人自身轨迹解算精度的问题，属于导航轨迹精度测试技术领域。本发明使用高精度激光扫描测距仪器，扫描环境获得场景完整点云和图像，采用虚拟轨迹和虚拟传感器类型与视角(与被测机器人使用的传感器类型和参数、视角相同)，对该场景完整点云和图像进行采样，生成虚拟轨迹和虚拟传感器类型与视角对应的传感器数据帧序列，被测机器人使用该传感器数据帧序列(等同于该机器人在该实际环境中进行运动和数据采集)，解算生成机器人运动轨迹，生成的机器人运动轨迹与虚拟轨迹(作为真值)进行比较，测试被测机器人轨迹解算精度和能力。

Description

移动机器人导航轨迹解算精度和能力测试方法

技术领域

本发明涉及一种移动机器人导航轨迹解算精度和能力测试方法，属于导航轨迹精度测试技术领域。

背景技术

移动机器人可依靠自身传感器，进行导航，并可解算自身的运动轨迹。提高机器人导航轨迹精度是机器人领域的一个重要研究方向，导航轨迹精度测试是其中关键环节。

通过在机器人上放置靶标，使用激光追踪仪获取机器人运行轨迹是目前测试机器人导航轨迹精度最高的方法之一。但是该方法成本高，操作复杂，并且激光跟踪仪给出的是靶标的运行轨迹，存在机器人旋转运行时，靶标平移变化小，即轨迹对旋转运动轨迹误差大等问题；存在移动机器人依靠自身传感器解算的轨迹与靶标的轨迹参照系不统一，不便于使用靶标轨迹评测机器人自身轨迹解算精度问题。

发明内容

针对现有不便使用靶标轨迹评测机器人自身轨迹解算精度的问题，本发明提供一种移动机器人导航轨迹解算精度和能力测试方法。

本发明的一种移动机器人导航轨迹解算精度和能力测试方法，所述方法包括:

S1、确定场景，场景内分布有多个站点，每个站点使用单线三维激光扫描仪对场景进行扫描，得到场景的三维点云，同时采集以激光光轴为中心的彩色图像，对采集的三维点云进行颜色映射，得到三维彩色点云和图像；

单线三维激光扫描仪的测距精度优于被测移动机器人传感器精度；

根据在不同站点得到的三维彩色点云之间重叠区域，求取不同站点云旋转平移矩阵，将多站点三维彩色点云融合到一起，最终得到场景完整点云和图像；

S2、根据被测机器人，设定虚拟轨迹、虚拟传感器的类型、内参数和视角，采用虚拟轨迹、虚拟传感器的类型、内参数和视角对场景完整点云和图像进行采样，生成与虚拟轨迹、虚拟传感器类型和视角对应的传感器数据帧序列；所述虚拟传感器类型为被测移动机器人的传感器类型和参数相同；

S3、被测移动机器人对S2生成的传感器数据帧序列进行解算生成机器人运动轨迹，将该机器人运动轨迹与S2的虚拟轨迹对比，评价导航轨迹精度和能力。

作为优选，所述S2包括：

设定虚拟传感器类型、内参数、视角、取景框IoU阈值；

在空间中生成取景框，取景框形状、视场角、深度根据设定的传感器内参数决定；在空间中生成取景框后，计算虚拟轨迹中相邻轨迹帧之间取景框的IoU,如果相邻轨迹帧对应的取景框的IoU低于取景框IoU阈值，则根据需要在这两帧之间进行线性插值，插入补充轨迹帧，直到所有相邻轨迹帧所对应的取景框之间的IoU都大于设定的取景框IoU阈值；

逐帧读取插帧后的轨迹，根据每一帧轨迹的位置、视角，结合虚拟传感器的类型、内参数，计算虚拟传感器到场景完整点云中各点的距离值或光度值作为像素值的图像后输出，或直接按虚拟传感器的空间分辨率捕获虚拟传感器处采集范围内的点云后输出，循环这个过程，直到所有轨迹帧对应的输出都被计算完毕，最终得到与虚拟轨迹对齐的传感器数据帧序列。

作为优选，S3中，采用相对位姿误差RPE和绝对轨迹误差ATE的统计值评价机器人导航轨迹精度和能力。

本发明的有益效果：本发明使用高精度激光扫描测距仪器，扫描环境获得场景完整点云和图像，采用虚拟轨迹和虚拟传感器类型与视角(与被测机器人使用的传感器类型和参数、视角相同)，对该场景完整点云和图像进行采样，生成虚拟轨迹和虚拟传感器类型与视角对应的传感器数据帧序列(是场景完整点云和图像的数据序列的子集合)，被测机器人使用该传感器数据帧序列(等同于该机器人在该实际环境中进行运动和数据采集)，解算生成机器人运动轨迹，生成的机器人运动轨迹与虚拟轨迹(作为真值)进行比较，测试被测机器人轨迹解算精度和能力。使用场景完整点云和图像数据，生成虚拟轨迹，再利用虚拟轨迹生成传感器的虚拟数据集，从原理上减少了误差；同时可以根据任意的虚拟轨迹生成被测机器人的传感器数据帧序列，极大地丰富了用于导航仿真的数据集的数据量，相比于其他方法，本发明可便捷、全面地测试评价机器人导航轨迹解算精度和能力。

附图说明

图1为本发明的流程示意图；

图2为多站点云融合的场景完整点云；

图3为虚拟轨迹设定；

图4为验证相邻帧之间取景框的IoU并插入轨迹帧，其中图4(a)为相邻帧对应的取景框的IoU低于取景框IoU阈值的情况，图4(b)为插入补充轨迹帧的情况。

具体实施方式

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

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

本实施方式的移动机器人导航轨迹解算精度和能力测试方法，包括：

步骤1、选定合适的场景，场景内分布有多个站点，每个站点使用单线高精度三维激光扫描仪对场景进行扫描，得到场景的三维点云，同时采集以激光光轴为中心的彩色图像，对采集的三维点云进行颜色映射，得到三维彩色点云和图像；单线高精度三维激光扫描仪的测距精度优于被测移动机器人传感器精度；根据在不同站点得到的三维彩色点云之间重叠区域，求取不同站点云旋转平移矩阵，将多站点维彩色点云融合到一起，最终得到场景完整点云和图像，如图2所示；

步骤2、设定虚拟轨迹、虚拟传感器的类型、内参数和视角，

采用虚拟轨迹和虚拟传感器类型与视角(与被测机器人使用的传感器类型和参数、视角相同)，对该场景完整点云和图像进行采样，生成虚拟轨迹和虚拟传感器类型与视角对应的传感器数据帧序列(是场景完整点云和图像的数据序列的子集合)；

步骤3、被测机器人使用步骤2生成的传感器数据帧序列(等同于该机器人在该实际环境中进行运动和数据采集)，解算生成机器人运动轨迹，生成的机器人运动轨迹与虚拟轨迹(作为真值)进行比较，测试被测机器人轨迹解算精度和能力。

本实施方式中使用场景完整点云和图像数据，生成虚拟轨迹，再利用虚拟轨迹生成传感器的虚拟数据集，从原理上减少了误差；同时可以根据任意的虚拟轨迹生成被测机器人的传感器数据帧序列，极大地丰富了用于导航仿真的数据集的数据量，相比于其他方法，本发明可便捷、全面地测试评价机器人导航轨迹解算精度和能力。优选实施例中，本实施方式的步骤2包括：

根据需要对被测移动机器人的运动路径，姿态，可自行设置行进路线，行进时视角，路径长短，路径是否闭环，生成虚拟轨迹。

根据被测机器人，设定虚拟传感器类型、内参数、视角、取景框IoU阈值；在实际生成的传感器数据帧序列前，需要验证虚拟轨迹。首先在空间中生成取景框，取景框形状、视场角、深度根据设定的传感器内参数决定；在空间中生成取景框后，计算虚拟轨迹中相邻轨迹帧之间取景框的IoU,如果相邻轨迹帧对应的取景框的IoU低于取景框IoU阈值，如图4(a)第四位姿帧与第五位姿帧，则根据需要在这两帧之间进行线性插值，插入补充轨迹帧，如图4(b)所示，直到所有相邻轨迹帧所对应的取景框之间的IoU都大于设定的阈值；新轨迹记为：Q₁,…,Q_n∈SE(3)。

接下来使用轨迹生成传感器数据帧序列。逐帧读取插帧后的轨迹，根据每一帧轨迹的位置、视角，结合虚拟传感器的类型、内参数，计算虚拟传感器到场景完整点云中各点的距离值或光度值作为像素值的图像后输出，或直接按虚拟传感器的空间分辨率捕获虚拟传感器处采集范围内的点云后输出，循环这个过程，直到所有轨迹帧对应的输出都被计算完毕，最终得到与虚拟轨迹对齐的传感器数据帧序列。

本实施方式的步骤3中，被测机器人根据提供的传感器类型、内参数配置完成，使用传感器数据帧序列进行测试。读取传感器数据帧序列作为导航的输入，运行导航任务，计算出导航过程中机器人的轨迹，记为P₁,…,P_n∈SE(3)。

本实施方式采用RPE(relative pose error,相对位姿误差)和ATE(absolutetrajectory error,绝对轨迹误差)的统计值评价机器人导航轨迹精度：

采用相对位姿误差RPE和绝对轨迹误差ATE的统计值评价机器人导航轨迹精度。

相对位姿误差RPE主要描述的是相隔固定帧数Δ的两帧位姿差的精度(相比真实位姿)，相当于直接测量里程计的误差。虚拟轨迹与机器人运动轨迹的第i帧的相对位姿误差RPE为：

n表示机器人运动轨迹的帧数，Δ表示间隔帧数，已知总数n与间隔Δ的情况下，m＝n-Δ，Q_i+Δ表示虚拟轨迹中第i+Δ帧，Q_i表示虚拟轨迹中第i帧，P_i表示机器人运动轨迹中第i帧，P_i+Δ表示机器人运动轨迹中第i+Δ帧；得到m＝n-Δ个RPE,然后用均方根误差RMSE统计这个误差，得到一个总体值:

RMSE(E_1:n,Δ)表示第1到n帧E_i的均方根误差；

根据RMSE(E_1:n,Δ)评价移动机器人导航轨迹精度。

绝对轨迹误差ATE是估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性，虚拟轨迹与机器人运动轨迹的第i帧的绝对轨迹误差为：

F_i表示使用最小二乘计算出一个虚拟轨迹到机器人运动轨迹的位姿变换矩阵S∈SE(3)，SE(3)表示特殊欧式群，用来表示参数空间；

同样适用RMSE反应统计属性：

RMSE(F_1:n,Δ)表示第1到n帧F_i的均方根误差。

机器人解算的轨迹对于虚拟轨迹的RPE和ATE的RMSE越小，则本评定其导航轨迹精度越高、能力越强。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (5)

1.移动机器人导航轨迹解算精度和能力测试方法，其特征在于，所述方法包括：

S1、确定场景，场景内分布有多个站点，每个站点使用单线三维激光扫描仪对场景进行扫描，得到场景的三维点云，同时采集以激光光轴为中心的彩色图像，对采集的三维点云进行颜色映射，得到三维彩色点云和图像；

单线三维激光扫描仪的测距精度优于被测移动机器人传感器精度；

根据在不同站点得到的三维彩色点云之间重叠区域，求取不同站点云旋转平移矩阵，将多站点三维彩色点云融合到一起，最终得到场景完整点云和图像；

S2、设定虚拟轨迹、虚拟传感器的类型、内参数和视角，采用虚拟轨迹、虚拟传感器的类型、内参数和视角对场景完整点云和图像进行采样，生成与虚拟轨迹、虚拟传感器类型和视角对应的传感器数据帧序列；所述虚拟传感器与被测移动机器人的传感器类型和参数相同；

S3、被测移动机器人对S2生成的传感器数据帧序列进行解算生成机器人运动轨迹，将该机器人运动轨迹与S2的虚拟轨迹对比，评价导航轨迹精度和能力；

所述S2包括：

根据被测机器人，设定虚拟传感器类型、内参数、视角、取景框IoU阈值；

在空间中生成取景框，取景框形状、视场角、深度根据设定的传感器内参数决定；在空间中生成取景框后，计算虚拟轨迹中相邻轨迹帧之间取景框的IoU,如果相邻轨迹帧对应的取景框的IoU低于取景框IoU阈值，则根据需要在这两帧之间进行线性插值，插入补充轨迹帧，直到所有相邻轨迹帧所对应的取景框之间的IoU都大于设定的取景框IoU阈值；

逐帧读取插帧后的轨迹，根据每一帧轨迹的位置、视角，结合虚拟传感器的类型、内参数，计算虚拟传感器到场景完整点云中各点的距离值或光度值作为像素值的图像后输出，或直接按虚拟传感器的空间分辨率捕获虚拟传感器处采集范围内的点云后输出，循环这个过程，直到所有轨迹帧对应的输出都被计算完毕，最终得到与虚拟轨迹对齐的传感器数据帧序列。

2.根据权利要求1所述的移动机器人导航轨迹解算精度和能力测试方法，其特征在于，所述S2中，

根据需要对被测移动机器人的运动路径，姿态，可自行设置行进路线，行进时视角，路径长短，路径是否闭环，生成虚拟轨迹。

3.根据权利要求1所述的移动机器人导航轨迹解算精度和能力测试方法，其特征在于，所述S3中，采用相对位姿误差RPE和绝对轨迹误差ATE的统计值评价机器人导航轨迹精度和能力。

4.根据权利要求3所述的移动机器人导航轨迹解算精度和能力测试方法，其特征在于，所述S3中，虚拟轨迹与机器人运动轨迹的第i帧的相对位姿误差RPE为：

n表示机器人运动轨迹的帧数，Δ表示间隔帧数，m＝n-Δ，Q_i+Δ表示虚拟轨迹中第i+Δ帧，Q_i表示虚拟轨迹中第i帧，P_i表示机器人运动轨迹中第i帧，P_i+Δ表示机器人运动轨迹中第i+Δ帧；

RMSE(E₁:_n,Δ)表示第1到n帧E_i的均方根误差；

根据RMSE(E_1:n,Δ)评价移动机器人导航轨迹精度。

5.根据权利要求4所述的移动机器人导航轨迹解算精度和能力测试方法，其特征在于，所述S3中，虚拟轨迹与机器人运动轨迹的第i帧的绝对轨迹误差ATE为：

使用最小二乘计算出一个虚拟轨迹到机器人运动轨迹的位姿变换矩阵S∈SE(3)，SE(3)表示特殊欧式群，用来表示参数空间；

RMSE(F₁:_n,Δ)表示第1到n帧F_i的均方根误差。

Images