基于多分辨率拓扑地图的路径搜索方法及系统

摘要

本发明提供了一种基于多分辨率拓扑地图的路径搜索方法及系统，属于路径导航技术领域。构建分辨率为半个机身栅格的低分辨率二值化栅格地图；根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展；本发明极大的提高了路径搜索效率和精度。

权利要求

1.一种基于多分辨率拓扑地图的路径搜索方法，其特征在于：包括以下过程：

根据离线地图构建分辨率为机器人内切半径的二值化栅格地图；

根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；

判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；

按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；

采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建高分辨率栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展；

将多边形的所有边向量按顺序依次两两叉积后，再将各个叉积相乘，最终结果小于零时为凹边形；

按照凹点位置将凹多边形进行切割，包括：

约定组成凹多边形的点按照顺时针排列，如果两条相邻边向量的叉积大于0，则判断这两条边的连接点为凹点，将这两条边定义为异号边，将任意一条异号边延长至与凹多边形的另一条边相交为止；依次对所有凹点执行上述操作，直至凹多边形全部被分割为凸多边形；

在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图，包括：已知各节点区域信息，将障碍物节点区域内的障碍物提取出来，结合节点区域信息，建立栅格地图信息，将栅格地图信息设置为栅格未在区域内、边界点和内部点，路径搜索在内部点进行完成。

2.如权利要求1所述的基于多分辨率拓扑地图的路径搜索方法，其特征在于：按照凸多边形的分布，采用cell decomposition算法在整个离线地图的区域分割出无障碍物节点。

3.如权利要求1所述的基于多分辨率拓扑地图的路径搜索方法，其特征在于：搜索终止条件是扩展至另一个节点的边界点，或者本节点全部扩展结束。

4.一种基于多分辨率拓扑地图的路径搜索系统，其特征在于：包括：

二值化栅格地图构建模块，被配置为：根据离线地图构建分辨率为机器人内切半径的二值化栅格地图；

多边形构建模块，被配置为：根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；

凸多边形构建模块，被配置为：判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；

拓扑地图生成模块，被配置为：按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；

路径搜索模块，被配置为：采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建高分辨率栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展；

将多边形的所有边向量按顺序依次两两叉积后，再将各个叉积相乘，最终结果小于零时为凹边形；

凸多边形构建模块中，按照凹点位置将凹多边形进行切割，包括：约定组成凹多边形的点按照顺时针排列，如果两条相邻边向量的叉积大于0，则判断这两条边的连接点为凹点，将这两条边定义为异号边，将任意一条异号边延长至与凹多边形的另一条边相交为止；依次对所有凹点执行上述操作，直至凹多边形全部被分割为凸多边形；

路径搜索模块中，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图，包括：已知各节点区域信息，将障碍物节点区域内的障碍物提取出来，结合节点区域信息，建立栅格地图信息，将栅格地图信息设置为栅格未在区域内、边界点和内部点，路径搜索在内部点进行完成。

5.如权利要求4所述的基于多分辨率拓扑地图的路径搜索系统，其特征在于：路径搜索模块中，在有障碍物节点进行路径搜索时，搜索终止条件是扩展至另一个节点的边界点，或者本节点全部扩展结束。

说明书

基于多分辨率拓扑地图的路径搜索方法及系统

技术领域

[0001] 本发明涉及路径导航技术领域，特别涉及一种基于多分辨率拓扑地图的路径搜索方法及系统。

背景技术

[0002] 本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。

[0003] 未来实现机器人的自主工作，机器人导航是个不可或缺的部分，通常移动机器人的导航问题可以分为三个部分：（1）机器人理解周围环境；（2）机器人实时定位；（3）机器人运动规划。现阶段的机器人运动规划大部分使用的地图是栅格地图和拓扑地图。

[0004] 发明人发现，栅格地图中分辨率越高对实际环境的描述就越详细，精度就会越高，但是搜索路径时将花费更多的时间，若分辨率越低，虽然能提高路径搜索能力，但是会造成障碍物描述不完整导致路径搜索失败；拓扑地图是以图模型来表示环境，节点表示为特定的地点或者地标，节点之间若有边界相邻则以边直接进行连接，在拓扑地图进行路径搜索能提升计算效率，但是拓扑地图对于某个特定区域的描述是不够详细的。

发明内容

[0005] 为了解决现有技术的不足，本发明提供了一种基于多分辨率拓扑地图的路径搜索方法及系统，克服了在工厂等大环境下、高分辨率栅格地图中进行启发式全局路径搜索容易出现的A*导航时间占时较长的问题，极大的提高了路径搜索效率和精度。

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

[0007] 本发明第一方面提供了一种基于多分辨率拓扑地图的路径搜索方法。

