CN112461230B - 机器人重定位方法、装置、机器人和可读存储介质 - Google Patents

Abstract

本发明实施例公开了机器人重定位方法、装置、机器人和可读存储介质，该方法包括：利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定机器人的第一估计位姿；根据第一估计位姿进行多线激光重定位，以确定机器人的第二估计位姿；根据重定位模式从第一估计位姿和第二估计位姿中选择待修正的估计位姿；利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于待修正的估计位姿对应的预设角度协方差阈值。本发明结合多目视觉和多线激光实现机器人的重定位，使得机器人重定位的准确度更高、鲁棒性更强。

Description

机器人重定位方法、装置、机器人和可读存储介质

技术领域

本发明涉及人工智能领域，尤其涉及一种机器人重定位方法、装置、机器人和可读存储介质。

背景技术

在商超、机场、办公、园区等大型导航应用场景中，因往往存在多动态物体、多重复场景以及多空旷狭长区域等高复杂环境的影响，单一传感器方案，如仅靠单目、多目视觉或单线、多线激光，和简单的多传感器融合方案，如单目/多目视觉+单线激光、单目视觉+多线激光等，由于其自身视野或量程、以及特征描述和庞大数据量处理等能力的局限性，往往难以同时提供高精度、高鲁棒性的重定位服务。

发明内容

鉴于上述问题，本发明提出一种机器人重定位方法、装置、机器人和可读存储介质。

本发明的一个实施例提出一种机器人重定位方法，该方法包括：

根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；

在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；

根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；

根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；

利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。

本发明实施例所述的机器人重定位方法，所述利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，包括：

对第

个图像帧提取对应的图像特征

和图像描述子

表示第j个同步相机采集的第i个图像帧；

利用图像特征

和图像描述子

在对应的回环数据库中进行回环检索，以确定第

个图像帧与回环数据库中各个回环候选帧的内敛点匹配数目；

若最大内敛点匹配数目小于预设的匹配数目阈值，则第

个图像帧对应的回环检索信息设置为空；

若最大内敛点匹配数目大于等于所述匹配数目阈值，则确定最大内敛点匹配数目对应的回环帧的第一回环索引，并根据所述第一回环索引对应的回环检索信息确定第

个图像帧对应的回环检索信息；

在全部图像帧对应的回环检索信息均为空时，所述多目视觉重定位失败；

在全部图像帧对应的回环检索信息不全部为空时，将所述第一回环索引对应的激光关键帧位姿作为所述第一估计位姿。

本发明实施例所述的机器人重定位方法，还包括：

确定用于获得最大内敛点匹配数目的第t个同步相机采集的第z个图像帧，以及第

个图像帧对应的回环检索信息；

确定除了第t个同步相机以外的各个同步相机采集的第z个图像帧与所述回环数据库中各个回环候选帧的内敛点匹配数目；

若除了第t个同步相机以外的各个同步相机对应的最大内敛点匹配数目对应的第二回环索引不等于所述第一回环索引，则利用对应的第二回环索引对应的回环检索结果更新对应的回环检索信息；

利用各个回环检索信息和各个同步相机采集的第z个图像帧优化所述第一估计位姿。

进一步的，本发明实施例所述的机器人重定位方法，还包括：

获取各个图像帧对应的里程计；

在所述机器人旋转完成时，确定当前图像帧对应的里程计；

利用所述当前图像帧对应的里程计与用于确定所述第一估计位姿的图像帧对应的里程计的差值补偿所述第一估计位姿的偏航角。

本发明实施例所述的机器人重定位方法，所述根据所述第一估计位姿进行多线激光重定位，包括：

以所述第一估计位姿对应的位置坐标为中心，预设的第一距离值为半径利用多线激光获取各个激光相邻关键帧；

分别计算所述第一估计位姿与所述各个激光相邻关键帧对应的位姿之间的距离；

确定距离所述第一估计位姿最近的激光相邻关键帧对应的位姿作为最佳相邻位姿；

以所述最佳相邻位姿对应的位置为中心，预设的第二距离值为半径利用所述多线激光获取所述第二距离值对应的各个相邻激光关键帧，所述第二距离值小于所述第一距离值；

利用各个相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，并确定补偿结果对应的ICP(Iterative Closest Point，迭代最近点)匹配的均方误差；

从各个相邻激光关键帧对应的ICP匹配的均方误差中确定最小均方误差；

若最小均方误差大于等于预设的均方误差阈值，则多线激光重定位失败；

若最小均方误差小于预设的均方误差阈值，则多线激光重定位成功，将经过补偿的最佳相邻位姿作为第二估计位姿。

本发明实施例所述的机器人重定位方法，每一个相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，包括：

在第p次补偿时，利用p-1倍的预设角度常量补偿最佳相邻位姿对应的偏航角，1≤p≤P，P为所述预设第二数目次补偿；

