CN113137968A - 基于多传感器融合的重定位方法、重定位装置和电子设备 - Google Patents

Abstract

本申请提供一种基于多传感器融合的重定位方法、重定位装置和电子设备。该方法包括：基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；确定与当前帧匹配的多个重定位候选关键帧；根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点，以及，基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。这样，能够有效地克服重定位过程受视场角的影响，以提高重定位精度。

Description

基于多传感器融合的重定位方法、重定位装置和电子设备

技术领域

本发明涉及SLAM领域，更具体地涉及基于多传感器融合的重定位方法、重定位装置和电子设备。

背景技术

SLAM(Simultaneous Localization and Mapping，同步定位与建图)技术，在AR(Augmented Reality，增强现实)、VR(Virtual Reality，虚拟现实)、机器人(例如，扫地机器人、服务机器人等)、无人驾驶设备(例如，无人驾驶汽车、无人机)等领域中得到广泛应用。SLAM问题可以描述为：设备在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现设备的自主定位和导航。

SLAM技术框架主要包括：传感器、里程计、后端处理和重定位。常见的SLAM传感器，包括两类：激光雷达和视觉传感器(例如，单目、双目和 RGB-D摄像模组等)。里程计模块通过传感器采集的数据进行定位来估计目标对象在两个时刻的相对运动。后端处理模块通过滤波方法或优化方法来解决SLAM问题。重定位模块又称为回环检测，其表示设备识别曾到达场景的能力。

在实际应用中，现有的SLAM系统遇到了诸多技术问题，例如，在动态环境中，定位可能丢失，需要进行重定位；在载入已有地图后，需要通过重定位来寻找设备的当前位姿等。

因此，需要一种改进的SLAM技术方案。

发明内容

本申请的一个优势在于提供一种基于多传感器融合的重定位方法、重定位装置和电子设备，其能够有效地克服重定位过程受视场角的影响，以提高重定位精度。

本申请的另一个优势在于提供一种基于多传感器融合的重定位方法、重定位装置和电子设备，其中，在重定位过程中，采用多传感器融合的方式获得融合位姿，以提供具有更高稳定性和准确度的位姿数据。

本申请的另一优势在于提供一种基于多传感器融合的重定位方法、重定位装置和电子设备，其中，采用多传感器融合的方式获得融合位姿，以使得所述重定位系统的环境适应性和兼容性。

为了实现上述至少一发明目的，本发明提供了一种基于多传感器融合的重定位方法，其包括：

基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；

确定与当前帧匹配的多个重定位候选关键帧；

根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；以及

基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。

在上述重定位方法中，确定与当前帧匹配的重定位候选关键帧，包括：根据词袋模型确定与当前帧匹配的重定位候选关键帧。

在上述重定位方法中，根据词袋模型确定与当前帧匹配的重定位候选关键帧，包括：获取当前帧的词袋数据，所述词袋数据包括用于表征所述当前帧的词节点；从词袋中筛选出与所述当前帧共享词节点的所有候选关键帧；去除所有所述候选关键帧中共享词节点的比例小于第一共享比例的候选关键帧；获取所有所述候选关键帧中剩下的候选关键帧与当前帧之间的相似性得分；以及，将相似性得分超过相似性最高得分的预设比例的所述候选关键帧确定为所述重定位候选关键帧。

在上述重定位方法中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿的过程，包括：响应于所述视觉里程计未丢失视觉，将由所述视觉里程计采集的当前帧的第一位姿确定为所述融合位姿。

在上述重定位方法中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿的过程，进一步包括：响应于所述视觉里程计丢失视觉且确定为转弯状态，获得由所述第三里程计采集的当前帧的第三位姿；以及，基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿。

在上述重定位方法中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿的过程，进一步包括：响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及,基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

在上述重定位方法中，基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定位姿修正量的过程，包括：获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿；获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；以及,基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。

在上述重定位方法中，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量，包括：基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，确定所述视觉修正量。

在上述重定位方法中，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量，包括：基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。

在上述重定位方法中，基于深度里程计获得当前帧的第二位姿的过程，包括：对获取的当前帧点云数据进行行列划分；判定待处理的每一行和每一列中的点云为线特征点或者面特征点；基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及，对所述代价函数进行优化，以确定当前帧的位姿。

在上述重定位方法中，基于历史点云数据确定每一线特征点对应的特征线，包括：在上一帧点云数据中搜索与所述当前帧点云数据中线特征点位于相同行或者列中距离最近的第一个点和位于不同行或者列中距离最近的第二个点；以及。确定所述第一个点和第二个点所确定的直线为相应线特征点对应的特征线。

在上述重定位方法中，其中，基于历史点云数据确定每一面特征点对应的特征面，包括：在上一帧点云数据中搜索与当前帧点云数据中相应面特征点位于相同行或者列中且距离最近的第三个点和第四个点，位于不同行或者列中且距离最近的第五个点；以及，确定所述第三个点、所述第四个点和所述第五个点所确定的面为相应面特征点对应的特征面。

在上述重定位方法中，在对获取的当前帧点云数据进行行列划分之后，以及，判定待处理的每一行和每一列中的点为线特征点或者面特征点之前，进一步包括：选择第一预设数量的行作为待处理的行；以及，选择第二预设数量的列作为待处理的列。

在上述重定位方法中，判定待处理的每一行和每一列中的点为线特征点或者面特征点，包括：获取待处理的每一行和每一列中点的曲率；响应于曲率小于或等于第一预设阈值，将对应点确定为面特征点；以及，响应于曲率大于所述第一预设阈值，将对应点确定为线特征点

在上述重定位方法中，获取待处理的每一行和每一列中点的曲率，包括：计算对应点与其所在行或列前预设数量的点和后预设数量的点之间的距离的平均值，作为对应点的曲率。

在上述重定位方法中，基于历史点云数据确定每一线特征点对应的特征线，包括：在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中线特征点周围的一组线特征点，并构建该组线特征点的协方差矩阵计算所述协方差矩阵的特征值V和特征向量E；确定所述特征值中最大者对应的特征向量的方向作为目标特征线的方向向量；以及，确定由所述一组线特征点中任一线特征点和所述方向向量所确定的直线为相应线特征点对应的特征线。

在上述重定位方法中，基于历史点云数据确定每一线特征面对应的特征面，包括：在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中面特征点周围的一组面特征点，并构建该组面特征点的协方差矩阵；计算所述协方差矩阵的特征值V和特征向量E；确定所述特征值中最小者对应的特征向量的方向作为目标特征面的法向量；以及，确定由所述一组线特征点中任一面特征点和所述法向量所确定的平面为相应面特征点对应的特征面。

