[发明专利]移动机器人导航轨迹解算精度和能力测试方法

权利要求书

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的均方根误差。