CN113902862B - 一种基于一致性集群的视觉slam回环验证系统 - Google Patents

Abstract

本发明涉及一种基于一致性集群的视觉SLAM回环验证系统，该系统包括：前端视觉里程计模块：用以对输入的图像帧提取特征，并确定相邻图像帧间的位置关系，构建局部地图；回环检测模块：以前端视觉里程计计算过的图像帧为基础，以相似度为依据进行图像帧为相同位置的判断，即判定回环；后端非线性优化模块：用以通过图优化方法对前端视觉里程计模块输入的局部地图和回环检测模块输出的回环约束进行全局优化，得到全局一致的轨迹和地图。与现有技术相比，本发明具有可靠性高、准确性好、资源消耗少等优点。

Description

一种基于一致性集群的视觉SLAM回环验证系统

技术领域

本发明涉及人工智能及机器人控制领域，尤其是涉及一种基于一致性集群的视觉SLAM回环验证系统。

背景技术

近些年来，人类社会朝着智能化和工业化的方向飞速发展，在此大环境下，国内外出现了一大批研发项目和机器人产品，涉及生活中的各个领域，移动机器人就是其中一类被广泛关注的机器人。例如，Google无人车项目，该项目旨在研发无人驾驶汽车，帮助驾驶员完成对车辆的启动、驾驶等操作。该项目已进入实际道路测试阶段，并完成了超百万公里的驾驶距离。另外，其他移动机器人，包括家庭清洁机器人、餐厅送餐机器人、安防机器人、搬运集装箱的大型货运自动导引机器人等，都已经被运用于各种场景，来提升工作效率，拓展人类的能力边界。机器人研究涉及多个工科研究领域，包括机械设计、智能控制、机器视觉等不同方面，因此对机器人的研究实质上是对多学科交叉融合课题的研究，可以推动不同技术的融合发展。

移动机器人的同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)技术是保证移动机器人可以在无需外界帮助条件下完成路径导航的必要技术，而其中尤为重要的是对机器人自身所在位置的精准确定，帮助机器人实现在运动层面的控制和在路径层面的规划等工作，推动移动机器人的智能化和完全自主化发展。

回环检测是视觉SLAM的重要组成部分，其主要作用是判断出当前位置是否为机器人先前经过的位置，若判断是相同位置则认为是回环，将该情况作为后端优化的约束条件，提高定位准确性。正确的回环检测算法可以优化节点位姿约束，克服机器人移动过程中因为计算误差累积导致的漂移问题，完成全局一致的地图构建，极大的帮助了机器人的自主导航工作。SLAM系统基于前端视觉里程计构建的地图因为累积的位姿漂移出现偏差，当移动到先前到达的位置后，回环检测模块识别出回环后，提供优化约束到后端，对轨迹进行全局一致性优化，修正了累积误差，确定移动轨迹并构建准确地图。

视觉SLAM回环检测算法通过图像帧判别出机器人到达相同位置后，将该信息作为优化约束条件添加到后端非线性优化过程中，帮助系统获取更加精准的地图和更加精确的定位，然而仅是基于两幅图像帧的相似性判定出的单个回环假设可靠性不够高，可能造成部分错误的约束条件添加，导致错误的地图估计，因此，需要对视觉SLAM回环验证系统的定位精确性进行进一步的研究。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于一致性集群的视觉SLAM回环验证系统。

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

一种基于一致性集群的视觉SLAM回环验证系统，该系统包括：

前端视觉里程计模块：用以对输入的图像帧提取特征，并确定相邻图像帧间的位置关系，构建局部地图；

回环检测模块：以前端视觉里程计计算过的图像帧为基础，以相似度为依据进行图像帧为相同位置的判断，即判定回环；

后端非线性优化模块：用以通过图优化方法对前端视觉里程计模块输入的局部地图和回环检测模块输出的回环约束进行全局优化，得到全局一致的轨迹和地图。

应用该系统的回环验证方法具体包括以下步骤：