将第p次补偿对应的估计位姿对应的激光点云信息映射至map坐标系下，以确定第p次补偿对应的估计位姿对应的点云图；

抽取在预先构建map地图时第p次补偿对应的估计位姿前后各预设数目帧激光关键帧对应的map坐标系下的局部激光点云信息，以利用各个局部激光点云信息确定第p次补偿对应的估计位姿对应局部匹配子图；

将第p次补偿对应的估计位姿对应的点云图与第p次补偿对应的估计位姿对应的局部匹配子图进行ICP匹配，并计算第p次补偿的ICP匹配的均方误差和相对变化位姿；

若第p次补偿的ICP匹配的均方误差大于等于预设的均方误差阈值，则第p次补偿无效；

若第p次补偿的ICP匹配的均方误差小于预设的均方误差阈值，则利用所述相对变化位姿对所述第p次补偿对应的估计位姿进行修正，直至所述预设第二数目次补偿完成。

本发明实施例所述的机器人重定位方法，所述根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式，包括：

若有预先设定的机器人初始位姿，则所述重定位模式为局部重定位模式；

若无预先设定的机器人初始位姿，则所述重定位模式为全局重定位模式；

进一步的，根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿，包括：

在所述重定位模式为局部重定位模式时：

若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的位置和所述第二估计位姿的位置的距离小于预设的距离阈值，以及所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；

若仅成功获取到所述第二估计位姿，则选择所述第二估计位姿作为待修正的估计位姿；

在所述重定位模式为全局重定位模式时：

若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；

若仅成功获取到所述第一估计位姿，则选择所述第一估计位姿作为待修正的估计位姿。

本发明的再一个实施例提供的一种机器人重定位装置，该装置包括：

重定位模式确定模块，用于根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；

第一估计位姿确定模块，用于在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；

第二估计位姿确定模块，用于根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；

待修正估计位姿选择模块，用于根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；

估计位姿修正模块，用于利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。

本发明实施例涉及一种机器人，包括存储器和处理器，所述存储器用于存储计算机程序，所述计算机程序在所述处理器上运行时执行本发明实施例所述的机器人重定位方法。

本发明实施例涉及一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行本发明实施例所述的机器人重定位方法。

本发明公开的机器人重定位方法包括：根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。本发明的技术方案结合多目视觉和多线激光实现机器人的重定位，使得机器人重定位的准确度更高、鲁棒性更强。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本发明实施例提出的一种机器人重定位方法的流程示意图；

图2示出了本发明实施例提出的一种多目相机和多线激光在机器人刚体结构上的布局示意图；

图3示出了本发明实施例提出的一种机器人利用多目相机进行多目视觉重定位的流程示意图；

图4示出了本发明实施例提出的另一种机器人利用多目相机进行多目视觉重定位的流程示意图；

图5示出了本发明实施例提出的另再种机器人利用多目相机进行多目视觉重定位的流程示意图；

图6示出了本发明实施例提出的一种机器人利用多线激光进行多线激光重定位的流程示意图；

图7示出了本发明实施例提出的一种机器人偏航角补偿方法的流程示意图；

图8示出了本发明实施例提出的一种机器人选择待修正估计位姿方法的流程示意图。

主要元件符号说明：

1-机器人重定位装置；10-重定位模式确定模块；20-第一估计位姿确定模块；30-第二估计位姿确定模块；40-待修正估计位姿选择模块；50-估计位姿修正模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

本发明公开的机器人重定位方法以四目相机和单颗多线激光雷达为例，如图2所示，图中矩形体代表四目相机和多线激光雷达安装的刚体结构；图中矩形体四周，即前后左右个安装一个相机，可以将四目相机前后左右分别布置在刚体中心的水平面上，多目相机时间同步；图中矩形体顶部安装一颗多线激光雷达，多线激光雷达布置于刚体正上方中心位置，并可以360度水平扫描视野。可以理解，图中的刚体结构示意性的表示机器人，机器人可以是具有人体结构的机器人、可以是圆柱体扫地机器人，还可以是任意结构的机器人，在此机器人的形状不做限制。

本发明包括全局重定位和局部重定位两种模式，全局重定位和局部重定位模式的区分取决于是否有用户指定的位置输入。本发明成立的先决条件主要有以下三点：

1.已成功由多线激光融合多目视觉完成环境地图的同步定位与地图构建(Simultaneous localization and mapping，SLAM)SLAM过程，成功获取激光关键帧点云特征和相应的位姿信息，并成功通过压缩3D点云的方式生成了应用场景的二维栅格导航地图。其中，应当理解，SLAM过程是机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征(比如，墙角，柱子等)定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目的。

2.视觉关键帧一一绑定激光关键帧，成功获取多目视觉关键帧二维特征信息，并结合所绑定的激光关键帧位姿以及相机和激光的相对位姿关系成功映射、优化生成了应用场景的多目视觉稀疏特征图。