[0008] 一种基于多分辨率拓扑地图的路径搜索方法，包括以下过程：

[0009] 根据离线地图构建分辨率为机器人内切半径的二值化栅格地图；

[0010] 根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；

[0011] 判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；

[0012] 按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；

[0013] 采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建高分辨率栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展。

[0014] 作为本发明第一方面可选的一种实现方式，将多边形的所有边向量按顺序依次两两叉积后，再将各个叉积相乘，最终结果小于零时为凹边形。

[0015] 作为本发明第一方面可选的一种实现方式，按照凹点位置将凹多边形进行切割，包括：

[0016] 约定组成凹多边形的点按照顺时针排列，如果两条相邻边向量的叉积大于0，则判断这两条边的连接点为凹点，将这两条边定义为异号边，将任意一条异号边延长至与凹多边形的另一条边相交为止；依次对所有凹点执行上述操作，直至凹多边形全部被分割为凸多边形。

[0017] 作为本发明第一方面可选的一种实现方式，按照凸多边形的分布，采用cell decomposition算法在整个离线地图的区域分割出无障碍物节点。

[0018] 作为本发明第一方面可选的一种实现方式，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图，包括：

[0019] 已知各节点区域信息，将障碍物节点区域内的障碍物提取出来，结合节点区域信息，建立栅格地图信息，将栅格地图信息设置为栅格未在区域内、边界点和内部点，路径搜索在内部点进行完成。

[0020] 作为本发明第一方面进一步的限定，搜索终止条件是扩展至另一个节点的边界点，或者本节点全部扩展结束。

[0021] 本发明第二方面提供了一种基于多分辨率拓扑地图的路径搜索系统。

[0022] 一种基于多分辨率拓扑地图的路径搜索系统，包括：

[0023] 二值化栅格地图构建模块，被配置为：根据离线地图构建分辨率为机器人内切半径的二值化栅格地图；

[0024] 多边形构建模块，被配置为：根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；

[0025] 凸多边形构建模块，被配置为：判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；

[0026] 拓扑地图生成模块，被配置为：按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；

[0027] 路径搜索模块，被配置为：采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建高分辨率栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展。

[0028] 作为本发明第二方面可选的一种实现方式，凸多边形构建模块中，按照凹点位置将凹多边形进行切割，包括：

[0029] 约定组成凹多边形的点按照顺时针排列，如果两条相邻边向量的叉积大于0，则判断这两条边的连接点为凹点，将这两条边定义为异号边，将任意一条异号边延长至与凹多边形的另一条边相交为止；依次对所有凹点执行上述操作，直至凹多边形全部被分割为凸多边形。

[0030] 作为本发明第二方面可选的一种实现方式，路径搜索模块中，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图，包括：

[0031] 已知各节点区域信息，将障碍物节点区域内的障碍物提取出来，结合节点区域信息，建立栅格地图信息，将栅格地图信息设置为栅格未在区域内、边界点和内部点，路径搜索在内部点进行完成。

[0032] 作为本发明第二方面进一步的限定，路径搜索模块中，在有障碍物节点进行路径搜索时，搜索终止条件是扩展至另一个节点的边界点，或者本节点全部扩展结束。

[0033] 与现有技术相比，本发明的有益效果是：

[0034] 1、本发明所述的基于多分辨率拓扑地图的路径搜索方法及系统，克服了在工厂等大环境下、高分辨率栅格地图中进行启发式全局路径搜索容易出现的A*导航时间占时较长的问题，极大的提高了路径搜索效率和精度。

[0035] 2、本发明所述的基于多分辨率拓扑地图的路径搜索方法及系统，提高了在障碍物分布不均匀的环境下的全局路径搜索效率，引入有障碍节点信息，避免拓扑地图在某种特定区域的描述不够详细的问题。

[0036] 3、本发明所述的基于多分辨率拓扑地图的路径搜索方法及系统，通过判断是否为凸多边形来设置有障碍物节点，能够更好的进行非障碍物节点区域和有障碍物节点区域的识别，进一步的提高了路径规划的效率。

[0037] 本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

[0038] 构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

[0039] 图1为本发明实施例1提供的拓扑地图生成方法的流程示意图；

[0040] 图2为本发明实施例1提供的凹多边形分割示意图；

[0041] 图3为本发明实施例1提供的拓扑地图构建示意图，其中， 为凹多边形的依次连接的边向量；

[0042] 图4为本发明实施例1提供的有障碍物节点和无障碍物节点的关系示意图一；

[0043] 图5为本发明实施例1提供的有障碍物节点和无障碍物节点的关系示意图二，其中，A代表在有障碍物节点之间，判断两节点之间的连线是否有可通行的位置，如有可通行的位置，则将两节点之间的边设置为可通行的位置（可存在多个可通行位置）；B代表无障碍物节点之间的边无需判断，直接设置边为两个节点连线的中点；C代表有障碍物节点之间，判断两节点之间的连线是否有可通行的位置，如没有可通行的位置，则两节点之间无边；

