一种基于双层图的大尺度已知环境地图创建方法与流程

本发明涉及环境地图创建方法，具体涉及一种基于双层图的大尺度已知环境地图创建方法。

背景技术：

目前，环境地图是移动机器人在当前环境下实现自主导航的关键部分，机器人地图创建问题，在结构化、静态、小尺度环境下已经取得很多研究成果，但对于大尺度环境下的研究还不够成熟，在移动机器人领域最常见的地图是栅格地图，但是当栅格地图的尺寸和分辨率提高时，将会大大降低路径搜索的效率，传统的基于切线图和voronoi图轨迹规划方法虽然能够解决这些问题，但是传统方法对障碍物的边缘和顶点比较敏感，当障碍物的边缘是曲线或者无顶点时，切线图和voronoi图方法均难以应用，并且传统方法生成的小车轨迹质量不高等诸多问题。

技术实现要素：

本发明的主要目的在于提供一种基于双层图的大尺度已知环境地图创建方法。

本发明采用的技术方案是：一种基于双层图的大尺度已知环境地图创建方法，包括以下步骤：

s1，使用九宫格方法对平面栅格地图进行分割；

s2，构建第一层图模型；

s3，构建第二层图模型。

进一步地，所述步骤s1包括：

s11，读取平面栅格地图，其中黑色部分表示经过膨胀的障碍物，白色部分表示移动小车的可行通道；

s12，将步骤s11中的地图划分为9个大小相同的方块；

s13，对方块4和方块5进一步分割；

s14，重复步骤s13，一直分割到地图中的格子分为两类，第一类是完全不包含障碍物的格子，第二类是完全包含障碍物的格子；

s15，对格子的面积设置一个最小阈值，当分割的格子面积小于这个阈值时则不再分割，具体阈值设置为多少需要根据地图中障碍物的复杂度和需要的精度进行设置；

s16，步骤s15中设置阈值的方法会产生大量的第三类格子，这类格子只包含部分障碍物，将这类格子当作第二类格子。

更进一步地，所述步骤s2包括：

s21，创建第一层节点；

s22，创建第一层边。

更进一步地，所述步骤s21包括：

s211，创建存储所有节点的空结构体nodes；

s212，取出其中一个第一类格子，创建该第一类格子对应的新节点；

s213，设置第一类格子的节点id；

s214，计算第一类格子的中心坐标并存储在当前节点中；

s215，计算第一类格子的长和宽并存储在当前节点中；

s216，计算左下角坐标、右上角坐标等，存储到当前节点中；

s217，将该节点放入存储所有节点的结构体nodes中；

s218，重复以上步骤s212到步骤s217，直到所有第一类格子都转化为节点。

更进一步地，所述步骤s22包括：

遍历每个节点，并且对节点进行一个像素的膨胀，如果有其它节点与之重叠则存在相邻关系，具体步骤为：

s221，创建一个二维的结构体edges，第一维表示当前节点的id，第二维表示当前节点的相邻节点；

s222，在第一层节点集中取出一个未遍历过的节点node；

s223，将节点node的四条边进行膨胀，得到膨胀后的边缘；

s224，判断膨胀后边缘与哪些节点有重叠关系；

s225，遍历每一个有重叠关系的节点，计算当前节点和相邻节点中心的欧式距离；

s226，根据当前节点的索引id、相邻节点的索引id和欧式距离等信息创建一个新的边；

s227，将新的边加入到结构体edges中；

s228，重复上述步骤5到步骤7，直到当前节点与所有相邻节点的边相邻关系都加入到结构体edges中；

s229，重复上述步骤s222到步骤s228，直到第一层中的所有节点都已经遍历过为止。

更进一步地，所述步骤s3包括：

s31，创建第二层节点；

s32，创建第二层边。

更进一步地，所述步骤s31包括：

重复使用第一层中的节点即可，创建第二层中第二类节点；将未被包含的第一层节点进行最大膨胀得到所有可能存在的凸安全节点；

创建凸安全节点的具体步骤为：

s311，创建第二层图模型中凸安全节点的结构体；

s312，从第一层图模型中取出一个未遍历过并且未被包含的节点；

s313，将该节点的边缘进行膨胀，当某一个方向碰到障碍物时则立即停止该方向的膨胀，并且继续其余方向的膨胀，直到所有方向都碰到障碍时，得到一个凸安全节点的形状和大小；

s314，判断当前凸安全节点都包含了第一层图模型中的哪些节点，并且将这些节点标记为已遍历过的节点，下次进行步骤s312时不再遍历；

s315，设置该凸安全节点的索引id，计算左下角坐标和右上角坐标；

s316，将步骤s315中的信息和步骤s314中包含的第一层节点id都保存到当前凸安全节点中，并且将该凸安全节点插入到第二层图模型中；

s317，重复上述步骤s312到步骤s316，直到第一层图模型中的所有节点都被包含为止，此时地图中所有不重复的凸安全节点均以创建完成。

更进一步地，所述步骤s32包括：

遍历第一层节点加入对应凸安全节点的id，具体步骤如下：