3.重定位过程中，要求定位机器人必须至少原地旋转一周，以提高视觉重定位的成功率；且每帧视觉图像需要一一同步绑定一帧底盘里程计信息，用于旋转停止时移动机器位姿的精确估计，其中每帧视觉图像指包含同一时刻各个相机组成的一组图像数据。

机器人重定位启动前，需要确保该应用场景中先决条件所述的3D激光关键帧位姿、点云信息、二维栅格导航地图信息、多目视觉关键帧二维特征及其所对应的稀疏点云信息均已成功完成加载。此处的视觉特征可以是harris、ORB、SIFT和SURF等任何人工角点特征。定位启动后，先查看用户在栅格导航地图中是否有指定位姿输入，如果有，则设置当前系统状态为局部重定位模式，否则，将当前系统状态设置为全局重定位模式。之后，移动机器会自发原地旋转一周回到起始位姿。在旋转过程中调用多目视觉重定位算法和多线激光重定位算法确定估计位姿，然后利用重定位修正算法对估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。

实施例1

本实施例，参见图1，示出了一种机器人重定位方法包括以下步骤：

S10：根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式。

预先设定的机器人初始位姿，用户可以手动输入二维栅格地图的坐标位置以及机器人的偏航角度作为机器人初始位姿，也可以在二维栅格地图中直接选定某一栅格位置，并设置机器人的偏航角度，进而确定机器人初始位姿。可以理解，机器人位姿包括坐标位置和偏航角度。

进一步的，若二维栅格地图中有预先设定的机器人初始位姿，则所述重定位模式为局部重定位模式；若二维栅格地图中无预先设定的机器人初始位姿，则所述重定位模式为全局重定位模式。

S20：在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿。

预设第一数目个同步相机构成多目相机，优选的预设第一数目可以是4，可以在机器人的前后左右分别安装用于获取环境图像的相机，4个相机同步获取环境图像。可以理解，四目相机同步获取的环境图像可以覆盖机器人四周的环境，理论上，无视觉死角，有利于机器人根据四目相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿。

S30：根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿。

多线激光雷达布置于机器人的正上方中心位置，并可以机器人为球心，全方位扫描视野，可以获取机器人周围的全部障碍物。可以根据多目视觉重定位确定的第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿。

S40：根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿。

应当理解，多目视觉重定位和多线激光重定位均存在失败的情况，在不同的重定位模式，可以根据多目视觉重定位和多线激光重定位成功或失败的情况从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿。

示范性的，在所述重定位模式为局部重定位模式时：

若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的位置和所述第二估计位姿的位置的距离小于预设的距离阈值，以及所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；若仅成功获取到所述第二估计位姿，则选择所述第二估计位姿作为待修正的估计位姿；

示范性的，在所述重定位模式为全局重定位模式时：

若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；若仅成功获取到所述第一估计位姿，则选择所述第一估计位姿作为待修正的估计位姿。

S50：利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。

重定位修正算法可以是自适应蒙特卡洛定位(adaptive Monte CarloLocalization，AMCL)。可以利用AMCL重定位修正算法连续进行多次的粒子重采样、优化操作用以收敛位置协方差和角度协方差，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。

本实施例公开的机器人重定位方法包括：根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。本实施例的技术方案结合多目视觉和多线激光实现机器人的重定位，使得机器人重定位的准确度更高、鲁棒性更强。

实施例2

本实施例，参见图3，示出了机器人的多目视觉重定位方法包括以下步骤：

S21：对第

个图像帧提取对应的图像特征

和图像描述子

表示第j个同步相机采集的第i个图像帧。

以四目相机为例，j的取值范围是1～4，1≤j≤4，代表相机的编号，本实施例中四目相机中的各个相机为同步相机，各个相机为同步运行。i代表机器人旋转一周采集的图像帧的编号。

利用特征提取算法，例如，Harris、SIFT、SURF、FAST、BRIEF、ORB，可以提取每一个图像帧

对应的图像特征

和图像描述子

S22：利用图像特征

和图像描述子

在对应的回环数据库中进行回环检索，以确定第

个图像帧与回环数据库中各个回环候选帧的内敛点匹配数目。

利用图像特征

和图像描述子

在对应的回环数据库中进行回环检索，可以理解，回环数据库中预先存有多目相机获取的多张回环候选帧，并且每一张回环候选帧都有对应的位姿信息。

可以通过确定第

个图像帧与回环数据库中各个回环候选帧的内敛点匹配数目，确定与第

个图像帧最相似的回环候选帧，进而可以根据最相似的回环候选帧对应的位姿信息估计机器人的位姿。

S23：判断最大内敛点匹配数目是否小于预设的匹配数目阈值。

优选的，匹配数目阈值可以为15，也可以选取更大的数值，应当理解，匹配数目阈值若过小，可能导致机器人定位的误差很大，但是，匹配数目阈值若过大，可能导致机器人的多目视觉定位过程成功率较低。

