CN109827585A - 一种快速填补栅格地图中凹形区域的方法 - Google Patents

Abstract

本发明属于机器人路径规划领域，具体涉及一种快速填补栅格地图中凹形区域的方法，包括以下步骤：获取占据栅格地图，选定一个重力作用方向，通常选择为上下左右四个方向，以下步骤选定重力方向向下；遍历地图寻找拥有支撑力的一个栅格单元，也即其正下方有障碍物栅格单元，并且该栅格单元本身不是障碍物；在该栅格单元左右开始进行水平方向上的扩充，直到填满整个水平层，也即扩展到左右两侧皆为障碍物，在该过程中持续检查是否存在某一格子上方有障碍物、或者某一格子下方没有障碍物的情况，如果存在，则返回上一步。本发明不依赖于计算机图形学中的凸包问题，是一种快速、直观、方便应用的方法。

Description

一种快速填补栅格地图中凹形区域的方法

技术领域

本发明属于机器人路径规划领域，具体涉及一种快速填补栅格地图中凹形区域的方法。

背景技术

路径规划是自主机器人研究的核心问题之一，旨在通过机器人对周边环境的感知或通过已有地图信息，规划出一条从起始点到目标点的安全路径。从机器人路径规划的目标范围来看可分为全局路径规划和局部路径规划。其中全局路径规划通常要求机器人初始位姿、环境信息和目标已知。而地图作为路径规划的基本元素，直接影响了全局规划算法的运行时间以及规划质量。机器人学中常见的地图表示方法有栅格地图、拓扑地图和语义地图，其中栅格地图易于建立、维护并且可以提供精确的度量地图。在很多实际应用场景中，如SLAM、游戏以及火星探险漫游者，栅格地图是最常用和最基本的地图形式。但是栅格地图在度量映射中包含了用于精确映射的冗余信息，因而不能有效地处理数据。这也导致了在栅格地图上进行路径规划往往会耗费大量时间。

路径规划中一个特殊的情况是对凹形区域的无效搜索。凹形区域是指地图中由房间、办公室隔间或者角落等不规则凹多边形所形成的区域，如果一个此类区域存在于起始点与终止点的路径上，会造成路径规划算法大量的无效搜索时间以及内存占用，甚至是导航的失败。因而通过一种预处理手段在栅格地图中提前标记出该类型区域十分必要。

发明内容

针对栅格地图中的凹形区域会造成路径规划算法无效搜索耗时和内存占用的问题，本发明提供一种快速排除凹形障碍物区域的地图预处理方法。该方法受物理现象启发，不受限于障碍物具体形状，具有直观、快速、实现方便的特点。

一种快速填补栅格地图中凹形区域的方法，包括以下步骤：

(1)获取占据栅格地图，选定一个重力作用方向，通常选择为上下左右四个方向，以下步骤选定重力方向向下；

(2)遍历地图寻找拥有支撑力的一个栅格单元，也即其正下方有障碍物栅格单元，并且该栅格单元本身不是障碍物；

(3)在该栅格单元左右开始进行水平方向上的扩充，直到填满整个水平层，也即扩展到左右两侧皆为障碍物，在该过程中持续检查是否存在某一格子上方有障碍物、或者某一格子下方没有障碍物的情况，如果存在，则返回步骤(2)；

(4)如果这一水平层是本次填补的第一层，则标记这一填补层为一个新填补区域；否则将这一层的格子标记为与下一层格子相同的填补区域，将水平面上涨一层，返回步骤(3)；

(5)对相邻的填补块创建关联，使用块关联表表示这种关联结构；

(6)建立排除列表L_exc。

所述对相邻的填补块创建关联，使用块关联表表示这种关联结构，包括：

将一个填补块b_n的类别序号标记为T_b(b_n)，下述为块关联表的构造规则：

(5.1)申请一个数组空间作为块关联表的索引目录，数组中的数据项为一个单链表的头结点，简记为B(n)，其中n为索引序号，其中单链表的数据项为填补块的类别序号；

(5.2)表中每个填补块b_n占据索引单元T_b(b_n)，随着程序运行，更多的块被添加进索引表中，数组空间需要重新分配；

(5.3)如果一个在建块b_new是相邻之前建立的块b_old而构建的，则T_b(b_new)会成为B(T_b(b_old))头结点下的新结点。

