一种结合栅格和拓扑地图的快速路径搜索方法与流程

1.本发明属于自动化技术领域，特别涉及地面移动机器人路径搜索方法。


背景技术：

2.随着相关产业智能化需求的不断增多，地面移动机器人被广泛应用于智能园区物流、智能矿山、建筑勘测、应急搜救等领域，因此自主导航能力是机器人技术的研究热点之一。其中，路径搜索是自主导航技术的核心，用于生成可通行路径，保障机器人在环境中安全快速地移动，以完成给定的具体任务。根据现有研究，移动机器人路径搜索技术主要在以下三个方面存在困难：(i)如何对环境进行建模，(ii)如何减小采样空间，(iii)如何提高搜索算法的收敛效率。
3.目前，已经在仿真实验和真实机器人上成功实现了一些可行的方法，其中基于贪婪搜索和快速探索随机树(rapid-exploration random tree，rrt)的方法已经有了较好的效果。经典的贪婪搜索算法，例如a*、d*，其思想是从起点出发，迭代地遍历并生长可通行像素点，直到找到目标点，该类算法实现简单，但效率极低，难以应对类似迷宫的环境，收敛速度慢。基于rrt的方法相当于加大了a*算法的步长，其核心思想是从起点出发，在迭代地生长一个随机树，直到随机树找到目标点，该类算法效率较高，但其随机性导致最终的路径可能不是最优路径，需要更长时间的优化来使得路径收敛。


技术实现要素：

