CN104898660A - 一种提高机器人路径规划效率的室内地图构建方法 - Google Patents

Abstract

本发明公开了一种提高机器人路径规划效率的室内地图构建方法，该方法将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图)算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加简单和高效。

Description

一种提高机器人路径规划效率的室内地图构建方法

技术领域

本发明涉及室内机器人领域，尤其涉及一种提高机器人路径规划效率的室内地图构建方法。

背景技术

近来年，随着科技的发展和进步，在机器人领域，室内服务机器人掀起了一股新的研究和应用热潮。

机器人在室内环境中的自主定位和导航需要活动空间的环境地图作为前提，精确地地图表示和创建成为机器人自主移动的关键技术，同时也是完成其它室内任务的基础。常用的环境表示方法有栅格地图，几何表示地图，特征地图和拓扑地图。

其中，栅格地图即将整个环境分为若干个大小相同的栅格。在栅格地图中，通过某种规划算法，找到一条代价最小，连结起点(机器人当前栅格位置)和终点(目标点所在的栅格位置)的路径。在实际应用中，栅格地图的精度和大小往往相互矛盾，精度越高，栅格的数目会相应增多，导致路径规划算法的规划空间过大，使得规划时间急剧增加，甚至无法响应实时需要。

拓扑地图相比栅格地图具有高度的抽象性，适用于环境大而简单的情况。但是，该方法缺乏环境的具体信息，难以适应定位等需要环境特征的任务。

发明内容

本发明的目的是提供一种提高机器人路径规划效率的室内地图构建方法，使得机器人导航中的路径规划算法更加简单和高效。

本发明的目的是通过以下技术方案实现的：

一种提高机器人路径规划效率的室内地图构建方法，该方法包括：

步骤A、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向图G＝＜V,E＞；

步骤B、初始化房间集合V＝{S₀}，门集合并标定机器人当前所在的房间节 点S_current＝S₀；利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获得栅格数据的标签值Label设为S₀，并从中提取出边界栅格，获得边界点集合；

步骤C、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从候选探索点集合中选择下一步探索的目标点；

步骤D、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为S₀；若检测到门，则将门同侧的栅格数据的标签值Label设为S₀，将门另一侧的栅格数据的标签值Label设为S₁，并更新房间集合V＝{S₀,S₁}，门集合E＝E∪{e＜S₀,S₁＞}；

步骤E、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤C～步骤D，直至完成当前室内环境的扫描过程。

进一步的，该方法还包括：当检测到门，并设置了门两侧栅格数据的标签值Label后，判断是否出现标签值Label被重复设置的情况；

若是，且设置前的标签值Label_old与设置后的标签值Label_new不相同，则将相应栅格数据的标签值为重新设置为设置前的标签值Label_old，并将房间集合V中标签值Label_old与Label_new对应的房间节点合并。

进一步的，所述对所述边界点集合进行预处理，获得候选探索点集合包括：

根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；

利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再对线段进行延伸与合并处理，获得若干线段；

记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。

进一步的，所述基于效用函数从候选探索点集合中选择下一步探索的目标点包括：

基于效用函数计算每一候选探索点的函数值，其公式为：

$U\left(P\right)=\frac{[\mathrm{\α}\·S\left(P\right)+(1-\mathrm{\α})\·C\left(P\right)]\·e^{-\mathrm{\Δ\θ}\left(P\right)}}{D\left(P\right)};$

其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，Δθ(P)为候选探索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点P的距离，α取值范围为[0,1]。

将函数值最大的候选点作为下一步探索的目标点。

进一步的，该方法还包括：根据激光数据检测室内环境中的门，其步骤为：

由于障碍物的存在，激光数据被划分为若干连续的区间，通过下述公式计算障碍物的宽度：

Width＝L₁·L₂·cos(b-a)

其中，L₁与L₂分别为机器人在当前位置处与障碍物两侧的距离，a与b分别为机器人当前位置处的水平方向与障碍物两侧的夹角；

若满足下式，则判定该障碍物为门：