1)通过前端视觉里程计模块对输入图像作特征提取，并基于对极几何确定相邻图像帧间的距离关系，构建局部地图；

2)回环检测模块基于输入图像帧的特征信息将当前图像帧与之前已计算的图像帧比较，并根据图像距离完成回环假设判断；

3)对所有回环假设作集群一致性处理，得到多个回环假设集群；

4)将回环假设集群中的多个回环假设作为约束条件提供到后端非线性优化模块中；

5)后端非线性优化模块基于回环假设对已构建的全局地图添加约束条件，通过图优化理论得到全局一致的轨迹和地图。

所述的步骤1)中，对输入图像帧进行特征提取，具体为：

提取输入图像帧的颜色特征、边缘特征和纹理特征作为图像帧信息内容的表示，分别以RGB特征、梯度幅值和方向角以及LBP纹理特征作为特征标准，通过不同特征间的关系构造特征向量F(x,y)表示图像区域信息，则有：

F(x,y)＝[R(x,y),G(x,y),B(x,y),G_r(x,y),θ(x,y),L_bp(x,y)]

其中，R(x,y)、G(x,y)、B(x,y)分别为像素点(x,y)的RGB特征，G_r(x,y)、θ(x,y)分别为像素点的梯度幅值和方向角，L_bp(x,y)为像素点的LBP值。

所述的步骤1)中，根据输入图像帧中待匹配区域I的协方差矩阵C_I，具体为：

根据图像特征间的相互关系构建与待匹配区域维度大小相同的图像信息协方差矩阵，对待匹配区域I的特征矩阵进行计算获取协方差矩阵C_I，则有：

式中，μ为特征向量的均值，M、N分别为图像的宽和高，F(i,j)为位置(i,j)处的特征向量，n为像素点个数，z_k为第k个像素点的特征向量。

所述的步骤2)中，基于距离阈值的回环判断具体为：

获取当前图像帧与之前图像帧的协方差矩阵间的相似度，即距离，并设定距离阈值，若相似度小于距离阈值，则认为两幅图像帧是单个回环假设，视为相同场景，即产生回环。

所述的相似度ρ(C_i,C_j)为对协方差矩阵广义特征值的对数平方和开根号，则有：

其中，λ_d(C_i,C_j)为协方差矩阵C_i和C_j通过|C_i-λC_j|＝0计算得到的第d个特征值，D为特征值总数。

所述的步骤3)中，回环验证系统对所有回环假设作集群一致性处理，得到多个回环假设集群，具体包括以下步骤：

41)根据步骤2)的回环假设判断得到多个回环假设；

42)根据时间增量关系将回环假设分组为多个回环假设集群；

43)对回环假设集群内部进行一致性校验，去除错误的回环假设，得到正确的回环假设集群。

所述的步骤42)中，对于获取的多个回环假设，将部分拓扑关系相同的回环假设合并为同一集群，使用时间增量方法进行分组，具体为：

将获取的第一个回环假设初始化为第一个集群R₁，之后判断下一个获取的回环假设是否属于同一个集群，若属于同一集群，则分组到第一个集群R₁中，否则初始化为一个新的集群，当下一个获取的回环假设若属于同一集群时，则对接下来获取的新的回环假设仍与第一个集群R₁比较，否则与新建的集群进行比较，当回环Z_pq的开始时间戳t_p和结束时间戳t_q满足与已在集群中的回环Z_ij的开始时间戳t_i和结束时间戳t_j的时间差距在t_g范围内时，则将Z_pq回环假设添加到同一个集群中，否则新建一个包含该回环假设的新集群。

所述的步骤43)中，对同一集群内部的所有回环假设作一致性验证，将其中明显偏离集群整体的部分回环假设移除，优化集群回环结果，再作为后续优化的约束条件，以提高系统性能，每个回环假设采用向量表示为：

L＝[x₁,y₁,x₂,y₂,dis]^T

其中，x₁、y₁分别为回环假设的当前帧计算得到的地图位置，x₂、y₂为之前帧计算得到的地图位置，dis为图像帧经过回环检测模块计算得到的距离；

