0 概述

同步定位与建图(Simultaneous Localization and Mapping, SLAM)是指机器人在一个陌生的环境中, 利用自身传感器获取的信息进行实时定位并构建周围环境地图^[1-2]。视觉传感器因其体积小、价格低廉并且蕴含丰富的环境信息, 被广泛地应用于单目视觉SLAM系统中^[3]。根据位姿估计算法的不同, 单目视觉SLAM可分为特征点法和直接法。

PTAM-SLAM^[4]算法是特征点法单目视觉SLAM的代表算法。该算法采用关键帧构建地图, 并使用BA(Bundle Adjustment)优化^[5]相机位姿, 替代了传统的基于滤波算法的特征点位置和相机位姿估计, 提高了位姿估计精度。但是PTAM-SLAM算法没有大型的闭环检测, 不能消除位姿估计中的累计误差, 同时视角不变性较差, 易丢失跟踪目标。文献[6-7]提出ORB-SLAM2算法, 其采用全局BA优化与闭环检测算法, 相比PTAM-SLAM算法具有更高的定位精度。

直接法单目视觉SLAM的代表算法有LSD-SLAM^[8]和DSO^[9]算法, LSD-SLAM算法可以在不使用GPU的情况下大范围构建半稠密地图, 同时直接优化位姿, 但是该算法定位精度明显低于PTAM-SLAM和ORB-SLAM2算法。DSO算法定位精度高于LSD-SLAM算法, 但是没有闭环检测。直接法虽然可以在不计算特征点和描述子的基础上构建半稠密地图, 但是其对光照变化较敏感, 限制了系统的使用环境。相比直接法单目视觉SLAM, 基于特征点的SLAM算法虽然需要进行两帧图像之间的特征匹配, 但是可以在一定程度上克服光照影响。

目前主流的特征匹配算法有暴力匹配、FLANN^[10-12]、RANSAC^[13-14]、CODE^[15]以及RepMatch^[16]等。其中:暴力匹配、FLANN和RANSAC是经典的匹配算法, 其匹配速度较快, 然而误匹配较多使得SLAM系统初始化速度慢, 定位精度不高; CODE对于具有重复结构的环境误匹配较多, RepMatch虽然解决了该问题, 匹配效果好, 但是这2种算法匹配速度慢, 主要应用于对实时性要求不高的三维重建系统中, 不适用于对实时性要求较高的SLAM系统。

为消除SLAM特征匹配过程中的误匹配, 保持系统的实时性, 同时获得较好的初始化位姿估计结果, 本文基于概率运动统计特征匹配设计一种单目视觉SLAM算法。

1 算法框架

基于概率运动统计特征匹配的单目视觉SLAM算法流程如图 1所示, 该算法主要分为特征提取、概率运动匹配、初始化以及位姿跟踪4个阶段。特征提取采用改进的ORB特征提取算法, 首先将当前帧图像通过图像金字塔生成8个不同尺度的图像, 然后将不同尺度的图像同时网格化, 利用自适应FAST角点提取算法在各个尺度上提取FAST^[17]特征点, 确保每个区域都有一定的特征点, 以提高鲁棒性, 最后计算特征点的BRIEF^[18]描述子, 生成ORB特征点, 保证旋转不变性, 同时以四叉树的形式保存特征点。概率运动特征匹配的核心思想是根据贝叶斯规则与运动的平滑性寻找图像中正确的特征匹配点。首先根据四叉树的结构特点, 计算四叉树同一层的ORB特征点之间的Hamming距离, 然后寻找每个匹配的特征点周围是否存在其他匹配点, 通过特征点周围匹配点的数量, 得到该特征点的匹配正确的概率, 最后使用fisher^[19]判别准则思想, 通过设定阈值, 判断匹配的正确性。特征匹配点数量达到设定值后, 使用匹配的特征点进行初始化。针对平面结构和非平面结构, 根据匹配的特征点分别计算出单应性矩阵H和基本矩阵F, 通过重投影误差计算出H矩阵和F矩阵的得分, 再经过三角化恢复出初始的地图点, 并据此进行位姿跟踪。