在上述重定位方法中，所述点云数据由TOF深度信息摄像模组采集。

在上述重定位方法中，所述第三里程计选自轮式编码器和惯性测量单元中任意一种

根据本发明的另一方面，还提供一种深度里程计方法，其包括：

对获取的当前帧点云数据进行行列划分；

判定待处理的每一行和每一列中的点为线特征点或者面特征点；

基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；

计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及

对所述代价函数进行优化，以确定当前帧的位姿。

在上述深度里程计方法中，基于历史点云数据确定每一线特征点对应的特征线，包括：在上一帧点云数据中搜索与所述当前帧点云数据中线特征点位于相同行或者列中距离最近的第一个点和位于不同行或者列中距离最近的第二个点；以及，确定所述第一个点和第二个点所设定的直线为相应线特征点对应的特征线。

在上述深度里程计方法中，基于历史点云数据确定每一线特征点对应的特征线，包括：在上一帧点云数据中搜索与当前帧点云数据中相应面特征点位于相同行或者列中且距离最近的第三个点和第四个点，位于不同行或者列中且距离最近的第五个点；以及，确定所述第三个点、所述第四个点和所述第五个点所设定的面为相应面特征点对应的特征面。

在上述深度里程计方法中，在对获取的当前帧点云数据进行行列划分之后，以及，判定待处理的每一行和每一列中的点为线特征点或者面特征点之前，进一步包括：选择第一预设数量的行作为待处理的行；以及，选择第二预设数量的列作为待处理的列。

在上述深度里程计方法中，判定待处理的每一行和每一列中的点为线特征点或者面特征点，包括：获取待处理的每一行和每一列中点的曲率；响应于曲率小于或等于第一预设阈值，将对应点确定为面特征点；以及，响应于曲率大于所述第一预设阈值，将对应点确定为线特征点。

在上述深度里程计方法中，获取待处理的每一行和每一列中点的曲率，包括：计算对应点与其所在行或列前预设数量的点和后预设数量的点之间的距离的平均值，作为对应点的曲率。

在上述深度里程计方法中，在判定待处理的每一行和每一列中的点为线特征点或者面特征点之后，进一步包括：识别并去除所述线特征点和所述面特征点中的异常点。

在上述深度里程计方法中，基于历史点云数据确定每一线特征点对应的特征线，包括：在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中线特征点周围的一组线特征点，并构建该组线特征点的协方差矩阵；计算所述协方差矩阵的特征值V和特征向量E；确定所述特征值中最大者对应的特征向量的方向作为目标特征线的方向向量；以及，确定由所述一组线特征点中任一线特征点和所述方向向量所设定的直线为相应线特征点对应的特征线。

在上述深度里程计方法中，基于历史点云数据确定每一线特征面对应的特征面，包括：在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中面特征点周围的一组面特征点，并构建该组面特征点的协方差矩阵；计算所述协方差矩阵的特征值V和特征向量E；确定所述特征值中最小者对应的特征向量的方向作为目标特征面的法向量；以及，确定由所述一组线特征点中任一面特征点和所述法向量所确定的平面为相应面特征点对应的特征面。

在上述深度里程计方法中，所述点云数据由TOF深度信息摄像模组采集。根据本申请又一方面，还提供一种定位方法，其包括：响应于视觉里程计未丢失视觉，将由视觉里程计采集的当前帧的第一位姿确定为所述融合位姿；响应于视觉里程计丢失视觉且确定为转弯状态，获得由第三里程计采集的当前帧的第三位姿；基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿；响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及，基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

在上述定位方法中，基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定位姿修正量的过程，包括：获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿；获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；以及，基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。

在上述定位方法中，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量，包括：基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，确定所述视觉修正量。

在上述定位方法中，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量，包括：基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。

在上述定位方法中，所述第三里程计选自轮式编码器和惯性测量单元中任意一种。

根据本申请又一方面，还提供一种重定位装置，其包括：候选帧确定单元，用于确定与当前帧匹配的重定位候选关键帧；融合位姿确定单元，用于基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；投影单元，用于根据所述融合位姿将所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；以及，重定位单元，用于基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。

根据本申请又一方面，还提供一种电子设备，其包括存储器和处理器，其中，在所述存储器中存储有计算机程序指令，所述计算机程序指令在被所述处理器运行时使得所述处理器执行如上所述的重定位方法和/定位方法和/ 或深度里程计方法。

根据本申请又一方面，还提供一种计算机可读存储介质，其中所述计算机可读存储介质上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行如上所述的重定位方法和/定位方法和/或深度里程计方法。

通过对随后的描述和附图的理解，本发明进一步的目的和优势将得以充分体现。

本发明的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。

附图说明

从下面结合附图对本发明实施例的详细描述中，本申请的这些和/或其它方面和优点将变得更加清楚并更容易理解，其中：

图1图示了根据本申请实施例的重定位方法的流程图。

图2图示了根据本申请实施例的所述重定位方法中基于深度里程计测量位姿的流程图。

图3图示了根据本申请实施例中确定特征线的示意图。

图4图示了根据本申请实施中确定特征面的示意图。

图5图示了根据本申请实施例的位姿融合过程的流程示意图。

图6图示了根据本申请实施中确定深度修正量的流程示意图。

图7图示了根据本申请实施中确定深度修正量的关系示意图。

图8图示了根据本申请实施例的重定位装置的框图示意图。

图9图示了根据本申请实施例的电子设备的示意图。

具体实施方式

以下说明书和权利要求中使用的术语和词不限于字面的含义，而是仅由本发明人使用以使得能够清楚和一致地理解本申请。因此，对本领域技术人员很明显仅为了说明的目的而不是为了如所附权利要求和它们的等效物所定义的限制本申请的目的而提供本申请的各种实施例的以下描述。

可以理解的是，术语“一”应理解为“至少一”或“一个或多个”，即在一个实施例中，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个，术语“一”不能理解为对数量的限制。

虽然比如“第一”、“第二”等的序数将用于描述各种组件，但是在这里不限制那些组件。该术语仅用于区分一个组件与另一组件。例如，第一组件可以被称为第二组件，且同样地，第二组件也可以被称为第一组件，而不脱离发明构思的教导。在此使用的术语“和/或”包括一个或多个关联的列出的项目的任何和全部组合。