因此，当对集群内的所有回环假设作一致性验证时，采用聚类方法对所有回环假设聚集成多个不同样本簇，将样本簇数量最多的定义为根簇，并获取根簇与其他簇中心的距离，若距离值小于分类阈值，则认为满足一致性验证，将其加入根簇，对不满足距离阈值的簇定义为异常簇，将异常簇中的回环假设从集群中移除，以避免作为后续优化的约束条件。

采用的聚类方法为K均值聚类方法。

与现有技术相比，本发明具有以下优点：

一、本发明中的回环验证系统的设计方法简单，但可以移除回环检测模块判断的错误回环，从而使得系统具有更高的可靠性。

二、本发明设计的集群一致性验证系统，在减少计算性能消耗的情况下，对集群内的回环假设做了准确性优化。

三、本发明的回环验证系统，作用于SLAM系统的回环检测模块，且对系统资源的消耗较少，可以较好的切入原SLAM系统，提升系统整体性能。

附图说明

图1为视觉SLAM系统结构示意图。

图2为图像协方差矩阵的构造过程。

图3为数据集场景，其中，图(3a)为TUM large-with-loop数据集场景，图(3b)为TUM long-office-household数据集场景，图(3c)为City Center数据集场景，图(3d)为NewCollege数据集场景。

图4为系统在TUM large-with-loop数据集的实验结果。

图5为系统在TUM long-office-household数据集的实验结果。

图6为系统在City Center数据集的实验结果。

图7为系统在New College数据集的实验结果。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

如图1所示，本发明提供一种基于一致性集群的视觉SLAM回环验证系统，该系统包括：

前端视觉里程计：对输入图像帧提取特征，并通过对极几何等方法计算得到相邻帧间的位置关系，构建局部地图。

回环检测模块：以前端视觉里程计计算过的图像帧为基础，对相似度较高的图像帧认为是相同位置，即判定回环，再将其结果作为优化约束提供到后端非线性优化中。

后端非线性优化：通过图优化的方法对前端视觉里程计输入的局部地图和回环检测模块提供的回环约束作全局优化，得到全局一致的轨迹和地图。

基于一致性集群的视觉SLAM回环验证系统的设计方法，该方法包括如下步骤：

(1)前端视觉里程计对输入图像作特征提取，并基于对极几何确定相邻图像帧间关系，构建局部地图；

(2)回环检测模块基于输入图像帧的特征信息将当前图像帧与之前已计算的图像帧比较，并根据图像距离完成回环假设判断；

21)输入图像帧特征提取：

对输入图像帧提取其颜色特征、边缘特征、纹理特征作为图像帧信息内容的表示，分别采取RGB特征、梯度幅值和方向角以及LBP纹理特征这三类特征作为特征标准，如图2所示，通过不同特征间的关系构造特征向量F(x,y)表示图像区域信息：

F(x,y)＝[R(x,y),G(x,y),B(x,y),G_r(x,y),θ(x,y),L_bp(x,y)] (1)

式中R(x,y),G(x,y),B(x,y)分别对应像素点的RGB特征，G_r(x,y),θ(x,y)表示像素点的梯度幅值和方向角，L_bp(x,y)表示像素点的LBP值，最终使用F特征向量表示图像区域信息。

22)基于协方差矩阵的距离度量：

对两幅图像提取相同类型的特征，计算特征间的相互关系构建与该区域维度大小相同的图像信息协方差矩阵，对区域I的特征矩阵进行计算获取协方差矩阵：

式中μ为特征向量均值，M、N为图像的宽和高，F(i,j)为位置(i,j)处的特征向量，n为像素点个数，z_k为第k个像素点的特征向量。

获取待匹配区域的协方差矩阵后，比较两矩阵间的相似度，将相似度高的结果视为相同场景，即产生回环。采用如下公式计算距离，即对不同矩阵广义特征值的对数平方和开根号：