4.本发明所要解决的问题是，如何结合栅格地图和拓扑地图，设计和实现快速的地面移动机器人路径搜索算法。本发明的目的是改进现有贪婪搜索算法和图搜索算法并利用其特点，实现大尺度环境下的实时路径搜索。
5.本发明采用的技术方案为：
6.一种结合栅格和拓扑地图的快速路径搜索方法，适用于地面移动机器人实时自主导航任务，包括以下步骤：
7.步骤1，基于改进的k3m算法，从栅格地图m中提取简化广义voronoi图g＝{e，v，m
dist
}作为拓扑地图，其中e为边，v为节点，m
dist
为图的距离矩阵；
8.步骤2，利用栅格地图m，在步骤1中得到的拓扑地图中，搜索候选起始边pse和候选目标边pte；
9.步骤3，以步骤2中找到的候选起始边pse和候选目标边pte为启发，查找候选起始路径ta→g和候选目标路径tg→b；对于其中的每一对候选起始路径和候选目标路径在拓扑地图中查找连接和的子图生成所有候选路径ta→b，取其中长度最短的路径作为结果路径。
10.进一步的，步骤2具体包括以下步骤：
11.步骤2.1，从栅格地图m中提取障碍物像素点集合o，如公式1所示：
[0012][0013]
式中，width为机器人运动模型的宽度，x是栅格地图m中的任一像素点，xi是栅格地图m中的障碍物像素点；
[0014]
步骤2.2，在拓扑地图中，找到所有候选起始边pse和候选目标边pte，如公式2和公式3所示：
[0015]
pse＝(ea，xa)＝{(e∈e，x∈e)|vt(la，e)≠none，x＝vt(la，e)}
ꢀꢀꢀ
(2)
[0016]
pte＝(eb，xb)＝{(e∈e，x∈e)|vt(lb，e)≠none，x＝vt(lb，e)}
ꢀꢀꢀ
(3)
[0017]
其中，ea和eb分别表示候选起始边和候选目标边的边集合，xa和xb表示对应的途经点的集合，起点la从一个途经点可以到达对应的候选起始边lb从一个途径点可以到达对应的候选目标边vt(la，e)和vt(lb，e)分别为起点la和终点lb与边e之间的可视性检测函数，其函数原型vt(xm，e)为像素点xm与边e之间的可视性检测函数，见公式4：
[0018][0019]
其中，函数的返回值为边e上距离xm最近且与xm可视的像素点，vt(xm，xn)为两个像素点xm和xn之间的可视性检测函数，见公式5：
[0020][0021]
其中，表示像素点xm和xn之间的线段。
[0022]
进一步的，步骤3具体包括以下步骤：
[0023]
步骤3.1，对于所有候选起始边pse，结合栅格地图m和可视性检测函数，找到所有候选起始路径ta→g，如公式6所示：
[0024][0025]
式中，是第t个候选起始边，是对应的途经点，和分别表示的两个节点，表示线段指示的路径，表示线段指示的路径，表示线段指示的路径；表示沿边e的，从到边的第1个节点的路径段，是边的
一部分；表示沿边e的，从到边的第2个节点的路径段，是边的一部分；
[0026]
步骤3.2，对于所有候选目标边pte，结合栅格地图m和可视性检测函数，找到所有的候选目标路径tg→b，如公式7所示：
[0027][0028]
式中，是第j个候选目标边，是对应的途经点，和分别表示的两个节点，表示线段指示的路径，表示线段指示的路径，表示线段指示的路径；表示沿边的，从边的第1个节点到的路径段，是边的一部分；表示沿边的，从边的第2个节点到的路径段，是边的一部分；
[0029]
步骤3.3，找到所有的常规候选路径如公式8所示：
[0030][0031]
式中，是拓扑地图g中连接节点和的子图，可用dijkstra算法查找；
[0032]
步骤3.4，在步骤3.3中的常规候选路径的基础上，还需要考虑两种情况：(i)la和lb可视，即vt(la，lb)＝1；(ii)候选起始边pse的边集合ea与候选目标边pte的边集合eb存在交集，即因此所有的候选路径ta→b包括如公式9所示的三种情况：
[0033][0034]
步骤3.5，选取候选路径ta→b中长度最短的一条路径中长度最短的一条路径作为最终的路径搜索结果，其中length(ti)代表路径ti的长度，使用欧氏距离来计算。
[0035]
本发明相比现有技术具有如下优点：
[0036]
本发明大幅度减少了在栅格地图中进行像素连通性遍历的计算，同时避免了传统
的基于快速探索随机树算法带来的随机性，显著提高了算法的收敛速度，极大程度地降低了传统方法的空间复杂度和时间复杂度，使得在大尺度场景下进行实时路径搜索成为可能，可以满足地面移动机器人自主导航的功能性、实时性和完整性需求。
附图说明
[0037]
图1为本发明实施例中的栅格地图m和拓扑地图g示意图。
[0038]
图2为本发明实施例中的候选起始边pse和候选目标边pte示意图。
[0039]
图3为本发明实施例中的候选起始路径和候选目标路径示意图。
[0040]
图4为本发明实施例中的最终路径搜索结果ta→b示意图。
具体实施方式
[0041]
为详细说明本发明的技术内容、特点和实现效果，以下结合具体实施方式并配合附图详细予以说明。
[0042]
本发明提供了一种结合栅格和拓扑地图的快速路径搜索方法，从环境的栅格地图中生成拓扑地图，结合两种地图各自的优势来设计和实现移动机器人路径搜索方法。拓扑地图能够在大尺度上表达整个环境的连通性和可达性，在大范围路径搜索上具有较大优势，避免了在整个栅格地图上进行像素遍历带来的巨大计算负担。栅格地图用于在小尺度上补足拓扑地图在路径细节上的缺失，由于只需要在栅格地图的较小范围内进行像素遍历。本发明提出的算法基于栅格和拓扑地图结合的思想，大幅度减少了在栅格地图中进行像素连通性遍历的计算，同时避免了传统的基于快速探索随机树(rapid-exploration random tree，rrt)算法带来的随机性，显著提高了算法的收敛速度，极大程度地降低了传统方法的空间复杂度和时间复杂度，使得在大尺度场景下进行实时路径搜索成为可能，可以满足地面移动机器人自主导航的功能性、实时性和完整性需求。
[0043]
下面对本发明步骤做进一步详细介绍：
[0044]
步骤1、基于改进的k3m算法，从栅格地图m中提取简化广义voronoi图g＝{e，v，m
dtst
}作为拓扑地图，其中e为边，v为节点，m
dist
为图的距离矩阵，如附图1所示，拓扑地图示例，白色区域为可通行区域，黑色区域为障碍物，灰色区域为未知区域，实线为拓扑地图的边，方块为拓扑地图的节点。具体算法参考专利[一种基于简化广义voronoi图的机器人自主探索方法:cn110703747a]。
[0045]
步骤2，利用栅格地图m，在步骤1中得到的拓扑地图中，搜索候选起始边和候选目标边结果如附图2所示，候选起始边和候选目标边示例。起点为la，终点为lb。为候选起始,，为候选目标边。包括以下子步骤：
[0046]
步骤2.1，从栅格地图m中提取障碍物像素点集合o，如公式1所示：
[0047][0048]
式中，width为机器人运动模型的宽度，x是栅格地图m中的任一像素点，xi是栅格地图m中的障碍物像素点；
[0049]
步骤2.2，在拓扑地图中，找到所有候选起始边pse和候选目标边pte，如公式2和公式3所示：
[0050]
pse＝(ea，xa)＝{(e∈e，x∈e)|vt(la，e)≠none，x＝vt(la，e)}
ꢀꢀꢀ
(2)
[0051]
pte＝(eb，xb)＝{(e∈e，x∈e)|vt(lb，e)≠none，x＝vt(lb，e)}
ꢀꢀꢀ
(3)
[0052]
其中，ea和eb分别表示候选起始边和候选目标边的边集合，xa和xb表示对应的途经点的集合，起点la从一个途经点可以到达对应的候选起始边lb从一个途径点可以到达对应的候选目标边例如在附图3中，la途径点到达边边lb途径到达边vt(la，e)和vt(lb，e)分别为起点la和终点lb与边e之间的可视性检测函数，其函数原型vt(xm，e)为像素点xm与边e之间的可视性检测函数，见公式4：
[0053][0054]
其中，函数的返回值为边e上距离xm最近且与xm可视的像素点，vt(xm，xn)为两个像素点xm和xn之间的可视性检测函数，见公式5：
[0055][0056]
其中，表示像素点xm和xn之间的线段。
[0057]
步骤3，以步骤2中找到的候选起始边pse和候选目标边pte为启发，查找候选起始路径ta→g和候选目标路径tg→b；对于其中的每一对候选起始路径和候选目标路径在拓扑地图中查找连接和的子图生成所有候选路径ta→b，取其中长度最短的路径作为结果路径。包括以下几个子步骤：
[0058]
步骤3.1，对于所有候选起始边pse，结合栅格地图m和可视性检测函数，找到所有候选起始路径ta→g，如公式6所示：
[0059][0060]
式中，是第i个候选起始边，是对应的途经点，和分别表示的两个节点，表示线段指示的路径，表示线段指示的路径，
表示线段指示的路径；指示的路径；表示沿边e的，从到边的第1个节点的路径段，是边的一部分；表示沿边e的，从到边的第2个节点的路径段，是边的一部分；结果见附图3，候选起始路径ta→
ragvg
(左侧虚线)和候选目标路径t
ragvg
→b(右侧虚线)。
[0061]
步骤3.2，对于所有候选目标边pte，结合栅格地图m和可视性检测函数，找到所有的候选目标路径tg→b，如公式7所示：
[0062][0063]
式中，是第j个候选目标边，是对应的途经点，和和分别表示的两个节点，表示线段指示的路径，表示线段指示的路径，表示线段指示的路径；表示沿边的，从边的第1个节点到的路径段，是边的一部分；表示沿边的，从边的第2个节点到的路径段，是边的一部分；结果见附图3。
[0064]
步骤3.3，找到所有的常规候选路径如公式8所示：
[0065][0066]
式中，是拓扑地图g中连接节点和的子图，可用dijkstra算法查找；
[0067]
步骤3.4，在步骤3.3中的常规候选路径的基础上，还需要考虑两种情况：(i)la和lb可视，即vt(la，lb)＝1；(ii)候选起始边pse的边集合ea与候选目标边pte的边集合eb存在交集，即因此所有的候选路径ta→b包括如公式9所示的三种情况：
[0068][0069]
步骤3.5，选取候选路径ta→b中长度最短的一条路径
作为最终的路径搜索结果，其中length(ti)代表路径ti的长度，使用欧氏距离来计算。如附图4所示，从左到右连接la与lb的虚线为最终的路径搜索结果。
[0070]
以上所述仅为本发明中的一个实例，并不用于限制本发明。凡在本发明的精神与原则之内，所做的任何修改、改进等，均应包含在本发明的保护范围之内。