定位信息和面片特征匹配的机器人重定位方法及系统与流程

本发明属于自主移动机器人定位导航，具体涉及一种定位信息和面片特征匹配的机器人重定位方法及系统。

背景技术：

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

2、机器人的自主导航准确性非常重要，尤其是工作在复杂环境下的机器人，如变电站巡检机器人、工业机器人等。

3、但据发明人了解，现有的机器日人定位多采用gps模块、imu模块或视觉传感器等，但是，单一的传感器如imu模块在机器人长时间做三维变速运动时易发生比较严重的漂移现象，gps模块在遮挡后定位准确度显著下降，而纯视觉定位需要较大的计算量，且变电站内有很多重复场景难以区分。

4、已经有部分研究提出将多个传感器结合，如依靠gps和imu原始数据进行重定位等，但对于很多场景，如变电站环境下，设备众多，较为复杂，数据准确性不能得到保证，或计算力需求高，且很难应对变电站内大量重复场景的情况。

技术实现思路

1、本发明为了解决上述问题，提出了一种定位信息和面片特征匹配的机器人重定位方法及系统，本发明可以在机器人丢失位置时，快速进行重定位，并提高机器人重定位的准确性，提高定位导航的可靠性。

2、根据一些实施例，本发明采用如下技术方案：

3、一种定位信息和面片特征匹配的机器人重定位方法，包括以下步骤：

4、获取机器人gps定位信息，并将其进行经纬度转换，得到激光雷达坐标系下的坐标值，确定体素特征匹配范围；

5、基于激光雷达数据，在预先构建的以面片为单位元素的体素地图中，在体素特征匹配范围内进行点和面的匹配；

6、根据匹配结果，进行状态估计，计算出机器人当前位姿。

7、作为可选择的实施方式，还包括以下步骤：

8、在激光定位过程中，通过迭代最近点点面配准确定配准误差，若配准误差大于设定的配准误差阈值时，判定为激光定位失败，进入上述重定位方法的步骤。

9、作为可选择的实施方式，将gps定位信息进行经纬度转换的过程包括，选取pgm地图上的两个点作为标定参考点，记录下这两个标定点的经纬度数值，通过平面几何的三角函数计算出gps经纬度高程坐标系和pgm地图坐标系之间的转换函数，将gps获得的经纬度高程数据与机器人的地图坐标一一映射，提供重定位的体素特征匹配范围。

10、作为可选择的实施方式，以面片为单位元素的体素地图的构建过程包括用哈希表和八叉树构建地图，哈希表管理最上层的地图结构，八叉树每个节点中存放一个平面，如果一个节点中的点不能被表示为一个特征，拆分这个节点。

11、作为进一步的，后续地图的点被送到对应的节点中，在面特征稳定后删除旧的观测，只保留最新观测，如果新的观测和旧的观测冲突，则删去旧估计位姿，重写估计位姿。

12、作为进一步的，新获取的激光雷达点云数据加入构建的体素地图，并且配准，估计其姿态，并使用估计的姿态将新点注册到全局地图中。

13、作为进一步的，将新点注册到全局地图中时，在新点位于一个未填充的体素中时，构造该体素；当将新点添加到现有体素时，更新体素中平面的参数和不确定性。

14、随着时间的累积，新的激光雷达点云帧不断的进入并且配准，进入的每个点的位置携带一个均值为零和一定方差的高斯噪声。在不确定性收敛后，只保留最新的n个点，n根据需求预先设置，并更新估计的平面参数(n,q)和协方差∑,q。如果新的法向量和先前收敛的法向量又出现大于设定值的差值，认为地图的这个区域发生了变化，需要重新计算法向量。

15、作为可选择的实施方式，在体素特征匹配范围内进行点和面的匹配时，根据在具有姿态先验的世界框架中预测的激光雷达点，在确定的体素特征匹配范围内，通过激光雷达点的哈希键找到其所在的根体素，对所有包含的子体素进行轮询，以此与点匹配。