式中，λ_k(C_i,C_j)为协方差矩阵C_i和C_j通过|C_i-λC_j|＝0计算得到的特征值。

23)基于距离阈值的回环判断：

对计算得到的两幅输入图像帧间的距离，将其与已定义的图像距离阈值作比较，对小于距离阈值的两幅图像帧认为是单个回环假设。

(3)回环验证系统对所有回环假设作集群一致性处理，得到多个回环假设集群；

31)根据权利要求3判断得到多个回环假设；

32)根据时间增量关系将回环假设分组为多个回环假设集群；

基于回环检测模块可获取多个回环假设，将部分拓扑关系相同的回环假设合并为同一集群，使用时间增量方法对它们进行分组，具体步骤如下：首先将获取的第一个回环假设初始化为第一个集群R₁，之后根据式(5)判断下一个获取的回环假设是属于同一个集群，还是需要初始化一个新的集群，若属于同一集群，则对接下来获取的新的回环假设仍与R₁比较，否则与新建的集群比较。

||t_i-t_p||≤t_g，||t_j-t_q||≤t_g (5)

式中i、j表示待比较的回环假设中的图像帧，t_i、t_j分别表示对应图像帧的时间节点，t_p、t_q表示新获取的回环假设图像p和q的时间节点。t_g表示时间的增量范围，用来判断是两者是否属于同一集群，与回环检测模块的运行速度相关。

当回环验证系统获取到第一个回环假设Z_ij时，会初始化为一个新的集群。继续获取到新的回环假设Z_pq时，如果该回环的开始时间戳t_p和结束时间戳t_q满足与已在集群中的回环Z_ij的开始时间戳t_i和结束时间戳t_j的时间差距在t_g范围内，则将Z_pq回环假设添加到同一个集群中，否则新建一个包含该回环假设的新集群，也就意味着只有轨迹相同的回环假设可以被添加到同一集群中。当验证系统获取到新的回环假设时，如果该回环满足式(5)，则将该回环添加到当前集群中，否则就新建集群，保证了集群的动态增长。

33)对回环假设集群内部进行一致性校验，去除错误的回环假设，得到正确的回环假设集群。

对同一集群内部的所有回环假设作一致性验证，将其中明显偏离集群整体的部分回环假设移除，优化集群回环结果，再作为后续优化的约束条件，提高系统性能。每个回环假设都可以用下列向量表示：

L＝[x₁,y₁,x₂,y₂,dis]^T (6)

式中，x₁、y₁分别为该回环假设的当前帧计算得到的地图位置，x₂、y₂为之前帧计算得到的地图位置，dis为图像帧经过回环检测模块计算得到的距离。

因此，当需要对集群内的所有回环假设作一致性验证时，采用聚类方法对所有假设聚集成多个不同样本簇，将样本簇数量最多的定义为根簇，计算其与其他簇中心的距离，若距离值小于分类阈值，则认为也满足一致性验证，将其加入根簇，对其他不满足距离阈值的情况定义为异常簇，对其中的回环假设从集群中移除，避免作为后续优化的约束条件。选取复杂度较低的K均值聚类方法，具体步骤如下：

331)选择初始的k个样本作为初始簇中心a＝a₁,a₂,a₃,a₄,a₅...a_k；

332)针对回环假设集群中的每个回环假设s_i，计算它到k个簇中心的距离并将其分到距离最小的簇中心所对应的类中；

333)针对每个类别a_j，重新计算它的簇中心(即属于该类的所有样本的质心)；

334)重复上面步骤2和步骤3，直到质心的变化小于设定的阈值时，即完成分类；

(4)将回环验证系统获得的回环假设集群中的多个回环假设作为约束条件提供到后端非线性优化中；

(5)后端非线性优化基于回环假设对已构建的全局地图添加约束条件，通过图优化理论得到全局一致的轨迹和地图。

实施例：

本节通过对移动机器人SLAM系统进行仿真实验来验证上文阐述的一种基于一致性集群的视觉SLAM回环验证系统对整体SLAM系统的准确性提升。

