一种提高机器人路径规划效率的室内地图构建方法
一种提高机器人路径规划效率的室内地图构建方法
【技术领域】
[0001] 本发明涉及室内机器人领域，尤其涉及一种提高机器人路径规划效率的室内地图 构建方法。
【背景技术】
[0002] 近来年，随着科技的发展和进步，在机器人领域，室内服务机器人掀起了一股新的 研宄和应用热潮。
[0003] 机器人在室内环境中的自主定位和导航需要活动空间的环境地图作为前提，精确 地地图表示和创建成为机器人自主移动的关键技术，同时也是完成其它室内任务的基础。 常用的环境表示方法有栅格地图，几何表示地图，特征地图和拓扑地图。
[0004] 其中，栅格地图即将整个环境分为若干个大小相同的栅格。在栅格地图中，通过某 种规划算法，找到一条代价最小，连结起点（机器人当前栅格位置）和终点（目标点所在的 栅格位置）的路径。在实际应用中，栅格地图的精度和大小往往相互矛盾，精度越高，栅格 的数目会相应增多，导致路径规划算法的规划空间过大，使得规划时间急剧增加，甚至无法 响应实时需要。
[0005]拓扑地图相比栅格地图具有高度的抽象性，适用于环境大而简单的情况。但是，该 方法缺乏环境的具体信息，难以适应定位等需要环境特征的任务。
【发明内容】
[0006] 本发明的目的是提供一种提高机器人路径规划效率的室内地图构建方法，使得机 器人导航中的路径规划算法更加简单和高效。
[0007] 本发明的目的是通过以下技术方案实现的：
[0008]-种提高机器人路径规划效率的室内地图构建方法，该方法包括：
[0009]步骤A、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向 图G= <V，E>;
[0010] 步骤B、初始化房间集合V= {SJ，门集合￡ = 0，并标定机器人当前所在的房间节 点利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获 得栅格数据的标签值Label设为\，并从中提取出边界栅格，获得边界点集合；
[0011] 步骤C、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从 候选探索点集合中选择下一步探索的目标点；
[0012] 步骤D、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更 新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为&若 检测到门，则将门同侧的栅格数据的标签值Label设为\，将门另一侧的栅格数据的标签值 Label设为Si，并更新房间集合V= {SQ，SJ，门集合E=EU{e<SQ，Sp};
[0013]步骤E、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤C~步骤D，直至完成当前室内环境的扫描过程。
[0014] 进一步的，该方法还包括：当检测到门，并设置了门两侧栅格数据的标签值Label 后，判断是否出现标签值Label被重复设置的情况；
[0015] 若是，且设置前的标签值Labels与设置后的标签值Labelnew不相同，则将相应栅 格数据的标签值为重新设置为设置前的标签值Labels，并将房间集合V中标签值Labels与Labelnew对应的房间节点合并。
[0016] 进一步的，所述对所述边界点集合进行预处理，获得候选探索点集合包括：
[0017] 根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；
[0018] 利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线 段，再对线段进行延伸与合并处理，获得若干线段；
[0019] 记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。
[0020] 进一步的，所述基于效用函数从候选探索点集合中选择下一步探索的目标点包 括：
[0021] 基于效用函数计算每一候选探索点的函数值，其公式为：
[0023] 其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在 候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，A0 (P)为候 选探索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点 P的距离，a取值范围为[0, 1]。
[0024] 将函数值最大的候选点作为下一步探索的目标点。
[0025] 进一步的，该方法还包括：根据激光数据检测室内环境中的门，其步骤为：
[0026]由于障碍物的存在，激光数据被划分为若干连续的区间，通过下述公式计算障碍 物的宽度：
[0027]Width=L: ?L2 ?cos(b-a)
[0028] 其中，1^与L2*别为机器人在当前位置处与障碍物两侧的距离，a与b分别为机 器人当前位置处的水平方向与障碍物两侧的夹角；
[0029] 若满足下式，则判定该障碍物为门：
[0031] 其中，fz是待扫描的室内环境中门宽度的平均值，DIST_T0L表示可以容忍的宽度 差别。
[0032] 由上述本发明提供的技术方案可以看出，将拓扑地图与栅格地图相结合，使得规 划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环 境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与 建图）算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导 航中的路径规划算法更加简单和高效。
【附图说明】
[0033] 为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用 的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本 领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他 附图。
[0034]图1为本发明实施例提供的一种提高机器人路径规划效率的室内地图构建方法；
[0035]图2为本发明实施例提供的激光数据被分割为连续区域的示意图；
[0036] 图3为本发明实施例提供的激光数据的直方图；
[0037]图4为本发明实施例提供的初始探索状态的示意图；
[0038] 图5为本发明实施例提供的机器人检测到门时的示意图；
[0039] 图6为本发明实施例提供的机器人检测到门时的示意图；
[0040]图7为本发明实施例提供的处理栅格数据标签值重复设置时的示意图；
[0041]图8为本发明实施例提供的处理栅格数据标签值重复设置时的示意图；
[0042] 图9为本发明实施例提供的实验时所构建实际环境的示意图；
[0043] 图10为本发明实施例提供的基于本发明方案进行实验的结果示意图。
【具体实施方式】
[0044] 下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整 地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本 发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施 例，都属于本发明的保护范围。
[0045] 实施例一
[0046] 图1为本发明实施例提供的一种提高机器人路径规划效率的室内地图构建方法。 如图1所示，该方法主要包括如下步骤：
[0047] 步骤11、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无 向图 G=<V，E>。
[0048]本发明实施例所建立的拓扑图以房间为节点，房间之间的门作为边，本方案中基 于门的检测和机器人的即时移动建立拓扑地图，而且同时创建栅格地图。
[0049] 室内环境中，通过门的位置就可以将区分不同的房间，而现有的房间分割方法都 是通过提取出已有的栅格地图上的线段，角点，多边形等几何特征，计算复杂且需要提前建 立栅格地图。
[0050]步骤12、初始化房间集合V= {S。}，门集合五二0，并标定机器人当前所在的房间 节点利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将 获得栅格数据的标签值Label设为\，并从中提取出边界栅格，获得边界点集合。
[0051] 步骤13、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从 候选探索点集合中选择下一步探索的目标点。
[0052] 步骤14、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行 更新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为 若检测到门，则将门同侧的栅格数据的标签值Label设为\，将门另一侧的栅格数据的标签 值Label设为Si，并更新房间集合V = {'SJ，门集合E = E U {eCS^Si〉}。
[0053] 更新后的门集合E = E U {e < Sd, Si> }表示新增一个用于连接房间节点S 与 Si的边元素e<SQ,Si>。
[0054] 进一步的，当检测到门，并设置了门两侧栅格数据的标签值Label后，判断是否出 现标签值Label被重复设置的情况；
[0055] 若是，且设置前的标签值Labels与设置后的标签值Label new不相同，则将相应栅 格数据的标签值为重新设置为设置前的标签值Labels，并将房间集合V中标签值Labels 与Label new对应的房间节点合并。
[0056] 步骤15、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步 骤13~步骤14,直至完成当前室内环境的扫描过程。
[0057] 为了便于理解本发明，下面分别针对门检测、探索策略及建立房间拓扑关系做详 细的说明。
[0058] 1、利用激光数据实现室内环境中的门检测。
[0059] 机器人在建图的过程中，不断接受周围环境的激光数据，对激光数据经行分析就 可以检测出环境中的门。
[0060] 如图2所示，由于障碍物的存在，激光数据被划分成为几个连续的区间，图2中的 门（door)即为一障碍物，a与b分别为机器人当前位置处的水平方向（X轴）与门两侧的 夹角；为了更为直观的表示图2,可以将其转换为图3所示的直方图，图3左侧的space则 为门之间的宽度。
[0061]计算障碍物宽度的公式为：
[0062] Width = L: ? L2 ? cos(b-a)
[0063] 其中，1^与L2分别为机器人在当前位置处与障碍物两侧的距离；
[0064] 若满足下式，则判定该障碍物为门：
[0066]其中，汞是待扫描的室内环境中门宽度的平均值，DIST_T0L表示可以容忍的宽度 差别。
[0067]当检测到门以后，则可根据机器人的位置计算得到门的两个端点的坐标，从而获 得门的位置和朝向。
[0068] 2、探索策略。
[0069] 机器人在探索的过程中不断的趋向于未知区域，获取未知区域的信息来扩充地 图，为了保证机器人探索的效率和地图的准确性，探索活动需要遵循一定的策略和方法。 [0 070] 2. 1提取未知边界，获得边界点集合。
[0071] 栅格地图中每个栅格单元的状态对应于空闲，占用，未知三种状态。为了建立完整 的环境地图，机器人只有不断靠近未知区域才是有意义的。
[0072] 本发明实施例中，定义栅格状态函数如下：
[0074]上式中，x，y为栅格的坐标，唯一确定一个栅格。若一个栅格A为边界栅格则需要 满足条件以下两个条件。
[0075] l)S(xA, yA) = 2 ；
[0076] 2)U为栅格A四邻域集合，若存在栅格B和C，
且 3C e "a ) = 1;其中，3为存在量词。
[0077] 通过上述方式找到所有的边界栅格，则完成了位置边界的提取，从而获得边界点 集合。
[0078] 2)确定候选点。
[0079] 首先，根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；其 次，利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再 对线段进行延伸与合并处理，获得若干线段；最后，记录各线段的端点和角度，取线段的中 点沿中垂线朝外的方向为候选探索点。
[0080] 3)确定探索目标点。
[0081] 评价一个环境探索策略的标准是能否高效准确的建立完整或近似完整的环境地 图。本发明实施例在确定下一步探索点时，综合考虑了路径优化，信息增益，定位准确等因 素。
[0082] 未知环境中的探索，机器人需要根据已有的局部栅格地图的信息，确定下一步继 续感知环境的最优位置，确保能够顺利完成对未知环境的探索和建图。具体的算法步骤如 下：
[0083] 1)若候选点集合为空，则探索结束；否则转向步骤2)。
[0084] 2)基于效用函数计算每一候选探索点的函数值，并将函数值最大的候选点作为下 一步探索的目标点，效用函数公式为：
[0086] 其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在 候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，A 0 (P)为候 选探索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索 点P的距离，a取值范围为[0, 1]。
[0087] 3)驱使机器人到达目标点，并读取传感器数据，更新局部栅格地图和障碍物地图。
[0088] 4)提取局部地图的边界点，计算出新的候选点加入候选点列表，转到步骤1)。
[0089] 需要说明的是，在上述探索过程中，机器人还会根据前述的方式实时判断是否检 测到门，以便建立房间的拓扑关系，具体的在后文中会进行详细的描述。
[0090] 3、建立房间拓扑关系。
[0091] 通常室内环境房间与房间之间都有一个或多个门，只要能正确检测出门就能够得 到单独的房间和房间之间的拓扑关系，而将这种拓扑关系应用到导航的路径规划算法，可 以使得规划更高效而且简单。
[0092] 本发明实施例中，将拓扑地图表示成为无向图G = < V，E >，V表示房间节点集合， E表示门集合；令为机器人的当前所在的房间节点；同时为每一个栅格增加一个标签 值Label，记录该栅格所在的房间节点。下面结合图4-图8来进行说明，具体的步骤如下：
[0093] 1)探索开始时，V = {S。}，￡ = 0，并设置Scu"ent= S。，如图4所示。
[0094] 2)根据前述方式来确定需探索的目标点，并移动至目标点，并判断是否检测到门， 若未检测到门转如步骤3)，若检测到门则转入步骤4)。
[0095] 3)当未检测到门时，将获得的栅格数据的标签值Label值设为S。。
[0096] 4)当未检测到门时，则将门同侧的栅格数据的标签值Label设为&，将门另一侧 的栅格数据的标签值Label设为Si，并更新房间集合V =队，SJ，门集合E = E U {e < \，Si > }，即对于无向图G而言，在节点\与S涧增加一条边（在E中新增一个门元素），如图 5_图6所示。同时，由于某些房间可能存在多个门，通过上述方式可能出现栅格数据的标签 值Label重复设置的情况，若出现这一情况，则转入步骤5)，否则，转入步骤6)。
[0097] 5)若设置前的标签值Label。；^与设置后的标签值Label new不相同，则将相应栅格 数据的标签值为重新设置为设置前的标签值Labels，并将房间集合V中标签值1^1^1。 1(1与 Labels对应的房间节点合并，并保持边数目不变，如图7-图8所示。
[0098] 6)更新Sramnt为机器人当前所在的房间节点。
[0099]上述所举例的图4-图8中，左侧为机器人探索的示意图，右侧为根据探索结果构 建的无向图G的示意图。
[0100] 本发明实施例通过将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点 表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房 间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图）算法中，最终得到 环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加 简单和高效。
[0101] 另一方面，还与本发明的上述实施例进行了实验。
[0102] 本实验中，采用了自主研发的服务机器人"可佳"，"可佳"的底盘装有Hokuyo UTM-30LN激光传感器，该激光传感器的范围为0. 1~30m，扫描频率为60Hz，角度分辨率为 0.25°。"可佳"的计算部件通常为笔记本或工控机（满足cpu intel i3及以上，内存4G 左右）。
[0103] 在应用中，操纵机器人在室内环境遍历一遍就能建立精确地识别出环境中的门， 并且建立栅格地图和拓扑地图。所建地图的实际环境模型如图9所示，实验结果如图10所 示。从实验结果可以看出，在比较复杂的家居环境中，利用本发明的方法能够比较精确地建 立环境地图，本发明实施例中的探索策略能够保证环境地图的准确性和完整性，门检测算 法能够快速有效的检测出门，从而建立基于门和房间之间的拓扑关系。
[0104] 通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可 以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解， 上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易 失性存储介质（可以是CD-ROM，U盘，移动硬盘等）中，包括若干指令用以使得一台计算机 设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。
[0105] 以上所述，仅为本发明较佳的【具体实施方式】，但本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换， 都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范 围为准。
【主权项】
1. 一种提高机器人路径规划效率的室内地图构建方法，其特征在于，该方法包括： 步骤A、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向图G =< V1E > ； 步骤Β、初始化房间集合V = {SJ，门集合￡ = 0，并标定机器人当前所在的房间节点 Scmrart= S ^利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获得 栅格数据的标签值Label设为Stl，并从中提取出边界栅格，获得边界点集合； 步骤C、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从候选 探索点集合中选择下一步探索的目标点； 步骤D、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更新， 并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为Stl;若检 测到门，则将门同侧的栅格数据的标签值Label设为Stl，将门另一侧的栅格数据的标签值 Label设为S1，并更新房间集合V = {SQ，SJ，门集合E = E U {e < SQ，S1S }; 步骤E、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤C~ 步骤D，直至完成当前室内环境的扫描过程。2. 根据权利要求1所述的方法，其特征在于，该方法还包括：当检测到门，并设置了门 两侧栅格数据的标签值Label后，判断是否出现标签值Label被重复设置的情况； 若是，且设置前的标签值Labeltjld与设置后的标签值Label new不相同，则将相应栅格数 据的标签值为重新设置为设置前的标签值Labeltjld，并将房间集合V中标签值1^1^1。1(1与 Labe Inew对应的房间节点合并。3. 根据权利要求1所述的方法，其特征在于，所述对所述边界点集合进行预处理，获得 候选探索点集合包括： 根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点； 利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再 对线段进行延伸与合并处理，获得若干线段； 记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。4. 根据权利要求1或3所述的方法，其特征在于，所述基于效用函数从候选探索点集合 中选择下一步探索的目标点包括： 基于效用函数计算每一候选探索点的函数值，其公式为：其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在候选 探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，Λ Θ (P)为候选探 索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点P的 距离，α取值范围为[〇, 1]。 将函数值最大的候选点作为下一步探索的目标点。5. 根据权利要求1所述的方法，其特征在于，该方法还包括：根据激光数据检测室内环 境中的门，其步骤为： 由于障碍物的存在，激光数据被划分为若干连续的区间，通过下述公式计算障碍物的 宽度： Width = L1 · L2 · cos(b-a) 其中，1^与L 2分别为机器人在当前位置处与障碍物两侧的距离，a与b分别为机器人 当前位置处的水平方向与障碍物两侧的夹角； 若满足下式，则判定该障碍物为门：其中，r是待扫描的室内环境中门宽度的平均值，dist_tol表示可以容忍的宽度差别。
【专利摘要】本发明公开了一种提高机器人路径规划效率的室内地图构建方法，该方法将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图)算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加简单和高效。
【IPC分类】G05D1/02
【公开号】CN104898660
【申请号】CN201510140528
【发明人】陈赢峰, 陈小平
【申请人】中国科学技术大学
【公开日】2015年9月9日
【申请日】2015年3月27日
【技术领域】
[0001] 本发明涉及室内机器人领域，尤其涉及一种提高机器人路径规划效率的室内地图 构建方法。
【背景技术】
[0002] 近来年，随着科技的发展和进步，在机器人领域，室内服务机器人掀起了一股新的 研宄和应用热潮。
[0003] 机器人在室内环境中的自主定位和导航需要活动空间的环境地图作为前提，精确 地地图表示和创建成为机器人自主移动的关键技术，同时也是完成其它室内任务的基础。 常用的环境表示方法有栅格地图，几何表示地图，特征地图和拓扑地图。
[0004] 其中，栅格地图即将整个环境分为若干个大小相同的栅格。在栅格地图中，通过某 种规划算法，找到一条代价最小，连结起点（机器人当前栅格位置）和终点（目标点所在的 栅格位置）的路径。在实际应用中，栅格地图的精度和大小往往相互矛盾，精度越高，栅格 的数目会相应增多，导致路径规划算法的规划空间过大，使得规划时间急剧增加，甚至无法 响应实时需要。
[0005]拓扑地图相比栅格地图具有高度的抽象性，适用于环境大而简单的情况。但是，该 方法缺乏环境的具体信息，难以适应定位等需要环境特征的任务。
【发明内容】
[0006] 本发明的目的是提供一种提高机器人路径规划效率的室内地图构建方法，使得机 器人导航中的路径规划算法更加简单和高效。
[0007] 本发明的目的是通过以下技术方案实现的：
[0008]-种提高机器人路径规划效率的室内地图构建方法，该方法包括：
[0009]步骤A、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向 图G= <V，E>;
[0010] 步骤B、初始化房间集合V= {SJ，门集合￡ = 0，并标定机器人当前所在的房间节 点利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获 得栅格数据的标签值Label设为\，并从中提取出边界栅格，获得边界点集合；
[0011] 步骤C、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从 候选探索点集合中选择下一步探索的目标点；
[0012] 步骤D、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更 新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为&若 检测到门，则将门同侧的栅格数据的标签值Label设为\，将门另一侧的栅格数据的标签值 Label设为Si，并更新房间集合V= {SQ，SJ，门集合E=EU{e<SQ，Sp};
[0013]步骤E、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤C~步骤D，直至完成当前室内环境的扫描过程。
[0014] 进一步的，该方法还包括：当检测到门，并设置了门两侧栅格数据的标签值Label 后，判断是否出现标签值Label被重复设置的情况；
[0015] 若是，且设置前的标签值Labels与设置后的标签值Labelnew不相同，则将相应栅 格数据的标签值为重新设置为设置前的标签值Labels，并将房间集合V中标签值Labels与Labelnew对应的房间节点合并。
[0016] 进一步的，所述对所述边界点集合进行预处理，获得候选探索点集合包括：
[0017] 根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；
[0018] 利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线 段，再对线段进行延伸与合并处理，获得若干线段；
[0019] 记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。
[0020] 进一步的，所述基于效用函数从候选探索点集合中选择下一步探索的目标点包 括：
[0021] 基于效用函数计算每一候选探索点的函数值，其公式为：
[0023] 其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在 候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，A0 (P)为候 选探索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点 P的距离，a取值范围为[0, 1]。
[0024] 将函数值最大的候选点作为下一步探索的目标点。
[0025] 进一步的，该方法还包括：根据激光数据检测室内环境中的门，其步骤为：
[0026]由于障碍物的存在，激光数据被划分为若干连续的区间，通过下述公式计算障碍 物的宽度：
[0027]Width=L: ?L2 ?cos(b-a)
[0028] 其中，1^与L2*别为机器人在当前位置处与障碍物两侧的距离，a与b分别为机 器人当前位置处的水平方向与障碍物两侧的夹角；
[0029] 若满足下式，则判定该障碍物为门：
[0031] 其中，fz是待扫描的室内环境中门宽度的平均值，DIST_T0L表示可以容忍的宽度 差别。
[0032] 由上述本发明提供的技术方案可以看出，将拓扑地图与栅格地图相结合，使得规 划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环 境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与 建图）算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导 航中的路径规划算法更加简单和高效。
【附图说明】
[0033] 为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用 的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本 领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他 附图。
[0034]图1为本发明实施例提供的一种提高机器人路径规划效率的室内地图构建方法；
[0035]图2为本发明实施例提供的激光数据被分割为连续区域的示意图；
[0036] 图3为本发明实施例提供的激光数据的直方图；
[0037]图4为本发明实施例提供的初始探索状态的示意图；
[0038] 图5为本发明实施例提供的机器人检测到门时的示意图；
[0039] 图6为本发明实施例提供的机器人检测到门时的示意图；
[0040]图7为本发明实施例提供的处理栅格数据标签值重复设置时的示意图；
[0041]图8为本发明实施例提供的处理栅格数据标签值重复设置时的示意图；
[0042] 图9为本发明实施例提供的实验时所构建实际环境的示意图；
[0043] 图10为本发明实施例提供的基于本发明方案进行实验的结果示意图。
【具体实施方式】
[0044] 下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整 地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本 发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施 例，都属于本发明的保护范围。
[0045] 实施例一
[0046] 图1为本发明实施例提供的一种提高机器人路径规划效率的室内地图构建方法。 如图1所示，该方法主要包括如下步骤：
[0047] 步骤11、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无 向图 G=<V，E>。
[0048]本发明实施例所建立的拓扑图以房间为节点，房间之间的门作为边，本方案中基 于门的检测和机器人的即时移动建立拓扑地图，而且同时创建栅格地图。
[0049] 室内环境中，通过门的位置就可以将区分不同的房间，而现有的房间分割方法都 是通过提取出已有的栅格地图上的线段，角点，多边形等几何特征，计算复杂且需要提前建 立栅格地图。
[0050]步骤12、初始化房间集合V= {S。}，门集合五二0，并标定机器人当前所在的房间 节点利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将 获得栅格数据的标签值Label设为\，并从中提取出边界栅格，获得边界点集合。
[0051] 步骤13、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从 候选探索点集合中选择下一步探索的目标点。
[0052] 步骤14、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行 更新，并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为 若检测到门，则将门同侧的栅格数据的标签值Label设为\，将门另一侧的栅格数据的标签 值Label设为Si，并更新房间集合V = {'SJ，门集合E = E U {eCS^Si〉}。
[0053] 更新后的门集合E = E U {e < Sd, Si> }表示新增一个用于连接房间节点S 与 Si的边元素e<SQ,Si>。
[0054] 进一步的，当检测到门，并设置了门两侧栅格数据的标签值Label后，判断是否出 现标签值Label被重复设置的情况；
[0055] 若是，且设置前的标签值Labels与设置后的标签值Label new不相同，则将相应栅 格数据的标签值为重新设置为设置前的标签值Labels，并将房间集合V中标签值Labels 与Label new对应的房间节点合并。
[0056] 步骤15、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步 骤13~步骤14,直至完成当前室内环境的扫描过程。
[0057] 为了便于理解本发明，下面分别针对门检测、探索策略及建立房间拓扑关系做详 细的说明。
[0058] 1、利用激光数据实现室内环境中的门检测。
[0059] 机器人在建图的过程中，不断接受周围环境的激光数据，对激光数据经行分析就 可以检测出环境中的门。
[0060] 如图2所示，由于障碍物的存在，激光数据被划分成为几个连续的区间，图2中的 门（door)即为一障碍物，a与b分别为机器人当前位置处的水平方向（X轴）与门两侧的 夹角；为了更为直观的表示图2,可以将其转换为图3所示的直方图，图3左侧的space则 为门之间的宽度。
[0061]计算障碍物宽度的公式为：
[0062] Width = L: ? L2 ? cos(b-a)
[0063] 其中，1^与L2分别为机器人在当前位置处与障碍物两侧的距离；
[0064] 若满足下式，则判定该障碍物为门：
[0066]其中，汞是待扫描的室内环境中门宽度的平均值，DIST_T0L表示可以容忍的宽度 差别。
[0067]当检测到门以后，则可根据机器人的位置计算得到门的两个端点的坐标，从而获 得门的位置和朝向。
[0068] 2、探索策略。
[0069] 机器人在探索的过程中不断的趋向于未知区域，获取未知区域的信息来扩充地 图，为了保证机器人探索的效率和地图的准确性，探索活动需要遵循一定的策略和方法。 [0 070] 2. 1提取未知边界，获得边界点集合。
[0071] 栅格地图中每个栅格单元的状态对应于空闲，占用，未知三种状态。为了建立完整 的环境地图，机器人只有不断靠近未知区域才是有意义的。
[0072] 本发明实施例中，定义栅格状态函数如下：
[0074]上式中，x，y为栅格的坐标，唯一确定一个栅格。若一个栅格A为边界栅格则需要 满足条件以下两个条件。
[0075] l)S(xA, yA) = 2 ；
[0076] 2)U为栅格A四邻域集合，若存在栅格B和C，
且 3C e "a ) = 1;其中，3为存在量词。
[0077] 通过上述方式找到所有的边界栅格，则完成了位置边界的提取，从而获得边界点 集合。
[0078] 2)确定候选点。
[0079] 首先，根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点；其 次，利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再 对线段进行延伸与合并处理，获得若干线段；最后，记录各线段的端点和角度，取线段的中 点沿中垂线朝外的方向为候选探索点。
[0080] 3)确定探索目标点。
[0081] 评价一个环境探索策略的标准是能否高效准确的建立完整或近似完整的环境地 图。本发明实施例在确定下一步探索点时，综合考虑了路径优化，信息增益，定位准确等因 素。
[0082] 未知环境中的探索，机器人需要根据已有的局部栅格地图的信息，确定下一步继 续感知环境的最优位置，确保能够顺利完成对未知环境的探索和建图。具体的算法步骤如 下：
[0083] 1)若候选点集合为空，则探索结束；否则转向步骤2)。
[0084] 2)基于效用函数计算每一候选探索点的函数值，并将函数值最大的候选点作为下 一步探索的目标点，效用函数公式为：
[0086] 其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在 候选探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，A 0 (P)为候 选探索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索 点P的距离，a取值范围为[0, 1]。
[0087] 3)驱使机器人到达目标点，并读取传感器数据，更新局部栅格地图和障碍物地图。
[0088] 4)提取局部地图的边界点，计算出新的候选点加入候选点列表，转到步骤1)。
[0089] 需要说明的是，在上述探索过程中，机器人还会根据前述的方式实时判断是否检 测到门，以便建立房间的拓扑关系，具体的在后文中会进行详细的描述。
[0090] 3、建立房间拓扑关系。
[0091] 通常室内环境房间与房间之间都有一个或多个门，只要能正确检测出门就能够得 到单独的房间和房间之间的拓扑关系，而将这种拓扑关系应用到导航的路径规划算法，可 以使得规划更高效而且简单。
[0092] 本发明实施例中，将拓扑地图表示成为无向图G = < V，E >，V表示房间节点集合， E表示门集合；令为机器人的当前所在的房间节点；同时为每一个栅格增加一个标签 值Label，记录该栅格所在的房间节点。下面结合图4-图8来进行说明，具体的步骤如下：
[0093] 1)探索开始时，V = {S。}，￡ = 0，并设置Scu"ent= S。，如图4所示。
[0094] 2)根据前述方式来确定需探索的目标点，并移动至目标点，并判断是否检测到门， 若未检测到门转如步骤3)，若检测到门则转入步骤4)。
[0095] 3)当未检测到门时，将获得的栅格数据的标签值Label值设为S。。
[0096] 4)当未检测到门时，则将门同侧的栅格数据的标签值Label设为&，将门另一侧 的栅格数据的标签值Label设为Si，并更新房间集合V =队，SJ，门集合E = E U {e < \，Si > }，即对于无向图G而言，在节点\与S涧增加一条边（在E中新增一个门元素），如图 5_图6所示。同时，由于某些房间可能存在多个门，通过上述方式可能出现栅格数据的标签 值Label重复设置的情况，若出现这一情况，则转入步骤5)，否则，转入步骤6)。
[0097] 5)若设置前的标签值Label。；^与设置后的标签值Label new不相同，则将相应栅格 数据的标签值为重新设置为设置前的标签值Labels，并将房间集合V中标签值1^1^1。 1(1与 Labels对应的房间节点合并，并保持边数目不变，如图7-图8所示。
[0098] 6)更新Sramnt为机器人当前所在的房间节点。
[0099]上述所举例的图4-图8中，左侧为机器人探索的示意图，右侧为根据探索结果构 建的无向图G的示意图。
[0100] 本发明实施例通过将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点 表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房 间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图）算法中，最终得到 环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加 简单和高效。
[0101] 另一方面，还与本发明的上述实施例进行了实验。
[0102] 本实验中，采用了自主研发的服务机器人"可佳"，"可佳"的底盘装有Hokuyo UTM-30LN激光传感器，该激光传感器的范围为0. 1~30m，扫描频率为60Hz，角度分辨率为 0.25°。"可佳"的计算部件通常为笔记本或工控机（满足cpu intel i3及以上，内存4G 左右）。
[0103] 在应用中，操纵机器人在室内环境遍历一遍就能建立精确地识别出环境中的门， 并且建立栅格地图和拓扑地图。所建地图的实际环境模型如图9所示，实验结果如图10所 示。从实验结果可以看出，在比较复杂的家居环境中，利用本发明的方法能够比较精确地建 立环境地图，本发明实施例中的探索策略能够保证环境地图的准确性和完整性，门检测算 法能够快速有效的检测出门，从而建立基于门和房间之间的拓扑关系。
[0104] 通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可 以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解， 上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易 失性存储介质（可以是CD-ROM，U盘，移动硬盘等）中，包括若干指令用以使得一台计算机 设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。
[0105] 以上所述，仅为本发明较佳的【具体实施方式】，但本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换， 都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范 围为准。
【主权项】
1. 一种提高机器人路径规划效率的室内地图构建方法，其特征在于，该方法包括： 步骤A、将待扫描的室内环境的拓扑地图表示为包含房间集合V与门集合E的无向图G =< V1E > ； 步骤Β、初始化房间集合V = {SJ，门集合￡ = 0，并标定机器人当前所在的房间节点 Scmrart= S ^利用机器人内置的激光传感器在当前位置对当前房间进行扫描；其中，将获得 栅格数据的标签值Label设为Stl，并从中提取出边界栅格，获得边界点集合； 步骤C、对所述边界点集合进行预处理，获得候选探索点集合，并基于效用函数从候选 探索点集合中选择下一步探索的目标点； 步骤D、该机器人向所述目标点移动，利用获得的栅格数据对初始栅格数据进行更新， 并判断是否检测到房门；若未检测到门，将获得的栅格数据的标签值Label设为Stl;若检 测到门，则将门同侧的栅格数据的标签值Label设为Stl，将门另一侧的栅格数据的标签值 Label设为S1，并更新房间集合V = {SQ，SJ，门集合E = E U {e < SQ，S1S }; 步骤E、提取所述目标点位置处采集到的栅格数据的边界点集合，并执行上述步骤C~ 步骤D，直至完成当前室内环境的扫描过程。2. 根据权利要求1所述的方法，其特征在于，该方法还包括：当检测到门，并设置了门 两侧栅格数据的标签值Label后，判断是否出现标签值Label被重复设置的情况； 若是，且设置前的标签值Labeltjld与设置后的标签值Label new不相同，则将相应栅格数 据的标签值为重新设置为设置前的标签值Labeltjld，并将房间集合V中标签值1^1^1。1(1与 Labe Inew对应的房间节点合并。3. 根据权利要求1所述的方法，其特征在于，所述对所述边界点集合进行预处理，获得 候选探索点集合包括： 根据点之间的距离将边界点分成若干子区域，并剔除子区域内的离异点； 利用Hough直线检测算法，用直线分别拟合每一子区域中的边界点，提取边界线段，再 对线段进行延伸与合并处理，获得若干线段； 记录各线段的端点和角度，取线段的中点沿中垂线朝外的方向为候选探索点。4. 根据权利要求1或3所述的方法，其特征在于，所述基于效用函数从候选探索点集合 中选择下一步探索的目标点包括： 基于效用函数计算每一候选探索点的函数值，其公式为：其中，S(P)表示在候选探索点P处激光覆盖范围内未知区域的面积，C(P)表示在候选 探索点P处激光覆盖范围内局部栅格地图和障碍物地图上角点的数目，Λ Θ (P)为候选探 索点P的朝向和当前机器人的朝向之间的夹角，D(P)为机器人当前位置与候选探索点P的 距离，α取值范围为[〇, 1]。 将函数值最大的候选点作为下一步探索的目标点。5. 根据权利要求1所述的方法，其特征在于，该方法还包括：根据激光数据检测室内环 境中的门，其步骤为： 由于障碍物的存在，激光数据被划分为若干连续的区间，通过下述公式计算障碍物的 宽度： Width = L1 · L2 · cos(b-a) 其中，1^与L 2分别为机器人在当前位置处与障碍物两侧的距离，a与b分别为机器人 当前位置处的水平方向与障碍物两侧的夹角； 若满足下式，则判定该障碍物为门：其中，r是待扫描的室内环境中门宽度的平均值，dist_tol表示可以容忍的宽度差别。
【专利摘要】本发明公开了一种提高机器人路径规划效率的室内地图构建方法，该方法将拓扑地图与栅格地图相结合，使得规划问题发生在拓扑节点表示的空间内，避免了全局空间的规划；同时，利用激光识别室内环境中的门，并建立起房间之间的拓扑关系，再将该过程融入到粒子滤波SLAM(同时定位与建图)算法中，最终得到环境表示的栅格地图和基于栅格地图的拓扑地图，使得机器人导航中的路径规划算法更加简单和高效。
【IPC分类】G05D1/02
【公开号】CN104898660
【申请号】CN201510140528
【发明人】陈赢峰, 陈小平
【申请人】中国科学技术大学
【公开日】2015年9月9日
【申请日】2015年3月27日
最新回复(0)