16、作为可选择的实施方式，根据匹配结果，进行状态估计的具体过程包括将点与面匹配得到的匹配的点到平面距离，融合进激光雷达里程计，形成最大后验估计，确定各个点到平面匹配的观测方程。

17、一种定位信息和面片特征匹配的机器人重定位系统，包括：

18、范围确定模块，被配置为获取机器人gps定位信息，并将其进行经纬度转换，得到激光雷达坐标系下的坐标值，确定体素特征匹配范围；

19、点面匹配模块，被配置为基于激光雷达数据，在预先构建的以面片为单位元素的体素地图中，在体素特征匹配范围内进行点和面的匹配；

20、状态估计模块，被配置为根据匹配结果，进行状态估计，计算出机器人当前位姿。

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

22、本发明创新性提供了定位信息和面片特征匹配的机器人重定位方法，通过增加gps模块解决通过激光雷达定位导航容易造成位置丢失的问题，并且考虑到室外机器人搭载的工控机算力较低，采用了节约算力的设计方案，增加变电站室外巡检机器人的定位导航可靠性。

23、为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

技术特征：

1.一种定位信息和面片特征匹配的机器人重定位方法，其特征是，包括以下步骤：

2.如权利要求1所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，还包括以下步骤：

3.如权利要求1所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，将gps定位信息进行经纬度转换的过程包括，选取pgm地图上的两个点作为标定参考点，记录下这两个标定点的经纬度数值，通过平面几何的三角函数计算出gps经纬度高程坐标系和pgm地图坐标系之间的转换函数，将gps获得的经纬度高程数据与机器人的地图坐标一一映射，提供重定位的体素特征匹配范围。

4.如权利要求1所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，以面片为单位元素的体素地图的构建过程包括用哈希表和八叉树构建地图，哈希表管理最上层的地图结构，八叉树每个节点中存放一个平面，如果一个节点中的点不能被表示为一个特征，拆分这个节点。

5.如权利要求4所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，后续地图的点被送到对应的节点中，在面特征稳定后删除旧的观测，只保留最新观测，如果新的观测和旧的观测冲突，则删去旧估计位姿，重写估计位姿。

6.如权利要求4或5所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，新获取的激光雷达点云数据加入构建的体素地图，并且配准，估计其姿态，并使用估计的姿态将新点注册到全局地图中。

7.如权利要求6所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，将新点注册到全局地图中时，在新点位于一个未填充的体素中时，构造该体素；当将新点添加到现有体素时，更新体素中平面的参数和不确定性；

8.如权利要求1所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，在体素特征匹配范围内进行点和面的匹配时，根据在具有姿态先验的世界框架中预测的激光雷达点，在确定的体素特征匹配范围内，通过激光雷达点的哈希键找到其所在的根体素，对所有包含的子体素进行轮询，以此与点匹配。

9.如权利要求1所述的一种定位信息和面片特征匹配的机器人重定位方法，其特征是，根据匹配结果，进行状态估计的具体过程包括将点与面匹配得到的匹配的点到平面距离，融合进激光雷达里程计，形成最大后验估计，确定各个点到平面匹配的观测方程。

10.一种定位信息和面片特征匹配的机器人重定位系统，其特征是，包括：

技术总结
本发明提供了一种定位信息和面片特征匹配的机器人重定位方法及系统，本发明可以在机器人丢失位置时，获取机器人GPS定位信息，并将其进行经纬度转换，得到激光雷达坐标系下的坐标值，确定体素特征匹配范围；基于激光雷达数据，在预先构建的以面片为单位元素的体素地图中，在体素特征匹配范围内进行点和面的匹配；根据匹配结果，进行状态估计，计算出机器人当前位姿，快速进行重定位，并提高机器人重定位的准确性，提高定位导航的可靠性。

技术研发人员：杨杰,巩方彬,刘加科,黄聪,明钰,薛双忠,张聪聪,黄倩菁,杜兴勋,侯继新,付学成,孙宗伟,翟义凯
受保护的技术使用者：国网智能科技股份有限公司
技术研发日：
技术公布日：2024/3/4