分别选择实验场景为TUM RGB-D基准数据集中的large-with-loop和long-office-household数据集、City Center数据集、New College数据集，场景如图3所示，将应用回环验证后的算法结果与未应用验证算法的结果进行比较，并分析实验结果。实验相关参数中，集群回环时间的增量范围设置为8s，簇中心距离阈值设置为15。

SLAM回环检测算法的性能评估标准使用以下两个参数：召回率(recall)和准确率(precision)，其计算方式分别如下：

式中，TP表示系统识别正确的回环，FP表示系统将是回环的情况错误的识别为不是回环，FN表示系统将不是回环的情况错误的识别为是回环。

召回率表示的是算法识别的正确回环占真实场景下所有回环中的比例，它衡量的是正确回环对整个场景中所有回环的覆盖率，即查全率。准确率表示的是算法识别的正确回环占算法识别出的所有回环的比例，即查准率。通常对于任意系统来说，追求的系统性能只能选择召回率和准确率其中之一。考虑在SLAM系统中，准确率的优先级高于召回率，因此主要通过准确率的不同来比较算法结果的差异。

图4到图7展示了不同数据集在应用基于一致性集群的回环验证算法后的PR曲线结果，其结果表明，回环验证算法对系统趋于稳定后的准确率或召回率没有明显影响，无法明显提升准确率或召回率，但其基于一致性集群来确定机器人移动过程中到达的相同位置，在系统在追求高召回率降低相似度判断标准时，对回环判断的准确率不会收到明显影响，保证了系统的可靠性。

表1展示了应用基于一致性集群的回环验证算法后的图像处理时间，其结果表明该验证算法增加了整个回环检测模块的运行时间，相较于原流程耗时增加约10％，但其整体时间仍然较小，存在可行性。

表1计算性能比较

综上，回环检测模块对移动机器人移动到相同位置时，识别到正确回环假设，将其作为优化约束可以很好的提高系统整体的准确性，但是若提供了错误的回环假设，则会对全局的轨迹和地图造成较大的干扰，本发明的基于一致性集群的视觉SLAM回环验证系统，作用于回环检测模块得出的多个回环假设，聚类得到多个回环假设集合，以满足距离条件的部分集合认为是正确回环，并且以集群的形式提供给后端优化。

Claims (5)

1.一种基于一致性集群的视觉SLAM回环验证系统，其特征在于，该系统包括：

前端视觉里程计模块：用以对输入的图像帧提取特征，并确定相邻图像帧间的位置关系，构建局部地图；

回环检测模块：以前端视觉里程计计算过的图像帧为基础，以相似度为依据进行图像帧为相同位置的判断，即判定回环；

后端非线性优化模块：用以通过图优化方法对前端视觉里程计模块输入的局部地图和回环检测模块输出的回环约束进行全局优化，得到全局一致的轨迹和地图；

应用该系统的回环验证方法具体包括以下步骤：

1)通过前端视觉里程计模块对输入图像作特征提取，并基于对极几何确定相邻图像帧间的距离关系，构建局部地图；

2)回环检测模块基于输入图像帧的特征信息将当前图像帧与之前已计算的图像帧比较，并根据图像距离完成回环假设判断；

3)对所有回环假设作集群一致性处理，得到多个回环假设集群；

4)将回环假设集群中的多个回环假设作为约束条件提供到后端非线性优化模块中；

5)后端非线性优化模块基于回环假设对已构建的全局地图添加约束条件，通过图优化理论得到全局一致的轨迹和地图；

所述的步骤1)中，对输入图像帧进行特征提取，具体为：

提取输入图像帧的颜色特征、边缘特征和纹理特征作为图像帧信息内容的表示，分别以RGB特征、梯度幅值和方向角以及LBP纹理特征作为特征标准，通过不同特征间的关系构造特征向量F(x,y)表示图像区域信息，则有：

