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

摘要

一种可提高机器人位姿一致性的ORB关键帧闭环检测SLAM方法，包括如下步骤：首先，采用RGB‑D传感器获取环境的彩色信息与深度信息，使用ORB特征提取图像特征；接着，通过基于RANSAC‑ICP帧间配准算法实现了机器人位姿估计，构建初始位姿图；最后，通过提取KeyFrame关键帧中的ORB特征构建BoVW(视觉词袋)，当前关键帧与BoVW中的单词进行相似性比较，实现闭环关键帧检测，通过关键帧帧间配准检测添加位姿图约束，得到全局最优的机器人位姿。本发明提供一种可提高机器人位姿一致性、环境地图构建质量较高以及优化效率高的ORB关键帧闭环检测SLAM方法。

说明书

技术领域

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

背景技术

为了在未知室内环境下实现自主移动与导航，智能机器人应具有构建室内环境地图，并同时具有在全局地图中定位的能力，上述过程同时实现称为机器人的同时定位与地图构建(SimultaneouslyLocalization 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优化目标函数：

$T^{*}=\mathrm{argmin}\left(\frac{1}{\left|A_f\right|}\underset{i\in A_f}{\Sigma }{|\pi \left(T\right(f_s^i\left)\right)-\pi \left(f_t^i\right)|}^2\right)---(1-10)$

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

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

$T^{*}=\underset{T}{\arg \min }[\alpha \left(\frac{1}{\left|A_f\right|}\underset{i\in A_f}{\Sigma }{|\pi \left(T\right(f_s^i\left)\right)-\pi \left(f_t^i\right)|}^2\right)+(1-\alpha )\left(\frac{1}{A_d}\underset{j\in A_d}{\Sigma }{w}_j{|\left(T\right(p_s^j)-p_t^j)·n_t^j)|}^2\right)]---(1-11)$

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

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

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

$F_{j+1}^k=\left(\begin{array}{cc}{F}_j& j=0\\ \left\{F_j\right|inlier(Perform\_RANSAC\_Alignment(F_i^k,F_j\left)\right)<\theta \}& j>0\end{array}\right)---(1-12)$

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

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

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

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

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

每一关键帧由不同权重的词汇聚类而成，每个词汇在全部图像N中出现的频率形成权重w_i：

w_i＝log₁₀(N/n_i)>

式中n_i表示d_i中包含图像的数量，若BoVW中包含|V|个不同的视觉单词，则图像矢量可描述为：其中关键帧中的单词权重为：

得到每个单词权重后，即可求出其所在图像的权重，通过相似函数S可计算键帧图像I_u和I_v之间的相似度：

$S(I_u,I_v)=\frac{\underset{i=0}{\overset{\left|V\right|}{\Sigma }}{u}_i{v}_i}{\sqrt{\underset{i=0}{\overset{\left|V\right|}{\Sigma }}{u}_i^2}·\sqrt{\underset{i=0}{\overset{\left|V\right|}{\Sigma }}{v}_i^2}}---(1-14)$

通过上式可得相似矩阵进而判断图像间的相似度，图像i和图像j通过相似函数得到相似度矩阵M_i,j；

通过视觉词袋方法，关键帧闭环检测问题可转化为从图像序列L＝[I₁,I₂,…]中找到两个相似度较高的子序列A＝[a₁,a₂,…]和B＝[b₁,b₂,…]，从而实现闭环检测。

本发明中，随机采样一致性(RANSAC)是一种可用于提出数据离群点的迭代算法。SLAM方法使用RANSAC算法可减少帧间配准过程中的图像特征匹配离群点，实现一种初始一致性较好的位姿估计，提高帧间匹配质量。RANSAC通过对输入数据进行多次随机采样，每 次抽取具有代表性的少量数据对估计模型进行评估，根据已有模型对所有数据进行筛选，保留误差范围内的数据称为有效数据点或内点(inliers)，筛除误差较大的数据离群点或外点(outliers)。离群点一般是噪声数据或异常数据，内点比例较高的帧间配准，所获得的位姿估计质量越高，越接近真实情况。