2 概率运动统计特征匹配SLAM

特征提取与匹配是单目视觉SLAM的关键环节。目前主要的特征提取算法有SIFT^[20]、SURF^[21]和ORB^[22]等。经典特征提取算法SIFT由于建立128维特征描述子, 因此运行速度较慢。SURF在SIFT上进行了改进, 虽然提高了算法运行速度, 但是其实时性依然不足。

ORB特征点由于其提取与匹配速度快, 是目前视觉SLAM主要使用的特征点之一。但是仅采用ORB特征点会导致较多误匹配, 同时针对于不同的场景ORB特征点分布可能集中于图像的一个区域, 使得位姿估计存在随机误差。采用改进的ORB特征提取和概率运动统计特征匹配相结合的算法, 可以得到分布均匀且正确的特征匹配, 以避免产生随机误差, 提高位姿跟踪精度。

2.1 改进的ORB特征提取

由于单目视觉SLAM中特征点的提取速度影响算法实时性, 同时图像中均匀分布的特征点能减少随机误差, 因此本文提出一种改进的ORB特征点提取算法, 其具体步骤如下:

1) 构建高斯图像金字塔, 设置金字塔尺度为1.2, 层数为8, 具有尺度不变性。将金字塔每层图像进行网格划分, 每个区域大小设置为30×30, 实现特征点均匀分配。采用自适应阈值的方法提取FAST特征点, 首先设定FAST的初始阈值为12, 最小阈值为6, 然后使用初始阈值在每层图像特定区域内提取FAST特征点, 若没有提取到特征点, 减小阈值再次提取, 保证特征点的均匀分布。

2) 在特征点半径为3的圆形区域内, 计算该区域图像质心, 得到特征点的主方向, 保证旋转不变性, 同时使用BRIEF算法计算该特征点的描述子。

3) 如图 2所示, 将图像中的特征点进行四叉树划分, 根据区域划分的大小与顺序依次将区域内的特征点放入四叉树的节点与叶子节点。由于只有在同一节点的特征点才可能是匹配点, 因此可以提高匹配精度和速率。

2.2 概率运动统计特征匹配

特征匹配是单目视觉SLAM关键环节之一, 将得到的特征点通过描述子进行匹配能有效地解决SLAM信息关联问题, 确定相机前后2帧之间的运动关系。基于概率运动统计的特征匹配算法可以提高前后2帧图像之间特征匹配的正确性, 算法具体步骤如下:

1) 特征点预匹配

对于前后2帧图像, 根据四叉树结构的特点, 将属于同一节点的特征点取出, 通过特征点的BRIEF描述子计算2个特征点之间的Hamming距离, 设定最小距离阈值为70, 对2幅图像的ORB特征点进行预匹配。

2) 概率模型估计

如图 3所示, 假设图像对{I_a, I_b}中特征点数分别为{M, N}, 所有的特征匹配点集X={x₁, x₂, …, x_N}, x_i代表第i个特征匹配点对。图像对{I_a, I_b}中区域{a, b}的特征点数为{n, m}, 特征匹配X_i⊆X, 记S_i为区域{a, b}中支持x_i的特征匹配的个数。

$ {S_i} = \left| {{X_i}} \right| - 1 $

(1)

其中, |X_i|表示区域内特征匹配个数的总和。

定义f_a^b为区域a中一个特征点的特征匹配点在区域b中, f _a^t表示区域a中一个特征点匹配正确的概率p(f_a^t)=t, 则f_a^f表示区域a中一个特征点匹配错误的概率p(f_a^f)=1－t。假设区域a中特征点匹配错误, 它的匹配点可以落在图像I_b中的任意位置, 则在匹配错误的条件下, 匹配点在区域b中的概率为:

$ p\left( {f_{\rm{a}}^{\rm{b}}|f_{\rm{a}}^{\rm{f}}} \right) = \beta m/M $

