CN111951341A - 一种基于rgb-d slam的闭环检测改进方法 - Google Patents

Abstract

本发明公开了一种基于RGB‑D SLAM的闭环检测改进方法，包括如下步骤：1)接收系统前端的关键帧序列F_s和新帧F_k+1，计算新帧F_k+1的运动估计，获得旋转矩阵R和平移向量t；2)计算新帧F_k+1的运动轨迹曲率c；3)选取阈值m的数值；4)比较c与m；5)检测完毕。这种方法能构建全局一致的轨迹和地图，能构建更准确地闭环，提高系统的可靠性。

Description

一种基于RGB-D SLAM的闭环检测改进方法

技术领域

本发明涉及RGB-D SLAM室内定位与地图构建系统相机位姿优化和闭环检测技术领域，具体是一种基于深度图像映射的(RGB+Depth Map simultaneous localization andmapping，简称RGB-D SLAM)的闭环检测改进方法。

背景技术

同时定位和地图构建(SLAM)是机器人实现自主移动的基本条件之一，近年来，随着非线性优化与图优化的研究的深入，SLAM技术的鲁棒性和实时性日渐提高，实际应用越来越广泛。在SLAM技术下，机器人能够在室内未知环境中，实时构建出室内地图，并且实现自主导航定位。该技术将为现实生活中的许多未知环境下的作业难题提供解决方案。

传统的SLAM主要基于扩展卡尔曼滤波器、Rao-CBlackwellized粒子滤波器和最大似然估计。随着传感器技术和优化算法的发展，Visual-SLAM成为主流。其通常使用深度相机，激光雷达，IMU作为前端传感器接收环境和机器人运动信息。在RGB-D SLAM中，深度相机获得的图像信息和深度信息可以有效地解决尺度不确定问题，前端使用ICP或PNP算法解算机器人位姿，后端使用非线性优化算法以及g2o优化框架对机器人位姿和运动情况进行优化，最后构建全局3D地图。但传统的RGB-D SLAM存在系统实时性较差，环路闭合不准等问题。

前端视觉里程计随着时间和帧数的增加，累计误差会越来越大。这将使得整个SLAM的结果不可靠，无法构建全局一致的轨迹和地图。回环检测提供了当前数据和所有历史数据的关联，在跟踪算法丢失后，能够利用回环检测进行重定位，它保证了估计的轨迹和地图在长时间下的正确性。为了提高闭环的准确性。将循环闭包检测算法与机器人运动轨迹曲率结合，该算法根据轨迹曲率值在全局环路检测和局部环路检测之间切换，在保证系统效率的同时，构建更准确的闭环。

发明内容

本发明的目的是针对现有技术的不足，而提供一种基于RGB-D SLAM的闭环检测改进方法。这种方法能构建全局一致的轨迹和地图，能构建更准确地闭环，提高系统的可靠性。

实现本发明目的的技术方案是：

一种基于RGB-D SLAM的闭环检测改进方法，包括如下步骤：

1)接收系统前端的关键帧序列F_s和新帧F_k+1，计算新帧F_k+1的运动估计，获得旋转矩阵R和平移向量t：

在RGB-D SLAM系统闭环检测过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时使用最小二乘优化重投影误差，过程如下：对关键帧F_k和新帧F_k+1，其中关键帧F_k为关键帧序列F_s中最后一帧关键帧，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁ ^k,s₂ ^k,...,s_n ^k}和S_k+1＝{s₁ ^k+1,s₂ ^k+1,...,s_n ^k+1}，结合给定图像深度的深度信息，得到3D特征点集P＝{p₁,p₂,...,p_n}和Q＝{q₁,q₂,...,q_n}，相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

由于存在误差，构建最小二乘来求解R和t，为公式(2)：

其中n表示特征点索引，最后采用ICP算法求解得旋转矩阵R和平移向量t的最优解；

2)计算新帧F_k+1的运动轨迹曲率c：轨迹曲率使用公式(3)计算：

3)选取阈值m的数值：为机器人轨迹曲率设定阈值m的范围，根据实测需要从设定阈值m的范围中选取阈值m的数值；

4)比较c与m：比较c与选取的阈值m的大小：

4-1)若c＜m，说明机器人在局部闭环的概率大，则进行局部闭环检测：将新帧F_k+1与关键帧序列F_s最后的u帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

4-2)若m＜c＜1，说明机器人在全局闭环的概率大，则进行全局闭环检测：将新帧F_k+1与关键帧序列F_s中的随机v帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

4-3)若c＝1，则先进行局部闭环检测，再进行全局闭环检测：将新帧F_k+1与关键帧序列F_s中的随机v帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

5)检测完毕：将符合步骤4)中任一条件下匹配成功的新帧F_k+1添加到关键帧序列F_s的末尾；对于后续的新帧，重复步骤1)到步骤4)的过程，直至闭环检测完毕即可。

与现有技术相比，本技术方案构造机器人运动轨迹曲率，减少前端视觉里程计累积误差的影响，在局部闭环检测和全局闭环检测策略中切换，提高系统检测闭环的效率和可靠性，构建更准确的系统闭环和全局一致的轨迹和地图。