在这里使用的术语仅用于描述各种实施例的目的且不意在限制。如在此使用的，单数形式意在也包括复数形式，除非上下文清楚地指示例外。另外将理解术语“包括”和/或“具有”当在该说明书中使用时指定所述的特征、数目、步骤、操作、组件、元件或其组合的存在，而不排除一个或多个其它特征、数目、步骤、操作、组件、元件或其组的存在或者附加。

申请概述

如上所述，在实际应用中，现有的SLAM系统遇到了诸多技术问题。

首先，在现有的SLAM系统中，弱纹理环境对视觉里程计的影响较大。在弱纹理环境下，需要其他里程计传感器提供位姿进行补足修正。轮式编码器是目前应用最广泛的用于位姿融合(目的是为了位姿补足和修正)的里程计之一，但它的漂移量随里程增大而增大。并且，轮式编码器在打滑或转弯时误差较大，因此，在转弯丢失视觉时，融合位姿误差较大。也有一些厂商采用激光雷达提供高精度的位姿来进行位姿融合，但激光雷达成本较高且传感器尺寸较为笨重。

除了在弱纹理环境中无法获得精确位姿外，还有一些场景下需要进行重定位，例如，在动态环境中，定位可能丢失，需要进行重定位；在载入已有地图后，需要通过重定位来寻找设备的当前位姿等。但是，现有的SLAM系统在重定位环节也遇到一些技术难题。

具体来说，传统视觉重定位方法通过词袋搜索并筛选出重定位候选关键帧，进而对每个候选关键帧进行词袋匹配获得匹配点对，接着，根据匹配点对进行PnP(Perspective-N-Point)及非线性优化求解位姿。但是，这种重定位方法受视角影响，特别是在当前帧视角和关键帧视角不一致的情况下，重定位较为困难。

针对上述技术问题，本申请的基本思路在于引入一种基于深度信息的深度里程计来提供额外的位姿数据，并结合现有的轮式编码器、惯性测量单元和视觉里程计以通过多传感器融合的方式来提高位姿估计精度，通过这样的方式，可同时增强所述SLAM系统的环境兼容性(尤其是弱纹理环境，因为在弱纹理环境下，所述深度里程计能够正常运转)。此外，在重定位过程中，通过将重定位候选关键帧中的地图点基于融合位姿投影到当前帧的视场角下，以避免因当前帧和候选关键帧存在视场角差异而引起重定位失败。

基于此，本申请提出一种重定位方法，其首先确定与当前帧匹配的重定位候选关键帧；然后基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；接着，根据所述融合位姿将所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；然后基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。这样，能够有效地克服重定位过程受视场角的影响，以提高重定位精度。并且，采用多传感器融合的方式获得融合位姿，以提供具有更高稳定性和准确度的位姿数据。

进一步地，本申请还提出了一种深度里程计方法，其首先对获取的当前帧点云数据进行行列划分；接着，判定待处理的每一行和每一列中的点云为线特征点或者面特征点；然后，基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；接着，计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；然后，对所述代价函数进行优化，以确定当前帧的位姿。

进一步地，本申请还提供了一种定位方法，其首先响应于视觉里程计未丢失视觉，将由视觉里程计采集的当前帧的第一位姿确定为所述融合位姿；或者，响应于视觉里程计丢失视觉且确定为转弯状态，获得由第三里程计采集的当前帧的第三位姿；以及，基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿；或者，响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及，基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。这样，采用多传感器融合的方式获得融合位姿，以提供具有更高稳定性和准确度的位姿数据。

在具体介绍本申请实施例涉及的方法之前，先明确一下，在本申请实施例中，所涉及的“位姿”表示位置和朝向，通常可以用一个六维的向量(x,y, z,θ,

ψ)表示。

示意性重定位方法

图1图示了根据本申请实施例的重定位方法的流程图。如图1所示，基于本申请实施例的重定位方法，包括：S110，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；S120，确定与当前帧匹配的多个重定位候选关键帧；S130，根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；以及，S140，基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。

在步骤S110中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿。这里，所述深度里程计表示基于深度信息获取被测目标位姿的装置或系统；所述视觉里程计表示基于图像信息获取被测目标位姿的装置或系统，其中，在具体实施中，所述视觉里程计，包括但不限于基于单目摄像模组的视觉里程计、基于双目摄像模组的视觉里程计；所述第三里程计表示除了所述深度里程计和所述视觉里程计这两类里程计之外的其他里程计，其中，在本申请实施例中，所述第三里程计包括但不限于惯性测量单(Inertialmeasurement unit，IMU)，轮式编码器等。

特别地，在本申请实施例中，所述深度里程计基于TOF摄像模组所采集的深度信息进行位姿测量，其中，所述TOF摄像模组包括但不限于基于散斑结构光的TOF摄像模组、基于编码结构光的TOF摄像模组等。图2图示了根据本申请实施例的所述重定位方法中基于深度里程计测量位姿的流程图。如图2所示，在本申请实施例中，通过所述深度里程计测量位姿的过程，包括：S210，获取由所述TOF摄像模组所采集的当前帧点云数据；S220，对当前帧点云数据进行行列划分；S230，判定待处理的每一行和每一列中的点云为线特征点或者面特征点；S240，基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；S250，计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及，S260，对所述代价函数进行优化，以确定当前帧的位姿。

在基于深度里程计测量位姿的过程中，在步骤S220之后和步骤S230之前，进一步包括：选择第一预设数量的行作为待处理的行和选择第二预设数量的列作为待处理的列。也就是说，在本申请实施中，对于按行划分的点，仅取部分数量的行来参与计算；同理，对于按照列划分的点，仅取部分数量的列来参与计算。

具体来说，在步骤S230中，以每个点具有的曲率来判断待处理的每一行和每一列中的点为线特征点还是面特征点。具体来说，在本申请实施例中，该过程，首先包括获取待处理的每一行和每一列中点的曲率，其中，如果该曲率小于或等于第一预设阈值，则将对应点确定为面特征点；如果该曲率大于所述第一预设阈值，将对应点确定为线特征点。特别地，在本申请实施例中，以对应点与其所在行或列前预设数量的点和后预设数量的点之间的距离的平均值，作为对应点的曲率。例如，在一种可能的实现方式中，以当前点与其之前的5个点和之后的5个点的距离平均值作为该点的曲率。当然，本领域普通技术人员应了解，在本申请其他示例中，还可以以其他方式来求解点云的曲率，对此，并不为本申请所局限。值得一提的是，在本申请实施中，在步骤S230后，还可以识别并去除所述线特征点和所述面特征点中的异常点，例如，去除所述线特征点和/或所述面特征点中深度为0的特征点。