(2)

其中, β为容错因子, 用来容纳个别违反该定义的特征点以提高准确性。

如图 3所示, 白色圆圈区域T ^ab为区域{a, b}在前后2帧场景中相同位置, 黑色圆圈区域F ^ab为区域{a, b}在前后2帧场景中的不同位置。根据贝叶斯准则, 在T ^ab的条件下, 区域a中的特征匹配点在b中的概率p_t为:

$ \begin{array}{l} {p_{\rm{t}}} = p\left( {f_{\rm{a}}^{\rm{b}}|{T^{{\rm{ab}}}}} \right) = p\left( {f_{\rm{a}}^{\rm{t}}|{T^{{\rm{ab}}}}} \right) + p\left( {f_{\rm{a}}^{\rm{f}},f_{\rm{a}}^{\rm{b}}|{T^{{\rm{ab}}}}} \right) = \\ \;\;\;\;\;\;\;p\left( {f_{\rm{a}}^{\rm{t}}|{T^{{\rm{ab}}}}} \right) + p\left( {f_{\rm{a}}^{\rm{f}}|{T^{{\rm{ab}}}}} \right)p\left( {f_{\rm{a}}^{\rm{b}}|f_{\rm{a}}^{\rm{f}},{T^{{\rm{ab}}}}} \right) = \\ \;\;\;\;\;\;\;p\left( {f_{\rm{a}}^{\rm{t}}} \right) + p\left( {f_{\rm{a}}^{\rm{f}}} \right)p\left( {f_{\rm{a}}^{\rm{b}}|f_{\rm{a}}^{\rm{f}}} \right) = t + \left( {1 - t} \right)\beta m/M \end{array} $

(3)

在F ^ab的条件下, 区域a中的特征匹配点在b中的概率p_f为:

$ \begin{array}{l} {p_{\rm{f}}} = p\left( {f_{\rm{a}}^{\rm{b}}|{F^{{\rm{ab}}}}} \right) = p\left( {f_{\rm{a}}^{\rm{f}},f_{\rm{a}}^{\rm{b}}|{F^{{\rm{ab}}}}} \right) = \\ \;\;\;\;\;\;\;p\left( {f_{\rm{a}}^{\rm{f}}|{F^{{\rm{ab}}}}} \right)p\left( {f_{\rm{a}}^{\rm{b}}|f_{\rm{a}}^{\rm{f}},{F^{{\rm{ab}}}}} \right) = \\ \;\;\;\;\;\;\;p\left( {f_{\rm{a}}^{\rm{f}}} \right)p\left( {f_{\rm{a}}^{\rm{b}}|f_{\rm{a}}^{\rm{f}}} \right) = (1 - t)\beta m/M \end{array} $

(4)

p_t的物理含义是指区域a中的特征点匹配在区域b中的概率, 即实际匹配正确的概率, p_f则是指实际匹配错误的概率。由于每个特征点是相互独立的, 因此区域a中的特征点匹配满足二项分布:

$ {S_i} \sim \left\{ {\begin{array}{*{20}{l}} {B\left( {n,{p_{\rm{t}}}} \right)}\\ {B\left( {n,{p_{\rm{f}}}} \right)} \end{array}} \right. $

(5)

其中, n表示区域a中所有的特征点数。

3) 概率运动统计

正确匹配点周围领域中的特征点是匹配的。当运动尺度过大时, 为保证算法的鲁棒性, 根据运动的平滑性, 可以将区域a扩展到其领域范围, 如图 4所示。

统计K个区域范围内的特征匹配对S_i有:

$ {S_i} = \sum\limits_{k = 1}^K {\left| {{X_{{a_k}{b_k}}}} \right|} - 1 $

(6)

其中, K表示区域a扩展区域的个数, 根据S_i满足二项分布, 如式(7)所示。