$\stackrel{\‾}{W}-\mathrm{DIST}\_\mathrm{TOL}\≤\mathrm{Width}\≤\stackrel{\‾}{W}+\mathrm{DIST}\_\mathrm{TOL};$

其中，是待扫描的室内环境中门宽度的平均值，DIST_TOL表示可以容忍的宽度差别。

由上述本发明提供的技术方案可以看出，将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图)算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加简单和高效。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。

图1为本发明实施例提供的一种提高机器人路径规划效率的室内地图构建方法；

图2为本发明实施例提供的激光数据被分割为连续区域的示意图；

图3为本发明实施例提供的激光数据的直方图；

图4为本发明实施例提供的初始探索状态的示意图；

图5为本发明实施例提供的机器人检测到门时的示意图；

图6为本发明实施例提供的机器人检测到门时的示意图；

图7为本发明实施例提供的处理栅格数据标签值重复设置时的示意图；

图8为本发明实施例提供的处理栅格数据标签值重复设置时的示意图；

图9为本发明实施例提供的实验时所构建实际环境的示意图；

图10为本发明实施例提供的基于本发明方案进行实验的结果示意图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

实施例一

图1为本发明实施例提供的一种提高机器人路径规划效率的室内地图构建方法。如图1所示，该方法主要包括如下步骤：

步骤11、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向图G＝＜V,E＞。

本发明实施例所建立的拓扑图以房间为节点，房间之间的门作为边，本方案中基于门的检测和机器人的即时移动建立拓扑地图，而且同时创建栅格地图。

室内环境中，通过门的位置就可以将区分不同的房间，而现有的房间分割方法都是通过提取出已有的栅格地图上的线段，角点，多边形等几何特征，计算复杂且需要提前建立栅格地图。

步骤12、初始化房间集合V＝{S₀}，门集合并标定机器人当前所在的房间节点S_current＝S₀；利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获得栅格数据的标签值Label设为S₀，并从中提取出边界栅格，获得边界点集合。

步骤13、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从候选探索点集合中选择下一步探索的目标点。

步骤14、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为S₀；若检测到门，则将门同侧的栅格数据的标签值Label设为S₀，将门另一侧的栅格数据的标签值Label设为S₁，并更新房间集合V＝{S₀,S₁}，门集合E＝E∪{e＜S₀,S₁＞}。

更新后的门集合E＝E∪{e＜S₀,S₁＞}表示新增一个用于连接房间节点S₀与S₁的边元素e＜S₀,S₁＞。

进一步的，当检测到门，并设置了门两侧栅格数据的标签值Label后，判断是否出现 标签值Label被重复设置的情况；

若是，且设置前的标签值Label_old与设置后的标签值Label_new不相同，则将相应栅格数据的标签值为重新设置为设置前的标签值Label_old，并将房间集合V中标签值Label_old与Label_new对应的房间节点合并。

步骤15、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤13～步骤14，直至完成当前室内环境的扫描过程。

为了便于理解本发明，下面分别针对门检测、探索策略及建立房间拓扑关系做详细的说明。

1、利用激光数据实现室内环境中的门检测。

机器人在建图的过程中，不断接受周围环境的激光数据，对激光数据经行分析就可以检测出环境中的门。

如图2所示，由于障碍物的存在，激光数据被划分成为几个连续的区间，图2中的门(door)即为一障碍物，a与b分别为机器人当前位置处的水平方向(X轴)与门两侧的夹角；为了更为直观的表示图2，可以将其转换为图3所示的直方图，图3左侧的space则为门之间的宽度。

计算障碍物宽度的公式为：

Width＝L₁·L₂·cos(b-a)

其中，L₁与L₂分别为机器人在当前位置处与障碍物两侧的距离；

若满足下式，则判定该障碍物为门：

$\stackrel{\‾}{W}-\mathrm{DIST}\_\mathrm{TOL}\≤\mathrm{Width}\≤\stackrel{\‾}{W}+\mathrm{DIST}\_\mathrm{TOL};$

其中，是待扫描的室内环境中门宽度的平均值，DIST_TOL表示可以容忍的宽度差别。

当检测到门以后，则可根据机器人的位置计算得到门的两个端点的坐标，从而获得门的位置和朝向。