进一步地，在步骤S240中，基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面。特别地，在一种可能的实现方式中，确定特征线的过程，首先包括：在上一帧点云数据中搜索与所述当前帧点云数据中线特征点位于相同行或者列中距离最近的第一个点和位于不同行或者列中距离最近的第二个点；接着，将所述第一个点和第二个点所确定的直线确定为相应线特征点对应的特征线。也就是说，对于当前帧的每一行或每一列的线特征点，在上一帧点云数据中，找到与该特征点相同的行或列中距离最近的一个点(例如设定为图3中的i点)，同时找到与该特征点不同行或不同列中距离最近的一个点(例如设定为图3中的j点)，进而，将i点和j 点所在的直线确定为对应线特征点的特征线。

相应地，在这种可能的实现方式中，确定特征面的过程，首先包括在上一帧点云数据中搜索与当前帧点云数据中相应面特征点位于相同行或者列中且距离最近的第三个点和第四个点，位于不同行或者列中且距离最近的第五点云；接着，将所述第三个点、所述第四个点和所述第五个点所确定的面为相应面特征点对应的特征面。也就是说，对于当前帧的每一行或每一列的面特征点，找到与特征点相同行或列中距离最近的两个点(例如，设定为图4 中的i点和j点)，接着找到与特征点不同行或列中距离最近的一个点(例如，设定为图4中的点k)，进而，将i点、j点和k点所在面作为特征点对应的特征面。

也就是说，在这种可能的实现方式中，仅基于历史点云数据中的上一帧点云数据来进行特征线和面特征线的确定。也就是说，在这种可能的实现方式中，通过相邻帧进行位姿估计。

在本申请另一种可能的实现方式中，基于历史点云数据确定每一线特征点对应的特征线的过程，首先包括：在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中线特征点周围的一组线特征点，并构建该组线特征点的协方差矩阵；然后，计算所述协方差矩阵的特征值V和特征向量 E；接着，确定所述特征值中最大者对应的特征向量的方向作为目标特征线的方向向量；进而，确定由所述一组线特征点中任一线特征点和所述方向向量所设定的直线为相应线特征点对应的特征线。也就是说，不同于上一种可能的实现方式，在另一种可能的实现方式中，对于特征线，在构建的点云地图中找到当前帧每一个线特征点周围的一组线特征点S构建协方差矩阵M，然后计算M的特征值V和特征向量E，如果其中一个特征值远大于另外两个，那么将该特征值对应的特征向量的方向作为特征线的方向向量，根据S中至少一个点和方向向量可得到一条直线即为线特征点对应的特征线；对于特征面，类似于特征线的计算方法，如果特征值中的其中一个远小于另外两个，最小的特征值对应的特征向量作为平面的法向量，根据法向量和至少一个点可得到一个平面，即为特征面。

也就是说，在这种可能的实现方式中，基于历史点云数据所构建的点云地图来进行线特征和面特征的确定。也就是说，在这种可能的实现方式中，通过历史帧进行位姿估计。相较于上一种可能的实现方式，其精度更高。

在步骤S250和步骤S260中，计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及，对所述代价函数进行优化，以确定当前帧的位姿。本领域普通技术人员应可以理解，在本申请其他示例中，还可以构建其他形式的代价函数，并且，对代价函数的优化方式也可以才有其他类型，对此，皆不为本申请所局限。

相应地，在本申请实施中，采用上述多种里程计传感器融合的方式来获取被测目标的位姿信息。也就是说，在本申请实施例中，通过特定的算法或者模式将所述视觉里程计所采集的第一位姿、所述深度里程计所采集的第二位姿和所述第三里程计所采集的第三位姿进行融合，以获得融合位姿，通过这样的方式，有效地提高了位姿测量的稳定性和准确度。

具体来说，图5图示了根据本申请实施例的位姿融合过程的流程示意图。如图5所示，根据本申请实施例的位姿融合过程，首先包括：当所述视觉里程计判定未丢失视觉时，将由所述视觉里程计采集的当前帧的第一位姿确定为所述融合位姿。也就是说，当所述视觉里程计能够采集到目标场景的图像时，将所述视觉里程计采集的当前帧的第一位姿确定为所述融合位姿。进一步地，如图5所示，当所述视觉里程计丢失视觉并且目标处于转弯状态时，所述位姿融合过程，首先包括获得由所述第三里程计采集的当前帧的第三位姿；然后，基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿。进一步地，如图5所示，当所述视觉里程计丢失视觉并且目标不处于转弯状态时，所述位姿融合过程，首先包括获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；然后，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；接着，基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

更具体地说，当所述视觉里程计丢失视觉并且目标不处于转弯状态时，基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定位姿修正量的过程的过程，首先包括：获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿，以及，获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；然后，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；接着，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；进而基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。这里，所述视觉特征表示用于形成追踪、构建地图且用于重定位的地图点数据。并且，在本申请实施例中，深度丢失表示所述深度里程计无法获得位姿数据，特别地，基于如上所述的深度里程计方法的论述，应可以理解，在本申请实施例中，所述深度里程计要想获得估计的位姿数据，其必须同时能够采集到线、面特征。然而，在一些场景中，例如，所述深度里程计与被测场景的距离太近只能采集到一个面的特征而无非采集到线特征，或者，环境结构化信息比较少(例如，只有一面墙等)，在这些场景中会导致深度丢失。

更细化地说，在本申请实施例中，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量的过程，包括：基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，并通过复杂的计算过程(虽然该过程复杂，但本领域普通技术人员可通过领域知识推知)，确定所述视觉修正量。

更细化地说，在本申请实施例中，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量的过程，包括：基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。图6图示了根据本申请实施中确定深度修正量的流程示意图，其中，该深度修正量的求解思路的核心在于：首先包括基于如上所述的深度里程计进行实际物理位姿估计，然后，引入所述第三里程计(在该示例中为轮式编码器)所采集的一个轴的位移作为辅助，以确定所述深度修正量。