$ {S_i} \sim \left\{ {\begin{array}{*{20}{l}} {B\left( {Kn,{p_{\rm{t}}}} \right)}\\ {B\left( {Kn,{p_{\rm{f}}}} \right)} \end{array}} \right. $

(7)

该区域内正确匹配和错误匹配的均值和标准差为:

$ \left\{ {\begin{array}{*{20}{l}} {{m_{\rm{t}}} = Kn{p_{\rm{t}}}}\\ {{s_{\rm{t}}} = \sqrt {Kn{p_{\rm{t}}}\left( {1 - {p_{\rm{t}}}} \right)} } \end{array}} \right. $

(8)

$ \left\{ {\begin{array}{*{20}{l}} {{m_{\rm{f}}} = Kn{p_{\rm{f}}}}\\ {{s_{\rm{f}}} = \sqrt {Kn{p_{\rm{f}}}\left( {1 - {p_{\rm{f}}}} \right)} } \end{array}} \right. $

(9)

其中, m_t、s_t是正确匹配的均值和方差, m_f、s_f是错误匹配的均值和方差。利用fisher判别思想, 定义区别正确匹配和错误匹配可分性度量P为:

$ P = \frac{{{m_{\rm{t}}} - {m_{\rm{f}}}}}{{{s_{\rm{t}}} - {s_{\rm{f}}}}} = \frac{{Kn{p_{\rm{t}}} - Kn{p_{\rm{f}}}}}{{\sqrt {Kn{p_{\rm{t}}}\left( {1 - {p_{\rm{t}}}} \right)} + \sqrt {Kn{p_{\rm{f}}}\left( {1 - {p_{\rm{f}}}} \right)} }} $

(10)

最大化P值就是使得正确匹配和错误匹配的均值之差足够大, 标准差之和足够小, 式(10)可以等价为:

$ P \propto \sqrt {Kn} $

(11)

由式(11)可以得到, 区域a中特征点数量越多或者K值越大, 则正确匹配和错误匹配可分性就越大。但是图像中的特征点个数和领域数K都与系统运行时间成正比, 通常适当增大K值提高区分度P值的大小。

4) 运动网格加速

由于对于每一个特征匹配对需要估计其周围领域得分S_i, 不满足实时性要求, 因此本文采用运动网格统计模式。将图像对划分成N×N网格, 每个网格仅计算一次领域得分S_i, 同时网格移动步长为半个格子的宽度, 保证格子边缘特征点的正确统计。如图 5所示, 每一对格子{a, b}的的得分S_ab为:

$ {S_{ij}} = \sum\limits_{k = 1}^{K = 9} {\left| {{X_{{i_k}{j_k}}}} \right|} $

(12)

其中, |X_ikjk|表示图像中网格{i_k, j_k}的特征匹配数量。

网格数量越多匹配的准确性越高, 但此时区域特征数量n值减少, 区分度P值降低。如果通过提高K值进行补偿, 算法运行时间将增多。因此, 本文实验采用10×10网格进行特征匹配的筛选, 能同时满足实时性与精度要求。

如图 6所示, 需要选择一个阈值区分网格对匹配的正确性。

阈值t的计算公式为:

$ t = {m_{\rm{f}}} + \alpha {s_{\rm{f}}} \approx \alpha \sqrt n $

(13)

当网格对的匹配得分S_ij>t时, 认为是正确匹配, 否则是错误匹配。

2.3 自动初始化

由于单目相机存在深度不确定性, 单目视觉SLAM需要进行初始化位姿估计, 构建初始的三维地图点。初始化算法的具体步骤如下:

1) 设定初始化特征提取数量为2 000, 采用改进的ORB特征提取算法对连续两帧图像进行特征提取, 只有当连续两帧图像提取的特征点数量均大于200时才进行初始化操作。

2) 假设参考帧F_r和当前帧F_c在图像中的特征匹配点为{x_r, x_c}, 根据式(14)采用8点法和SVD^[23]相结合的方式求解单应性矩阵H_cr和基本矩阵F_cr。