假设前后帧间共有n对匹配的三维点，变换参数可用旋转矩阵R和平移向量t表示，则变换参数与三维点集之间的关系可表示为：

p_ci＝rp_pi+t>

式中p_ci与p_pi分别代表当前帧和上一帧匹配的第i对三维点坐标，i＝1,2,…,n。设θ和ψ分别为绕y轴、x轴和z轴旋转的角度，称为偏航角、俯仰角及翻滚角，统称为姿态角，则r与三个姿态角的关系为：

代表姿态增量的旋转参数也可用四元数表示。二维平面中单位圆上任意一点对应一个旋转角度，可表示二维空间的点姿态；在三维空间中，单位球上任意一点应绕两轴旋转的角度，可描述某点的三维空间位姿；可推，在四维空间中，三维空间中任意的旋转可由四维单位球上的点q＝[q₀,q₁,q₂,q₃]^T表示，q＝[q₀,q₁,q₂,q₃]^T称为四元数，即四维空间单位球面上任意一点可描述绕三个轴的旋转角度。三维旋转通过单位四元数表示有两种表示方式：q、-q，但给定一个四元数，有且仅有一个三维旋转可与之对应。使用四元数表示三维空间的旋转运动，可有效减少中间变量；与欧拉角相比，四元具有更稳定的数值解。旋转 变换矩阵r可由四元数q表示：

$r=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---(1-3)$

$\xi =\underset{\{{\xi }_1\mathrm{...},{\xi }_n\in SE\left(3\right)\}}{\mathrm{argmin}}\underset{<i,j>\in C}{\Sigma }e{({\xi }_i,{\xi }_j,{\tilde{\xi }}_{i,j})}^T{\Omega }_{i,j}({\xi }_i,{\xi }_j,{\tilde{\xi }}_{i,j})---(1-4)$

对方程(1-4)求解，若忽略三维点重建误差，可以使用直接线性变换方程(Direct Linear transformation，DLT)，但该方法精度较差，对输入数据比较敏感。考虑到三维点重建误差，运动参数的估计可转化为非线性最小化误差和的优化问题：

$(\hat{r},\hat{t})=\underset{r,t}{\mathrm{argmin}}\underset{i=1}{\overset{n}{\Sigma }}\left|\right|p_{ci}-({\mathrm{rp}}_{pi}+t)|{|}^2---(1-5)$

式(1-5)是一个最小二乘估计问题，通过最大似然法可实现非线性参数估计。本发明通过RANSAC去除数据中的离群点，以提高位姿估计的准确性。

迭代最近点(ICP)是一种可用于计算具有深度和彩色信息的立体点云数据间匹配关系的算法，可筛选出满足匹配关系的有效数据，实现位姿间的精确估计。RANSAC与ICP算法相结合，可解决机器人初始配准不确定条件下的位姿准确估计，防止位姿估计陷入局部最优。其基本思想是先通过ORB特征匹配与RANSAC算法为ICP配准提供初始对齐，随后通过优化特征点与深度点云的联合误差的ICP误差来实现精确化。

设存在两组对应点云A＝{a_i}_i＝1,…,N,B＝{b_i}_i＝1,…,N，且存在||a_i-T·b_i||<d_max，其中存在集合假设点云A和B满足正态分布：其中分别为A、B的 协方差矩阵。如果集合完全对应，且所求协方差矩阵具有极小值，则有：

${\hat{b}}_i=T*{\hat{a}}_i---(1-6)$

定义T为任意刚性变换。已知和均服从高斯分布，的概率分布可表示为：

$d_i^{T*}~N({\hat{b}}_i-(T^{*}){\hat{a}}_i,C_i^B+(T^{*}\left)C_i^A{\left(T^{*}\right)}^T\right)=N(0,C_i^B+(T^{*}\left)C_i^A{\left(T^{*}\right)}^T\right)---(1-7)$

使用最大似然估计可迭代计算出转换矩阵T：

$T=\underset{T}{\mathrm{argmax}}\underset{t}{\Pi }p\left(d_i^{\left(T\right)}\right)=\underset{T}{\mathrm{argmax}}\underset{T}{\Sigma }\log \left(p\right(d_i^{\left(T\right)}\left)\right)---(1-8)$

