[发明专利]一种三维激光点云快速重定位方法

说明书

本发明提出一种三维激光点云快速重定位方法，在先验地图的基础上，基于二维栅格化减少数据运算量提高实时性，同时基于Jaccard系数得到初步候选场景集合，在此基础上基于数据主方向与Pearson相关系数对初步候选场景集合进行筛选后得到候选场景集合，提高了实时性，再基于欧几里得聚类法进行三维聚类后对候选场景集合中每一历史帧场景构建二分图，并基于Hungarian匹配算法寻找到最大匹配关系后基于余弦相似性完成三维相似性度量，最终得到唯一候选场景，使得得到的对应关系求解更可靠，鲁棒性更高，配准结果更准确，方法整体实时性高，最后利用3D‑NDT算法得到两帧间的姿态变换关系矩阵完成重定位，本发明基于余弦相似度进行唯一候选场景筛选。

技术领域

本发明属于同步定位与建图技术，特别是一种三维激光点云快速重定位方法。

背景技术

重定位算法是指基于先验地图完成定位的过程。重定位技术直接利用先验地图与当前三维激光雷达所获取的场景信息进行全局定位，其允许机器人在不用重新构图的基础上完成定位。

传统的三维激光点云重定位算法通常基于三维场景原始点云与关键帧进行匹配以期获得候选场景，这导致算法实时性差，往往需要对重定位算法或者三维点云输入降频以达到实时性的要求；

同时，传统重定位算法基于激光里程计的匹配结果，需要在其设定的欧氏距离范围内进行候选场景搜索，这往往使得重定位结果高度依赖激光里程计结果，一旦激光里程计配准失败则会导致重定位结果不可信，由此基于重定位结果进行优化的里程计结果也将继续发散，最终整个构图与重定位算法失败。

发明内容

本发明的目的在于提出一种三维激光点云快速重定位方法，以解决三维激光点云在先验地图中快速重定位的问题。

实现本发明的技术解决方案为：一种三维激光点云快速重定位方法，具体步骤为：

步骤1、对分割好当前场景的原始激光图点云进行二维栅格化处理；

步骤2、基于Jaccard系数对先验地图中关键帧进行初筛得到候选场景集合；

步骤3、基于数据主方向与Pearson相关系数对候选场景集合进行进一步筛选；

步骤4、对原始帧与步骤3筛选出的候选场景帧进行基于欧几里得聚类算法的三维点云聚类，得到聚类集合；

步骤5、对步骤4得到的聚类集合基于Hungarian匹配构建当前帧与候选场景集合中每一帧的最大对应关系，得到对应的集合；

步骤6、基于余弦相似度进行唯一候选场景筛选；

步骤7、基于唯一候选场景帧，由3D-NDT算法得到变换矩阵，并依此确定定位矩阵，实现三维点云配准重定位。

本发明与现有技术相比，其显著优点为：

(1)本发明基于Jaccard系数、数据主方向与Pearson相关系数对当前场景进行二维化处理提高了实时性。

(2)本发明基于二分图构建基当前场景与候选场景之间的对应关系，使得得到的对应关系求解更可靠，鲁棒性更高。

(3)本发明基于余弦相似度进行唯一候选场景筛选，使得配准结果更准确，方法整体实时性高。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1为本发明中三维激光点云快速重定位方法整体步骤流程图。

图2为本发明中原始点云中心化处理效果示意图，其中图2(a)为原始点云图，图2(b)为剔除地面-雪面后的点云图，图2(c)为点云中心化处理后的图。

具体实施方式

一种三维激光点云快速重定位方法，包括以下步骤：