基于动态点云与拓扑路网的机器人全局定位方法及系统

本发明属于机器人定位，具体涉及一种基于动态点云与拓扑路网的机器人全局定位方法及系统。

背景技术：

1、本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

2、精确稳定的定位是自主导航的前提，在无人驾驶中，全局定位一般指在开机后在建好的地图上定位或在机器人绑定后定位，此时当前帧的位姿可能出现在已有地图中的任意位姿，甚至在已有地图之外。在没有先验信息的前提下，机器人仅依靠自身传感器估计其在已知全局地图中的位姿。传统的全局定位方法主要有三种：一是采用rtk（载波相位差分技术）作为全局约束信息加入到定位中；二是直接在rviz（开源机器人操作系统ros中的一款三维可视化平台）上手动给予初始位姿；三是直接利用当前帧的所有点云与地图进行暴力搜索匹配。

3、机器人全局定位时，导航过程中由于某种外部因素 (如人为搬离、外部碰撞等)机器人位姿发生突变，从而导致原有依赖位姿连续变化的定位算法失效。在室内环境下，组合导航失去了全球导航卫星系统（global navigation satellite system，gnss）信号，无法建立全局约束；在rviz上直接手动获得初始位姿存在一定的局限性，在自动驾驶中是一种不太科学的方法。相似环境下的关键帧不仅会造成冗余，还容易造成误匹配。直接用激光进行匹配的方法速度较慢，不适用于较大场景下的全局定位。

4、据发明人了解，现有的全局定位存在以下缺陷：（1）缺乏先验知识；即前一瞬间机器人的位置是未知的；（2）在重新定位和闭环检测的时候，即机器人回到某个曾访问过的位置，会发生各种各样的变化，包括光照、视角的变化，同时也有一些动态的物体造成的变化。

技术实现思路

1、为了解决上述问题，本发明提出了一种基于动态点云与拓扑路网的机器人全局定位方法及系统，应用于室内环境的全局定位，在室内结构化环境下实现机器人重新上电或位姿发生突变后的重新定位，有效解决在缺乏先验知识下的重定位问题以及机器人回到曾经访问过的位置因环境变化的回环检测及全局定位问题，有效提高定位的稳定性及鲁棒性。

2、根据一些实施例，本发明的第一方案提供了一种基于动态点云与拓扑路网的机器人全局定位方法，采用如下技术方案：

3、一种基于动态点云与拓扑路网的机器人全局定位方法，包括：

4、构建室内环境的点云地图；

5、提取所构建点云地图的拓扑结构，得到室内环境的拓扑路网和拓扑点位姿；

6、根据所得到的拓扑路网和拓扑点位姿进行机器人定位的粗匹配，确定机器人当前点云的全局位姿；

7、根据当前点云的全局位姿，加载机器人的分块地图；

8、基于实时动态点云进行分块地图的精匹配，得到机器人的全局位姿，完成机器人的全局定位。

9、作为进一步的技术限定，在根据所得到的拓扑路网和拓扑点位姿进行机器人定位的粗匹配的过程中，判断拓扑节点的类型，根据拓扑节点类型判断该拓扑节点的特征与匹配节点特征是否相符，完成机器人定位的粗匹配；其中，拓扑节点的类型为转角处、交叉路口或走廊。

10、进一步的，在确定机器人当前点云的全局位姿的过程中，根据当前点云和拓扑节点的类型，得到当前点云的平移矩阵；根据预设分辨率遍历所有姿态角；根据点云对应点间距进行当前点云的匹配，确定当前点云的位置和姿态，得到机器人当前点云的全局位姿。

11、作为进一步的技术限定，在分块地图的精匹配的过程中，采用icp（point cloudregistration）点云配准匹配法。

12、进一步的，在icp点云配准匹配的过程中，把分块地图转换成地图点云，把当前帧的激光变成激光点云，对所得到的地图点云与激光点云进行配准，得到地图点云与激光点云之间的旋转量和平移量，根据所得到的旋转量和平移量进行地图点云的旋转平移，实现地图点云与激光点云的重合，完成分块地图的精匹配。

13、作为进一步的技术限定，在构建室内环境的点云地图的过程中，通过激光雷达扫描室内的结构化环境，再将惯性传感器（inertial measurement unit，简称imu）和里程计所获取的信息融入到即时定位与地图构建（simultaneous localization and mapping，简称slam）中，完成室内环境点云地图的构建。

14、作为进一步的技术限定，所提取的点云地图的拓扑结构包括拓扑节点的位置以及拓扑节点之间的连接关系。

15、根据一些实施例，本发明的第二方案提供了一种基于动态点云与拓扑路网的机器人全局定位系统，采用如下技术方案：