$ \left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{x}}_{\rm{c}}} = {\mathit{\boldsymbol{H}}_{{\rm{cr}}}}{\mathit{\boldsymbol{x}}_{\rm{r}}}}\\ {\mathit{\boldsymbol{x}}_{\rm{c}}^{\rm{T}}{F_{{\rm{cr}}}}{\mathit{\boldsymbol{x}}_{\rm{r}}} = 0} \end{array}} \right. $

(14)

其中, 参考帧和当前帧图像特征点坐标分别为x_c=(u_c, v_c, 1)^T, x_r=(u_r, v_r, 1)^T。单应性矩阵H_cr表示处于同一平面上的匹配点在2帧图像之间的映射关系, 基本矩阵F_cr表示2个匹配点之间的空间位置关系, 两者均为3×3矩阵。

3) 利用重投影误差对H_cr和F_cr进行得分统计, 根据其得分S_H和S_F采用式(15)判断使用哪种矩阵进行相机当前位姿估计。

$ {R_{\rm{H}}} = \frac{{{S_{\rm{H}}}}}{{{S_{\rm{H}}} + {S_{\rm{F}}}}} $

(15)

其中, R_H表示评估参数。当R_H>0.45时, 选用单应性矩阵H_cr, 否则选择基本矩阵F_cr恢复当前相机位姿。

4) 判断参考帧和当前帧之间的视差角是否大于5°, 大于5°则使用当前相机位姿和三角化算法恢复出当前初始三维地图点, 同时判断地图点数量是否大于90, 小于90则重新初始化。

2.4 位姿跟踪

在获得正确的初始化位姿和点云地图之后, 采用参考帧模式和恒速模式对相机位姿进行跟踪。如图 7所示, 将初始化的最后一帧设置为当前帧的参考帧。

参考帧模式为:

$ SE{3_k} \approx SE{3_{{\rm{RF}}}} $

(16)

其中, SE3_k表示当前帧位姿矩阵的李群, SE3_RF表示参考帧位姿矩阵的李群, 矩阵大小均为4×4。采用式(16)将参考帧的位姿设置为当前帧位姿, 同时根据概率统计模型对当前帧和参考帧进行特征匹配。将匹配部分的参考帧的地图点设置为当前帧特征点对应的地图点, 通过BA优化当前帧位姿。

在参考帧模式设置成功之后, 采用恒速模式对相机位姿进行跟踪, 如图 8所示, 恒速模式认为相机近似匀速运动。

恒速模式为:

$ \Delta {R_k} \approx \Delta {R_{k - 1}} $

(17)

其中, ΔR_k和ΔR_k－1表示前后2帧之间的相对旋转, 均为3×3的矩阵。采用式(17)得到当前帧的旋转矩阵, 通过近似匀速运动的特点得到平移向量, 进而得到当前帧的位姿。再通过上一帧特征点对应的地图点重投影到当前帧, 搜索与当前帧的匹配点, 构建当前帧的特征点对应的地图点, 同时使用BA优化得到最终当前帧的位姿矩阵。

在跟踪相机位姿过程中, 将获得的地图点存储在局部地图中构建稀疏地图, 同时采用DBoW2^[24-25]进行图像相似度的判断。当检测到闭环时, 采用全局BA对相机位姿进行优化, 得到最后的相机运动轨迹。

3 实验验证与结果分析

本文在ubuntu14.04操作系统系统上采用C++编程, 系统CPU为Intel i7 2.20 GHz, 内存为16 GB。

3.1 特征提取

本文使用TUM^[26]中的fr2/desk数据集对ORB和改进的ORB特征点提取算法进行测试, 实验提取1 000个特征点, 结果如图 9所示。从图 9可以看出, 改进后的ORB特征点提取算法可以在图像上获得均匀分布的特征点。

本文同时将改进的ORB特征点提取算法与SIFT、SURF、ORB特征点提取算法进行实时性对比。各对比算法分别对图 3场景提取1 000个的特征点, 为了消除随机误差, 计算10次特征提取的平均值, 结果如表 1所示。