若最大内敛点匹配数目小于预设的匹配数目阈值，则执行步骤S24；若最大内敛点匹配数目大于等于所述匹配数目阈值，则执行步骤S25。

S24：第

个图像帧对应的回环检索信息设置为空。

S25：确定最大内敛点匹配数目对应的回环帧的第一回环索引，并根据所述第一回环索引对应的回环检索信息确定第

个图像帧对应的回环检索信息。

第

个图像帧对应的回环检索信息可以记为

回环检索信息

一般包含最大内敛点匹配数目对应的回环帧的回环索引、回环帧二维特征、回环帧二维特征描述子、第

个图像帧与最大内敛点匹配数目对应的回环帧的内敛点匹配关系以及内敛点匹配数目和最大内敛点匹配数目对应的回环帧所绑定的位姿。

S26：在获取到全部图像帧对应的回环检索信息后，判断全部图像帧对应的回环检索信息是否均为空。

若全部图像帧对应的回环检索信息均为空，则多目视觉重定位失败；若全部图像帧对应的回环检索信息不全部为空，则执行步骤S28。

S27：多目视觉重定位失败。

S28：将所述第一回环索引对应的激光关键帧位姿作为所述第一估计位姿。

第一回环索是最大内敛点匹配数目对应的回环帧的回环索引，可以将第一回环索引对应的激光关键帧位姿作为所述第一估计位姿。此时，第一估计位姿对应的第一位置协方差阈值和第一角度协方差阈值可以分别预设为1和1.08。

实施例3

进一步的，本实施例，参见图4，示出了机器人的多目视觉重定位方法还包括以下步骤：

S281：确定用于获得最大内敛点匹配数目的第t个同步相机采集的第z个图像帧，以及第

个图像帧对应的回环检索信息。

S282：确定除了第t个同步相机以外的各个同步相机采集的第z个图像帧与所述回环数据库中各个回环候选帧的内敛点匹配数目。

S283：判断除了第t个同步相机以外的各个同步相机对应的最大内敛点匹配数目对应的第二回环索引是否等于所述第一回环索引。

若除了第t个同步相机以外的各个同步相机对应的最大内敛点匹配数目对应的第二回环索引等于所述第一回环索引，则执行步骤S284；若除了第t个同步相机以外的各个同步相机对应的最大内敛点匹配数目对应的第二回环索引不等于所述第一回环索引，则执行步骤S285～S286。

S284：保留各个同步相机的第z个图像帧对应的回环检索信息。

S285：利用对应的第二回环索引对应的回环检索结果更新对应的回环检索信息。

S286：利用各个回环检索信息和各个同步相机采集的第z个图像帧优化所述第一估计位姿。

本实施例利用BA优化算法，各个回环检索信息和各个同步相机采集的第z个图像帧建立超定方程，解出最优的位姿矩阵或空间点坐标，优化所述第一估计位姿，优化后的第一估计位姿对应的第一位置协方差阈值和第一角度协方差阈值可以分别预设为0.25和0.11。

实施例4

进一步的，本实施例，参见图5，示出了机器人的多目视觉重定位方法还包括以下步骤：

S211：获取各个图像帧对应的里程计。

S212：在所述机器人旋转完成时，确定当前图像帧对应的里程计。

S213：利用所述当前图像帧对应的里程计与用于确定所述第一估计位姿的图像帧对应的里程计的差值补偿所述第一估计位姿的偏航角。

示范性的，机器人旋转停止，如果多目视觉定位成功，则同步获取最后多目相机获取的最后一帧图像和最后一帧图像对应的里程计，即分别为

和

M⁰表示每一个同步相机获取到的图像帧的总数，计算多目视觉定位成功时到机器人旋转停止时刻，机器人位姿中的角度变化，记作▲od，有

表示机器人旋转停止时刻的偏航角，od^z表示机器人多目视觉定位成功时的偏航角。进一步的，利用▲od补偿所述第一估计位姿的偏航角。

实施例5

本实施例，参见图6，示出了机器人的多线激光重定位方法包括以下步骤：

S31：以所述第一估计位姿对应的位置坐标为中心，预设的第一距离值为半径利用多线激光获取各个激光相邻关键帧。

预设的第一距离值可以是5m，利用kdtree快速检索算法，以第一估计位姿对应的位置坐标为中心，预设的第一距离值为半径利用多线激光获取各个激光相邻关键帧。

应当理解，若激光相邻关键帧的数目为0，多线激光重定位失败。

S32：分别计算所述第一估计位姿与所述各个激光相邻关键帧对应的位姿之间的距离。

S33：确定距离所述第一估计位姿最近的激光相邻关键帧对应的位姿作为最佳相邻位姿。

S34：以所述最佳相邻位姿对应的位置为中心，预设的第二距离值为半径利用所述多线激光获取所述第二距离值对应的各个相邻激光关键帧，所述第二距离值小于所述第一距离值。