所述建立排除列表L_exc，包括：

(1)建立一个空排除列表L_exc；

(2)将起始点或目标点所在填补块b_start与b_end的T_b(b_start)以及T_b(b_end)添加进L_exc，根据到块关联表查询相应的头结点B(T_b(b_start))以及B(T_b(b_end))，并将它们的所有后继结点也加入L_exc；

(3)将所有后接结点作为头结点，查询块关联表，并将它们的后接结点添加进入排除列表L_exc，重复这一过程，直到最后添加进来的结点无后接结点。

本发明的有益效果在于：

(1)抽象化容器与流入其中的流体相互作用力的现象，得到一种填补栅格地图中的凹形障碍物区域的方法。本发明中的FC算法对障碍物边缘不敏感，只要地图中存在凹形障碍物，即可进行一次拥有较佳结果的填补。

(2)本发明不依赖于计算机图形学中的凸包问题，是一种快速、直观、方便应用的方法。

(3)如果一个凹形障碍物中不存在起始点与目标点，那么该凹形障碍物中的格子不会出现在最优路径上。填补占据栅格地图中的凹形障碍物可以大大减少路径规划算法的无效搜索耗时与内存占用。

(4)构建块关联表与排除列表可使得在同一个地图上进行重复路径规划无需重复使用FC算法。

附图说明

图1是块关联表结构图；

图2是地图中某一点拥有支撑力的示意图；

图3是水平层扩展示意图；

图4是水平层缺少支撑力，终止扩展示意图；

图5是完成一次凹陷区域填补过程示意图；

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及机器人路径规划领域，具体涉及一种快速填补栅格地图中凹形区域的方法。

路径规划是自主机器人研究的核心问题之一，旨在通过机器人对周边环境的感知或通过已有地图信息，规划出一条从起始点到目标点的安全路径。从机器人路径规划的目标范围来看可分为全局路径规划和局部路径规划。其中全局路径规划通常要求机器人初始位姿、环境信息和目标已知。而地图作为路径规划的基本元素，直接影响了全局规划算法的运行时间以及规划质量。机器人学中常见的地图表示方法有栅格地图、拓扑地图和语义地图，其中栅格地图易于建立、维护并且可以提供精确的度量地图。在很多实际应用场景中，如SLAM、游戏以及火星探险漫游者，栅格地图是最常用和最基本的地图形式。但是栅格地图在度量映射中包含了用于精确映射的冗余信息，因而不能有效地处理数据。这也导致了在栅格地图上进行路径规划往往会耗费大量时间。

路径规划中一个特殊的情况是对凹形区域的无效搜索。凹形区域是指地图中由房间、办公室隔间或者角落等不规则凹多边形所形成的区域，如果一个此类区域存在于起始点与终止点的路径上，会造成路径规划算法大量的无效搜索时间以及内存占用，甚至是导航的失败。因而通过一种预处理手段在栅格地图中提前标记出该类型区域十分必要。

针对栅格地图中的凹形区域会造成路径规划算法无效搜索耗时和内存占用的问题，本发明提供一种快速排除凹形障碍物区域的地图预处理方法。该方法受物理现象启发，不受限于障碍物具体形状，具有直观、快速、实现方便的特点。

为了实现上述目的，本发明提供了如下方案：

一种快速填补凹形区域的FC(Fill The Container)算法。该算法不是一个最优化方案，它不能够保证做到完整的凹多边形补全，但它对凹陷障碍物的形状不敏感，在大多数情况下都能取得比较好的效果。抽象化容器与流入其中的流体相互作用力的现象，得到填补栅格地图中的凹形障碍物区域的步骤：

步骤1：获取占据栅格地图，选定一个重力作用方向，通常可选择为上下左右四个方向，以下步骤以重力方向向下为基础。

步骤2：遍历地图寻找拥有支撑力的一个栅格单元，也即其正下方有障碍物栅格单元，并且该栅格单元本身不是障碍物。

步骤3：在该栅格单元左右开始进行水平方向上的扩充，直到填满整个水平层，也即扩展到左右两侧皆为障碍物，在该过程中持续检查是否有某一格子上方有障碍物，或者下方没有障碍物的情况，如果发生，则跳回到步骤2。

