基于3D激光点云的移动机器人全局重定位方法及装置与流程

本发明涉及移动机器人自主导航，尤其涉及一种基于3d（dimensions，维度）激光点云的移动机器人全局重定位方法及装置。

背景技术：

1、实时定位是移动机器人自主导航中至关重要的一环，其决定机器人能否准确地感知自身位置和环境信息，从而有效地完成导航任务。虽然同时定位与建图（simultaneouslocalization and mapping,slam）技术可以实现机器人在未知场景和未知状态的前提下建立环境地图并确定机器人相对环境地图的位姿，但传统slam方法中，机器人每次开机时机器人相对已有环境地图的初始位姿是未知的，因而每次开机后都需要重新构建地图，环境地图无法得到有效的复用，而建图过程往往需要人工辅助，极大地增加了建图工作量和人工时间成本，且要完成重定位必须机器人运动一段距离才能构建出包含足够环境信息的子图，而机器人在初始未知的前提下运行会存在运动碰撞风险。

2、为解决机器人相对已有地图的初始位姿定位问题，现有技术中一种解决策略是默认机器人每次的起始出发点都是同一个点，以将机器人相对已有环境地图的起始位姿变为一个已知问题，通过这种策略可以绕开机器人的初始定位问题，但是机器人每次都需要从同一个固定位置出发，极大地限制了机器人任务的灵活性。另一种解决策略是机器人在导航开始前会请求用户指定一个初始位姿，但是该类导航方式需要依赖人工的参与，无法实现完全自主的移动机器人导航。另外，部分室外机器人可以依靠gps（global positioningsystem，全球定位系统）获取初始位置，但是gps无法提供方向信息，这使得依靠gps获取初始位姿的方法还需要结合其他传感器来共同确定机器人的初始方向，实现成本较高，且在室内和一些室外gps遮挡区域下，难以保证gps设备正常工作。

技术实现思路

1、本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现方法简单、成本低、定位效率与可靠性高的基于3d激光点云的移动机器人全局重定位方法及装置。

2、为解决上述技术问题，本发明提出的技术方案为：

3、一种基于3d激光点云的移动机器人全局重定位方法，步骤包括：

4、获取移动机器人当前所在环境的3d点云地图，并在移动机器人行驶过程中获取激光雷达扫描移动机器人周围环境所得到的3d激光点云，其中3d点云地图为预先根据移动机器人遍历当前环境得到的3d激光点云构建得到；

5、使用所述3d激光点云生成2d激光点云以及使用3d点云地图生成2d的栅格地图；

6、将所述2d激光点云与所述栅格地图进行相关扫描匹配，根据匹配结果得到最佳2d估计位姿；

7、将所述3d激光点云变换至所述最佳2d估计位姿的附近，并通过将3d激光点云与所述3d点云地图进行配准得到最佳3d估计位姿；

8、根据3d激光点云与3d点云地图之间的配准结果对当前得到的最佳2d估计位姿进行校验，如果校验通过则将所述最佳2d估计位姿、所述最佳3d估计位姿作为当前的估计位姿输出。

9、进一步的，所述将所述2d激光点云与所述栅格地图进行相关扫描匹配包括：

10、将2d激光点云进行旋转和平移操作以使得2d激光点云中的激光点落在栅格地图中；

11、计算2d激光点云中的激光点击中栅格地图的概率，根据计算的概率搜索当前2d激光点云与栅格地图的最佳匹配位姿，即得到所述最佳2d估计位姿。

12、进一步的，所述根据计算的概率搜索当前2d激光点云与栅格地图的最佳匹配位姿包括：

13、根据不同的分辨率在栅格地图上生成所有可能的位姿解集合；

14、使用分支定界方法在所述位姿解集合中搜索最佳匹配位姿以得到所述最佳2d估计位姿；

15、所述分支定界方法通过获取所述位姿解集合作为初始候选位姿集并使用栅格地图层构建多分辨率地图，先对当前候选位姿集中候选位姿进行分支扩展得到候选位姿子集，然后在所述候选位姿子集中进行剪枝，对每个候选位姿子集递归地执行所述分支扩展、所述剪枝的操作直到满足终止条件为止，最终选择具有最高得分的位姿作为最佳匹配位姿，即最佳2d估计位姿。

16、进一步的，使用所述分支定界方法的过程中，在不同分辨率地图上采用不同大小的角度步长，其中在第一分辨率地图上使用第一步长和第一角度分辨率生成候选位姿，在第二分辨率地图上使用第二步长和第二角度分辨率生成候选位姿，所述第一分辨率小于所述第二分辨率，所述第一步长大于所述第二步长，所述第一角度分辨率大于第二角度分辨率。

17、进一步的，得到最佳2d估计位姿后还包括根据当前激光帧与栅格地图的匹配状态对最佳2d位姿进行评分，根据评分结果判断当前位姿估计的有效性，其中如果评分值大于预设阈值则继续执行后续的3d激光点云与3d点云地图之间的配准，否则判定当前位姿估计失败。

18、进一步的，按照下式对最佳2d位姿进行评分：

19、

20、其中，x表示估计的最佳2d估计位姿，表示位姿x在栅格地图中的得分，h表示当前激光帧击中栅格地图的激光点数量，n表示当前激光帧未击中栅格地图的激光点数量。

21、进一步的，所述根据3d激光点云与3d点云地图之间的配准结果对当前得到的最佳2d估计位姿进行校验包括：根据3d激光点云与3d点云地图之间的点云相似度计算3d位姿评分，如果计算得到的3d位姿评分大于预设阈值则判定校验通过，否则校验失败。

22、进一步的，所述将所述3d激光点云转换为2d激光点云包括：

23、对每帧3d激光点云进行滤波，去除不在预设截取区间内的激光点后将3d激光点云投影成2d激光点云；

24、对2d激光点云进行降采样以去除重复的激光点，得到最终的2d激光点云。

25、一种基于3d激光点云的移动机器人全局重定位装置，包括处理器以及存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序以执行如上述方法。

26、一种存储有计算机程序的计算机可读存储介质，所述计算机程序执行时实现如上述方法。

27、与现有技术相比，本发明的优点在于：

28、1、本发明通过融合2d与3d点云匹配方式实现移动机器人的全局重定位，利用2d激光点云与栅格地图进行相关扫描匹配搜索出最佳2d位姿估计，利用3d激光点云帧和点云地图进行点云匹配得到最佳3d位姿估计，同时使用点云配准结果对估计的位姿进行校验，将校验通过后的位姿估计作为重定位的定位结果，能够充分利用2d点云匹配方式加速位姿估计的搜索速度，同时利用3d点云匹配方式校验2d位姿搜索结果，能够兼顾搜索速度和搜索结果可信度实现快速、精准的全局重定位。

29、2、本发明可以在未知机器人相对环境地图的初始位姿情况下为机器人需要重定位时提供一个可靠的初始位姿，且可以实现移动导航机器人对环境地图的重复使用，无需每次开机时重新建图，减少重复建图的成本提升机器人的导航效率，还可以在导航过程中发生位姿丢失时快速恢复机器人位姿，提高导航成功率和导航效率。

30、3、本发明在重定位过程中机器人保持静止即可完成整个重定位过程，还可以减少机器人发生运动碰撞的危险性。