CN111912411B - 一种机器人导航定位方法、系统及存储介质 - Google Patents

Abstract

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

Description

一种机器人导航定位方法、系统及存储介质

技术领域

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

背景技术

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

发明内容

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

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

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

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

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

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

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

为0，则满足连通性；

其中，

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

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

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

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

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

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

f_m(P)＝f(p_{loop-closoure})+λf(p_end)

其中，f_m(P)为成本，f(p_{loop-closoure})为从当前位置到第一个预测环路闭合边缘的边缘序列，f(p_end)是完成探索的剩余部分，λ是调制因子，p_{loop-closoure}为最短路径子计划，p_end为闭合子计划。

一种机器人导航定位系统，包括：

计算模块，用于根据环境数据制定拓扑图，根据拓扑图计算全局规划路径；

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

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如上所述的机器人导航定位方法。

与现有技术相比，本发明具有的有益效果：本发明根据机器人导航定位范围或区域的环境数据制定拓扑图，通过拓扑图提供先验信息，根据拓扑图计算全局规划路径，并判断机器人经过的路径形成的图形是否符合连通性；若不符合连通性，则采用图优化技术进行优化，通过将图优化技术应用到机器人上，以达到更好的精度要求。将优化后的图作为连通图，该连通图最大化了相对于先验和所探索的图的似然性。本发明的方法能够结合拓扑图的先验信息映射环境，利用这些信息计算全局规划路径，探索时，判断否到达预期位置，如果到达预期位置，则返回原点，若没有达到预期位置，则以最接近原点的点为起点，返回到原点，主动形成循环闭合。由于在一个局域网内，机器人可以连接到人机交互界面的主机，通过界面可以控制机器人进行探索，根据用户可以指定地图上的点，机器人可以自动进行路径规划，移动到目的地。

进一步的，本发明中采用图优化技术进行优化时，创建临时图，根据临时图得到循环闭包候选者，创建包含候选者的探索计划路径；比较探索计划路径与全局规划路径的成本，将成本较小的计划作为遵循计划路径，判断遵循计划路径是否符合连通性，若不符合，则重复采用图优化技术进行优化，直至符合连通性。可见，在探索过程中，机器人不断监测其姿势定位精度，最终它可以切换到一个新的计划，如果作为判断图形符合连通性步骤的结果，边缘的长度发生了实质性的变化，则机器人可以再次计算其全局规划。

附图说明

图1是使用拓扑图来探索地图的主动SLAM的框架。

具体实施方式

下面结合附图对本发明进行详细说明。

SLAM系统是被动的：机器人由操作员驱动，用其车载传感器收集环境数据。这些数据通过处理，以估计机器人的地图和轨迹。

参见图1，本发明的一种机器人导航定位方法，包括以下步骤：

1)根据机器人导航定位范围或区域的环境数据制定一个拓扑图，该拓扑图表示移动机器人在基于SLAM的映射过程中必须探索的区域，任务为找到一种在每个边缘上导航并返回起始位置的方法，必须始终保持足够高的定位精度，以便能够依赖机器人的姿势进行一致的映射。拓扑图符合连通性。

一般情况下，一些关于环境的先验信息以拓扑图的形式存在。拓扑地图是拓扑图的一个子集，拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图，由节点和边组成，只考虑节点间的连通性。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。可以根据真实的地形获取相应的拓扑地图的数据，将真实地形的数据处理成拓扑地图格式的数据。拓扑地图的数据包含：拓扑地图的结点，拓扑地图的边，其由开始顶点、结束顶点和权重组成，整个拓扑地图由边集和顶点集构成。

2)在机器人启动时提供了拓扑图，机器人根据拓扑图计算出全局规划路径；

全局规划是一次遍历拓扑图的所有边的计划，具体过程为：将平面P定义为边的有序列表，首先从原点探索到第一个预测环路闭合边缘的边缘序列，然后完成探索的剩余部分。

在处理具有拓扑地图特征的环境时，循环闭合的一个充分条件是机器人遍历已访问的边，不包括上次遍历的边，以避免频繁闭合。