更具体地说，如图6所示，所述深度修正量的确定过程，首先包括利用所述TOF摄像模组所采集的点云数据，对点云数据进行面或线特征检测(值得一提的是，可通过对点云进行预处理和/或采用一些优化手段，来提高线或面检测效率及效果)；然后通过前后帧相同线/面判断，确保角度变化是针对同一个线/面；接着，根据线/面与所述TOF摄像模组的物理关系，可得到光轴与它们之间的夹角，通过这样的方式，即可得到相邻帧的夹角变化，从而得到所述TOF摄像模组的旋转角度△θ，同时也能求出前后帧所述TOF摄像模组与线或面的距离d，为后面计算位移提供参考(为了便于计算，需要将轮式编码器的坐标系转化到TOF摄像模组的坐标系下)；接着，根据轮式编码器前后帧变化可计算出相邻帧在x轴方向上的位移△x，并与距离d结合可计算出在z轴方向上相邻帧位移△z，其具体推导和计算过程如图7所示；最后，将得到的相邻帧位姿变化转化到世界坐标系下，以获得所述深度修正量。

更细化地说，在本申请实施例中，基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量的过程中，当所述视觉特征数量越高时，所述视觉修正量对应的权重越大，反之则越小。

在步骤S120中，确定与当前帧匹配的多个重定位候选关键帧。在本申请一种可能的实现方式中，可根据词袋模型来确定与当前帧匹配的重定位候选帧。具体来说，根据词袋模型确定与当前帧匹配的重定位候选关键帧的过程，首先包括获取当前帧的词袋数据，所述词袋数据包括用于表征所述当前帧的词节点；然后，从词袋中筛选出与所述当前帧共享词节点的所有候选关键帧；接着，去除所有所述候选关键帧中共享词节点的比例小于第一共享比例的候选关键帧；然后，获取所有所述候选关键帧中剩下的候选关键帧与当前帧之间的相似性得分；接着，将相似性得分超过相似性最高得分的预设比例的所述候选关键帧确定为所述重定位候选关键帧。在一个具体的示例中，第一共享比例设置为0.8，计算相似度得分的算法模型为BOW算法。

值得一提的是，本领域普通技术应可以理解，在本申请其他示例中，还可以通过其他方式来确定重定位候选关键帧，例如，通过哈希表，神经网络，分类器等，对此，并不为本申请所局限。

在步骤S130中，根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点。应可以理解，如前所述，现有的重定位方法受视角影响，特别是在当前帧视角和候选关键帧视角不一致的情况下，重定位较为困难。相应地，在本申请实施例中，通过将所有重定位候选关键帧中的不重复的地图点基于融合位姿投影到当前帧的视场角下，以避免因当前帧和候选关键帧存在视场角差异而引起重定位失败，这样，有效地克服重定位过程受视场角的影响，以提高重定位精度和效率。

在步骤S140中，基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。例如，在一种可能的实现方式中，可根据 PnP问题的求解方法估计一个当前帧的RT(旋转量和平移量)，再通过非线性优化的方法对所估计位姿进行优化。值得一提的是，如果在优化过程中内点数量较少，可以放宽搜索范围，然后再使用如前所述Pnp估计出的相机位姿进行再次投影，补充匹配对，再次通过非线性优化算法确定目标位姿。

综上，基于本申请实施例的重定位方法被阐明，其能够有效地克服重定位过程受视场角的影响，以提高重定位精度和效率。并且，在重定位过程中，采用多传感器融合的方式获得融合位姿，以提高具有更高稳定性和准确度的位姿数据。并且，采用多传感器融合的方式获得融合位姿，以提高系统的环境适应性和兼容性。

示意性深度里程计方法

根据本申请另一方面，还提供一种视觉里程计方法，其包括：S210，获取由所述TOF摄像模组所采集的当前帧点云数据；S220，对当前帧点云数据进行行列划分；S230，判定待处理的每一行和每一列中的点为线特征点或者面特征点；S240，基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；S250，计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及，S260，对所述代价函数进行优化，以确定当前帧的位姿。

特别地，在本申请实施例中，所述深度里程计基于TOF摄像模组所采集的深度信息进行位姿测量，其中，所述TOF摄像模组包括但不限于基于散斑结构光的TOF摄像模组、基于编码结构光的TOF摄像模组等。

在基于深度里程计测量位姿的过程中，在步骤S220之后和步骤S230之前，进一步包括：选择第一预设数量的行作为待处理的行和选择第二预设数量的列作为待处理的列。也就是说，在本申请实施中，对于按行划分的点，仅取部分数量的行来参与计算；同理，对于按照列划分的点，仅取部分数量的列来参与计算。

具体来说，在步骤S230中，以每个点具有的曲率来判断待处理的每一行和每一列中的点为线特征点还是面特征点。具体来说，在本申请实施例中，该过程，首先包括获取待处理的每一行和每一列中点云的曲率，其中，如果该曲率小于或等于第一预设阈值，则将对应点确定为面特征点；如果该曲率大于所述第一预设阈值，将对应点确定为线特征点。特别地，在本申请实施例中，以对应点与其所在行或列前预设数量的点和后预设数量的点之间的距离的平均值，作为对应点云的曲率。例如，在一种可能的实现方式中，以当前点云与其之前的5个点和之后的5个点的距离平均值作为该点云的曲率。当然，本领域普通技术人员应了解，在本申请其他示例中，还可以以其他方式来求解点的曲率，对此，并不为本申请所局限。值得一提的是，在本申请实施中，在步骤S230后，还可以识别并去除所述线特征点和所述面特征点中的异常点，例如，去除所述线特征点和/或所述面特征点中深度为0的特征点。

进一步地，在步骤S240中，基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面。特别地，在一种可能的实现方式中，确定特征线的过程，首先包括：在上一帧点云数据中搜索与所述当前帧点云数据中线特征点位于相同行或者列中距离最近的第一个点和位于不同行或者列中距离最近的第二个点；接着，将所述第一个点和第二个点所确定的直线确定为相应线特征点对应的特征线。也就是说，对于当前帧的每一行或每一列的线特征点，在上一帧点云数据中，找到与该特征点相同的行或列中距离最近的一个点(例如设定为图3中的i点)，同时找到与该特征点不同行或不同列中距离最近的一个点(例如设定为图3中的j点)，进而，将i点和j 点所在的直线确定为对应线特征点的特征线。

相应地，在这种可能的实现方式中，确定特征面的过程，首先包括在上一帧点云数据中搜索与当前帧点云数据中相应面特征点位于相同行或者列中且距离最近的第三个点和第四个点，位于不同行或者列中且距离最近的第五个点；接着，将所述第三个点、所述第四个点和所述第五个点所设定的面为相应面特征点对应的特征面。也就是说，对于当前帧的每一行或每一列的面特征点，找到与特征点相同行或列中距离最近的两个点(例如，设定为图4 中的i点和j点)，接着找到与特征点不同行或列中距离最近的一个点(例如，设定为图4中的点k)，进而，将i点、j点和k点所在面作为特征点对应的特征面。