步骤4：如果这一水平层是本次填补的第一层，则标记这一填补层为一个新填补区域，否则将这一层的格子标记为与下一层格子相同的填补区域。将水平面上涨一层，继续步骤3。

现实地图中往往会有若干凹陷区域，而每个凹陷区域会被一个或多个填补块所填补。填补块在FC算法构造时互相提供支撑力，这导致在路径规划时只消解一个填补块不足以还原路径，因而需要对相邻的填补块创建关联，使用块关联表表示这种关联结构。将一个填补块b_n的类别序号标记为T_b(b_n)。下述为块关联表的构造规则：

规则1：申请一个数组空间作为块关联表的索引目录，数组中的数据项为一个单链表的头结点，简记为B(n)，其中n为索引序号。其中单链表的数据项为填补块的类别序号。

规则2：表中每个填补块b_n占据索引单元T_b(b_n)，随着程序运行，更多的块被添加进索引表中，数组空间可能需要重新分配。

规则3：如果一个在建块b_new是相邻之前建立的块b_old而构建的，则T_b(b_new)会成为B(T_b(b_old))头结点下的新结点。

在一张已经建立好填补块的地图上执行路径规划，则需要一些额外的处理工作，因为随机设定的起始点或目标点很可能存在于某一填补块之中。以下步骤为阻挡路径的填补块建立一个排除列表L_exc的过程：

步骤1：建立一个空排除列表L_exc。

步骤2：将起始点或目标点所在填补块b_start与b_end的T_b(b_start)以及T_b(b_end)添加进L_exc，根据到块关联表查询相应的头结点B(T_b(b_start))以及B(T_b(b_end))，并将它们的所有后继结点也加入L_exc。

步骤3：将所有后接结点作为头结点，查询块关联表，并将它们的后接结点添加进入排除列表L_exc，重复这一过程，直到最后添加进来的结点无后接结点。

在路径规划算法运行时，不要将排除列表L_exc中类别号所表示的填补块当作障碍物，而其它未被获取的块则依然作为障碍物。

下面将结合附图对本发明加以详细说明，应指出的是，所描述的实施例仅旨在便于解释本发明，并不构成对本发明的限制。

设定二维栅格地图空间中的单元关系由平面直角坐标系表示，每个栅格单元由横坐标x与纵坐标y标识其位置。此外，每个格子都有一个值以表示其类别type，这与常规栅格地图不同，例如在二值栅格地图中，1表示障碍，0表示自由区；或者在概率栅格地图中，格子的数值表示该栅格的占据可能性。对于占据栅格地图中的每个格子，其类别定义如下：

(1)如果一个格子是未被占据的，则其类别type为0，或记录为T(x，y)＝0；

(2)如果一个格子在是被占据的，则其类别type为1，或T(x，y)＝1；

(3)如果一个格子处于FC算法的填补区域之中，则其类别大于等于2，具体数值会与算法本身的处理顺序相关，通常来说，处于同一填补区域的格子会有相同的类别。

现实地图中往往会有若干凹陷区域，通常来说，每个凹陷区域会被一个或多个填补块所填补。如在图5中，灰色区域为一个填补块。这些填补块，或简称块，是在FC算法运行的同一阶段创建的。每个块由若干具有相同类型的栅格单元组成，可使用该类别号来标识每个块，并将一个填补块b_n的类别标记为T_b(b_n)。随着算法的运行，更多的块被创建，而它们的类别号也会从2逐渐增大。

为方便后续处理，要求填补块之间并非完全互相独立，所以必须为相邻的块创建关联。这不需要一个类似多分辨率栅格地图中的四叉树结构，索引表通常足够处理这种关联关系，称该索引表为块关联表。该表中每个填补块占据一个索引单元，随着程序运行，更多的块被添加进索引表中。每个索引单元是一个单链表的头结点，简记为B(type)，也即索引表中第type号头结点。如果一个在建块b_new是相邻之前建立的块b_old而构建的，则T_b(b_new)会成为B(T_b(b_old))头结点下的新结点。这个结构与哈希表相似，只是并没有哈希函数映射。该表的结构可见图1。

下述过程为图1所示的块关联表的构造过程：

步骤1：申请一个数组空间作为块关联表的索引目录，数组中的数据项为一个单链表的头结点。其中0号索引项以及1号索引项将不会拥有后继链表结构，因为标记为type＝0以及type＝1的栅格单元并非是填补块。

