一种室内机器人的重定位方法及装置的制造方法

一种室内机器人的重定位方法及装置的制造方法
【专利摘要】本发明提供了一种室内机器人的重定位方法及装置，该方法包括：控制视觉传感器采集视觉图像数据和控制激光传感器采集激光点云数据；根据激光点云数据判断机器人在当前环境地图中是否丢失；若是，则根据激光点云数据进行激光重定位、并根据视觉图像数据进行视觉重定位，根据激光重定位结果和视觉重定位结果确定是否存在候选区域；当存在候选区域时，对机器人进行姿态优化，确定机器人当前位置信息和当前姿态信息；当不存在候选区域时，根据激光点云数据控制机器人进行避障移动，直到确定出存在候选区域。本发明实施例采用激光传感器和视觉传感器相结合的方式对机器人进行重定位，提高了机器人重定位的准确性，从而准确地对机器人进行自主导航。
【专利说明】
一种室内机器人的重定位方法及装置
技术领域
[0001] 本发明涉及室内机器人的控制技术领域，具体而言，涉及一种室内机器人的重定 位方法及装置。
【背景技术】
[0002] 目前，移动机器人通过传感器感知环境和自身状态，进而实现在有障碍物的环境 中面向目标自主运动，这就是通常所说的智能自主移动机器人的导航技术。而定位则是确 定移动机器人在工作环境中相对于全局坐标的位置及其本身的姿态，是移动机器人导航的 基本环节。然而，在机器人的系统关闭或者断电的情况下，当时机器人的位置和姿态发生变 化时，机器人启动后无法定位其所在地图位置和自身姿态，此时需要人为将机器人移动到 初始位置重新启动系统获取初始位置和姿态后才能进行自主导航。因此，为了实现机器人 在异常情况下自动定位，无须人为干预移动，给出了一系列关于机器人重定位的方法。
[0003] 当前，相关技术中提供了一种室内机器人的重定位方法，其中，常用的三种重定位 方法有:基于信标定位方法、环境地图模型匹配方法、或者基于视觉的定位方法。
[0004] 具体的，上述基于信标定位方法，具体包括:机器人通过各种传感器接收或观测环 境中已知位置的信标，经过计算得出机器人与信标的相对位置，再代入已知的信标位置坐 标，解出机器人的绝对坐标来实现定位。但该基于信标定位方法存在安装和维护信标成本 高的问题。
[0005] 具体的，上述环境地图模型匹配方法，具体包括:机器人通过自身的各种传感器探 测周围环境，利用感知到的局部环境信息进行局部的地图构造，并与其内部事先存储的完 整地图进行匹配。通过匹配关系获得自己在全局环境中的位置，从而确定自身的位置。但该 环境地图模型匹配方法仅适于一些结构相对简单的环境，存在应用场景受限的问题。
[0006] 具体的，上述基于视觉的定位方法，具体包括:利用图像特征点和目标特征点之间 的对应关系，根据摄像机模型，由获得的图像特征点坐标和世界坐标求解摄像机在世界坐 标系中的位姿，从而求得定位信息，另外，还可以根据序列图像之间的关系，通过一系列已 知变换，得出摄像机的三维位置信息。但该基于视觉的定位方法存在定位精度受图像质量 影响较大的问题。
[0007] 在实现本发明的过程中，发明人发现相关技术中至少存在以下问题：由于相关技 术中给出的室内机器人的重定位方法存在定位准确度低、应用场景受限的技术问题，无法 准确地确定出的机器人的当前位置信息和当前姿态信息，进而无法准确地对机器人进行自 主导航。

【发明内容】