3)然后机器人根据全局规划路径进行探索过程，探索过程包括迭代地从全局规划路径中提取下一条边，并沿着这条边行驶时，判断是否到达预期位置，如果到达预期位置，则返回原点，若没有达到预期位置，则以最接近原点的点为起点，创建新图，并根据新建图返回到原点。

具体的，机器人使用反应式规划技术(反应式规划技术即机器人移动的过程中都是在不断的规划当前的路线，每移动到下一个点就重新规划)沿边缘移动，判断是否到达预期位置，如果由于任何原因，机器人发现它无法到达预期位置，将最接近原点的点作为起点，遍历其他的边，创建新图，然后根据新图回到原点；若到达预期位置，则回到原点。

4)一旦到达原点，机器人就必须判断到目前为止已探索的图形即经过的路径形成的图形是否符合连通性属性。

若一元因子

为0，则已探索的图形满足连通性；

其中，

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

通过计算一元因子，以检查图的所有顶点x^[i]是否满足最小许可间隙C，若满足，即一元因子

为0，则符合连通性。

若已探索的图形符合连通性，将已探索的图形作为连通图，根据连通图计算下一次导航的全局规划路径，即返回步骤2)；

若已探索的图形符合连通性，意味着可以在没有额外动作的情况下穿过最后一条边，到达全局规划的目标顶点，并且这是安全的，不会发生碰撞，若已完成整个地图的探索，返回连通图，若未完成探索，则返回步骤2)。

若已探索的图形不符合连通性，那么机器人将采用图优化技术进行优化，得到连通图，采用图优化技术进行优化得到连通图的具体过程如下：

1.1)进行自适应优化得到优化后的路径图，为了找到可能的循环闭包候选者，根据优化后的路径图，创建了一个临时图，其中最后遍历的边已被删除。然后，计算从这个临时图的当前位置可以到达哪些已访问的顶点：从这些顶点之一经过的任何已访问的边都是一个有效的循环闭包候选者。这样做的效果是排除一些琐碎的闭包，比如沿着最后一条边返回。

其中，进行自适应优化的具体过程为：首先计算误差，误差包括当前边的长度与拓扑图中的参考长度之间的差值以及当前图和拓扑图中两个边之间的角度差，假设误差为高斯分布，通过非线性最小二乘估计问题计算最大似然估计，根据最大似然估计得到优化后的图形。

基于最小二乘优化算法在线来补偿最常见的误差源(激光、雷达、点位的误差)，使机器人能够重建更精确的图形。通过假设误差为高斯分布，可以通过非线性最小二乘估计问题计算最大似然估计(最大似然原理的直观理解是：设一个随机试验有若干个可能的结果，在一次试验中，结果a出现，则一般认为实验对a的出现最有利，即a出现的概率较大)。这种优化的状态空间由图顶点的二维坐标组成。通过因子图可以图形化地表示这类问题。因子图是二部图。节点可以是变量，也可以是因子，因子只能连接到变量。变量编码问题的状态空间，而因子编码变量之间的约束。

为了实现图优化过程，运用了一个稳健的最小二乘优化算法。最小二乘优化问题的状态空间将只包含到目前为止访问过的顶点。经过图优化步骤后，机器人更新其对图形的估计。原始的拓扑图存储在内存中，因为它是一些成本函数所必需的。在探索过程中，机器人不断监测其姿势定位精度，最终它可以切换到一个新的计划，如果作为判断图形符合连通性步骤的结果，边缘的长度发生了实质性的变化，则机器人可以再次计算其全局规划路径。只有在经过最小二乘优化算法后，充分探索边缘周围的区域，边缘才是有效访问的。

计算从这个临时图的当前位置可以到达哪些已访问的顶点是公知技术，原因在于拓扑图是由点和边连接起来的，只需要从当前点沿着边看能到达哪些点。