预设的第二距离值小于所述第一距离值，第二距离值可以是3m，利用kdtree快速检索算法，以最佳相邻位姿对应的位置坐标为中心，预设的第二距离值为半径利用多线激光获取各个相邻激光关键帧。可以理解，以第二距离值为半径获取的各个相邻激光关键帧区别于以第以第一距离值为半径获取的各个激光相邻关键帧。

S35：利用各个相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，并确定补偿结果对应的ICP匹配的均方误差。

通过遍历所有相邻激光关键帧，分别利用每一相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，并通过ICP匹配算法确定补偿结果对应的ICP匹配的均方误差。

S36：从各个相邻激光关键帧对应的ICP匹配的均方误差中确定最小均方误差。

S37：判断最小均方误差是否大于等于预设的均方误差阈值。

若最小均方误差大于等于预设的均方误差阈值，则多线激光重定位失败；若最小均方误差小于预设的均方误差阈值，多线激光重定位成功，执行步骤S39。

S38：多线激光重定位失败。

S39：将经过补偿的最佳相邻位姿作为第二估计位姿。

示范性的，第二估计位姿对应的第二位置协方差阈值和第二角度协方差阈值可以分别预设为0.1和0.031。

实施例6

本实施例，参见图7，示出了机器人的多线激光重定位方法中偏航角补偿包括以下步骤：

S351：在第p次补偿时，利用p-1倍的预设角度常量补偿最佳相邻位姿对应的偏航角，1≤p≤P，P为所述预设第二数目次补偿。

预设角度常量可表示为▲θ，▲θ＝2*PI/N，其中，PI＝3.1415926，N的取值范围为0＜N＜36，优选的，N＝8，在N＝8机器人的偏航角会得到较理想的补偿，N若过大，过度增加算法的复杂度，计算时间较长。

示范性的，第p次补偿对应的偏航角yaw_θ_P＝yaw_θ+(p-1)*▲θ，其中，yaw_θ为最佳相邻位姿对应的偏航角。可以理解，在p＝1时，即第1次补偿时，第1次补偿对应的偏航角yaw_θ₁＝yaw_θ。

S352：将第p次补偿对应的估计位姿对应的激光点云信息映射至map坐标系下，以确定第p次补偿对应的估计位姿对应的点云图。

可以理解，环境地图数据库中预先存储有多个位姿，每一位姿有对应的激光点云信息，将第p次补偿对应的估计位姿对应的激光点云信息映射至map坐标系下，可以确定第p次补偿对应的估计位姿对应的点云图。

S353：抽取在预先构建map地图时第p次补偿对应的估计位姿前后各预设数目帧激光关键帧对应的map坐标系下的局部激光点云信息，以利用各个局部激光点云信息确定第p次补偿对应的估计位姿对应局部匹配子图。

可以理解，预先构建的map地图是利用多个激光关键帧构建的，在确定第p次补偿对应的估计位姿后，可以抽取在预先构建map地图时第p次补偿对应的估计位姿前后各预设数目帧激光关键帧对应的map坐标系下的局部激光点云信息，以利用各个局部激光点云信息确定第p次补偿对应的估计位姿对应局部匹配子图。

示范性的，第p次补偿对应的估计位姿前后各预设数目帧激光关键帧，可以是第p次补偿对应的估计位姿前后各15帧激光关键帧，利用前后15帧激光关键帧以及第p次补偿对应的估计位姿对应的激光关键帧，即31帧激光关键帧确定第p次补偿对应的估计位姿对应局部匹配子图。

S354：将第p次补偿对应的估计位姿对应的点云图与第p次补偿对应的估计位姿对应的局部匹配子图进行ICP匹配，并计算第p次补偿的ICP匹配的均方误差和相对变化位姿。

S355：第p次补偿的ICP匹配的均方误差大于等于预设的均方误差阈值。

其中，均方误差阈值可以取值0.1。若第p次补偿的ICP匹配的均方误差大于等于预设的均方误差阈值，则第p次补偿无效，可以放弃本次补偿；若第p次补偿的ICP匹配的均方误差小于预设的均方误差阈值，则执行步骤S356～S358。

S356：利用所述相对变化位姿对所述第p次补偿对应的估计位姿进行修正。

示范性的，相对变化位姿为▲T，第p次补偿对应的估计位姿为T’，对所述第p次补偿对应的估计位姿进行修正的修正结果为T_p＝T’*▲T或T_p＝▲T*T’。

S357：p＝p+1。

S358：判断p是否大于预设的第二数目。

若大于预设的第二数目，则偏航角补偿完成。若小于等于预设的第二数目，则重复执行S351～SS358，直至p大于预设的第二数目，偏航角补偿完成。

实施例7

本实施例，参见图8，示出了一种机器人重定位装置1包括：重定位模式确定模块10、第一估计位姿确定模块20、第二估计位姿确定模块30、待修正估计位姿选择模块40和估计位姿修正模块50。

