[发明专利]一种融合ND和ICP匹配的移动机器人重定位方法

说明书

本发明适用于机器人定位技术领域，提供了一种融合ND和IPC匹配的移动机器人重定位方法，包括：对激光帧初始点云进行滤波，获得激光帧点云S；基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分，提取前m个最高ND概率得分的撒阵列粒子；基于m个撒阵列粒子构建矩阵集T2，基于各矩阵将激光帧点云S换到栅格地图，得到m个激光帧点云S″；将m个激光帧点云S″和分别地图点云M'进行ICP匹配，输出前n个误差较小的撒阵列粒子及对应的变换矩阵T_e；计算n个撒阵列粒子的ND概率得分，输出最高得分撒阵列粒子对应的变换矩阵T_e，基于该变化矩阵T_e来计算机器人的当前位姿。仅通过用激光雷达进行定位，降低定位的成本的同时，实现了高精度的重定位。

技术领域

本发明属于机器人定位技术领域，提供了一种融合ND和ICP匹配的移动机器人重定位方法。

背景技术

随着移动机器人在服务业和仓储物流领域有着越来越广泛的应用，其自主定位导航技术也越发重要。现主要的技术有磁导航、视觉导航和激光导航，其中磁导航工作轨迹固定，路径需要铺设电磁导线或磁条。视觉导航轨迹较为自由，但是环境中多使用二维码或特殊图形码等标记物，还需要定期维护。激光导航定位精准，路径灵活多变，适应多种现场环境，因此成为了移动机器人主流的定位导航方式，但是也显露出一系列问题，如机器人的重启或关机、打滑漂移和人为移动等，导致机器人绑架和定位失败问题，此时重定位技术就显得越发重要。

为了解决上述问题，现有的方案主要包含如下三种：3)AMCL定位。在地图中生成随机粒子，计算粒子权重，重采样剔除权重低的粒子，保存和复制权重高的粒子，多次迭代后粒子群收敛于真实机器人位姿。2)机器人回到初始位置。定位失败后，人为将机器人推到初始位置，然后重新开机。3)人为标记机器人新位姿，根据导航地图和真实环境，人为标记机器人的大致位置。这些定位方法都需要人工参与，无法保证重定位的精度。

发明内容

本发明实施例提供一种融合ND和ICP匹配的移动机器人重定位方法，仅通过激光雷达来实现了高精度的重定位。

本发明是这样实现的，一种融合ND和ICP匹配的移动机器人重定位方法，所述方法具体包括如下步骤：

S1、获取激光帧的初始点云，并对激光帧初始点云进行滤波，获得激光帧点云S；

S2、获取栅格导航地图的撒阵列粒子集，基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分，提取前m个最高ND概率得分的撒阵列粒子；

S3、基于m个撒阵列粒子构建矩阵集T2，矩阵集T2中包括m个矩阵，基于各矩阵将激光帧点云S换到栅格地图，得到变换后的m个激光帧点云S″；

S4、将m个激光帧点云S”和分别地图点云M'进行ICP匹配，输出前n个误差较小的撒阵列粒子及对应的变换矩阵T_e；

S5、计算n个撒阵列粒子的ND概率得分，输出最高得分撒阵列粒子对应的变换矩阵T_e，基于该变化矩阵T_e来计算机器人的当前位姿。

进一步的，撒阵列粒子的ND概率得分获取方法具体如下：

基于撒阵列粒子集来构建矩阵集T1，矩阵集T1中包括K个矩阵，基于各矩阵将激光帧点云S换到栅格导航地图上，形成K个激光帧点云S'；

基于地图点云的ND模型来计算各帧激光帧点云S'的ND概率得分，即为帧激光帧点云S'对应撒阵列粒子的ND概率得分。

进一步的，撒阵列粒子集的获取方法具体如下：

遍历栅格导航地图中的像素点，若当前像素值≥254，即且所述像素点的周围距离阈值内的像素值均≥254，将所述像素坐标作为撒阵列粒子保存到撒阵列粒子集。