F(x,y)＝[R(x,y),G(x,y),B(x,y),G_r(x,y),θ(x,y),L_bp(x,y)]

其中，R(x,y)、G(x,y)、B(x,y)分别为像素点(x,y)的RGB特征，G_r(x,y)、θ(x,y)分别为像素点的梯度幅值和方向角，L_bp(x,y)为像素点的LBP值；

所述的步骤1)中，根据输入图像帧中待匹配区域I的协方差矩阵C_I，具体为：

根据图像特征间的相互关系构建与待匹配区域维度大小相同的图像信息协方差矩阵，对待匹配区域I的特征矩阵进行计算获取协方差矩阵C_I，则有：

式中，μ为特征向量的均值，M、N分别为图像的宽和高，F(i,j)为位置(i,j)处的特征向量，n为像素点个数，z_k为第k个像素点的特征向量；

所述的步骤3)中，回环验证系统对所有回环假设作集群一致性处理，得到多个回环假设集群，具体包括以下步骤：

41)根据步骤2)的回环假设判断得到多个回环假设；

42)根据时间增量关系将回环假设分组为多个回环假设集群；

43)对回环假设集群内部进行一致性校验，去除错误的回环假设，得到正确的回环假设集群；

所述的步骤42)中，对于获取的多个回环假设，将部分拓扑关系相同的回环假设合并为同一集群，使用时间增量方法进行分组，具体为：

将获取的第一个回环假设初始化为第一个集群R₁，之后判断下一个获取的回环假设是否属于同一个集群，若属于同一集群，则分组到第一个集群R₁中，否则初始化为一个新的集群，当下一个获取的回环假设若属于同一集群时，则对接下来获取的新的回环假设仍与第一个集群R₁比较，否则与新建的集群进行比较，当回环Z_pq的开始时间戳t_p和结束时间戳t_q满足与已在集群中的回环Z_ij的开始时间戳t_i和结束时间戳t_j的时间差距在t_g范围内时，则将Z_pq回环假设添加到同一个集群中，否则新建一个包含该回环假设的新集群。

2.根据权利要求1所述的一种基于一致性集群的视觉SLAM回环验证系统，其特征在于，所述的步骤2)中，基于距离阈值的回环判断具体为：

获取当前图像帧与之前图像帧的协方差矩阵间的相似度，即距离，并设定距离阈值，若相似度小于距离阈值，则认为两幅图像帧是单个回环假设，视为相同场景，即产生回环。

3.根据权利要求2所述的一种基于一致性集群的视觉SLAM回环验证系统，其特征在于，所述的相似度ρ(C_i,C_j)为对协方差矩阵广义特征值的对数平方和开根号，则有：

其中，λ_d(C_i,C_j)为协方差矩阵C_i和C_j通过|C_i-λC_j|＝0计算得到的第d个特征值，D为特征值总数。

4.根据权利要求1所述的一种基于一致性集群的视觉SLAM回环验证系统，其特征在于，所述的步骤43)中，对同一集群内部的所有回环假设作一致性验证，将其中明显偏离集群整体的部分回环假设移除，优化集群回环结果，再作为后续优化的约束条件，以提高系统性能，每个回环假设采用向量表示为：

L＝[x₁,y₁,x₂,y₂,dis]^T

其中，x₁、y₁分别为回环假设的当前帧计算得到的地图位置，x₂、y₂为之前帧计算得到的地图位置，dis为图像帧经过回环检测模块计算得到的距离；

因此，当对集群内的所有回环假设作一致性验证时，采用聚类方法对所有回环假设聚集成多个不同样本簇，将样本簇数量最多的定义为根簇，并获取根簇与其他簇中心的距离，若距离值小于分类阈值，则认为满足一致性验证，将其加入根簇，对不满足距离阈值的簇定义为异常簇，将异常簇中的回环假设从集群中移除，以避免作为后续优化的约束条件。

5.根据权利要求4所述的一种基于一致性集群的视觉SLAM回环验证系统，其特征在于，采用的聚类方法为K均值聚类方法。