重定位模式确定模块10，用于根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；第一估计位姿确定模块20，用于在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；第二估计位姿确定模块30，用于根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；待修正估计位姿选择模块40，用于根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；估计位姿修正模块50，用于利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值。

进一步的，第一估计位姿确定模块20包括：

图像特征和描述子提取单元，用于对第

个图像帧提取对应的图像特征

和图像描述子

表示第j个同步相机采集的第i个图像帧。

第一内敛点匹配数目确定单元，用于利用图像特征

和图像描述子

在对应的回环数据库中进行回环检索，以确定第

个图像帧与回环数据库中各个回环候选帧的内敛点匹配数目。

第一回环检索信息设置单元，用于若最大内敛点匹配数目小于预设的匹配数目阈值，则第

个图像帧对应的回环检索信息设置为空。

第一回环检索信息设置单元，用于若最大内敛点匹配数目大于等于所述匹配数目阈值，则确定最大内敛点匹配数目对应的回环帧的第一回环索引，并根据所述第一回环索引对应的回环检索信息确定第

个图像帧对应的回环检索信息；

视觉重定位失败判断单元，用于在全部图像帧对应的回环检索信息均为空时，所述多目视觉重定位失败。

视觉重定位成功判断单元，用于在全部图像帧对应的回环检索信息不全部为空时，将所述第一回环索引对应的激光关键帧位姿作为所述第一估计位姿。

进一步的，第一估计位姿确定模块20还包括：

目标图像帧确定单元，用于确定用于获得最大内敛点匹配数目的第t个同步相机采集的第z个图像帧，以及第

个图像帧对应的回环检索信息。

第二内敛点匹配数目确定单元，用于确定除了第t个同步相机以外的各个同步相机采集的第z个图像帧与所述回环数据库中各个回环候选帧的内敛点匹配数目。

回环检索信息更新单元，用于若除了第t个同步相机以外的各个同步相机对应的最大内敛点匹配数目对应的第二回环索引不等于所述第一回环索引，则利用对应的第二回环索引对应的回环检索结果更新对应的回环检索信息；

第一估计位姿优化单元，用于利用各个回环检索信息和各个同步相机采集的第z个图像帧优化所述第一估计位姿。

第一估计位姿确定模块20还包括：

里程计获取单元，用于获取各个图像帧对应的里程计。

当前里程计确定单元，用于在所述机器人旋转完成时，确定当前图像帧对应的里程计。

第一偏航角补偿单元，用于利用所述当前图像帧对应的里程计与用于确定所述第一估计位姿的图像帧对应的里程计的差值补偿所述第一估计位姿的偏航角。

进一步的，第二估计位姿确定模块30包括：

激光相邻关键帧获取单元，用于以所述第一估计位姿对应的位置坐标为中心，预设的第一距离值为半径利用多线激光获取各个激光相邻关键帧；

相邻关键帧距离计算单元，用于分别计算所述第一估计位姿与所述各个激光相邻关键帧对应的位姿之间的距离；

最佳相邻位姿确定单元，用于确定距离所述第一估计位姿最近的激光相邻关键帧对应的位姿作为最佳相邻位姿；

相邻激光关键帧获取单元，用于以所述最佳相邻位姿对应的位置为中心，预设的第二距离值为半径利用所述多线激光获取所述第二距离值对应的各个相邻激光关键帧，所述第二距离值小于所述第一距离值；

第二偏航角补偿单元，用于利用各个相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，并确定补偿结果对应的ICP匹配的均方误差；

最小均方误差确定单元，用于从各个相邻激光关键帧对应的ICP匹配的均方误差中确定最小均方误差。

多线激光重定位失败判断单元，用于若最小均方误差大于等于预设的均方误差阈值，则多线激光重定位失败；

多线激光重定位成功判断单元，用于若最小均方误差小于预设的均方误差阈值，则多线激光重定位成功，将经过补偿的最佳相邻位姿作为第二估计位姿。

进一步的，第二偏航角补偿单元包括：

估计位姿初始补偿子单元，用于在第p次补偿时，利用p-1倍的预设角度常量补偿最佳相邻位姿对应的偏航角，1≤p≤P，P为所述预设第二数目次补偿；

点云图确定子单元，用于将第p次补偿对应的估计位姿对应的激光点云信息映射至map坐标系下，以确定第p次补偿对应的估计位姿对应的点云图。

局部匹配子图确定子单元，用于抽取在预先构建map地图时第p次补偿对应的估计位姿前后各预设数目帧激光关键帧对应的map坐标系下的局部激光点云信息，以利用各个局部激光点云信息确定第p次补偿对应的估计位姿对应局部匹配子图。

均方误差和相对变化位姿确定子单元，用于将第p次补偿对应的估计位姿对应的点云图与第p次补偿对应的估计位姿对应的局部匹配子图进行ICP匹配，并计算第p次补偿的ICP匹配的均方误差和相对变化位姿；