[0044] 图6为本发明实施例1提供的栅格地图构建示意图；

[0045] 图7为本发明实施例1提供的路径搜索方法的流程示意图；

[0046] 图8为本发明实施例1提供的路径搜索过程的流程示意图；

[0047] 图9为本发明实施例1提供的栅格地图与世界坐标的转换示意图。

具体实施方式

[0048] 实施例1：

[0049] 本发明实施例1提供了一种基于多分辨率拓扑地图的路径搜索方法，包括以下过程：

[0050] 根据离线地图构建分辨率为半个机身栅格的二值化栅格地图；

[0051] 根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；

[0052] 判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；

[0053] 按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；

[0054] 采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展。

[0055] 具体的，包括以下内容：

[0056] S1：构建低分辨率二值化栅格地图

[0057] 根据凸多边形中任意两点的连线仍然在图形内部的特性，将拓扑地图中的全部节点均设计为凸多边形，以便后续进行路径规划，如图1所示。

[0058] S1.1：加载工厂中的离线地图，根据地图的大小构建以机器人内切半径为分辨率的低分辨率二值化栅格地图；

[0059] S1.2：提取障碍物的外轮廓信息，并将轮廓点按顺时针连接构成多边形；

[0060] S1.3：判断多边形是否为凸多边形，如果是，将该多边形各顶点的最小x和最小y坐标以及轮廓点的信息放在有障碍节点中，如果不是，按照凹点位置将凹多边形进行切割，直到区域中不存在凹多边形；

[0061] （1）凹多边形的判断

[0062] 将所有的相邻边向量进行两两叉积，再将所有叉积相乘后，如果小于零，则为凹多边形，如图2所示：

[0063] 当 时，为凹多边形。

[0064] （2）凹点的选择

[0065] 约定组成该凹多边形的点是按照顺时针排列的，所以如果边向量叉积中出现大于0的情况，则可以判断这两条边是凹进去的位置，例如 和 向量的叉积大于0，则凹点出现在 和 向量的连接点位置。

[0066] （3）凹多边形的分割

[0067] 以 和 向量对应的边为“异号边”，将任一条“异号边”延长，延长至跟多边形的一条边相交为止，完成分割。

[0068] S2：拓扑地图信息生成以及路径搜索策略

[0069] 按照有障碍物凸多边形的分布，结合cell decomposition算法将区域分割无障碍物节点，并形成拓扑地图信息（记录节点之间连接的中点，作为计算代价函数的目标位置），进入路径搜索。

[0070] 其中，拓扑地图的构造方式为：利用cell decomposition进行竖直或者水平方向切割，如图3、图4和图5所示。

[0071] 进入A*路径搜索，在有障碍物节点进行路径搜索时，按照节点存放的offset等信息构建栅格地图，在无障碍物节点进行路径搜索时，直接拓扑地图周围节点进行扩展（节点之间代价为上一个节点到这个节点连线中点的欧式距离）。

[0072] 本实施例中，多目标点A*搜索的逻辑为：首先是将周围可扩展节点的边（两节点之间连线的中点）作为目标点集，选择目标点集中距离当前点欧式距离最短的作为启发方向，从当前点开始进行路径搜索，当路径搜索过程中，扩展到了两节点之间连线上的N点时，此时删除目标点集中属于该两节点边界连线上的栅格，并修改两节点之间边关系为N点，继续进行搜索，直到所有可扩展节点和当前点具备路径或者有障碍物节点内全部栅格点被访问，并选择不出路径为止。

[0073] 栅格地图的构建，如图6所示，包括：

[0074] 根据栅格地图的构建方式，已知当前坐标在地图中的节点信息，将障碍物节点区域内的障碍物提取出来，并结合区域信息，建立栅格地图信息，将栅格地图信息设置为超出节点栅格out_area_grid、边界栅格boundary_grid、障碍物栅格obstacle_grid_cost以及节点内部栅格inner_grid，路径搜索在节点内部栅格inner_grid进行完成，搜索终止条件是扩展至另一个节点的边界点或者本节点全部扩展结束。

[0075] 进行路径搜索，如图7和图8所示，包括：

[0076] 步骤（1）：输入构建的拓扑地图G，起点S，终点T；

[0077] 步骤（2）：将起点S对应的坐标、在拓扑地图中的节点性质、代价值放在开集Openlist中；

[0078] 步骤（3）：判断开集Openlist是否为空，如果为空，则直接输出路径搜索结果，不为空执行步骤（4）；