从表 1可以看出, 改进后的ORB特征点提取算法运行速度快于原始的ORB特征点提取算法, 实时性远高于SIFT和SURF算法, 满足单目视觉SLAM的实时性要求。

3.2 特征匹配

基于TUM中的fr2/desk数据集, 使用改进的ORB特征点进行概率运动统计匹配, 并且与BF(Brute Force)匹配和FLANN匹配进行对比, 实验结果如图 10所示。

如图 10(a)所示, FLANN匹配完全是混乱的, 前后2帧图像存在大量误匹配。图 10(b)中黑色矩形框内的圆圈区域表示前后两帧图像部分错误的特征匹配, 可以看出BF匹配依然存在较多的误匹配。如图 10(c)所示, 概率运动匹配可以获得正确的特征匹配, 确保位姿估计的正确性。

分别对前后2帧图像提取1 000和1 500个改进的ORB特征点, 进行概率运动匹配、BF匹配和FLANN匹配3种算法运行时间的对比, 如表 2所示。

从表 2可以看出, FLANN匹配耗时长, BF匹配在特征点较少时耗时较短, 但是随着特征点的增多, BF匹配时间明显上升。概率运动统计匹配相对于BF匹配和FLANN匹配耗时最少, 完全满足视觉SLAM中实时性要求。

3.3 初始化

由于采用概率运动统计特征匹配能得到正确的特征匹配, 因此当相机运行到具有足够的视差(实验设置为5°)时, 根据式(15)的得分选择平面结构或是非平面结构, 恢复出足够的地图点, 完成初始化过程。在TUM中的fr2/desk数据集上进行初始化测试, 实验结果如图 11所示。

从图 11可以看出, PTAM-SLAM提取的特征点和LSD-SLAM提取的关键点均位于场景的同一平面上, 初始化结构为平面型, 而基于概率运动统计SLAM (PMS-SLAM)提取的改进的ORB特征分布于场景各个位置。这是因为PTAM-SLAM和LSD-SLAM均以平面结构初始化, 限制了该类算法的使用场景, 当环境为非平面结构时, 容易初始化不成功或是跟踪丢失。

在ORB-SLAM2系统中当相机运动获得足够的视差时, 由于特征匹配存在误匹配, 使得位姿估计存在较大误差或估计错误, 进而重复进入初始化过程。本文提出的基于概率运动统计SLAM只要获得足够的视差, 系统就能完成初始化, 获得较好的初始位姿估计, 在TUM数据集上进行测试, 初始化时间、图像帧数、地图点构建数对比如表 3所示, 其中, “6.707 75(-∞)”表示如果初始化成功, 其时间为6.707 75 s, 也可能初始化不成功, 时间为无穷大。从表 3可以看出, 由于获得足够的正确匹配, PMS-SLAM能够快速初始化。在fr2/desk数据集上, PMS-SLAM最优的初始化的时间为1.751 s, 帧数为17, 创建地图点数为305, 明显优于单目ORB-SLAM2, 在其他数据集上也有相同的结果, 并且其初始化的稳定性也具有明显的优势, 避免了单目ORB-SLAM2中初始化不成功的风险。

3.4 定位精度与实时性分析 3.4.1 离线定位精度分析

由于单目相机存在深度不确定性, 因此通过刚体变换将相机轨迹与真实轨迹对齐后采用均方根误差(Root Mean Square Error, RMSE)进行定位精度分析。选取TUM数据集中的fr1/desk、fr1/floor、fr2/desk等10个数据集进行测试, 将PMS-SLAM与ORB-SLAM2(单目)、LSD-SLAM、PTAM、RGBD-SLAM^[27]进行对比分析, 实验结果如表 4所示。其中, RGBD-SLAM运动轨迹从TUM数据集官方发布数据获取, “—”表示未对此数据集进行测试, “X”表示跟踪丢失。