步骤2：随着FC算法运行，填补块b₀被创建，且T_b(b₀)＝2，在索引目录中添加索引为2的单元，默认填补块b_n的索引序号为T_b(b_n)。随后填补块b₁被创建，且T_b(b₁)＝3，此时b₀与b₁未发生边缘接触，因而彼此之间将不会成为后继结点。

步骤3：填补块b₂被创建，且在b₂的构造过程中，与b₀发生了接触。因为在FC算法的支撑力判定过程中，原生障碍物也即type＝1的栅格单元，以及所有type≥1的格子都可为当前判定格子提供支撑力，这便造成了已构造的填补块与新构造的填补块相接触的情况。此时，一个新的单链表结点将被创建，其数据项被设置为T_b(b₂)＝4，并将该结点设置为B(2)的后继结点。填补块b₂未与其它块接触，因而只存在于B(2)的后继结点中。

步骤4：填补块b₃被创建，且在b₃的构造过程中，与b₀以及b₁发生了接触。构造两个单链表结点，其数据项被设置为T_b(b₃)＝5，分别将它们设置为B(2)与B(3)的后继结点。

步骤5：持续表项的添加与链表节点的创建，直到FC算法运行完毕。

FC算法具体作用过程，包括以下步骤：

步骤1：获取占据栅格地图M，生成一个空的块关联表B。设置当前类别号type为2。

步骤2：选取一个重力方向，通常可选择为上下左右四个方向，或者加上四个对角线方向。以下步骤皆以重力方向向下为基础。

步骤3：逐一遍历除去边界格子以外的所有栅格单元，设置一个块扩展标识符exp_b，并置为真。使用当前单元的纵坐标y记录扩展层次level。

步骤4：如果块扩展标识符exp_b为真，此时意味着可以开始构造一个新的填补块，或者已经完成了一次或多次水平层扩展填补，可以继续上一轮填补块构造过程。对每一个格子(x，y)尝试进行扩展以构建填补块。检查其下方格子是否为障碍物，并检查该栅格单元是否为障碍物。如果满足T(x，y-1)＝1并且T(x，y)≠1，如图2所示情况，则进入步骤5，否则终止填补过程，返回步骤3。

步骤5：使用当前单元横坐标x记录水平层扩展的左端标志l与右端标志r，并尝试进行水平方向扩展，一次水平层扩展结果如图3所示。

步骤6：尝试进行左向扩展。如果左端不是障碍物，即T(l-1，level)＝0，并且块扩展标识符exp_b为真，则将水平填补层向左扩展一格，即将l设置为l-1。随后检查该左端格子的上层和下层是否为障碍物，如图4所示，下层障碍物或者先前构建的填补单元将为当前栅格单元提供支撑力，以阻止其下迭；检查上层障碍物可避免过度填充淹没了可通行路径。如果满足T(l，level-1)＝0或者T(l，level+1)≠0，则终止当前左向扩展，并设置块扩展标识符exp_b为假。重复该步骤直到扩展到左端为障碍物或块扩展标识符exp_b为假或者到达地图左边界为止。

步骤7：尝试进行右向扩展。如果右端不是障碍物，即T(r+1，level)＝0，并且块扩展标识符exp_b为真，则将水平填补层向右扩展一格，即将r设置为r+1。随后检查该右端格子的上层和下层是否为障碍物，如果满足T(r，level-1)＝0或者T(l，level+1)≠0，则终止当前右向扩展，并设置块扩展标识符exp_b为假。重复该步骤直到扩展到右端为障碍物或块扩展标识符exp_b为假，或者到达地图右边界为止。

步骤8：如果块扩展标识符exp_b为假，并且level≠y，则将当前类别号type设置为type+1。因为满足上述两个条件时意味着已经构建完成了一个新的填补块，且上一个填补块b_n的T_b(b_n)＝type。如果不满足块扩展标识符exp_b为假，则进入步骤9。

步骤9：若满足块扩展标识符exp_b为真，这意味着在步骤5以及步骤6中水平层扩展的过程中已经在对凹形障碍物进行填充。无论是一个填补块的最底层还是其它层，其中所有格子将享有相同的类别号type。对于所有i∈(l，r+1)，将type赋给T(i，level)。