补偿无效判断子单元，用于若第p次补偿的ICP匹配的均方误差大于等于预设的均方误差阈值，则第p次补偿无效；

补偿完成确定子单元，用于若第p次补偿的ICP匹配的均方误差小于预设的均方误差阈值，则利用所述相对变化位姿对所述第p次补偿对应的估计位姿进行修正，直至所述预设第二数目次补偿完成。

进一步的，重定位模式确定模块10包括：

局部重定位确定单元，用于若有预先设定的机器人初始位姿，则所述重定位模式为局部重定位模式。

全局重定位确定单元，用于若无预先设定的机器人初始位姿，则所述重定位模式为全局重定位模式。

进一步的，待修正估计位姿选择模块40包括：

在所述重定位模式为局部重定位模式时：若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的位置和所述第二估计位姿的位置的距离小于预设的距离阈值，以及所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；若仅成功获取到所述第二估计位姿，则选择所述第二估计位姿作为待修正的估计位姿；

在所述重定位模式为全局重定位模式时：若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；若仅成功获取到所述第一估计位姿，则选择所述第一估计位姿作为待修正的估计位姿。

本实施例公开的一种机器人重定位装置1通过重定位模式确定模块10、第一估计位姿确定模块20、第二估计位姿确定模块30、待修正估计位姿选择模块40和估计位姿修正模块50的配合使用，用于执行上述实施例所述的机器人重定位方法，上述实施例所涉及的实施方案以及有益效果在本实施例中同样适用，在此不再赘述。

可以理解，本发明实施例涉及一种机器人，包括存储器和处理器，所述存储器用于存储计算机程序，所述计算机程序在所述处理器上运行时执行本发明实施例所述的机器人重定位方法。

可以理解，本发明实施例涉及一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行本发明实施例所述的机器人重定位方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

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

Claims (9)

1.一种机器人重定位方法，其特征在于，该方法包括：

根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；

在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；

根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；

根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；

利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值；

其中，所述利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，包括：

对第

个图像帧提取对应的图像特征

和图像描述子

表示第j个同步相机采集的第i个图像帧；

利用图像特征

和图像描述子

在对应的回环数据库中进行回环检索，以确定第

个图像帧与回环数据库中各个回环候选帧的内敛点匹配数目；

若最大内敛点匹配数目小于预设的匹配数目阈值，则第

个图像帧对应的回环检索信息设置为空；

若最大内敛点匹配数目大于等于所述匹配数目阈值，则确定最大内敛点匹配数目对应的回环帧的第一回环索引，并根据所述第一回环索引对应的回环检索信息确定第

个图像帧对应的回环检索信息；

在全部图像帧对应的回环检索信息均为空时，所述多目视觉重定位失败；

在全部图像帧对应的回环检索信息不全部为空时，将所述第一回环索引对应的激光关键帧位姿作为所述第一估计位姿。

2.根据权利要求1所述的机器人重定位方法，其特征在于，还包括：

确定用于获得最大内敛点匹配数目的第t个同步相机采集的第z个图像帧，以及第

个图像帧对应的回环检索信息；

确定除了第t个同步相机以外的各个同步相机采集的第z个图像帧与所述回环数据库中各个回环候选帧的内敛点匹配数目；

若除了第t个同步相机以外的各个同步相机对应的最大内敛点匹配数目对应的第二回环索引不等于所述第一回环索引，则利用对应的第二回环索引对应的回环检索结果更新对应的回环检索信息；

利用各个回环检索信息和各个同步相机采集的第z个图像帧优化所述第一估计位姿。

3.根据权利要求1或2所述的机器人重定位方法，其特征在于，还包括：

获取各个图像帧对应的里程计；

在所述机器人旋转完成时，确定当前图像帧对应的里程计；

利用所述当前图像帧对应的里程计与用于确定所述第一估计位姿的图像帧对应的里程计的差值补偿所述第一估计位姿的偏航角。

4.根据权利要求1所述的机器人重定位方法，其特征在于，所述根据所述第一估计位姿进行多线激光重定位，包括：

以所述第一估计位姿对应的位置坐标为中心，预设的第一距离值为半径利用多线激光获取各个激光相邻关键帧；

分别计算所述第一估计位姿与所述各个激光相邻关键帧对应的位姿之间的距离；

确定距离所述第一估计位姿最近的激光相邻关键帧对应的位姿作为最佳相邻位姿；

以所述最佳相邻位姿对应的位置为中心，预设的第二距离值为半径利用所述多线激光获取所述第二距离值对应的各个相邻激光关键帧，所述第二距离值小于所述第一距离值；

利用各个相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，并确定补偿结果对应的ICP匹配的均方误差；

从各个相邻激光关键帧对应的ICP匹配的均方误差中确定最小均方误差；

若最小均方误差大于等于预设的均方误差阈值，则多线激光重定位失败；