也就是说，在这种可能的实现方式中，仅基于历史点云数据中的上一帧点云数据来进行特征线和面特征线的确定。也就是说，在这种可能的实现方式中，通过相邻帧进行位姿估计。

在本申请另一种可能的实现方式中，基于历史点云数据确定每一线特征点对应的特征线的过程，首先包括：在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中线特征点周围的一组线特征点，并构建该组线特征点的协方差矩阵；然后，计算所述协方差矩阵的特征值V和特征向量 E；接着，确定所述特征值中最大者对应的特征向量的方向作为目标特征线的方向向量；进而，确定由所述一组线特征点中任一线特征点和所述方向向量所设定的直线为相应线特征点对应的特征线。也就是说，不同于上一种可能的实现方式，在另一种可能的实现方式中，对于特征线，在构建的点云地图中找到当前帧每一个线特征点周围的一组线特征点S构建协方差矩阵M，然后计算M的特征值V和特征向量E，如果其中一个特征值远大于另外两个，那么将该特征值对应的特征向量的方向作为特征线的方向向量，根据S中至少一个点和方向向量可得到一条直线即为线特征点对应的特征线；对于特征面，类似于特征线的计算方法，如果特征值中的其中一个远小于另外两个，最小的特征值对应的特征向量作为平面的法向量，根据法向量和至少一个点可得到一个平面，即为特征面。

也就是说，在这种可能的实现方式中，基于历史点云数据所构建的点云地图来进行线特征和面特征的确定。也就是说，在这种可能的实现方式中，通过历史帧进行位姿估计。相较于上一种可能的实现方式，其精度更高。

在步骤S250和步骤S260中，计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及，对所述代价函数进行优化，以确定当前帧的位姿。本领域普通技术人员应可以理解，在本申请其他示例中，还可以构建其他形式的代价函数，并且，对代价函数的优化方式也可以才有其他类型，对此，皆不为本申请所局限。

综上，基于本申请实施例的深度里程计方法被阐明，其基于TOF摄像模组所采集的点云数据进行位姿估计。值得一提的是，本申请实施例涉及的所述深度里程计方法以及能够实施所述深度里程计方法的装置、电子设备和计算机可读存储介质等的应用范围不仅仅局限于上述重定位方法中，其还能够单独使用或者在其他领域中得以应用，例如，AR/VR领域、无人驾驶领域等。

示意性定位方法

根据本申请又一方面，本申请还提供一种定位方法，其包括：响应于视觉里程计未丢失视觉，将由视觉里程计采集的当前帧的第一位姿确定为所述融合位姿；响应于视觉里程计丢失视觉且确定为转弯状态，获得由第三里程计采集的当前帧的第三位姿；基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿；响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及，基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

在上述定位方法中，在本申请一个示例中，基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定位姿修正量的过程，包括：获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿；获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；以及，基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。

在上述定位方法中，在本申请一个示例中，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量，包括：基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，确定所述视觉修正量。

在上述定位方法中，在本申请一个示例中，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量，包括：基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。

在上述定位方法中，在本申请一个示例中，所述第三里程计选自轮式编码器和惯性测量单元中任意一种。

综上，基于本申请实施例的定位方法被阐明，其采用多传感器融合的方式获得融合位姿，以提高系统的环境适应性和兼容性。值得一提的是，本申请实施例涉及的所述定位方法以及能够实施所述定位方法的装置、电子设备和计算机可读存储介质等的应用范围不仅仅局限于上述重定位方法中，其还能够单独使用或者在其他领域中得以应用，例如，AR/VR领域、无人驾驶领域等。

示意性重定位装置

根据本申请另一方面，本申请还提供一种重定位装置。

图8图示了根据本申请实施例的重定位装置的框图示意图。如图7所示，所述重定位装置800，包括：融合位姿确定单元810，用于基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；候选帧确定单元820，用于确定与当前帧匹配的多个重定位候选关键帧；投影单元830，用于根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；以及，重定位单元840，用于基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。

在上述重定位装置800中，在本申请一实施例中，所述候选帧确定单元 820，进一步用于：根据词袋模型确定与当前帧匹配的多个重定位候选关键帧。

在上述重定位装置800中，在本申请一实施例中，所述候选帧确定单元 820，进一步用于：获取当前帧的词袋数据，所述词袋数据包括用于表征所述当前帧的词节点；从词袋中筛选出与所述当前帧共享词节点的所有候选关键帧；去除所有所述候选关键帧中共享词节点的比例小于第一共享比例的候选关键帧；获取所有所述候选关键帧中剩下的候选关键帧与当前帧之间的相似性得分；以及，将相似性得分超过相似性最高得分的预设比例的所述候选关键帧确定为所述重定位候选关键帧。

在上述重定位装置800中，在本申请一实施例中，所述融合位姿确定单元810，进一步用于：响应于所述视觉里程计未丢失视觉，将由所述视觉里程计采集的当前帧的第一位姿确定为所述融合位姿。

在上述重定位装置800中，在本申请一实施例中，所述融合位姿确定单元810，进一步用于：响应于所述视觉里程计丢失视觉且确定为转弯状态，获得由所述第三里程计采集的当前帧的第三位姿；以及，基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿

在上述重定位装置800中，在本申请一实施例中，所述融合位姿确定单元810，进一步用于：响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及，基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

在上述重定位装置800中，在本申请一实施例中，所述融合位姿确定单元810，进一步用于：获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿；获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；以及，基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。

在上述重定位装置800中，在本申请一实施例中，所述融合位姿确定单元810，进一步用于：基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，确定所述视觉修正量。

在上述重定位装置800中，在本申请一实施例中，所述融合位姿确定单元810，进一步用于：基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。

在上述重定位装置800中，在本申请一实施例中，所述第三里程计选自轮式编码器和惯性测量单元中任意一种。

这里，本领域技术人员可以理解，上述重定位装置800中的各个单元和模块的具体功能和操作已经在上面参考图1到图7描述的重定位方法中详细介绍，并因此，将省略其重复描述。

如上所述，根据本申请实施例的重定位装置可以实现在各种终端设备中，例如扫地机器人中。在一个示例中，根据本申请实施例的重定位装置可以作为一个软件模块和/或硬件模块而集成到所述终端设备中。例如，该重定位装置可以是该终端设备的操作系统中的一个软件模块，或者可以是针对于该重定位装置所开发的一个应用程序；当然，该重定位装置同样可以是该终端设备的众多硬件模块之一。