[0079] 步骤（4）：取出Openlist中代价最小的点作为当前点current_point，判断当前点current_point是否和终点T在拓扑地图中的同一个节点，如果是执行步骤（5），如果不是执行步骤（6）；

[0080] 步骤（5）：判断终点T所在节点是否为有障碍物节点，如果是，则按照A*的算法进行路径搜索，直到搜索到终点为止；如果不是，直接返回路径搜索成功；

[0081] 步骤（6）：判断当前点current_point所在节点是否为有障碍物节点，如果是，则执行步骤（7），如果不是则执行步骤（8）；

[0082] 步骤（7）：根据拓扑地图对应的该有障碍物节点信息，构建分辨率为0.05m的高分辨率的栅格地图，并将周围可扩展节点（扩展节点可以理解是当前节点在拓扑地图中可建立边关系的节点）边上的多个目标点（多个目标点是周围可扩展节点与当前节点之间的边，即节点边界的中间点）作为扩展目标，利用多目标点A*进行路径搜索，终止条件为：

[0083] 1）多目标点路径搜索时，周围可扩展节点的边全部打通（具有路径通过）；

[0084] 2）该节点中全部栅格点被访问结束未选择到边界；

[0085] 更新节点间代价为A*路径搜索的路径长度，根据有障碍物节点中路径搜索的结果，修改拓扑地图中节点边的关系，修改为真正有路径通过的坐标点，将周围可扩展节点对应的目标点及节点性质放在开集Openlist中；

[0086] 步骤（8）：无障碍物节点则直接在拓扑地图中进行路径搜索，更新扩展节点坐标启发值为节点当前点current_point与可扩展节点边之间的欧式距离，将周围可扩展节点对应的目标点及节点性质放在开集Openlist中；

[0087] 步骤（9）：继续进入步骤（3）。

[0088] 本实施例中，栅格坐标系与世界坐标系的关系，如图9所示，具体的，包括：

[0089] 分辨率：resolution；

[0090] 偏移坐标：offset_x，offset_y；

[0091] 地图大小：宽度width，高度height；

[0092] 世界坐标pos_x，pos_y转向栅格地图坐标grid_x，grid_y，包括：

[0093] grid_x =(pos_x‑offsetpos_x)/resolution；

[0094] grid y=height‑pos_y*offsetpos_y/resolution；

[0095] 栅格地图坐标grid_x，grid_y转向世界坐标pos_x，pos_y，包括：

[0096] pos_x =offsetpos_x + grid_x * resolution；

[0097] pos_y =offsetpos_y+(height‑grid_y)*resolution。

[0098] 实施例2：

[0099] 本发明实施例2提供了一种基于多分辨率拓扑地图的路径搜索系统，包括：

[0100] 二值化栅格地图构建模块，被配置为：根据离线地图构建分辨率为机器人内切半径的二值化栅格地图；

[0101] 多边形构建模块，被配置为：根据得到的二值化栅格地图，提取障碍物的外轮廓点，将轮廓点按顺时针连接构成多边形；

[0102] 凸多边形构建模块，被配置为：判断多边形是否为凸多边形，如是，将多边形设置为有障碍物节点；否则，按照凹点位置将凹多边形进行切割，直到全部为凸多边形；

[0103] 拓扑地图生成模块，被配置为：按照凸多边形的分布，在整个离线地图的区域分割出无障碍物节点，形成拓扑地图；

[0104] 路径搜索模块，被配置为：采用A*算法进行路径搜索，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建高分辨率栅格地图；在无障碍物节点进行路径搜索时，直接将拓扑地图的周围节点进行扩展。

[0105] 具体的，凸多边形构建模块中，按照凹点位置将凹多边形进行切割，包括：

[0106] 约定组成凹多边形的点按照顺时针排列，如果两条相邻边向量的叉积大于0，则判断这两条边的连接点为凹点，将这两条边定义为异号边，将任意一条异号边延长至与凹多边形的另一条边相交为止；依次对所有凹点执行上述操作，直至凹多边形全部被分割为凸多边形。

[0107] 具体的，路径搜索模块中，在有障碍物节点进行路径搜索时，按照有障碍物节点的信息构建栅格地图，包括：

[0108] 已知各节点区域信息，将障碍物节点区域内的障碍物提取出来，结合节点区域信息，建立栅格地图信息，将栅格地图信息设置为栅格未在区域内、边界点和内部点，路径搜索在内部点进行完成；

[0109] 在有障碍物节点进行路径搜索时，搜索终止条件是扩展至另一个节点的边界点，或者本节点全部扩展结束。

[0110] 具体的，所述系统的详细工作方法与实施例1中提供的基于多分辨率拓扑地图的路径搜索方法相同，这里不再赘述。