2、探索策略。 

机器人在探索的过程中不断的趋向于未知区域，获取未知区域的信息来扩充地图，为了保证机器人探索的效率和地图的准确性，探索活动需要遵循一定的策略和方法。

2.1提取未知边界，获得边界点集合。

栅格地图中每个栅格单元的状态对应于空闲，占用，未知三种状态。为了建立完整 的环境地图，机器人只有不断靠近未知区域才是有意义的。

本发明实施例中，定义栅格状态函数如下：

上式中，x，y为栅格的坐标，唯一确定一个栅格。若一个栅格A为边界栅格则需要满足条件以下两个条件。

1)S(x_A,y_A)＝2；

2)U为栅格A四邻域集合，若存在栅格B和C，且 其中，为存在量词。

通过上述方式找到所有的边界栅格，则完成了位置边界的提取，从而获得边界点集合。

2)确定候选点。 

首先，根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；其次，利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再对线段进行延伸与合并处理，获得若干线段；最后，记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。

3)确定探索目标点。

评价一个环境探索策略的标准是能否高效准确的建立完整或近似完整的环境地图。本发明实施例在确定下一步探索点时，综合考虑了路径优化，信息增益，定位准确等因素。

未知环境中的探索，机器人需要根据已有的局部栅格地图的信息，确定下一步继续感知环境的最优位置，确保能够顺利完成对未知环境的探索和建图。具体的算法步骤如下：

1)若候选点集合为空，则探索结束；否则转向步骤2)。

2)基于效用函数计算每一候选探索点的函数值，并将函数值最大的候选点作为下一步探索的目标点，效用函数公式为：

$U\left(P\right)=\frac{[\mathrm{\α}\·S\left(P\right)+(1-\mathrm{\α})\·C\left(P\right)]\·e^{-\mathrm{\Δ\θ}\left(P\right)}}{D\left(P\right)};$

其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，Δθ(P)为候选探 索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点P的距离，α取值范围为[0,1]。

3)驱使机器人到达目标点，并读取传感器数据，更新局部栅格地图和障碍物地图。

4)提取局部地图的边界点，计算出新的候选点加入候选点列表，转到步骤1)。

需要说明的是，在上述探索过程中，机器人还会根据前述的方式实时判断是否检测到门，以便建立房间的拓扑关系，具体的在后文中会进行详细的描述。

3、建立房间拓扑关系。

通常室内环境房间与房间之间都有一个或多个门，只要能正确检测出门就能够得到单独的房间和房间之间的拓扑关系，而将这种拓扑关系应用到导航的路径规划算法，可以使得规划更高效而且简单。

本发明实施例中，将拓扑地图表示成为无向图G＝＜V,E＞，V表示房间节点集合，E表示门集合；令S_current为机器人的当前所在的房间节点；同时为每一个栅格增加一个标签值Label，记录该栅格所在的房间节点。下面结合图4-图8来进行说明，具体的步骤如下：

1)探索开始时，V＝{S₀}，并设置S_current＝S₀，如图4所示。

2)根据前述方式来确定需探索的目标点，并移动至目标点，并判断是否检测到门，若未检测到门转如步骤3)，若检测到门则转入步骤4)。

3)当未检测到门时，将获得的栅格数据的标签值Label值设为S₀。

4)当未检测到门时，则将门同侧的栅格数据的标签值Label设为S₀，将门另一侧的栅格数据的标签值Label设为S₁，并更新房间集合V＝{S₀,S₁}，门集合E＝E∪{e＜S₀,S₁＞}，即对于无向图G而言，在节点S₀与S₁间增加一条边(在E中新增一个门元素)，如图5-图6所示。同时，由于某些房间可能存在多个门，通过上述方式可能出现栅格数据的标签值Label重复设置的情况，若出现这一情况，则转入步骤5)，否则，转入步骤6)。

5)若设置前的标签值Label_old与设置后的标签值Label_new不相同，则将相应栅格数据的标签值为重新设置为设置前的标签值Label_old，并将房间集合V中标签值Label_old与Label_new对应的房间节点合并，并保持边数目不变，如图7-图8所示。