这种方法能提高系统检测闭环的效率和可靠性，能构建全局一致的轨迹和地图。

附图说明

图1为实施例的流程示意图；

图2为实施例的结构示意图；

图3为实施例的效果图。

具体实施方式

下面结合附图和实施例对本发明内容作进一步的说明，但不是对本发明的限定。

实施例：

参照图1，一种基于RGB-D SLAM的闭环检测改进方法，包括如下步骤：

1)接收系统前端的关键帧序列F_s和新帧F_k+1，计算新帧F_k+1的运动估计，获得旋转矩阵R和平移向量t：

在RGB-D SLAM系统闭环检测过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时使用最小二乘优化重投影误差，过程如下：对关键帧F_k和新帧F_k+1，其中关键帧F_k为关键帧序列F_s中最后一帧关键帧，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁ ^k,s₂ ^k,...,s_n ^k}和S_k+1＝{s₁ ^k+1,s₂ ^k+1,...,s_n ^k+1}，结合给定图像深度的深度信息，得到3D特征点集P＝{p₁,p₂,...,p_n}和Q＝{q₁,q₂,...,q_n}，相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

由于存在误差，构建最小二乘来求解R和t，为公式(2)：

其中n表示特征点索引，最后采用ICP算法求解得旋转矩阵R和平移向量t的最优解；

2)计算新帧F_k+1的运动轨迹曲率c：轨迹曲率使用公式(3)计算：

3)选取阈值m的数值：为机器人轨迹曲率设定阈值m的范围，根据实测需要从设定阈值m的范围中选取阈值m的数值；

4)比较c与m：比较c与选取的阈值m的大小：

4-1)若c＜m，说明机器人在局部闭环的概率大，则进行局部闭环检测：将新帧F_k+1与关键帧序列F_s最后的u帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

4-2)若m＜c＜1，说明机器人在全局闭环的概率大，则进行全局闭环检测：将新帧F_k+1与关键帧序列F_s中的随机v帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

4-3)若c＝1，则先进行局部闭环检测，再进行全局闭环检测：将新帧F_k+1与关键帧序列F_s中的随机v帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

5)检测完毕：将符合步骤4)中任一条件下匹配成功的新帧F_k+1添加到关键帧序列F_s的末尾；对于后续的新帧，重复步骤1)到步骤4)的过程，直至闭环检测完毕即可。

如图2所示，本例通过构造机器人运动轨迹曲率，减少前端视觉里程计累积误差的影响，在局部闭环检测和全局闭环检测策略中切换，提高系统检测闭环的效率和可靠性，构建更准确的系统闭环和全局一致的轨迹和地图。

使用本例方法在Ubuntu 16.4中具有4GB内存的双核CPU上进行实验，使用nynv2和TUM RGB-D的室内数据集验证系统方法。图3显示了改进闭环检测方法的效果，其中图3的a图是由56帧关键帧构成的3D点云图，图3的b图是由改进闭环检测方法得到的位姿图，从图3可以看出本例方法对闭环检测具有良好的鲁棒性。

Claims (1)

1.一种基于RGB-D SLAM的闭环检测改进方法，其特征在于，包括如下步骤：

1)接收系统前端的关键帧序列F_s和新帧F_k+1，计算新帧F_k+1的运动估计，获得旋转矩阵R和平移向量t：

在RGB-D SLAM系统闭环检测过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时使用最小二乘优化重投影误差，过程如下：对关键帧F_k和新帧F_k+1，其中关键帧F_k为关键帧序列F_s中最后一帧关键帧，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁ ^k,s₂ ^k,...,s_n ^k}和S_k+1＝{s₁ ^k+1,s₂ ^k+1,...,s_n ^k+1}，结合给定图像深度的深度信息，得到3D特征点集P＝{p₁,p₂,...,p_n}和Q＝{q₁,q₂,...,q_n}，相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

由于存在误差，构建最小二乘来求解R和t，为公式(2)：

其中n表示特征点索引，最后采用ICP算法求解得旋转矩阵R和平移向量t的最优解；

2)计算新帧F_k+1的运动轨迹曲率c：轨迹曲率使用公式(3)计算：

3)选取阈值m的数值：为机器人轨迹曲率设定阈值m的范围，根据实测需要从设定阈值m的范围中选取阈值m的数值；

4)比较c与m：比较c与选取的阈值m的大小：

4-1)若c＜m，说明机器人在局部闭环的概率大，则进行局部闭环检测：将新帧F_k+1与关键帧序列F_s最后的u帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

4-2)若m＜c＜1，说明机器人在全局闭环的概率大，则进行全局闭环检测：将新帧F_k+1与关键帧序列F_s中的随机v帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

4-3)若c＝1，则先进行局部闭环检测，再进行全局闭环检测：将新帧F_k+1与关键帧序列F_s中的随机v帧进行特征匹配，如果匹配成功，在优化求解器中添加一条边，如果匹配不成功，舍弃该帧；

5)检测完毕：将符合步骤4)中任一条件下匹配成功的新帧F_k+1添加到关键帧序列F_s的末尾；对于后续的新帧，重复步骤1)到步骤4)的过程，直至闭环检测完毕即可。

Images