1.2)在确定一个循环闭包候选者之后，可以创建一个包含候选者的探索计划(当探索到拓扑图的中间的点的时候，它的周围会连接多个点形成不同的路径，每一条路径就对应一个探索计划)，然后，将这个新的包含候选者的探索计划与全局规划(即一次遍历拓扑图的所有边的计划)进行比较，得到比较结果，将比较结果中成本较少的计划作为下一步探索要遵循的路径，包括两个子计划即最短路径子计划p_{loop-closoure}和闭合子计划p_end，这两个子计划必须独立计算以下两项内容：

第一项为：从机器人当前位置到候选边缘的最短路径。通过应用在临时图上的标准最短路径技术来计算这个最短路径子计划p_{loop-closoure}；

其中，标准最短路径技术是本领域的公知的术语，最短路径问题是图论研究中的一个经典算法问题，旨在寻找图(由结点和路径组成的)中两结点之间的最短路径。算法具体的过程包括：确定起点的最短路径问题，确定终点的最短路径问题，确定起点终点的最短路径问题，全局最短路径问题。用于解决最短径问题的算法被称做“最短路径算法”。

第二项为：从循环闭包候选者开始到完成探索所需遍历的边序列。

当机器人在探索一个图时，它可以找到几个候选的循环闭合边，并自动丢弃值高于当前计划值的所有候选项，然后将所有剩余的候选的循环闭合边作为闭合子计划p_end，将p_end代入计算下面的f_m(P)，并使用计算得到的成本值来比较包含候选者的探索计划与全局规划路径，以决定要遵循哪一个计划。

通过下面的公式计算成本，并使用计算得到的成本值来比较新计划(即新的包含候选者的探索计划)和当前计划(即全局规划)，选择成本低的计划，即下一步探索要遵循的路径。在不确定性无限的极端情况下，最好的计划将是允许以尽可能低的成本达到循环闭包的计划，而不考虑完成勘探任务的额外开销。

f_m(P)＝f(p_{loop-closoure})+λf(p_end)

其中，f_m(P)为成本，f(p_{loop-closoure})为从当前位置到第一个预测环路闭合边缘的边缘序列，(p_end)是完成探索的剩余部分，函数f(x)是指遵循计划x的标准成本，即其边缘成本之和，λ是调制因子，调制因子λ是一个反比标量项。

采用下面的两种成本函数中的一种计算误差，来保持拓扑图的初始形状和避免变形，这种方法不同于仅仅计算边缘成本的总和，它将考虑机器人的姿态信息。

第一种成本函数：当前边的长度L^[i,j](x)与拓扑图中的参考长度

之间的差异

第二种成本函数：当前图A^[i,j,k](x)和拓扑图

中两个边之间的角度差

为了确保机器人能够安全地穿越所有的边缘，还需要在每个边缘的最关键点上评估该成本函数。

使用拓扑图作为先验信息的主要困难在于，由于多种误差来源，它不能准确地表示环境。为了纠正这些错误，所以必须做一个额外的假设。真实环境(即机器人建图的环境)应该允许存在一个拓扑结构与先前图相似的连通图，即具有相同数量的顶点和边。连通性属性意味着机器人可以在每个顶点位置自由旋转，并且可以安全地遍历边，而不会发生碰撞。每当机器人检测到提供先验信息的拓扑图违反此连通性属性时，它必须执行图优化技术以强制执行观察到的环境的物理约束。

将优化后的图作为连通图，该连通图最大化了相对于先验和所探索的图的似然性。

本发明主要是建立了一个主动SLAM的框架，它利用一个图结构来提高探测时间和精度。当机器人在环境中移动时，为了最大限度地提高其姿态定位精度并触发主动环路闭合，这方面得到改进。提供的拓扑图中的错误可能导致探测效率低下，并可能将机器人导向不允许移动的区域。另外，通过基于最小二乘优化的在线算法来补偿最常见的误差源，使机器人能够重建更精确的图形。本发明的方法能够结合先验信息以映射环境，利用这些信息来追求主动循环闭合，自动检测和纠正所提供数据中的不一致。