6)更新S_current为机器人当前所在的房间节点。

上述所举例的图4-图8中，左侧为机器人探索的示意图，右侧为根据探索结果构建的无向图G的示意图。

本发明实施例通过将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图)算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加简单和高效。

另一方面，还与本发明的上述实施例进行了实验。

本实验中，采用了自主研发的服务机器人“可佳”，“可佳”的底盘装有Hokuyo UTM-30LN激光传感器，该激光传感器的范围为0.1～30m，扫描频率为60Hz，角度分辨率为0.25°。“可佳”的计算部件通常为笔记本或工控机(满足cpu intel i3及以上，内存4G左右)。

在应用中，操纵机器人在室内环境遍历一遍就能建立精确地识别出环境中的门，并且建立栅格地图和拓扑地图。所建地图的实际环境模型如图9所示，实验结果如图10所示。从实验结果可以看出，在比较复杂的家居环境中，利用本发明的方法能够比较精确地建立环境地图，本发明实施例中的探索策略能够保证环境地图的准确性和完整性，门检测算法能够快速有效的检测出门，从而建立基于门和房间之间的拓扑关系。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (5)

1.一种提高机器人路径规划效率的室内地图构建方法，其特征在于，该方法包括：

步骤A、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向图G＝＜V,E＞；

步骤B、初始化房间集合V＝{S₀}，门集合并标定机器人当前所在的房间节点S_current＝S₀；利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获得栅格数据的标签值Label设为S₀，并从中提取出边界栅格，获得边界点集合；

步骤C、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从候选探索点集合中选择下一步探索的目标点；

步骤D、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为S₀；若检测到门，则将门同侧的栅格数据的标签值Label设为S₀，将门另一侧的栅格数据的标签值Label设为S₁，并更新房间集合V＝{S₀,S₁}，门集合E＝E∪{e＜S₀,S₁＞}；

步骤E、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤C～步骤D，直至完成当前室内环境的扫描过程。

2.根据权利要求1所述的方法，其特征在于，该方法还包括：当检测到门，并设置了门两侧栅格数据的标签值Label后，判断是否出现标签值Label被重复设置的情况；

若是，且设置前的标签值Label_old与设置后的标签值Label_new不相同，则将相应栅格数据的标签值为重新设置为设置前的标签值Label_old，并将房间集合V中标签值Label_old与Label_new对应的房间节点合并。

3.根据权利要求1所述的方法，其特征在于，所述对所述边界点集合进行预处理，获得候选探索点集合包括：

根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；

利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再对线段进行延伸与合并处理，获得若干线段；

记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。

4.根据权利要求1或3所述的方法，其特征在于，所述基于效用函数从候选探索点集合中选择下一步探索的目标点包括：

基于效用函数计算每一候选探索点的函数值，其公式为：

$U\left(P\right)=\frac{[\mathrm{\α}\·S\left(P\right)+(1-\mathrm{\α})\·C\left(P\right)]\·e^{-\mathrm{\Δ\θ}\left(P\right)}}{D\left(P\right)};$

其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，Δθ(P)为候选探索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点P的距离，α取值范围为[0,1]。

将函数值最大的候选点作为下一步探索的目标点。

5.根据权利要求1所述的方法，其特征在于，该方法还包括：根据激光数据检测室内环境中的门，其步骤为：

由于障碍物的存在，激光数据被划分为若干连续的区间，通过下述公式计算障碍物的宽度：

Width＝L₁·L₂·cos(b-a)

其中，L₁与L₂分别为机器人在当前位置处与障碍物两侧的距离，a与b分别为机器人当前位置处的水平方向与障碍物两侧的夹角；

若满足下式，则判定该障碍物为门：

$\stackrel{\‾}{W}-\mathrm{DISY}\_\mathrm{TOL}\≤\mathrm{Width}\≤\stackrel{\‾}{W}+\mathrm{DIST}\_\mathrm{TOL};$

其中，是待扫描的室内环境中门宽度的平均值，DIST_TOL表示可以容忍的宽度差别。