替换地，在另一示例中，该重定位装置与该终端设备也可以是分立的终端设备，并且该重定位装置可以通过有线和/或无线网络连接到该终端设备，并且按照约定的数据格式来传输交互信息。

示意性电子设备

下面，参考图9来描述根据本申请实施例的电子设备。

图9图示了根据本申请实施例的电子设备的框图。

如图9所示，电子设备10包括一个或多个处理器11和存储器12。

处理器11可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备10中的其他组件以执行期望的功能。

存储器12可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器11可以运行所述程序指令，以实现上文所述的本申请的各个实施例的重定位方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如点云数据、融合位姿、参数等各种内容。

在一个示例中，电子设备10还可以包括：输入装置13和输出装置14，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。

例如，该输入装置13可以是例如键盘、鼠标等等。

该输出装置14可以向外部输出各种信息，包括融合位姿、点云地图等。该输出设备14可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。

当然，为了简化，图9中仅示出了该电子设备10中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备10还可以包括任何其他适当的组件。

示意性计算机程序产品

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的重定位方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的重定位方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims (38)

1.一种基于多传感器融合的重定位方法，其特征在于，包括：

基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；

确定与当前帧匹配的多个重定位候选关键帧；

根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；以及

基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。

2.根据权利要求1所述的重定位方法，其中，确定与当前帧匹配的多个重定位候选关键帧，包括：

根据词袋模型确定与当前帧匹配的多个重定位候选关键帧。

3.根据权利要求2所述的重定位方法，其中，根据词袋模型确定与当前帧匹配的多个重定位候选关键帧，包括：

获取当前帧的词袋数据，所述词袋数据包括用于表征所述当前帧的词节点；

从词袋中筛选出与所述当前帧共享词节点的所有候选关键帧；

去除所有所述候选关键帧中共享词节点的比例小于第一共享比例的候选关键帧；

获取所有所述候选关键帧中剩下的候选关键帧与当前帧之间的相似性得分；以及

将相似性得分超过相似性最高得分的预设比例的所述候选关键帧确定为所述重定位候选关键帧。

4.根据权利要求1所述的重定位方法，其中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿的过程，包括：

响应于所述视觉里程计未丢失视觉，将由所述视觉里程计采集的当前帧的第一位姿确定为所述融合位姿。

5.根据权利要求4所述的重定位方法，其中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿的过程，进一步包括：

响应于所述视觉里程计丢失视觉且确定为转弯状态，获得由所述第三里程计采集的当前帧的第三位姿；以及

基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿。

6.根据权利要求5所述的重定位方法，其中，基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿的过程，进一步包括：

响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；

获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及

基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

7.根据权利要求6所述的重定位方法，其中，基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定位姿修正量的过程，包括：

获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿；

获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；

基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；

基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；以及

基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。

8.根据权利要求7所述的重定位方法，其中，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量，包括：

基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，确定所述视觉修正量。

9.根据权利要求7所述的重定位方法，其中，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量，包括：

基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。

10.根据权利要求1-9任一所述的重定位方法，其中，基于深度里程计获得当前帧的第二位姿的过程，包括：

对获取的当前帧点云数据进行行列划分；

判定待处理的每一行和每一列中的点为线特征点或者面特征点；

基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；

计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及

对所述代价函数进行优化，以确定当前帧的位姿。

11.根据权利要求10所述的重定位方法，其中，基于历史点云数据确定每一线特征点对应的特征线，包括：

在上一帧点云数据中搜索与所述当前帧点云数据中线特征点位于相同行或者列中距离最近的第一个点和位于不同行或者列中距离最近的第二个点；以及

确定所述第一个点和第二个点所设定的直线为相应线特征点对应的特征线。

12.根据权利要求11所述的重定位方法，其中，基于历史点云数据确定每一线特征点对应的特征线，包括：

在上一帧点云数据中搜索与当前帧点云数据中相应面特征点位于相同行或者列中且距离最近的第三个点和第四个点，位于不同行或者列中且距离最近的第五个点；以及

确定所述第三个点、所述第四个点和所述第五个点所设定的面为相应面特征点对应的特征面。

13.根据权利要求10所述的重定位方法，其中，在对获取的当前帧点云数据进行行列划分之后，以及，判定待处理的每一行和每一列中的点为线特征点或者面特征点之前，进一步包括：

选择第一预设数量的行作为待处理的行；以及

选择第二预设数量的列作为待处理的列。

14.根据权利要求13所述的重定位方法，其中，判定待处理的每一行和每一列中的点为线特征点或者面特征点，包括：

获取待处理的每一行和每一列中点的曲率；

响应于曲率小于或等于第一预设阈值，将对应点确定为面特征点；以及

响应于曲率大于所述第一预设阈值，将对应点确定为线特征点。

15.根据权利要求14所述的重定位方法，其中，获取待处理的每一行和每一列中点的曲率，包括：

计算对应点云与其所在行或列前预设数量的点和后预设数量的点之间的距离的平均值，作为对应点云的曲率。

16.根据权利要求14所述的重定位方法，其中，在判定待处理的每一行和每一列中的点为线特征点或者面特征点之后，进一步包括：

识别并去除所述线特征点和所述面特征点中的异常点。

17.根据权利要求10所述的重定位方法，其中，基于历史点云数据确定每一线特征点对应的特征线，包括：

在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中线特征点周围的一组线特征点，并构建该组线特征点的协方差矩阵；

计算所述协方差矩阵的特征值V和特征向量E；

确定所述特征值中最大者对应的特征向量的方向作为目标特征线的方向向量；以及

确定由所述一组线特征点中任一线特征点和所述方向向量所设定的直线为相应线特征点对应的特征线。

18.根据权利要求17所述的重定位方法，其中，基于历史点云数据确定每一线特征面对应的特征面，包括：

在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中面特征点周围的一组面特征点，并构建该组面特征点的协方差矩阵；

计算所述协方差矩阵的特征值V和特征向量E；

确定所述特征值中最小者对应的特征向量的方向作为目标特征面的法向量；以及

确定由所述一组线特征点中任一面特征点和所述法向量所设定的平面为相应面特征点对应的特征面。

19.根据权利要求10所述的重定位方法，其中，所述点云数据由TOF深度信息摄像模组采集。

20.根据权利要求1-9任一所述的重定位方法，其中，所述第三里程计选自轮式编码器和惯性测量单元中任意一种。

