[发明专利]一种可提高机器人位姿一致性的ORB关键帧闭环检测SLAM方法

说明书

技术领域

本发明涉及机器人的同时定位与地图构建(Simultaneously Localization and Mapping，SLAM)领域，尤其是一种可提高机器人位姿一致性的ORB关键帧闭环检测SLAM方法。

背景技术

为了在未知室内环境下实现自主移动与导航，智能机器人应具有构建室内环境地图，并同时具有在全局地图中定位的能力，上述过程同时实现称为机器人的同时定位与地图构建(Simultaneously Localization and Mapping，SLAM)。当机器人处于室外环境时，可通过高精度的GPS和先验地图实现全局地图下的定位。但当机器人处于室内环境，或处于GPS不可用的环境(水下、矿洞)时，就必须采取其他方式实现机器人SLAM。

现有的室内SLAM方法存在的缺陷：定位一致性差，环境地图构建质量不高以及优化效率低。

发明内容

为了克服已有室内机器人定位与环境地图构建中定位一致性差，环境地图构建质量不高以及优化效率低的不足，本发明提供一种可提高机器人位姿一致性、环境地图构建质量较高以及优化效率高的ORB关键帧闭环检测SLAM方法。

本发明解决其技术问题所采用的技术方案是：

一种可提高机器人位姿一致性的ORB关键帧闭环检测SLAM方法，包括如下步骤：

首先，采用RGB-D传感器获取环境的彩色信息与深度信息，使用ORB特征提取图像特征；

接着，通过基于RANSAC-ICP帧间配准算法实现了机器人位姿估计，构建初始位姿图；

最后，通过提取KeyFrame关键帧中的ORB特征构建BoVW(视觉词袋)，当前关键帧与BoVW中的单词进行相似性比较，实现闭环关键帧检测，通过关键帧帧间配准检测添加位姿图约束，得到全局最优的机器人位姿。

进一步，所述基于RANSAC-ICP帧间配准算法包括基于RANSAC算法的初始粗配准与基于ICP算法的精确配准，过程如下：

首先通过RANSAC算法对特征集进行初始配准，找到两帧中的最佳平移旋转变换，随后通过重投影误差RANSAC优化目标函数：

式中，A_f包含了特征内点；分别表示场景帧和参考帧上关联点对的空间坐标；T(p)＝R·p+t表示平移旋转变换矩阵；π(p)为参考帧摄像头上的投影变换函数。

优选的，式(1-10)通过帧间稀疏BA方法进行优化；ICP迭代过程中，首先通过K-D树检索最近点实现点云关联，通过优化关联特征点与点云的联合误差函数实现全局位姿变换矩阵的优化，权重因子w_j表示关联点对在误差函数中的重要性；

式中，α为各投影变换函数件的权重因子。

再进一步，所述闭环检测方法中，通过KeyFrame选取算法，选取过程为：当前帧与上一关键帧通过RANSAC进行配准，如果配准内点值小于规定阈值，则认定其为关键帧；采用关键帧视觉词袋模型，该模型将关键帧的ORB特征聚类，利用视觉词典将特征描述子映射为视觉单词，通过对比关键帧中的“单词”，可判断两个关键帧是否形成闭环。

所述关键帧选取过程描述为：在完成RANSAC-ICP精确估计后，利用当前帧与上一关键帧再次进行RANSAC-ICP配准。如果当前帧与关键帧之间的配准误差大于阈值，则认为该帧所在场景为新场景，该帧设为关键帧，反之，该帧为普通帧，用于位姿图构建，选取下一帧进行关键帧比对，其数学过程可描述为：

其中，为离j时刻最近的关键帧；F_j为当前帧。

更进一步，所述闭环检测方法的流程如下：

1)首先从所有图像中检测并提取关键帧，假设机器人位于地图场景中的某一关键帧位置，将生成的关键帧图像使用ORB特征描述符进行描述，关键帧图像I可表示为描述符集合D:I→{d₁,…,d_n}；

2)提取每个关键帧场景的ORB特征并投影到BoVW中，将关键帧ORB特征点与视觉词典中的单词进行关联，视觉词典可表示为通过BoVW相似描述符聚类可构建视觉词典V，通过映射形成最邻近视觉字典，每一个视觉单词的ORB描述向量都是一个关联的视觉词表；

3)采用单词相似度的方式计算“单词”间距，形成关键帧场景的相似度，为获得场景相似度，计算关键帧图像I_u和I_v之间的余弦距离可得到二者BoVW的相似度，其过程为：