[0008] 有鉴于此，本发明实施例的目的在于提供一种室内机器人的重定位方法，以提高 机器人重定位的准确性，从而准确地对机器人进行自主导航。
[0009 ]第一方面，本发明实施例提供了一种室内机器人的重定位方法，该方法包括：
[0010] 控制视觉传感器采集视觉图像数据和控制激光传感器采集激光点云数据；
[0011] 根据所述激光点云数据判断室内机器人在当前环境地图中是否丢失；
[0012] 若是，则根据所述激光点云数据进行激光重定位、以及根据所述视觉图像数据进 行视觉重定位，根据激光重定位结果和视觉重定位结果确定是否存在候选区域；
[0013] 当确定出存在候选区域时，对所述室内机器人进行姿态优化，确定所述室内机器 人的当前位置信息和当前姿态信息；
[0014]当确定出不存在候选区域时，根据所述激光点云数据控制所述室内机器人进行避 障移动，直到确定出存在候选区域。
[0015] 结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，所 述根据所述激光点云数据判断室内机器人在当前环境地图中是否丢失，包括：
[0016] 调取室内机器人当前位姿信息对应的激光点云数据；
[0017] 验证采集到的所述激光点云数据与调取的所述激光点云数据的一致性；
[0018] 若验证结果为不一致，则确定所述室内机器人在当前环境地图中已丢失。
[0019] 结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第二种 可能的实施方式，其中，所述验证采集到的所述激光点云数据与调取的所述激光点云数据 的一致性，包括：
[0020] 根据采集到的所述激光点云数据提取第一距离方向直方图；
[0021] 根据调取的所述激光点云数据提取第二距离方向直方图；
[0022] 对所述第一距离方向直方图和所述第二距离方向直方图进行直方图特征匹配，确 定所述第一距离方向直方图与所述第二距离方向直方图之间的直方图相似度；
[0023] 当所述直方图相似度小于第一预设阈值时，则确定验证结果为不一致。
[0024] 结合第一方面，本发明实施例提供了第一方面的第三种可能的实施方式，其中，所 述根据所述激光点云数据进行激光重定位、以及根据所述视觉图像数据进行视觉重定位， 根据激光重定位结果和视觉重定位结果确定是否存在候选区域，包括：
[0025] 根据所述激光点云数据进行激光重定位，根据激光重定位结果确定是否存在第一 候选区域；
[0026] 根据所述视觉图像数据进行视觉重定位，根据视觉重定位结果确定是否存在第二 候选区域；
[0027] 当至少存在所述第一候选区域和所述第二候选区域中的一个时，确定存在候选区 域。
[0028] 结合第一方面的第三种可能的实施方式，本发明实施例提供了第一方面的第四种 可能的实施方式，其中，根据所述激光点云数据进行激光重定位，根据激光重定位结果确定 是否存在第一候选区域，包括：
[0029]对所述激光点云数据对应的距离方向直方图和所述当前环境地图对应的关键帧 集合进行直方图特征匹配；
[0030] 计算所述第一距离方向直方图分别与所述关键帧集合中各个关键帧的相似度；
[0031] 将所述相似度大于第二预设阈值的关键帧作为第一候选区域。
[0032] 结合第一方面的第三种可能的实施方式，本发明实施例提供了第一方面的第五种 可能的实施方式，其中，根据所述视觉图像数据进行视觉重定位，根据视觉重定位结果确定 是否存在第二候选区域，包括：
[0033] 利用图像金字塔提取方法对所述视觉图像数据进行0RB特征提取；
[0034] 利用视觉词袋模型对提取到的0RB特征和所述当前环境地图对应的关键帧集合进 行场景匹配，计算所述0RB特征分别与所述关键帧集合中各个关键帧的场景相似度；
[0035] 将场景相似度大于第三预设阈值的关键帧作为候选关键帧；
[0036] 对所述0RB特征和确定出的所述候选关键帧进行0RB特征匹配，计算所述0RB特征 与所述候选关键帧中各个关键帧的特征相似度；
[0037] 将所述特征相似度大于第四预设阈值的关键帧作为第二候选区域。
[0038] 结合第一方面的第三种可能的实施方式，本发明实施例提供了第一方面的第六种 可能的实施方式，其中，对所述室内机器人进行姿态优化，确定所述室内机器人的当前位置 信息和当前姿态信息，包括：
[0039] 将所述第一候选区域和/或所述第二候选区域作为初始位姿信息估计值；
[0040] 利用迭代最近点算法以所述初始位姿信息估计值作为起始值、以预设步长、以及 预设旋转角开始，并缩小所述预设步长，直到缩小后的所述预设步长小于预设的步长阈值， 对所述激光点云数据与所述当前环境地图进行特征匹配，得到多个特征匹配相似度；
[0041] 将多个所述特征匹配相似度中最大值对应的位姿信息作为所述室内机器人的当 前位置信息和当前姿态信息。
[0042] 结合第一方面，本发明实施例提供了第一方面的第七种可能的实施方式，其中，根 据所述激光点云数据控制所述室内机器人进行避障移动，包括：
[0043]根据所述激光点云数据确定障碍物与所述室内机器人距离最远的方向角度；
[0044] 根据所述方向角度和公式distance〉=R/cos(beta)控制所述室内机器人进行避 障移动，其中，distance表示在角度beta下障碍物到激光传感器的距离，betae [alpha-Ji/ 2，alpha+Ji/2]，R表不室内机器人底盘的半径。
[0045] 第二方面，本发明实施例还提供了一种室内机器人的重定位装置，该装置包括：
[0046] 数据采集模块，用于控制视觉传感器采集视觉图像数据和控制激光传感器采集激 光点云数据；
[0047] 当前状态确定模块，用于根据所述激光点云数据判断室内机器人在当前环境地图 中是否丢失；
[0048]重定位模块，用于当室内机器人在当前环境地图中丢失时，则根据所述激光点云 数据进行激光重定位、以及根据所述视觉图像数据进行视觉重定位，根据激光重定位结果 和视觉重定位结果确定是否存在候选区域；
[0049] 姿态优化模块，用于当确定出存在候选区域时，对所述室内机器人进行姿态优化， 确定所述室内机器人的当前位置信息和当前姿态信息；
[0050] 避障移动模块，用于当确定出不存在候选区域时，根据所述激光点云数据控制所 述室内机器人进行避障移动，直到确定出存在候选区域。
[0051] 结合第二方面，本发明实施例提供了第二方面的第一种可能的实施方式，其中，所 述当前状态确定模块包括：
[0052]激光数据调取单元，用于调取室内机器人当前位姿信息对应的激光点云数据； [0053] 一致性验证单元，用于验证采集到的所述激光点云数据与调取的所述激光点云数 据的一致性；
[0054] 当前状态确定单元，用于若验证结果为不一致，则确定所述室内机器人在当前环 境地图中已丢失。
[0055] 结合第二方面的第一种可能的实施方式，本发明实施例提供了第二方面的第二种 可能的实施方式，其中，所述一致性验证单元包括：
[0056]第一直方图提取子单元，用于根据采集到的所述激光点云数据提取第一距离方向 直方图；
[0057]第二直方图提取子单元，用于根据调取的所述激光点云数据提取第二距离方向直 方图；
[0058]直方图相似度确定子单元，用于对所述第一距离方向直方图和所述第二距离方向 直方图进行直方图特征匹配，确定所述第一距离方向直方图与所述第二距离方向直方图之 间的直方图相似度；
[0059] 验证结果确定子单元，用于当所述直方图相似度小于第一预设阈值时，则确定验 证结果为不一致。
[0060] 结合第二方面，本发明实施例提供了第二方面的第三种可能的实施方式，其中，所 述重定位模块包括：
[0061] 激光重定位单元，用于根据所述激光点云数据进行激光重定位，根据激光重定位 结果确定是否存在第一候选区域；
[0062] 视觉重定位单元，用于根据所述视觉图像数据进行视觉重定位，根据视觉重定位 结果确定是否存在第二候选区域；
[0063] 候选区域确定单元，用于当至少存在所述第一候选区域和所述第二候选区域中的 一个时，确定存在候选区域。
[0064] 结合第二方面的第三种可能的实施方式，本发明实施例提供了第二方面的第四种 可能的实施方式，其中，所述激光重定位单元包括：
[0065]直方图特征匹配子单元，用于对所述激光点云数据对应的距离方向直方图和所述 当前环境地图对应的关键帧集合进行直方图特征匹配；
[0066] 相似度计算子单元，用于计算所述第一距离方向直方图分别与所述关键帧集合中 各个关键帧的相似度；
[0067] 第一候选区域确定子单元，用于将所述相似度大于第二预设阈值的关键帧作为第 一候选区域。
[0068] 结合第二方面的第三种可能的实施方式，本发明实施例提供了第二方面的第五种 可能的实施方式，其中，所述视觉重定位单元包括：
[0069] 0RB特征提取子单元，用于利用图像金字塔提取装置对所述视觉图像数据进行0RB 特征提取；
[0070] 场景相似度计算子单元，用于利用视觉词袋模型对提取到的0RB特征和所述当前 环境地图对应的关键帧集合进行场景匹配，计算所述0RB特征分别与所述关键帧集合中各 个关键帧的场景相似度；
[0071] 候选关键帧确定子单元，用于将场景相似度大于第三预设阈值的关键帧作为候选 关键帧；
[0072] 特征相似度计算子单元，用于对所述ORB特征和确定出的所述候选关键帧进行ORB 特征匹配，计算所述ORB特征与所述候选关键帧中各个关键帧的特征相似度；
[0073] 第二候选区域确定子单元，用于将所述特征相似度大于第四预设阈值的关键帧作 为第二候选区域。
[0074] 结合第二方面的第三种可能的实施方式，本发明实施例提供了第二方面的第六种 可能的实施方式，其中，所述姿态优化模块包括：
[0075] 初始值确定单元，用于将所述第一候选区域和/或所述第二候选区域作为初始位 姿信息估计值；
[0076] 特征匹配相似度确定单元，用于利用迭代最近点算法以所述初始位姿信息估计值 作为起始值、以预设步长、以及预设旋转角开始，并缩小所述预设步长，直到缩小后的所述 预设步长小于预设的步长阈值，对所述激光点云数据与所述当前环境地图进行特征匹配， 得到多个特征匹配相似度；
[0077] 当前位姿信息确定单元，用于将多个所述特征匹配相似度中最大值对应的位姿信 息作为所述室内机器人的当前位置信息和当前姿态信息。
[0078] 结合第二方面，本发明实施例提供了第二方面的第七种可能的实施方式，其中，所 述避障移动_吴块包括：
[0079]方向角度确定单元，用于根据所述激光点云数据确定障碍物与所述室内机器人距 离最远的方向角度；
[0080] 避障移动单元，用于根据所述方向角度和公式distance > =R/cos (beta)控制所 述室内机器人进行避障移动，其中，distance表示在角度beta下障碍物到激光传感器的距 离，betae [alpha-3i/2，alpha+3i/2]，R表示室内机器人底盘的半径。
[0081] 在本发明实施例提供的室内机器人的重定位方法及装置中，该方法包括:控制视 觉传感器采集视觉图像数据和控制激光传感器采集激光点云数据;根据激光点云数据判断 机器人在当前环境地图中是否丢失;若是，则根据激光点云数据进行激光重定位、并根据视 觉图像数据进行视觉重定位，根据激光重定位结果和视觉重定位结果确定是否存在候选区 域；当存在候选区域时，对机器人进行姿态优化，确定机器人当前位置信息和当前姿态信 息；当不存在候选区域时，根据激光点云数据控制机器人进行避障移动，直到确定出存在候 选区域。本发明实施例采用激光传感器和视觉传感器相结合的方式对机器人进行重定位， 提高了机器人重定位的准确性，从而准确地对机器人进行自主导航。
[0082] 为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合 所附附图，作详细说明如下。
【附图说明】
[0083] 为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附 图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对 范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这 些附图获得其他相关的附图。
[0084] 图1示出了本发明实施例所提供的一种室内机器人的重定位方法的流程示意图；
[0085] 图2示出了本发明实施例所提供的一种室内机器人的重定位装置的结构示意图。
【具体实施方式】
[0086] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例 中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅 是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实 施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的 实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实 施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所 有其他实施例，都属于本发明保护的范围。
[0087] 考虑到由于相关技术中给出的室内机器人的重定位方法存在定位准确度低、应用 场景受限的技术问题，无法准确地确定出的机器人的当前位置信息和当前姿态信息，进而 无法准确地对机器人进行自主导航。基于此，本发明实施例提供了一种室内机器人的重定 位方法及装置，下面通过实施例进行描述。
[0088] 如图1所示，本发明实施例提供了一种室内机器人的重定位方法，该方法包括步骤 S102-S110,具体如下：
[0089]步骤S102:控制视觉传感器采集视觉图像数据和控制激光传感器采集激光点云数 据；
[0090] 步骤S104:根据上述激光点云数据判断室内机器人在当前环境地图中是否丢失；
[0091] 步骤S106:若是，则根据上述激光点云数据进行激光重定位、以及根据上述视觉图 像数据进行视觉重定位，根据激光重定位结果和视觉重定位结果确定是否存在候选区域； [00 92] 步骤S108:当确定出存在候选区域时，对上述室内机器人进行姿态优化，确定上述 室内机器人的当前位置信息和当前姿态信息；
[0093] 步骤S110:当确定出不存在候选区域时，根据上述激光点云数据控制上述室内机 器人进行避障移动，直到确定出存在候选区域。
[0094] 其中，定位是指确定移动机器人在工作环境中相对于坐标的位置及该移动机器人 的姿态，是移动机器人导航的基本环节，但移动机器人由于系统关闭或者断电后被人移动 等异常情况，移动机器人再次启动后无法定位其所在地图位置和自身姿态，此时需要人为 将移动机器人移动到初始位置重新启动系统获取初始位置和姿态后才能进行自主导航，因 而，为了达到无需人员参与即可完成自主导航的目的，需要对该移动机器人进行重定位，确 定该移动机器人的当前位置信息和当前姿态信息。在二维世界中，机器人的位姿通常使用 三元组( XQ，yQ，0)来表示，其中，（XQ，yQ)表示机器人在地图中位置信息，Θ表示机器人的姿态 信息(旋转分量），在本发明提供的实施例中，首先，根据获取的激光点云数据自动判断机器 人在当前环境地图中是否丢失;然后，在丢失的情况下，结合激光传感器和视觉传感器进行 机器人重定位，进而获取机器人当前所在地图中的位置信息( XQ，yQ)和姿态信息Θ。
[0095] 其中，由于考虑到激光传感器不受光照变化的影响，视觉定位方法在复杂环境性 能表现优异，因而，采用基于视觉传感器和激光传感器相融合的室内机器人重定位技术方 法，利用激光传感器采集的数据进行环境地图模型匹配来弥补视觉定位受关照变化影响的 缺点，利用视觉重定位弥补环境地图模型匹配方法受复杂环境影响的不足。
[0096] 在本发明提供的实施例中，采用激光传感器和视觉传感器相结合的方式对机器人 进行重定位，提高了机器人重定位的准确性，从而准确地对机器人进行自主导航。
[0097] 其中，上述根据上述激光点云数据判断室内机器人在当前环境地图中是否丢失， 具体包括：
[0098] 调取室内机器人当前位姿信息对应的激光点云数据；
[0099]验证采集到的上述激光点云数据与调取的上述激光点云数据的一致性；
[0100] 若验证结果为不一致，则确定上述室内机器人在当前环境地图中已丢失。
[0101]具体的，上述验证采集到的上述激光点云数据与调取的上述激光点云数据的一致 性，具体包括：
[0102] 根据采集到的上述激光点云数据提取第一距离方向直方图；
[0103] 根据调取的上述激光点云数据提取第二距离方向直方图；
[0104] 对上述第一距离方向直方图和上述第二距离方向直方图进行直方图特征匹配，确 定上述第一距离方向直方图与上述第二距离方向直方图之间的直方图相似度；
[0105] 当上述直方图相似度小于第一预设阈值时，则确定验证结果为不一致。
[0106] 其中，上述距离方向直方图的提取方法具体包括：
[0107] 按照bin的预设数量将360度进行等分划分，得到包含预设数量bin的直方图；
[0108] 按照障碍物点的采集角度对激光点云数据进行bin划分；
[0109] 将各个bin对应的幅值累加至上述直方图对应的bin中，生成距离方向直方图。
[0110] 具体的，判断当前机器人在所建环境地图中的状态信息，利用当前的激光数据和 机器人获取位姿信息在该环境地图中所对应的数据进行一致性判断。将激光传感器采集的 激光点云数据转化为距离方向直方图特征，进行直方图匹配，如果匹配得到的直方图相似 度超过设定阈值thresholdl，则确定当前机器人没有丢失，否则启动重定位功能。
[0111]具体的，距离方向直方图：360度线性激光采集的激光点云数据为激光传感器旋转 一周获取到各个角度的障碍物到激光传感器的距离distance，将360度（2*PI)根据需要分 割成若干个bin，例如：分割成12个bin，每个bin包含30度，整个直方图包含12维，即12个 bin，然后，根据每个障碍物点的采集角度进行bin划分，其幅值累加到直方图中即生成了距 离方向直方图。
[0112] 其中，上述根据上述激光点云数据进行激光重定位、以及根据上述视觉图像数据 进行视觉重定位，根据激光重定位结果和视觉重定位结果确定是否存在候选区域，具体包 括：
[0113] 根据上述激光点云数据进行激光重定位，根据激光重定位结果确定是否存在第一 候选区域；
[0114] 根据上述视觉图像数据进行视觉重定位，根据视觉重定位结果确定是否存在第二 候选区域；
[0115] 当至少存在上述第一候选区域和上述第二候选区域中的一个时，确定存在候选区 域。
[0116] 具体的，根据上述激光点云数据进行激光重定位，根据激光重定位结果确定是否 存在第一候选区域，具体包括：
[0117]对上述激光点云数据对应的距离方向直方图和上述当前环境地图对应的关键帧 集合进行直方图特征匹配；
[0118] 计算上述第一距离方向直方图分别与上述关键帧集合中各个关键帧的相似度；
[0119] 将上述相似度大于第二预设阈值的关键帧作为第一候选区域。
[0120] 具体的，利用当前的激光距离方向直方图特征与构建环境地图过程中的各个关键 帧进行直方图匹配，如果超过设定阈值threSh 〇ld2,则作为第一候选区域筛选出来，记作： CandiateLaser〇
[0121] 具体的，根据上述视觉图像数据进行视觉重定位，根据视觉重定位结果确定是否 存在第二候选区域，具体包括：
[0122] 利用图像金字塔提取方法对上述视觉图像数据进行0RB特征提取；
[0123] 利用视觉词袋模型对提取到的0RB特征和上述当前环境地图对应的关键帧集合进 行场景匹配，计算上述0RB特征分别与上述关键帧集合中各个关键帧的场景相似度；
[0124] 将场景相似度大于第三预设阈值的关键帧作为候选关键帧；
[0125] 对上述0RB特征和确定出的上述候选关键帧进行0RB特征匹配，计算上述0RB特征 与上述候选关键帧中各个关键帧的特征相似度；
[0126] 将上述特征相似度大于第四预设阈值的关键帧作为第二候选区域。
[0127] 具体的，0RB特征是目前最优秀的特征提取与匹配算法之一，对于旋转和光照变化 有一定的容忍性，同时利用图像金字塔的提取方法，达到尺度不变形的效果。例如，考虑到 在一个20m*20m的办公室场景，M = N = 50cm
的情况下，需要采集19200张RGB关键 帧，在这么大范围的数据集上进行0RB特征匹配，效率比较低。在本发明提供的实施例中，为 了提高视觉重定位的效率，利用视觉词袋模型进行相似性区域判断，从而将相似度较高的 关键帧作为候选关键帧，记做RelocCanditates，在RelocCanditates中进行0RB特征匹配极 大的提高了重定位效率，如果匹配上则作为第二候选区域筛选出，记作:CandiateVision。
[0128] 其中，视觉词袋模型来源于词袋模型，词袋模型最先应用于自然语言处理中。类比 一篇文章有很多文字组合而成，如果将一张图片表示成由许多视觉单词组合而成，就能将 在文本检索领域的技巧逆向文件索引的直接利用在图像检索中，以文字检索的效率，图像 表示的"文字化"也有助于大规模图像检索系统的效率。
[0129] 在本发明提供的实施例中，利用视觉词袋模型进行视觉重定位，在视觉匹配时利 用视觉词典进行匹配加速，即先利用视觉词袋模型确定候选关键帧，再在确定出的候选关 键帧中进行特征匹配的方式，从而提高视觉重定位的效率。
[0130] 具体的，对上述室内机器人进行姿态优化，确定上述室内机器人的当前位置信息 和当前姿态信息，包括：
[0131] 将上述第一候选区域和/或上述第二候选区域作为初始位姿信息估计值；
[0132] 利用迭代最近点算法以上述初始位姿信息估计值作为起始值、以预设步长、以及 预设旋转角开始，并缩小上述预设步长，直到缩小后的上述预设步长小于预设的步长阈值， 对上述激光点云数据与上述当前环境地图进行特征匹配，得到多个特征匹配相似度；
[0133] 将多个上述特征匹配相似度中最大值对应的位姿信息作为上述室内机器人的当 前位置信息和当前姿态信息。
[0134] 其中，当确定出存在第一候选区域时，则将第一候选区域作为初始位姿信息估计 值，同样的，当确定出存在第二候选区域时，则将第二候选区域作为初始位姿信息估计值， 当确定出第一候选区域和第二候选区域同时存在时，则将第一候选区域和第二候选区域作 为初始位姿信息估计值。
[0135] 其中，将候选CandiateLaser和CandiateVision候选区域位姿作为初始位姿估计 值，使用迭代最近点（ICP)算法将当前激光采集的激光点云数据精确配准到当前环境地图 点云中，从而获得当前机器人在该环境地图中的准确位置信息和姿态信息。实验表明，这种 方法对于物体位姿识别有很强的稳健性，能够准确地实现机器人位姿计算。
[0136] 具体的，经过关键帧检索得到L个候选区域(该候选区域包括第一候选区域和/或 第二候选区域），可以进一步进行更为精确的匹配，在本发明提供的实施例中采用改进的贪 心算法，以候选区域为起始位置，步长StepLen(x，y)和旋转角stepRot开始进行激光点云数 据和环境地图的匹配，并不断缩短步长，达到局部收敛，最终选择匹配相似度最大的位置作 为重定位输出的位置。
[0137] 具体的，根据上述激光点云数据控制上述室内机器人进行避障移动，包括：
[0138] 根据上述激光点云数据确定障碍物与上述室内机器人距离最远的方向角度；
[0139] 根据上述方向角度和公式distance〉=R/cos(beta)控制上述室内机器人进行避 障移动，其中，distance表示在角度beta下障碍物到激光传感器的距离，betae [alpha-Ji/ 2，alpha+Ji/2]，R表不室内机器人底盘的半径。
[0140] 具体的，机器人在当前点进行匹配时，考虑到可能存在机器人所在位置的特征信 息不明显或者没有采集过关键帧的情况，因而，需要机器人自主移动到下一个区域进行重 定位。此时机器人处于丢失状态，所以系统创建的环境地图已经无法指导机器人进行移动 导航功能，为了保证机器人在移动过程中不碰撞障碍物，需要利用机器人当前采集的激光 点云数据指导移动。基于激光数据找到障碍物距离机器人最远的方向alpha，考虑到机器人 底盘的半径R，需要保证在机器人左右半径两侧无障碍物，其中，判断方法如下，在[alpha-3i/2，alpha+3i/2]范围内，激光采集的数据角度为betae [alpha-3i/2，alpha+3i/2]，保证 distance〉=R/cos(beta)，distance为在beta角度下障碍物到激光采集器的距离。
[0141] 在本发明提供的实施例中，当确定出机器人丢失且未匹配到候选区域时，首先在 保证机器人与障碍物不发生碰撞的情况下，控制机器人移动至下一个区域，然后重新对机 器人进行重定位，从而提高重定位成功率。
[0142] 进一步的，考虑到室内机器人可能进入一个新的环境中，因此需要构建当前环境 地图，其中，构建当前环境地图的过程具体包括：
[0143] 步骤a:获取机器人的运行记录；
[0144] 步骤b:根据获取的运行记录判断机器人在当前环境下是否是第一次运行；
[0145] 步骤c:若是，则根据激光传感器采集的激光点云数据构建环境地图模型；
[0146] 步骤d:按照预设栅格大小进行关键帧采集，对采集后的栅格进行标记，并确定各 个栅格的位置信息和姿态信息；
[0147] 步骤e:对采集到的关键帧进行特征提取，得到关键帧集合，其中，所述关键帧集合 中包含各个栅格的激光特征、视觉特征、位置信息和姿态信息。
[0148] 另外，为了提高关键帧采集效率，上述步骤还包括:检测当前栅格是否有标记，若 否，则执行步骤d，若是，则控制机器人移动至下一栅格。
[0149] 具体的，在机器人到达新环境第一次启动时，需要进行室内环境的建图。在建图的 过程中
的栅格大小进行关键帧采集，Μ、N分别表示栅格的长宽，单位 为厘米，step为姿态角度采样间隔，针对激光关键帧采集step = 2π，实时获取当前机器人的 位置(XQ，y〇)和姿态信息Θ，判断每个栅格是否已采集关键帧，若未采集则进行激光数据和 RGB数据的关键帧采集并记录当前关键帧的位姿信息(xo，yo，Θ )，并将该栅格标记为1，再次 来到该区域则不进行关键帧采集已节省存储空间。地图创建完成后，对采集的关键帧进行 特征提取操作并保存到关键帧集合中，用KeyframeSet表示。
[0150] 在本发明实施例提供的室内机器人的重定位方法中，采用激光传感器和视觉传感 器相结合的方式对机器人进行重定位，提高了机器人重定位的准确性，从而准确地对机器 人进行自主导航;进一步的，利用视觉词袋模型进行视觉重定位，在视觉匹配时利用视觉词 典进行匹配加速，即先利用视觉词袋模型确定候选关键帧，再在确定出的候选关键帧中进 行特征匹配的方式，从而提高视觉重定位的效率;更进一步的，当确定出机器人丢失且未匹 配到候选区域时，首先在保证机器人与障碍物不发生碰撞的情况下，控制机器人移动至下 一个区域，然后重新对机器人进行重定位，从而提高重定位成功率。
[0151] 本发明实施例还提供一种室内机器人的重定位装置，如图2所示，该装置包括：
[0152] 数据采集模块202,用于控制视觉传感器采集视觉图像数据和控制激光传感器采 集激光点云数据；
[0153]当前状态确定模块204,用于根据上述激光点云数据判断室内机器人在当前环境 地图中是否丢失；
[0154] 重定位模块206,用于当室内机器人在当前环境地图中丢失时，则根据上述激光点 云数据进行激光重定位、以及根据上述视觉图像数据进行视觉重定位，根据激光重定位结 果和视觉重定位结果确定是否存在候选区域；
[0155] 姿态优化模块208,用于当确定出存在候选区域时，对上述室内机器人进行姿态优 化，确定上述室内机器人的当前位置信息和当前姿态信息；
[0156] 避障移动模块210,用于当确定出不存在候选区域时，根据上述激光点云数据控制 上述室内机器人进行避障移动，直到确定出存在候选区域。
[0157] 在本发明提供的实施例中，采用激光传感器和视觉传感器相结合的方式对机器人 进行重定位，提高了机器人重定位的准确性，从而准确地对机器人进行自主导航。
[0158] 进一步的，上述当前状态确定模块204包括：
[0159] 激光数据调取单元，用于调取室内机器人当前位姿信息对应的激光点云数据； [0160] 一致性验证单元，用于验证采集到的上述激光点云数据与调取的上述激光点云数 据的一致性；
[0161] 当前状态确定单元，用于若验证结果为不一致，则确定上述室内机器人在当前环 境地图中已丢失。
[0162] 进一步的，上述一致性验证单元包括：
[0163] 第一直方图提取子单元，用于根据采集到的上述激光点云数据提取第一距离方向 直方图；
[0164] 第二直方图提取子单元，用于根据调取的上述激光点云数据提取第二距离方向直 方图；
[0165] 直方图相似度确定子单元，用于对上述第一距离方向直方图和上述第二距离方向 直方图进行直方图特征匹配，确定上述第一距离方向直方图与上述第二距离方向直方图之 间的直方图相似度；
[0166] 验证结果确定子单元，用于当上述直方图相似度小于第一预设阈值时，则确定验 证结果为不一致。
[0167] 进一步的，上述重定位模块206包括：
[0168] 激光重定位单元，用于根据上述激光点云数据进行激光重定位，根据激光重定位 结果确定是否存在第一候选区域；
[0169] 视觉重定位单元，用于根据上述视觉图像数据进行视觉重定位，根据视觉重定位 结果确定是否存在第二候选区域；
[0170] 候选区域确定单元，用于当至少存在上述第一候选区域和上述第二候选区域中的 一个时，确定存在候选区域。
[0171 ]进一步的，上述激光重定位单元包括：
[0172]直方图特征匹配子单元，用于对上述激光点云数据对应的距离方向直方图和上述 当前环境地图对应的关键帧集合进行直方图特征匹配；
[0173] 相似度计算子单元，用于计算上述第一距离方向直方图分别与上述关键帧集合中 各个关键帧的相似度；
[0174] 第一候选区域确定子单元，用于将上述相似度大于第二预设阈值的关键帧作为第 一候选区域。
[0175] 进一步的，上述视觉重定位单元包括：
[0176] 0RB特征提取子单元，用于利用图像金字塔提取装置对上述视觉图像数据进行0RB 特征提取；
[0177]场景相似度计算子单元，用于利用视觉词袋模型对提取到的0RB特征和上述当前 环境地图对应的关键帧集合进行场景匹配，计算上述0RB特征分别与上述关键帧集合中各 个关键帧的场景相似度；
[0178]候选关键帧确定子单元，用于将场景相似度大于第三预设阈值的关键帧作为候选 关键帧；
[0179]特征相似度计算子单元，用于对上述0RB特征和确定出的上述候选关键帧进行0RB 特征匹配，计算上述ORB特征与上述候选关键帧中各个关键帧的特征相似度；
[0180] 第二候选区域确定子单元，用于将上述特征相似度大于第四预设阈值的关键帧作 为第二候选区域。
[0181] 进一步的，上述姿态优化模块208包括：
[0182] 初始值确定单元，用于将上述第一候选区域和/或上述第二候选区域作为初始位 姿信息估计值；
[0183] 特征匹配相似度确定单元，用于利用迭代最近点算法以上述初始位姿信息估计值 作为起始值、以预设步长、以及预设旋转角开始，并缩小上述预设步长，直到缩小后的上述 预设步长小于预设的步长阈值，对上述激光点云数据与上述当前环境地图进行特征匹配， 得到多个特征匹配相似度；
[0184] 当前位姿信息确定单元，用于将多个上述特征匹配相似度中最大值对应的位姿信 息作为上述室内机器人的当前位置信息和当前姿态信息。
[0185] 进一步的，上述避障移动模块210包括：
[0186] 方向角度确定单元，用于根据上述激光点云数据确定障碍物与上述室内机器人距 离最远的方向角度；
[0187] 避障移动单元，用于根据上述方向角度和公式distance > =R/cos( beta)控制上 述室内机器人进行避障移动，其中，distance表示在角度beta下障碍物到激光传感器的距 离，betae [alpha-3i/2，alpha+3i/2]，R表示室内机器人底盘的半径。
[0188] 在本发明实施例提供的室内机器人的重定位装置中，采用激光传感器和视觉传感 器相结合的方式对机器人进行重定位，提高了机器人重定位的准确性，从而准确地对机器 人进行自主导航;进一步的，利用视觉词袋模型进行视觉重定位，在视觉匹配时利用视觉词 典进行匹配加速，即先利用视觉词袋模型确定候选关键帧，再在确定出的候选关键帧中进 行特征匹配的方式，从而提高视觉重定位的效率;更进一步的，当确定出机器人丢失且未匹 配到候选区域时，首先在保证机器人与障碍物不发生碰撞的情况下，控制机器人移动至下 一个区域，然后重新对机器人进行重定位，从而提高重定位成功率。
[0189] 本发明实施例所提供的室内机器人的重定位装置可以为设备上的特定硬件或者 安装于设备上的软件或固件等。本发明实施例所提供的装置，其实现原理及产生的技术效 果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施 例中相应内容。所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，前述描述的 系统、装置和单元的具体工作过程，均可以参考上述方法实施例中的对应过程，在此不再赘 述。
[0190] 在本发明所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方 式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻 辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可 以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间 的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连 接，可以是电性，机械或其它的形式。
[0191]所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显 示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个 网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目 的。
[0192] 另外，在本发明提供的实施例中的各功能单元可以集成在一个处理单元中，也可 以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
[0193] 所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以 存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说 对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计 算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个 人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。 而前述的存储介质包括:U盘、移动硬盘、只读存储器(Read-Only Memory，R0M)、随机存取存 储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
[0194] 应注意到:相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一 个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，此外，术语"第 一"、"第二"、"第三"等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0195] 最后应说明的是：以上所述实施例，仅为本发明的【具体实施方式】，用以说明本发明 的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发 明进行了详细的说明，本领域的普通技术人员应当理解:任何熟悉本技术领域的技术人员 在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻 易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使 相应技术方案的本质脱离本发明实施例技术方案的精神和范围。都应涵盖在本发明的保护 范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。
【主权项】
1. 一种室内机器人的重定位方法，其特征在于，所述方法包括： 控制视觉传感器采集视觉图像数据和控制激光传感器采集激光点云数据； 根据所述激光点云数据判断室内机器人在当前环境地图中是否丢失； 若是，则根据所述激光点云数据进行激光重定位、以及根据所述视觉图像数据进行视 觉重定位，根据激光重定位结果和视觉重定位结果确定是否存在候选区域； 当确定出存在候选区域时，对所述室内机器人进行姿态优化，确定所述室内机器人的 当前位置信息和当前姿态信息； 当确定出不存在候选区域时，根据所述激光点云数据控制所述室内机器人进行避障移 动，直到确定出存在候选区域。2. 根据权利要求1所述的方法，其特征在于，所述根据所述激光点云数据判断室内机器 人在当前环境地图中是否丢失，包括： 调取室内机器人当前位姿信息对应的激光点云数据； 验证采集到的所述激光点云数据与调取的所述激光点云数据的一致性； 若验证结果为不一致，则确定所述室内机器人在当前环境地图中已丢失。3. 根据权利要求2所述的方法，其特征在于，所述验证采集到的所述激光点云数据与调 取的所述激光点云数据的一致性，包括： 根据采集到的所述激光点云数据提取第一距离方向直方图； 根据调取的所述激光点云数据提取第二距离方向直方图； 对所述第一距离方向直方图和所述第二距离方向直方图进行直方图特征匹配，确定所 述第一距离方向直方图与所述第二距离方向直方图之间的直方图相似度； 当所述直方图相似度小于第一预设阈值时，则确定验证结果为不一致。4. 根据权利要求1所述的方法，其特征在于，所述根据所述激光点云数据进行激光重定 位、以及根据所述视觉图像数据进行视觉重定位，根据激光重定位结果和视觉重定位结果 确定是否存在候选区域，包括： 根据所述激光点云数据进行激光重定位，根据激光重定位结果确定是否存在第一候选 区域； 根据所述视觉图像数据进行视觉重定位，根据视觉重定位结果确定是否存在第二候选 区域； 当至少存在所述第一候选区域和所述第二候选区域中的一个时，确定存在候选区域。5. 根据权利要求4所述的方法，其特征在于，根据所述激光点云数据进行激光重定位， 根据激光重定位结果确定是否存在第一候选区域，包括： 对所述激光点云数据对应的距离方向直方图和所述当前环境地图对应的关键帧集合 进行直方图特征匹配； 计算所述第一距离方向直方图分别与所述关键帧集合中各个关键帧的相似度； 将所述相似度大于第二预设阈值的关键帧作为第一候选区域。6. 根据权利要求4所述的方法，其特征在于，根据所述视觉图像数据进行视觉重定位， 根据视觉重定位结果确定是否存在第二候选区域，包括： 利用图像金字塔提取方法对所述视觉图像数据进行ORB特征提取； 利用视觉词袋模型对提取到的ORB特征和所述当前环境地图对应的关键帧集合进行场 景匹配，计算所述ORB特征分别与所述关键帧集合中各个关键帧的场景相似度； 将场景相似度大于第三预设阈值的关键帧作为候选关键帧； 对所述ORB特征和确定出的所述候选关键帧进行ORB特征匹配，计算所述ORB特征与所 述候选关键帧中各个关键帧的特征相似度； 将所述特征相似度大于第四预设阈值的关键帧作为第二候选区域。7. 根据权利要求4所述的方法，其特征在于，对所述室内机器人进行姿态优化，确定所 述室内机器人的当前位置信息和当前姿态信息，包括： 将所述第一候选区域和/或所述第二候选区域作为初始位姿信息估计值； 利用迭代最近点算法以所述初始位姿信息估计值作为起始值、以预设步长、以及预设 旋转角开始，并缩小所述预设步长，直到缩小后的所述预设步长小于预设的步长阈值，对所 述激光点云数据与所述当前环境地图进行特征匹配，得到多个特征匹配相似度； 将多个所述特征匹配相似度中最大值对应的位姿信息作为所述室内机器人的当前位 置信息和当前姿态信息。8. 根据权利要求1所述的方法，其特征在于，根据所述激光点云数据控制所述室内机器 人进行避障移动，包括： 根据所述激光点云数据确定障碍物与所述室内机器人距离最远的方向角度； 根据所述方向角度和公式distance〉= R/cos( beta)控制所述室内机器人进行避障移 动，其中，distance表示在角度beta下障碍物到激光传感器的距离，betae [alpha-Ji/2， alpha+Ji/2]，R表不室内机器人底盘的半径。9. 一种室内机器人的重定位装置，其特征在于，所述装置包括： 数据采集模块，用于控制视觉传感器采集视觉图像数据和控制激光传感器采集激光点 云数据； 当前状态确定模块，用于根据所述激光点云数据判断室内机器人在当前环境地图中是 否丢失； 重定位模块，用于当室内机器人在当前环境地图中丢失时，则根据所述激光点云数据 进行激光重定位、以及根据所述视觉图像数据进行视觉重定位，根据激光重定位结果和视 觉重定位结果确定是否存在候选区域； 姿态优化模块，用于当确定出存在候选区域时，对所述室内机器人进行姿态优化，确定 所述室内机器人的当前位置信息和当前姿态信息； 避障移动模块，用于当确定出不存在候选区域时，根据所述激光点云数据控制所述室 内机器人进行避障移动，直到确定出存在候选区域。10. 根据权利要求9所述的装置，其特征在于，所述当前状态确定模块包括： 激光数据调取单元，用于调取室内机器人当前位姿信息对应的激光点云数据； 一致性验证单元，用于验证采集到的所述激光点云数据与调取的所述激光点云数据的 一致性； 当前状态确定单元，用于若验证结果为不一致，则确定所述室内机器人在当前环境地 图中已丢失。11. 根据权利要求10所述的装置，其特征在于，所述一致性验证单元包括： 第一直方图提取子单元，用于根据采集到的所述激光点云数据提取第一距离方向直方 图； 第二直方图提取子单元，用于根据调取的所述激光点云数据提取第二距离方向直方 图； 直方图相似度确定子单元，用于对所述第一距离方向直方图和所述第二距离方向直方 图进行直方图特征匹配，确定所述第一距离方向直方图与所述第二距离方向直方图之间的 直方图相似度； 验证结果确定子单元，用于当所述直方图相似度小于第一预设阈值时，则确定验证结 果为不一致。12. 根据权利要求9所述的装置，其特征在于，所述重定位模块包括： 激光重定位单元，用于根据所述激光点云数据进行激光重定位，根据激光重定位结果 确定是否存在第一候选区域； 视觉重定位单元，用于根据所述视觉图像数据进行视觉重定位，根据视觉重定位结果 确定是否存在第二候选区域； 候选区域确定单元，用于当至少存在所述第一候选区域和所述第二候选区域中的一个 时，确定存在候选区域。13. 根据权利要求12所述的装置，其特征在于，所述激光重定位单元包括： 直方图特征匹配子单元，用于对所述激光点云数据对应的距离方向直方图和所述当前 环境地图对应的关键帧集合进行直方图特征匹配； 相似度计算子单元，用于计算所述第一距离方向直方图分别与所述关键帧集合中各个 关键帧的相似度； 第一候选区域确定子单元，用于将所述相似度大于第二预设阈值的关键帧作为第一候 选区域。14. 根据权利要求12所述的装置，其特征在于，所述视觉重定位单元包括： ORB特征提取子单元，用于利用图像金字塔提取装置对所述视觉图像数据进行ORB特征 提取； 场景相似度计算子单元，用于利用视觉词袋模型对提取到的ORB特征和所述当前环境 地图对应的关键帧集合进行场景匹配，计算所述ORB特征分别与所述关键帧集合中各个关 键帧的场景相似度； 候选关键帧确定子单元，用于将场景相似度大于第三预设阈值的关键帧作为候选关键 帧； 特征相似度计算子单元，用于对所述ORB特征和确定出的所述候选关键帧进行ORB特征 匹配，计算所述ORB特征与所述候选关键帧中各个关键帧的特征相似度； 第二候选区域确定子单元，用于将所述特征相似度大于第四预设阈值的关键帧作为第 二候选区域。15. 根据权利要求12所述的装置，其特征在于，所述姿态优化模块包括： 初始值确定单元，用于将所述第一候选区域和/或所述第二候选区域作为初始位姿信 息估计值； 特征匹配相似度确定单元，用于利用迭代最近点算法以所述初始位姿信息估计值作为 起始值、以预设步长、以及预设旋转角开始，并缩小所述预设步长，直到缩小后的所述预设 步长小于预设的步长阈值，对所述激光点云数据与所述当前环境地图进行特征匹配，得到 多个特征匹配相似度； 当前位姿信息确定单元，用于将多个所述特征匹配相似度中最大值对应的位姿信息作 为所述室内机器人的当前位置信息和当前姿态信息。16.根据权利要求9所述的装置，其特征在于，所述避障移动模块包括： 方向角度确定单元，用于根据所述激光点云数据确定障碍物与所述室内机器人距离最 远的方向角度； 避障移动单元，用于根据所述方向角度和公式distance〉= R/cos (beta)控制所述室内 机器人进行避障移动，其中，distance表示在角度beta下障碍物到激光传感器的距离，beta e [alpha-3i/2，alpha+3r/2]，R表不室内机器人底盘的半径。
【文档编号】G01C21/20GK106092104SQ201610738132
【公开日】2016年11月9日
【申请日】2016年8月26日
【发明人】魏磊磊
【申请人】深圳微服机器人科技有限公司