可简化为：

$T=\underset{T}{\mathrm{argmax}}\underset{T}{\Sigma }{d}_i^{{\left(T\right)}^T}{(C_i^B+(T^{*}\left)C_i^A{\left(T^{*}\right)}^T\right)}^{-1}{d}_i^{\left(T\right)}---(1-9).$

本发明的技术构思为：室内SLAM首先应通过传感器获取环境信息，如采用声呐测距传感器获取传感器与障碍物之间的距离信息；二维激光测距仪使用线扫描的方式获取本体与障碍物之间的距离和角度信息；RGB-D视觉传感器通过深度相机获取外界环境距离信息(Depthimage)，同时可获得与之对应的彩色信息(RGB image)，是一种新型视觉传感器。RGB-D信息经过处理后可直接用于机器人定位、地图创建与导航。与传统视觉传感器相比，RGB-D传感器可直接获得环境深度信息，数据处理过程简单，传感器数据适合三维地图重建，实时性高，地图精度好。

通过ORB算子提取室内环境图像数据帧中鲁棒性较好的特征点，通过RANSAC-ICP算法实现了初始位姿估计一致性不高情况下的帧间配准；通过RANSAC算法筛选符合内点阈值条件的关键帧，以关键帧作为校验约束，最终实现了一种全局一致性较高的位姿估计，保证 了机器人在室内环境下的定位精度与自建地图模型的准确性。

通过RANSAC算法减少ORB特征离群点，提高初始位姿配准精度，与ICP算法相结合实现了机器人在初始配准不确定的条件下精确位姿估计；基于ORB特征的关键帧词袋闭环检测算法，将关键帧特征点聚类成为字典，根据字典单词间的相似度判断是否形成场景闭环，修正配准错误，减少地图冗余结构，生成一致性最优的地图。

本发明的有益效果主要表现在：

1)位姿估计一致性高：本发明基于RANSAC-ICP帧间配准算法筛除了图像特征离群点，利用内点最多的特征匹配点实现了六自由度位姿估计，提高帧间配准生成的位姿一致性；采用ORB关键帧闭环检测算法添加额外全局位姿约束。由上述方法生成的机器人位姿一致性高，定位与地图较为精确。

2)鲁棒性：RANSAC算法可剔除图像特征匹配过程中产生的错误匹配点，基于ORB关键帧的闭环检测算法可校正由传感器测量及帧间配准导致的位姿估计错误。通过不断校正错误，使SLAM系统更健壮。

3)运行效率：本发明中通过ORB算子提高了特征点检测速度与特征点匹配速度，通过改进RANSAC算法，减少了数据离群点，提高了图像配准重合率，较少了错误匹配，增强了SLAM系统前端的时效性；基于视觉词袋模型的关键帧ORB特征增强了位姿约束，减少了位姿估计中的冗余结构，提高SLAM系统后端的时效性。因此，基于ORB关键帧闭环检测的SLAM方法的运行效率较高。

附图说明

图1是可提高机器人位姿一致性的ORB关键帧闭环检测SLAM方 法系统框图；

图2是ORB关键帧闭环检测SLAM方法的功能模块关系图；

图3是ORB关键帧闭环检测SLAM方法的RANSAC-ICP方法流程图；

图4是ORB关键帧闭环检测SLAM方法的关键帧与闭环检测关系图；

图5是ORB关键帧闭环检测SLAM方法中基于视觉词袋的关键帧场景描述方法的流程图。

具体实施方式

下面结合附图对本发明作进一步描述。

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

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

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

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

图1所示一种可提高机器人位姿一致性的ORB关键帧闭环检测SLAM系统框架，其中提出使用基于RANSAC-ICP帧间配准的位姿估计算法和基于ORB关键帧(KeyFrame)的闭环检测方法。通过Asus Xtion或同类型的RGB-D传感器获取环境色彩信息与深度信息，通过ORB算法加快图像特征点检测与描述子建立速度；结合相机小孔模型与深度信息，将二维图像转换为三维彩色点云，并进行存储；通过基于改进RANSAC-ICP帧间配准的位姿估计算法实现机器人在初始配准不确定的条件下精确位姿估计，提高了机器人位姿估计一致性。基于ORB特征的关键帧词袋闭环检测算法将关键帧特征点聚类成为字典，根据字典单词间的相似度判断是否形成场景闭环，修正配准错误。通过闭环检测可减少地图冗余结构，生成具有一致性的位姿估计与地图模型。