21.一种深度里程计方法，其特征在于，包括：

对获取的当前帧点云数据进行行列划分；

判定待处理的每一行和每一列中的点为线特征点或者面特征点；

基于历史点云数据确定每一线特征点对应的特征线和每一面特征点对应的特征面；

计算所有线特征点到对应特征线的距离和面特征点到对应特征面的距离之和，作为代价函数；以及

对所述代价函数进行优化，以确定当前帧的位姿。

22.根据权利要求21所述的深度里程计方法，其中，基于历史点云数据确定每一线特征点对应的特征线，包括：

在上一帧点云数据中搜索与所述当前帧点云数据中线特征点位于相同行或者列中距离最近的第一个点和位于不同行或者列中距离最近的第二个点；以及

确定所述第一个点和第二个点所设定的直线为相应线特征点对应的特征线。

23.根据权利要求22所述的深度里程计方法，其中，基于历史点云数据确定每一线特征点对应的特征线，包括：

在上一帧点云数据中搜索与当前帧点云数据中相应面特征点位于相同行或者列中且距离最近的第三个点和第四个点，位于不同行或者列中且距离最近的第五个点；以及

确定所述第三个点、所述第四个点和所述第五个点所设定的面为相应面特征点对应的特征面。

24.根据权利要求21所述的深度里程计方法，其中，在对获取的当前帧点云数据进行行列划分之后，以及，判定待处理的每一行和每一列中的点为线特征点或者面特征点之前，进一步包括：

选择第一预设数量的行作为待处理的行；以及

选择第二预设数量的列作为待处理的列。

25.根据权利要求24所述的深度里程计方法，其中，判定待处理的每一行和每一列中的点为线特征点或者面特征点，包括：

获取待处理的每一行和每一列中点的曲率；

响应于曲率小于或等于第一预设阈值，将对应点确定为面特征点；以及

响应于曲率大于所述第一预设阈值，将对应点确定为线特征点。

26.根据权利要求25所述的深度里程计方法，其中，获取待处理的每一行和每一列中点的曲率，包括：

计算对应点与其所在行或列前预设数量的点和后预设数量的点之间的距离的平均值，作为对应点的曲率。

27.根据权利要求25所述的深度里程计方法，其中，在判定待处理的每一行和每一列中的点为线特征点或者面特征点之后，进一步包括：

识别并去除所述线特征点和所述面特征点中的异常点。

28.根据权利要求21所述的深度里程计方法，其中，基于历史点云数据确定每一线特征点对应的特征线，包括：

在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中线特征点周围的一组线特征点，并构建该组线特征点的协方差矩阵；

计算所述协方差矩阵的特征值V和特征向量E；

确定所述特征值中最大者对应的特征向量的方向作为目标特征线的方向向量；以及

确定由所述一组线特征点中任一线特征点和所述方向向量所设定的直线为相应线特征点对应的特征线。

29.根据权利要求28所述的深度里程计方法，其中，基于历史点云数据确定每一线特征面对应的特征面，包括：

在基于历史点云数据构建的点云地图中，搜索位于所述当前帧点云数据中面特征点周围的一组面特征点，并构建该组面特征点的协方差矩阵；

计算所述协方差矩阵的特征值V和特征向量E；

确定所述特征值中最小者对应的特征向量的方向作为目标特征面的法向量；以及

确定由所述一组线特征点中任一面特征点和所述法向量所设定的平面为相应面特征点对应的特征面。

30.根据权利要求21-29任一所述的深度里程计方法，其中，所述点云数据由TOF深度信息摄像模组采集。

31.一种定位方法，其特征在于，包括：

响应于视觉里程计未丢失视觉，将由视觉里程计采集的当前帧的第一位姿确定为所述融合位姿；

响应于视觉里程计丢失视觉且确定为转弯状态，获得由第三里程计采集的当前帧的第三位姿；

基于所述第三里程计和所述视觉里程计之间的相对位置关系，对所述第三位姿进行递推并将递推后的位姿确定为所述融合位姿；

响应于所述里程计丢失视觉且确定为非转弯状态，获得由所述深度里程计采集的当前帧的第二位姿和由所述第三里程计采集的当前帧的第三位姿；

获取基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定的位姿修正量；以及

基于所述位姿修正量对所述第三位姿进行修正，以获得所述融合位姿。

32.根据权利要求31所述的定位方法，其中，基于视觉丢失前的最后一帧的位姿数据和深度丢失前最后一帧的位姿数据确定位姿修正量的过程，包括：

获得由所述视觉里程计采集的视觉丢失前最后一帧的第一位姿以及由所述第三里程计采集的视觉丢失前最后一帧的第三位姿；

获得由所述深度里程计采集的深度丢失前最后一帧的第二位置以及由所述第三里程计采集的深度丢失前最后一帧的第三位姿；

基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量；

基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量；以及

基于视觉特征数量，确定所述视觉修正量和所述深度修正量对应的权重，以获得所述位姿修正量。

33.根据权利要求32所述的定位方法，其中，基于视觉丢失前最后一帧的所述第一位姿和所述第三位姿，确定视觉修正量，包括：

基于所述第三里程计所设定的坐标系和所述视觉里程计所设定的坐标系之间的相互转换关系，确定所述视觉修正量。

34.根据权利要求32所述的定位方法，其中，基于深度丢失前最后一帧的所述第二位姿和所述第三位姿，确定深度修正量，包括：

基于所述第三里程计所设定的坐标系和所述深度里程计所设定的坐标系之间的相互位置关系，确定所述深度修正量。

35.根据权利要求31-34任一所述的定位方法，其中，所述第三里程计选自轮式编码器和惯性测量单元中任意一种。

36.一种重定位装置，其特征在于，包括：

融合位姿确定单元，用于基于深度里程计、视觉里程计和第三里程计确定当前帧的融合位姿；

候选帧确定单元，用于确定与当前帧匹配的多个重定位候选关键帧；

投影单元，用于根据所述融合位姿将所有所述重定位关键帧中的地图点投影到当前帧对应的视场角下，以获得一系列投影点；以及

重定位单元，用于基于所述投影点与所述当前帧中的地图点之间的特征匹配结果，确定当前帧的目标位姿。

37.一种电子设备，其特征在于，包括：

存储器；以及

处理器，在所述存储器中存储有计算机程序指令，所述计算机程序指令在被所述处理器运行时使得所述处理器执行如权利要求1-20中任一项所述的重定位方法。

38.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行如权利要求1-20中任一项所述的重定位方法。

Images