s321，遍历所有凸安全节点，取出其中一个凸安全节点node_expand；

s322，遍历当前凸安全节点包含的所有第一层节点，并且取出其中一个节点node；

s323，将当前凸安全节点node_expand的id信息保存到当前节点node中；

重复步骤s321到步骤s323。

本发明的优点：

本发明针对大尺度已知环境下的轨迹规划问题，提前构建地图中的凸安全节点，从而构建双层图模型，避免了在实时规划过程中重复生成凸安全区域造成的效率降低问题，进一步提升了大尺度已知环境下轨迹规划的速度，无需在实时的规划中重新生成凸安全节点，减少了机器人在实时规划中的计算量。与传统的基于切线图和voronoi图轨迹规划方法相比，本专利的方法在第二层图模型中凸安全节点上生成轨迹的质量更高。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

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

图1是本发明的平面栅格地图示意图；

图2是本发明的第一次九宫格划分图；

图3是本发明的第二次九宫格划分图；

图4是本发明的九宫格分割效果图；

图5是本发明的节点膨胀过程示意图（a为节点膨胀方向图，b为当某一方向碰障碍物时的图）；

图6是本发明的简单的平面栅格地图；

图7是本发明的九宫格分割图；

图8是本发明的第一层图模型图；

图9是本发明的凸安全节点示意图；

图10是本发明的第二层图模型图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

由于本发明针对的是已知环境下的小车轨迹规划问题，假设已经通过激光slam扫描并建立了小车移动环境中完整的平面栅格地图。为了避免小车边缘与障碍物边缘发生碰撞，根据小车的最大半径预先对障碍物的边缘进行相应的膨胀，从而将小车简化为一个质点，因此，只需要考虑该质点在膨胀后地图中的移动即可避免小车的边缘与障碍物的边缘碰撞，假设下面使用的栅格地图均为经过障碍物膨胀处理的栅格地图。

参考图1至图10，如图1至图10所示，一种基于双层图的大尺度已知环境地图创建方法，包括以下步骤：

s1，使用九宫格方法对平面栅格地图进行分割；

s2，构建第一层图模型；

s3，构建第二层图模型。

本发明针对大尺度已知环境下的轨迹规划问题，提前构建地图中的凸安全节点，从而构建双层图模型，避免了在实时规划过程中重复生成凸安全区域造成的效率降低问题，进一步提升了大尺度已知环境下轨迹规划的速度，无需在实时的规划中重新生成凸安全节点，减少了机器人在实时规划中的计算量。与传统的基于切线图和voronoi图（泰森多边形）轨迹规划方法相比，本专利的方法在第二层图模型中凸安全节点上生成轨迹的质量更高。

所述步骤s1包括：

s11，读取平面栅格地图，见图1，其中黑色部分表示经过膨胀的障碍物，白色部分表示移动小车的可行通道；

s12，将步骤s1中的地图划分为9个大小相同的方块，如图2；

s13，其中方块1、2、3、6、7、8、9中均不包含障碍物，所以无需再进行分割，而方块4和方块5中均部分包含了障碍物，所以需要进一步分割，对方块4和方块5进行下一步九宫格划分的效果如图3，需要提醒的一点是不同格子之间不会有任何重叠；

s14，重复步骤3，一直分割到地图中的格子分为两类，第一类是完全不包含障碍物的格子，如图2中的方块1、2、3、6、7、8、9，第二类是完全包含障碍物的格子，如图3中的方块5.1；

s15，值得注意的一点是，上述划分方法最后一般都会在障碍物的边缘划分到1x1像素大小的格子，当地图稍复杂的时候，需要消耗大量的时间才能完成全部格子的分割，并且大量1x1大小的格子会大量增加图的节点和边的数量，但是对最短路径的搜索和轨迹的生成却不会有多少提升，因此这里采取的实际做法是对格子的面积设置一个最小阈值，当分割的格子面积小于这个阈值时则不再分割，具体阈值设置为多少需要根据地图中障碍物的复杂度和需要的精度进行设置；

s16，步骤5中设置阈值的方法会产生大量的第三类格子，这类格子只包含部分障碍物，为了避免机器人与障碍物碰撞，将这类格子当作第二类格子；

最终九宫格分割的效果如图4所示。

所述步骤s2包括：

s21，创建第一层节点；

s22，创建第一层边。

所述步骤s21包括：

当使用九宫格分割方法对平面栅格地图进行分割完成后，产生了两类格子，接下来需要根据这些格子的大小、空间分布等信息创建图模型中的节点，这里只将第一类不包含障碍物的格子转换成节点，而忽略了包含障碍物的节点，这样构建的图模型将会是无碰撞的图，即减少了节点和边的数量，又减少了搜索最短路径时的计算量。

创建节点的方法比较简单，只需要遍历九宫格分割得到的所有第一类不包含障碍物的格子，并且提取相应的信息即可，这里以图3中的方块7为例，具体步骤如下：

s211，创建存储所有节点的空结构体nodes；

s212，取出其中一个第一类格子，创建该第一类格子对应的新节点；

s213，设置第一类格子的节点id；