16、一种基于动态点云与拓扑路网的机器人全局定位系统，包括：

17、构建模块，其被配置为构建室内环境的点云地图；

18、提取模块，其被配置为提取所构建点云地图的拓扑结构，得到室内环境的拓扑路网和拓扑点位姿；

19、粗匹配模块，其被配置为根据所得到的拓扑路网和拓扑点位姿进行机器人定位的粗匹配，确定机器人当前点云的全局位姿；

20、加载模块，其被配置为根据当前点云的全局位姿，加载机器人的分块地图；

21、定位模块，其被配置为基于实时动态点云进行分块地图的精匹配，得到机器人的全局位姿，完成机器人的全局定位。

22、根据一些实施例，本发明的第三方案提供了一种计算机可读存储介质，采用如下技术方案：

23、一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方案所述的基于动态点云与拓扑路网的机器人全局定位方法中的步骤。

24、根据一些实施例，本发明的第四方案提供了一种电子设备，采用如下技术方案：

25、一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方案所述的基于动态点云与拓扑路网的机器人全局定位方法中的步骤。

26、与现有技术相比，本发明的有益效果为：

27、本发明中拓扑地图与其它地图的信息耦合度低，定位存储空间小，匹配速度快；通过拓扑路网进行粗匹配，采用动态点云地图分块加载进行精匹配，避免加载整张全局地图造成匹配速度慢，通过粗匹配与精匹配的相互配合提高定位鲁棒性。

28、本发明在室内结构化环境下实现机器人重新上电或位姿发生突变后的重新定位，有效解决在缺乏先验知识下的重定位问题以及机器人回到曾经访问过的位置因环境变化的回环检测及全局定位问题，有效提高定位的稳定性及鲁棒性。

技术特征：

1.一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，包括：

2.如权利要求1中所述的一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，在根据所得到的拓扑路网和拓扑点位姿进行机器人定位的粗匹配的过程中，判断拓扑节点的类型，根据拓扑节点类型判断该拓扑节点的特征与匹配节点特征是否相符，完成机器人定位的粗匹配；其中，拓扑节点的类型为转角处、交叉路口或走廊。

3.如权利要求2中所述的一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，在确定机器人当前点云的全局位姿的过程中，根据当前点云和拓扑节点的类型，得到当前点云的平移矩阵；根据预设分辨率遍历所有姿态角；根据点云对应点间距进行当前点云的匹配，确定当前点云的位置和姿态，得到机器人当前点云的全局位姿。

4.如权利要求1中所述的一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，在分块地图的精匹配的过程中，采用icp点云配准匹配法。

5.如权利要求4中所述的一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，在icp点云配准匹配的过程中，把分块地图转换成地图点云，把当前帧的激光变成激光点云，对所得到的地图点云与激光点云进行配准，得到地图点云与激光点云之间的旋转量和平移量，根据所得到的旋转量和平移量进行地图点云的旋转平移，实现地图点云与激光点云的重合，完成分块地图的精匹配。

6.如权利要求1中所述的一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，在构建室内环境的点云地图的过程中，通过激光雷达扫描室内的结构化环境，再将惯性传感器和里程计所获取的信息融入到即时定位与地图构建中，完成室内环境点云地图的构建。

7.如权利要求1中所述的一种基于动态点云与拓扑路网的机器人全局定位方法，其特征在于，所提取的点云地图的拓扑结构包括拓扑节点的位置以及拓扑节点之间的连接关系。

8.一种基于动态点云与拓扑路网的机器人全局定位系统，其特征在于，包括：

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现了如权利要求1-7任一项所述的基于动态点云与拓扑路网的机器人全局定位方法的步骤。

10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现了如权利要求1-7任一项所述的基于动态点云与拓扑路网的机器人全局定位方法的步骤。

技术总结
本发明属于机器人定位技术领域，具体涉及一种基于动态点云与拓扑路网的机器人全局定位方法及系统，包括：构建室内环境的点云地图；提取所构建点云地图的拓扑结构，得到室内环境的拓扑路网和拓扑点位姿；根据所得到的拓扑路网和拓扑点位姿进行机器人定位的粗匹配，确定机器人当前点云的全局位姿；根据当前点云的全局位姿，加载机器人的分块地图；基于实时动态点云进行分块地图的精匹配，得到机器人的全局位姿，完成机器人的全局定位。本发明在室内结构化环境下实现机器人重新上电或位姿发生突变后的重新定位，有效解决重定位问题及机器人回到曾经访问过的位置因环境变化的回环检测及全局定位问题，提高定位的稳定性。

技术研发人员：陈雪梅,李健,孙静,桑迪,陈庆伟,郭坤
受保护的技术使用者：北京理工大学前沿技术研究院
技术研发日：
技术公布日：2024/1/15