步骤10：检查这一水平层扩展是否与其它已经构建的填补块相接触。对于所有i∈(l，r+1)，如果T(i，level)≠1并且则在B(T(i，level-1))下创建一个新的结点，并将type作为其数据项。此外，检查该水平扩展层的左端与右端是否与其它块接触。如果B(T(l-1，level))≠1或者B(T(r+1，level))≠1，则在B(T(l-1，level))或B(T(r+1，level))下创建一个新的结点，并将type作为其数据项。

步骤11：此时已经结束了一次水平层扩展，令level加1，也即水平面上涨一层，返回步骤4，并继续进行凹形区域填补直到一个填补块完成，如图5所示。

在一张已经建立好填补块的地图上执行路径规划，则需要一些额外的处理工作，因为随机设定的起始点或目标点很可能存在于某一填补块之中，这一处理过程通常很简单，通常并不需要对地图进行二次处理。以下步骤为阻挡路径的填补块建立一个排除列表L_exc的过程：

步骤1：建立一个空排除列表L_exc。

步骤2：将起始点或目标点所在填补块b_start与b_end的T_b(b_start)以及T_b(b_end)添加进L_exc，根据到块关联表查询相应的头结点B(T_b(b_start))以及B(T_b(b_end))，并将它们的所有后继结点也加入L_exc。

步骤3：将所有后接结点作为头结点，查询块关联表，并将它们的后接结点添加进入排除列表L_exc，重复这一过程，直到最后添加进来的结点无后接结点。

这一过程并不会造成过于复杂的递归调用，因为头结点的类别号永远会小于其后接结点的类别号，因为头结点所表示的填补块是更早建立的。由于块关联表的规模相对于地图来说通常是很小的，所以这一过程的时间损耗很低。在路径规划算法运行时，不要将排除列表L_exc中类别号所表示的填补块当作障碍物，而其它未被获取的块则依然作为障碍物。

Claims (3)

1.一种快速填补栅格地图中凹形区域的方法，其特征在于，包括以下步骤：

(1)获取占据栅格地图，选定一个重力作用方向，通常选择为上下左右四个方向，以下步骤选定重力方向向下；

(2)遍历地图寻找拥有支撑力的一个栅格单元，也即其正下方有障碍物栅格单元，并且该栅格单元本身不是障碍物；

(3)在该栅格单元左右开始进行水平方向上的扩充，直到填满整个水平层，也即扩展到左右两侧皆为障碍物，在该过程中持续检查是否存在某一格子上方有障碍物、或者某一格子下方没有障碍物的情况，如果存在，则返回步骤(2)；

(4)如果这一水平层是本次填补的第一层，则标记这一填补层为一个新填补区域；否则将这一层的格子标记为与下一层格子相同的填补区域，将水平面上涨一层，返回步骤(3)；

(5)对相邻的填补块创建关联，使用块关联表表示这种关联结构；

(6)建立排除列表L_exc。

2.根据权利要求1所述的一种快速填补栅格地图中凹形区域的方法，其特征在于，所述对相邻的填补块创建关联，使用块关联表表示这种关联结构，包括：

将一个填补块b_n的类别序号标记为T_b(b_n)，下述为块关联表的构造规则：

(5.1)申请一个数组空间作为块关联表的索引目录，数组中的数据项为一个单链表的头结点，简记为B(n)，其中n为索引序号，其中单链表的数据项为填补块的类别序号；

(5.2)表中每个填补块b_n占据索引单元T_b(b_n)，随着程序运行，更多的块被添加进索引表中，数组空间需要重新分配；

(5.3)如果一个在建块b_new是相邻之前建立的块b_old而构建的，则T_b(b_new)会成为B(T_b(b_old))头结点下的新结点。

3.根据权利要求1所述的一种快速填补栅格地图中凹形区域的方法，其特征在于，所述建立排除列表L_exc，包括：

(1)建立一个空排除列表L_exc；

(2)将起始点或目标点所在填补块b_start与b_end的T_b(b_start)以及T_b(b_end)添加进L_exc，根据到块关联表查询相应的头结点B(T_b(b_start))以及B(T_b(b_end))，并将它们的所有后继结点也加入L_exc；

(3)将所有后接结点作为头结点，查询块关联表，并将它们的后接结点添加进入排除列表L_exc，重复这一过程，直到最后添加进来的结点无后接结点。