在使用LSD-SLAM估计位姿时, 由于采用随机的深度进行初始化, 因此去掉前20帧数据进行误差估计。从表 4可以看出, PTAM-SLAM和LSD-SLAM对部分数据集跟踪丢失, 而PMS-SLAM在所有数据集上均能成功跟踪, 表明本文算法具有较强的的鲁棒性。在fr2/xyz数据集上本文算法定位精度略低于PTAM-SLAM, 这是因为PTAM-SLAM是在人为干预下拥有高精度的平面结构初始化。此外, 本文算法定位精度均高于ORB-SLAM2。在上述数据集中, 将本文算法计算出的相机运动轨迹与真实运动轨迹对比, 结果如图 12所示。

从图 12可以看出, 相机运动轨迹与真实的轨迹十分相近, 验证了本文算法的正确性和稳定性。

3.4.2 离线实时性分析

PMS-SLAM系统通过多线程进行设计, 将追踪、建图、位姿优化、显示使用4个不同线程并行执行, 以提高系统的实时性。使用TUM数据集对PMS-SLAM的实时性进行分析, 在系统初始化完成之后, 计算出系统对后续的单帧图像进行特征提取, 与上一帧图像特征匹配, 估计出当前帧相机位姿和恢复出当前帧观测到的三维空间点的总时间, 即系统对单帧图像的跟踪时间。系统跟踪单帧图像所耗时间的均值如表 5所示。

根据表 5中的数据, 计算得到跟踪的平均时间为0.040 9 s, 即系统运行的帧率为25 Hz, 满足单目视觉SLAM的实时性要求。

3.4.3 在线定位精度与实时性分析

为了进一步验证系统的定位精度和实时性, 本文利用Turtlebot2移动机器人进行实际环境的实验测试。设定FAST算法的初始阈值为12, 最小阈值为6, 金字塔尺度为1.2, 层数为8, 在每一层均提取FAST特征点。每帧图像提取的改进的ORB特征点数最多为1 000个, 采用10×10网格进行特征匹配筛选。初始化的最小视差角为5°, 最少的地图点为90个。实验系统如图 13所示。

首先对Turtlebot2移动机器人上的相机进行标定, 然后通过摄像头获取图像, 最后进行特征点提取和位姿跟踪。实验环境和特征点提取如图 14所示。

使用Turtlebot2上的激光雷达(思岚RPLIDAR A2), 将激光雷达SLAM^[28-29]算法作为定位精度分析的真实轨迹, 统计6次实验下PMS-SLAM和ORB-SLAM的定位精度, 实验结果如表 6所示。

从表 6可以看出, 在以激光数据作为真实轨迹的情况下, PMS-SLAM算法在每次实验的定位精度均高于ORB-SLAM2算法的定位精度。绘制出PMS-SLAM算法其中一次的轨迹图, 如图 15所示。

从图 15可以看出, 采用PMS-SLAM算法得到的运动轨迹和激光雷达得到的轨迹十分接近, 从而表明本文算法具有较高的定位精度。

在上述6次实验中, 本文算法的跟踪时间如表 7所示。

根据表 7中的数据, 计算得到跟踪的平均时间为0.039 0 s, 即在线实测运行帧率为25.6 Hz, 满足单目视觉SLAM的实时性要求。

图 16为一次实验生成的实际运动轨迹。

4 结束语

本文提出PMS-SLAM算法, 采用基于概率统计模型的特征匹配方式, 根据运动的平滑性和正确的特征匹配在图像中的一致性, 对特征匹配进行筛选, 快速得到正确的初始化位姿, 同时恢复出足够的地图点以进行位姿跟踪。实验结果表明, 相比于传统的SLAM算法, PMS-SLAM具有较高的定位精度, 并且满足机器人定位的实时性需求。由于本文算法采用的是特征点法, 因此其在低纹理特征的情况下容易出现跟踪丢失的现象, 而在这种环境下, 直接法的视觉SLAM就具有明显的优势, 下一步将结合直接法并基于深度学习实现复杂环境下机器人的高精度定位, 同时提高系统的稳定性和实时性。