[发明专利]一种机器人导航定位方法、系统及存储介质

说明书

一种机器人导航定位方法、系统及存储介质，根据环境数据制定拓扑图，根据拓扑图计算全局规划路径；根据全局规划路径进行探索时，判断是否到达预期位置，如果到达预期位置，则返回原点，若没有达到预期位置，则创建新图，并根据新建图返回到原点；判断是否符合连通性；若不符合连通性，则采用图优化技术进行优化，得到连通图，根据连通图计算下一次导航的全局规划路径。本发明将3D导航和通过拓扑图提供先验信息的路径优化应用到机器人上达到更好的精度要求。本发明的方法能够结合先验信息以映射环境，利用这些信息来追求主动循环闭合，自动检测和纠正所提供数据中的不一致。

技术领域

本发明属于智导导航控制领域，涉及在远程控制智能小车的方法，具体涉及一种机器人导航定位方法、系统及存储介质。

背景技术

近年来，具备学习环境模型和自我定位能力的自主移动机器人的应用越来越普遍。各种激光建图定位的算法也有很多，然而算法的精度不同，用到的传感器数量和总类也不同，算法的执行时间也有优劣之分，一般都是要综合各项指标来选择综合效果最好的算法。基于2D的导航和路径规划的程序有很多，做法也已经很成熟，然而基于3D的导航和路径规划程序数量却很少。目前也有很多自主移动机器人已经应用到人们的日常生活中，这需要一套完整的人机交互界面来指导用户使用，人机交互界面也有基于不同的框架来搭建开发，但宗旨都是为了方便用户操作，使用户便于理解，使用户有一个良好的体验。

发明内容

为克服现有技术中的问题，本发明的目的在于提供一种机器人导航定位方法、系统及存储介质，以提高导航定位的精度。

为实现上述目的，本发明采用的技术方案如下：

一种机器人导航定位方法，包括以下步骤：

根据机器人导航定位范围的环境数据制定拓扑图，根据拓扑图计算全局规划路径；

根据机器人对全局规划路径进行探索后，判断机器人经过的路径形成的图形是否符合连通性；若符合连通性，将机器人经过的路径形成的图形作为连通图，若不符合连通性，则采用图优化技术进行优化，得到连通图，根据连通图计算下一次导航的全局规划路径。

本发明进一步的改进在于，根据机器人对全局规划路径进行探索后判断是否到达预期位置，如果到达预期位置，则返回原点，若没有达到预期位置，则以最接近原点的点为起点，返回到原点；全局规划路径是：遍历拓扑图或连通图的所有边。

本发明进一步的改进在于，通过下式判断机器人经过的路径形成的图形是否满足连通性，若一元因子为0，则满足连通性；

其中，为一元因子，x^[i]为顶点，C为最小许可间隙，damp(x^[i])为最近障碍物。

本发明进一步的改进在于，采用图优化技术进行优化的具体过程为：进行自适应优化得到优化后的路径图，根据优化后的路径图，创建临时图，根据临时图得到循环闭包候选者，创建包含候选者的探索计划路径；比较探索计划路径与全局规划路径的成本，得到比较结果，将比较结果中成本较小的计划作为下一步探索要遵循的路径，并判断是否符合连通性，若不符合，则重复采用图优化技术进行优化，直至符合连通性。

本发明进一步的改进在于，进行自适应优化的具体过程为：计算误差，误差包括当前边的长度与拓扑图中的参考长度之间的差值以及当前图和拓扑图中两个边之间的角度差，假设误差为高斯分布，计算最大似然估计，根据最大似然估计得到优化后的路径图。

本发明进一步的改进在于，比较结果中成本较小的计划包括最短路径子计划和闭合子计划；最短路径子计划是根据临时图采用标准最短路径技术计算得到。

本发明进一步的改进在于，通过非线性最小二乘估计问题计算最大似然估计。

本发明进一步的改进在于，成本通过下式计算：