本发明将3D导航和通过拓扑图提供先验信息的路径优化应用到机器人上以达到更好的精度要求。将优化后的图作为连通图，该连通图最大化了相对于先验和所探索的图的似然性。本发明的方法能够结合先验信息以映射环境，利用这些信息来追求主动循环闭合，自动检测和纠正所提供数据中的不一致。由于在一个局域网内，机器人可以连接到人机交互界面的主机，通过界面可以控制机器人建图、定位、停止建图，重新加载建好的地图，用户可以通过指定地图上的点，机器人可以自动进行路径规划，移动到目的地，用户还可以用手柄控制机器人。

一种机器人导航定位系统，包括：

计算模块，用于根据环境数据制定拓扑图，根据拓扑图计算全局规划路径；

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

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行上述的机器人导航定位方法。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (6)

1.一种机器人导航定位方法，其特征在于，包括以下步骤：

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

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

其中，采用图优化技术进行优化的具体过程为：进行自适应优化得到优化后的路径图，根据优化后的路径图，创建临时图，根据临时图得到循环闭包候选者，创建包含候选者的探索计划路径；比较探索计划路径与全局规划路径的成本，得到比较结果，将比较结果中成本小的计划路径作为下一步探索要遵循的路径，并判断是否符合连通性，若不符合，则重复采用图优化技术进行优化，直至符合连通性；所述进行自适应优化的具体过程为：计算误差，误差包括当前边的长度与拓扑图中的参考长度之间的差值以及当前图和拓扑图中两个边之间的角度差，假设误差为高斯分布，计算最大似然估计，根据最大似然估计得到优化后的路径图；

比较结果中成本小的计划路径包括最短路径子计划和闭合子计划；最短路径子计划是根据临时图采用标准最短路径技术计算得到；

成本通过下式计算：

f_m(P)＝f(p_{loop-closoure})+λf(p_end)

其中，f_m(P)为成本，f(p_{loop-closoure})为从当前位置到第一个预测环路闭合边缘的边缘序列，f(p_end)是完成探索的剩余部分，λ是调制因子，p_{loop-closoure}为最短路径子计划，p_end为闭合子计划。

2.根据权利要求1所述的一种机器人导航定位方法，其特征在于，根据机器人对全局规划路径进行探索后判断是否到达预期位置，如果到达预期位置，则返回原点，若没有达到预期位置，则以最接近原点的点为起点，返回到原点；全局规划路径是：遍历拓扑图或连通图的所有边。

3.根据权利要求1所述的一种机器人导航定位方法，其特征在于，通过下式判断机器人经过的路径形成的图形是否满足连通性，若一元因子

为0，则满足连通性；

其中，

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

4.根据权利要求1所述的一种机器人导航定位方法，其特征在于，通过非线性最小二乘估计方法计算最大似然估计。

5.一种机器人导航定位系统，其特征在于，包括：

计算模块，用于根据环境数据制定拓扑图，根据拓扑图计算全局规划路径；

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

其中，采用图优化技术进行优化的具体过程为：进行自适应优化得到优化后的路径图，根据优化后的路径图，创建临时图，根据临时图得到循环闭包候选者，创建包含候选者的探索计划路径；比较探索计划路径与全局规划路径的成本，得到比较结果，将比较结果中成本小的计划路径作为下一步探索要遵循的路径，并判断是否符合连通性，若不符合，则重复采用图优化技术进行优化，直至符合连通性；所述进行自适应优化的具体过程为：计算误差，误差包括当前边的长度与拓扑图中的参考长度之间的差值以及当前图和拓扑图中两个边之间的角度差，假设误差为高斯分布，计算最大似然估计，根据最大似然估计得到优化后的路径图；

比较结果中成本小的计划路径包括最短路径子计划和闭合子计划；最短路径子计划是根据临时图采用标准最短路径技术计算得到；

成本通过下式计算：

f_m(P)＝f(p_{loop-closoure})+λf(p_end)

其中，f_m(P)为成本，f(p_{loop-closoure})为从当前位置到第一个预测环路闭合边缘的边缘序列，f(p_end)是完成探索的剩余部分，λ是调制因子，p_{loop-closoure}为最短路径子计划，p_end为闭合子计划。

6.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如权利要求1至4任一项所述的机器人导航定位方法。

Images