图2所示为基于ORB关键帧闭环检测SLAM方法中各功能模块之间的关系。ORB关键帧闭环检测SLAM方法以RGB-D三维点云序列{F}(其中，I为RGB图像，D为深度图像，p为图像匹配矩阵)为输入，输出为场景地图的网格模型与位姿估计轨迹，模型中的网格顶点为点云模型的数据点。

图3所示为ORB关键帧闭环检测SLAM方法的RANSAC-ICP算法流程图。通过RANSAC粗配准可得到相对变换T(p)将作为RANSAC-ICP的输入。T(p)函数表示对点p进行刚性变换，则T(p)＝Rp+T。RANSAC-ICP最终输出为最优相对变换T^*(p)。

Extract_RGB_Point_Featrures函数分别可从待配准帧与参考帧中提取稀疏的ORB特征点云P_s、P_t。

随后使用RANSAC算法从两组特征点云中找到初始的刚性变换T(p)，通过在两帧之间找到匹配的特征完成刚性变换初始化。随后，通过重复采样，可以确定特征点的内点数，变换后内点数最大的变换 即为最优变换。Perform_RANSAC_Alignment函数首先使用RANSAC算法对特征集进行初始配准，找到两帧中的最佳平移旋转变换。随后通过重投影误差RANSAC优化目标函数：

$T^{*}=\mathrm{argmin}\left(\frac{1}{\left|A_f\right|}\underset{i\in A_f}{\Sigma }{|\pi \left(T\right(f_s^i\left)\right)-\pi \left(f_t^i\right)|}^2\right)---(1-10)$

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

步骤4-6为基于最小二乘法的ICP配准算法。该算法选择对应点对进行迭代计算，计算最优刚体变换估计，直到满足配准的收敛精度。其具体步骤描述如下：

第一步，计算P_s中的每一个点在P_t点集的最近对应点；

第二步，求得使上述对应点对的平均距离最小的刚体变换T*

$T^{*}=\underset{T}{\arg \min }[\alpha \left(\frac{1}{\left|A_f\right|}\underset{i\in A_f}{\Sigma }{|\pi \left(T\right(f_s^i\left)\right)-\pi \left(f_t^i\right)|}^2\right)+(1-\alpha )\left(\frac{1}{A_d}\underset{j\in A_d}{\Sigma }{w}_j{|\left(T\right(p_s^j)-p_t^j)·n_t^j)|}^2\right)]---(1-11)$

第三步，对P_t使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步，如果新的变换点集与参考点集满足平均距离小于阈值时，则停止迭代计算；否则新的变换点集作为新的P_t继续迭代，直到满足目标函数的要求为止。

图4所示为ORB关键帧闭环检测SLAM方法的关键帧与闭环检测关系图。可描述为：关键帧闭环检测实现过程为：输入连续标记的图像，根据RANSAC内点作为筛选条件选取关键帧；以关键帧的ORB 特征建立词典，比对词典相似性实现全局闭环检测，经过关键帧位姿变换，输出闭环校正后的位姿图，实现机器人位姿估计校正。关键帧闭环检测算法可在保证帧间配准的有效性的同时提高了检测实时性，实现了一种健壮、实时的SLAM后端优化算法。

图5所示为ORB关键帧闭环检测SLAM方法中基于视觉词袋的关键帧场景描述算法。闭环检测首先通过将关键帧的ORB特征进行聚类，采用视觉词典(BoVW)将特征描述子映射为视觉单词，通过对比关键帧视觉词典中的“单词”即可判断是否形成闭环。BoVW通过结构化描述关键帧，可使图像特征具有不变性，采取离散的单词表示方法可提高闭环检测效率。

对于本领域技术人员而言，显然本发明不限于示例性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明额范围由所附权力要求而不是上述说明先定，因为旨在将落在权力要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为了清除期间，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。