s214，计算第一类格子的中心坐标并存储在当前节点中；

s215，计算第一类格子的长和宽并存储在当前节点中；

s216，计算左下角坐标、右上角坐标等，存储到当前节点中；

s217，将该节点放入存储所有节点的结构体nodes中；

s218，重复以上步骤s212到步骤s217，直到所有第一类格子都转化为节点。

所述步骤s22包括：

遍历每个节点，并且对节点进行一个像素的膨胀，如果有其它节点与之重叠则存在相邻关系，具体步骤为：

s221，创建一个二维的结构体edges，第一维表示当前节点的id，第二维表示当前节点的相邻节点；

s222，在第一层节点集中取出一个未遍历过的节点node；

s223，将节点node的四条边进行膨胀，得到膨胀后的边缘；

s224，判断膨胀后边缘与哪些节点有重叠关系；

s225，遍历每一个有重叠关系的节点，计算当前节点和相邻节点中心的欧式距离；

s226，根据当前节点的索引id、相邻节点的索引id和欧式距离等信息创建一个新的边；

s227，将新的边加入到结构体edges中；edges为用来存储“边”的一个集合；

s228，重复上述步骤5到步骤7，直到当前节点与所有相邻节点的边相邻关系都加入到结构体edges中；

s229，重复上述步骤s222到步骤s228，直到第一层中的所有节点都已经遍历过为止。

第一层节点之间均为相邻关系，而不会有任何重叠，第一层的所有节点根据相邻关系拼接起来即可构成原始地图中的所有可行通道，而自动忽略了障碍物的信息。

所述步骤s3包括：

s31，创建第二层节点；

s32，创建第二层边。

所述步骤s31包括：

第二层的第一类节点无需另外生成，只需重复使用第一层中的节点即可，因此下面只关注第二层中第二类节点的创建。创建第二类节点需要保证每一个第一类节点都必定被至少一个凸安全节点包含，由于不同的凸安全节点之间存在部分区域重合，所以出于重合区域的第一层节点将会被多个凸安全节点包含。由于凸安全节点的包含性质，只需将未被包含的第一层节点进行最大膨胀即可得到所有可能存在的凸安全节点。

创建凸安全节点的具体步骤为：

s311，创建第二层图模型中凸安全节点的结构体；

s312，从第一层图模型中取出一个未遍历过并且未被包含的节点（由于第一类节点与第一层图中的节点基本相同，直接使用第一层图中的节点作为第二层图的第一类节点即可，下面不再重复说明）；

s313，将该节点的边缘进行膨胀，当某一个方向碰到障碍物时则立即停止该方向的膨胀，并且继续其余方向的膨胀，直到所有方向都碰到障碍时，得到一个凸安全节点的形状和大小；

s314，判断当前凸安全节点都包含了第一层图模型中的哪些节点，并且将这些节点标记为已遍历过的节点，下次进行步骤s312时不再遍历；

s315，设置该凸安全节点的索引id，计算左下角坐标和右上角坐标；

s316，将步骤s315中的信息和步骤s314中包含的第一层节点id都保存到当前凸安全节点中，并且将该凸安全节点插入到第二层图模型中；

s317，重复上述步骤s312到步骤s316，直到第一层图模型中的所有节点都被包含为止，此时地图中所有不重复的凸安全节点均以创建完成。

所述步骤s32包括：

第二层的边与第一层的边完全不同，前者表示的是包含关系，而后者表示的是相邻关系。创建第二层图的边的方法比较简单，无需另外创建结构体来保存，能够在第一层节点中查询到相应的凸安全节点，并且在凸安全节点中也能够查询到包含的第一层节点有哪些即可。在上一小节创建第二层节点时，已经将每个凸安全节点包含的第一类节点id放入凸安全节点的信息中，故只需遍历第一层节点加入对应凸安全节点的id即可，具体步骤如下：

遍历第一层节点加入对应凸安全节点的id，具体步骤如下：

s321，遍历所有凸安全节点，取出其中一个凸安全节点node_expand；

s322，遍历当前凸安全节点包含的所有第一层节点，并且取出其中一个节点node；

s323，将当前凸安全节点node_expand的id信息保存到当前节点node中；

重复步骤s321到步骤s323。

本专利构建的图模型分为两层，其中第一层的节点表示为九宫格分割后的可行通道，第一层的边表示第一层节点之间的相邻关系，不相邻的节点不存在边；而第二层的节点有两类，第一类与第一层中的节点相同，而第二类节点表示凸安全节点，凸安全节点指其中一部分可行通道的最大凸安全区域构成的节点，凸安全节点可能包含有多个第一层节点，凸安全节点之间可以相互重叠，但是不会完全重叠，也不会存在包含关系，并且每一个凸安全节点都必须是凸的，凸性质的定义为凸安全区域中的任意两个像素点的连接线上的每一个像素点都属于该区域，而第二层图的边表示为第一类和第二类节点之间的包含关系，当第一类某节点被第二类某节点完全包含时，在这两个节点之间创建第二层图的边；以上就是本专利构建的两层图模型中节点和边的具体含义。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。