若最小均方误差小于预设的均方误差阈值，则多线激光重定位成功，将经过补偿的最佳相邻位姿作为第二估计位姿。

5.根据权利要求4所述的机器人重定位方法，其特征在于，每一个相邻激光关键帧对所述最佳相邻位姿对应的偏航角进行预设第二数目次补偿，包括：

在第p次补偿时，利用p-1倍的预设角度常量补偿最佳相邻位姿对应的偏航角，1≤p≤P，P为所述预设第二数目次补偿；

将第p次补偿对应的估计位姿对应的激光点云信息映射至map坐标系下，以确定第p次补偿对应的估计位姿对应的点云图；

抽取在预先构建map地图时第p次补偿对应的估计位姿前后各预设数目帧激光关键帧对应的map坐标系下的局部激光点云信息，以利用各个局部激光点云信息确定第p次补偿对应的估计位姿对应局部匹配子图；

将第p次补偿对应的估计位姿对应的点云图与第p次补偿对应的估计位姿对应的局部匹配子图进行ICP匹配，并计算第p次补偿的ICP匹配的均方误差和相对变化位姿；

若第p次补偿的ICP匹配的均方误差大于等于预设的均方误差阈值，则第p次补偿无效；

若第p次补偿的ICP匹配的均方误差小于预设的均方误差阈值，则利用所述相对变化位姿对所述第p次补偿对应的估计位姿进行修正，直至所述预设第二数目次补偿完成。

6.根据权利要求1所述的机器人重定位方法，其特征在于，

所述根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式，包括：

若有预先设定的机器人初始位姿，则所述重定位模式为局部重定位模式；

若无预先设定的机器人初始位姿，则所述重定位模式为全局重定位模式；

根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿，包括：

在所述重定位模式为局部重定位模式时：

若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的位置和所述第二估计位姿的位置的距离小于预设的距离阈值，以及所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；

若仅成功获取到所述第二估计位姿，则选择所述第二估计位姿作为待修正的估计位姿；

在所述重定位模式为全局重定位模式时：

若成功获取到所述第一估计位姿和所述第二估计位姿，则在所述第一估计位姿的导航角度和所述第二估计位姿的导航角度的之差的绝对值小于预设的角度差阈值时，选择所述第二估计位姿作为待修正的估计位姿；

若仅成功获取到所述第一估计位姿，则选择所述第一估计位姿作为待修正的估计位姿。

7.一种机器人重定位装置，其特征在于，该装置包括：

重定位模式确定模块，用于根据二维栅格地图中是否有预先设定的机器人初始位姿确定所述机器人的重定位模式；

第一估计位姿确定模块，用于在所述机器人原地旋转的过程中，利用预设第一数目个同步相机获取的各个图像帧进行多目视觉重定位，以确定所述机器人的第一估计位姿；

第二估计位姿确定模块，用于根据所述第一估计位姿进行多线激光重定位，以确定所述机器人的第二估计位姿；

待修正估计位姿选择模块，用于根据所述重定位模式从所述第一估计位姿和所述第二估计位姿中选择待修正的估计位姿；

估计位姿修正模块，用于利用重定位修正算法对所选择的待修正的估计位姿进行迭代修正，直至迭代修正的位置协方差收敛并小于所述待修正的估计位姿对应的预设位置协方差阈值，且迭代修正的角度协方差收敛并小于所述待修正的估计位姿对应的预设角度协方差阈值；

其中，所述第一估计位姿确定模块包括：

图像特征和描述子提取单元，用于对第

个图像帧提取对应的图像特征

和图像描述子

表示第j个同步相机采集的第i个图像帧；

第一内敛点匹配数目确定单元，用于利用图像特征

和图像描述子

在对应的回环数据库中进行回环检索，以确定第

个图像帧与回环数据库中各个回环候选帧的内敛点匹配数目；

第一回环检索信息设置单元，用于若最大内敛点匹配数目小于预设的匹配数目阈值，则第

个图像帧对应的回环检索信息设置为空；

第一回环检索信息设置单元，用于若最大内敛点匹配数目大于等于所述匹配数目阈值，则确定最大内敛点匹配数目对应的回环帧的第一回环索引，并根据所述第一回环索引对应的回环检索信息确定第

个图像帧对应的回环检索信息；

视觉重定位失败判断单元，用于在全部图像帧对应的回环检索信息均为空时，所述多目视觉重定位失败；

视觉重定位成功判断单元，用于在全部图像帧对应的回环检索信息不全部为空时，将所述第一回环索引对应的激光关键帧位姿作为所述第一估计位姿。

8.一种机器人，其特征在于，包括存储器和处理器，所述存储器用于存储计算机程序，所述计算机程序在所述处理器上运行时执行权利要求1至6任一项所述的机器人重定位方法。

9.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时执行权利要求1至6任一项所述的机器人重定位方法。

Images