面对家居服务机器人和医疗机器人技术的发展，室内同步定位和绘图(Simultaneous Localization and Mapping，SLAM)这种实现机器人定位与构建环境地图的技术也在快速发展，在实际应用中如何实现SLAM系统快速准确定位和构建丰富环境信息的地图也逐渐成为视觉SLAM研究的热点问题。

SLAM技术的发展历程可分为以下3个阶段^[1]。传统时代(1986—2004年)：通过求解系统随时间变化的状态估计模型来解决SLAM问题^[2]。算法分析时代(2004—2015年)：研究SLAM的基本特性，包括观测性、收敛性和一致性^[2]。鲁棒性−预测性时代(2015年至今)：SLAM需要在不同环境保持良好的鲁棒性，能获取更准确的场景信息。视觉SLAM根据其应用的图像特征可以分为直接法和特征点法^[3]。单目SLAM系统^[4]采用激光方法使用扩展卡尔曼滤波框架成功构建完整的视觉SLAM系统，在此基础上建立的MonoSLAM系统将三维重建方法应用到SLAM系统来构建点云地图^[5]。PTAM(parallel tracking and mapping)系统中开创性地提出了SLAM的多线程框架，将系统分为跟踪和建图线程，并在系统中广泛采用Bundle Adjustment (BA)算法进行非线性优化，正式取代了传统的EKF(extended kalman filter)框架^[6]。ORB-SLAM2^[7]沿用了PTAM框架，该系统适用于单目、双目和RGB-D相机，并且可应用于室内和室外的大小规模场景，在保持高精度的基础上表现出良好的鲁棒性。整个系统采用ORB(oriented fast and rotated Brief)进行所有的关于特征的计算，引入回环检测线程使得在PTAM基础上增加对全局位姿的优化，有效地减少了系统的累计误差。

本文在ORB-SLAM2系统基础上进行改进，针对ORB特征点提取耗时影响系统处理速度问题，引入关键点替代ORB特征点，设计了新的关键帧提取策略，并添加稠密点云地图构建线程和八叉树地图构建线程，实现视觉SLAM系统的快速准确定位与地图构建。

1 关键点信息获取 1.1 Fast角点检测

Fast特征可以定义为：若某个像素点在其周围一定范围内与其他像素点存在一定差异，则将中心像素称为角点。为避免局部角点集中影响后续匹配结果，采用非极大值抑制方法对密集角点进行剔除，结果如图1所示。

1.2 角点的旋转不变性与尺度不变性

角点虽然获取较快的计算速度，但是如果相机位置发生旋转变化，会造成匹配的大量丢失和误匹配现象，所以使角点保持旋转不变性和尺度不变性。

角点获得旋转不变性是基于灰度质心法实现的，灰度质心指的是以某一图形块灰度值作为权重的中心。具体实现步骤如下。

1) 在以一个Fast特征P为圆心、半径为3像素的图像块B中，定义图像块的矩为

${m_{pq}}{\rm{ = }}\sum\limits_{x,y \in B} {{x^p}{y^q}I(x,y){\rm{ , }}\;\;\;p,q{\rm{ ∈ }}\left\{ {0,{\rm{ 1}}} \right\}} $

2) 通过矩可以找到图形块的灰度质心

$C{\rm{ = }}\left( {\frac{{{m_{10}}}}{{{m_{00}}}},{\rm{ }}\frac{{{m_{01}}}}{{{m_{00}}}}} \right)$

3)连接图像块的几何中心P与灰度质心C，得到一个方向向量 $\overrightarrow {PC} $ ，于是特征点的方向(单位：°)就可以表示为

$\theta {\rm{ = arctan( }}{m_{01}}{\rm{/}}{m_{10}}{\rm{ )}}$

本文引入图像金字塔池化模型来获取不同尺度图像的角点信息，使得获得的角点具有尺度不变性。将具有旋转不变性和尺度不变性的角点作为关键点。

1.3 关键点与ORB特征点提取速度对比实验

基于改进Fast角点作为关键点在提取速度上较ORB特征点明显更快，节约特征提取时间，为定量分析两者的差别，现对500张慕尼黑工业大学(Technische Universität München，TUM)数据集fr2_desk结果进行基于2种方法的特征提取实验，每张图片提取特征点数目为600，在实验中对提取时间进行统计，统计结果如图2所示。

由图2可以看出，本文对于关键点的提取速度明显快于基于ORB特征点的提取方法，因为本文方法没有提取特征点的描述子，所以耗时较短。

2 关键点的帧间匹配 2.1 基于关键点的帧间匹配

在LK(Lucas–Kanade)光流中将图像看作时间的函数 $I(t)$ ，那么在t时刻位于 $(x,{\rm{ }}y)$ 处的像素值可以表示为 $I(x,{\rm{ }}y,{\rm{ }}t)$ ^[8]。为使得计算准确，引入光流法的2个假设条件：亮度恒定和空间一致性。

亮度恒定：对于时刻t位于(x, y)处的像素点，假设其在 $t + {\rm{d}}t$ 时刻，该点新的像素点坐标值为 $(x + {\rm{d}}x,{\rm{ }}y + {\rm{d}}y)$ ，根据亮度恒定假设有

$ \begin{array}{l} I(x + {\rm{d}}x,{{ y}} + {\rm{d}}y,{\rm{ }}t + {\rm{d}}t){\rm{ = }}I(x,y,t)\\ I(x + {\rm{d}}x,{{ y}} + {\rm{d}}y,{\rm{ }}t + {\rm{d}}t){\rm{ = }}I(x,y,t) \end{array}$

因为假设亮度不变，所以下一时刻的灰度值等于前一时刻灰度值，从而

$\frac{{\partial I}}{{\partial x}}\frac{{{\rm{d}}x}}{{{\rm{d}}t}} + \frac{{\partial I}}{{\partial y}}\frac{{{\rm{d}}y}}{{{\rm{d}}t}}= - \frac{{\partial I}}{{\partial t}}$

(1)

式中 ${\rm{d}}x/{\rm{d}}t$ 和 ${\rm{d}}y/{\rm{d}}t$ 是像素在x和y轴方向运动速度，记为u，v。通过式(1)计算像素运动u、v，式(1)是具有2个变量的一次方程，无法求2个变量解，所以引入额外约束即空间一致性假设：相邻像素具有相似运动，即可得到像素在图像间的运动速度u、v。

因为引入最小二乘法求解像素运动即光流，所以可以把光流看作优化问题：通过最小化灰度误差来估计最优的像素偏移。主要解决如下问题：

$\mathop {\min }\limits_{\Delta x,\Delta y} ||{I_1}(x,y) - {I_2}(x + \Delta x,y + \Delta y)|{|_2}^2$

单层图像可能由于相机快速运动引起图像差异较大而在优化时到达局部最小值，无法继续优化，使得最后优化效果较差。对原始图像进行一定比例缩放得到不同分辨率图像，以原始图像作为底层(共4层)，每向上一层进行0.6倍缩小得到图像金字塔。单层LK光流和多层LK光流跟踪结果如图3所示。

参考帧与当前帧计算光流，结果在当前帧上用绿色点进行标注。如图4所示，相机在参考帧和当前帧间进行明显的右移，可以看出多层光流结果偏移方向更趋于一致，单层光流结果偏移方向表现散乱，所以多层光流结果表现优于单层光流结果。

2.2 关键点的误匹配剔除

每帧图像上获取对应的关键点经过LK光流法进行基于关键点的帧间匹配过程会出现误匹配。所以首先利用前文提取的关键点方向信息来进行初步的误匹配结果剔除，根据参考帧和当前帧图像的LK跟踪结果对匹配关键点做差值，每30°表示一个区间，匹配点基本保持在第一二个区间内，所以本文保留初次匹配结果的第一二区间的关键点作为正确匹配关键点。

经过上述简单误匹配剔除后，匹配结果中仍存在误匹配现象，继续采用随机抽样一致算法计算变换矩阵，通过变换矩阵判断关键点是内点(正确匹配点)还是外点(错误匹配点)。计算2帧图片变化的变换矩阵H。

${{H}}=\left[ {\begin{array}{*{20}{c}} {{h_{11}}}&{{h_{12}}}&{{h_{13}}} \\ {{h_{21}}}&{{h_{22}}}&{{h_{23}}} \\ {{h_{31}}}&{{h_{32}}}&{{h_{33}}} \end{array}} \right]$

RANSAC算法的目标是找到一个H矩阵使得其能包含最多的数据点，即通过变换后2帧间的关键点尽可能多的进行匹配，变换公式可以写为

$s\left[ {\begin{array}{*{20}{c}} {x'} \\ {y'} \\ 1 \end{array}} \right]=\left[ {\begin{array}{*{20}{c}} {{h_{11}}}&{{h_{12}}}&{{h_{13}}} \\ {{h_{21}}}&{{h_{22}}}&{{h_{23}}} \\ {{h_{31}}}&{{h_{32}}}&{{h_{33}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} x \\ y \\ 1 \end{array}} \right]$

式中： $(x,y)$ 和 $(x',y')$ 为对应2帧的关键点像素坐标；s为尺度因子。

因为H矩阵可以通过令h₃₃=1进行归一化处理，然后计算满足该变换矩阵的内点，代价函数为

${\rm{Error}}={\sum\limits_{i=1}^n {\left( {{x_i}^{'}\frac{{{h_{11}}{x_i} + {h_{12}}{y_i} + {h_{13}}}}{{{h_{31}}{x_i} + {h_{32}}{y_i} + {h_{33}}}}} \right)} ^2} + {\left( {{y_i}^{'}\frac{{{h_{21}}{x_i} + {h_{22}}{y_i} + {h_{23}}}}{{{h_{31}}{x_i} + {h_{32}}{y_i} + {h_{33}}}}} \right)^2}$

具体步骤为：

1)从所有样本数据集中随机选取不共线的4对点计算H矩阵，记为模型M。

2)计算数据集中所有数据与模型M的投影误差，若误差小于阈值，加入内点集I。

3)如当前内点集I元素个数大于最优内点集I_best, 则更新I_best=I，同时更新迭代次数k。

4)如果迭代次数大于k，则退出; 否则迭代次数加1，并重复上述步骤。

经过2次误匹配剔除后，图4(a)表示LK光流法直接匹配结果，图4(b)表示2次误匹配剔除后的匹配结果，可以看出匹配结果中误匹配对明显减少，说明本文方法能获取较好的效果。

3 关键帧检测

通过跟踪线程确定的关键帧可以作为后续帧的参考帧，并且检测到的关键帧会进入到后续的建图和优化线程，具体方法如下。

1)根据以上内容获取当前帧的相机位姿，转化为旋转矩阵R和偏移量t，根据R和t来评价当前帧与参考关键帧的运动差异来获取候选关键帧。

旋转矩阵R和t分别为

${{R}}=\left[ {\begin{array}{*{20}{c}} {{R_{00}}}&{{R_{01}}}&{{R_{02}}} \\ {{R_{10}}}&{{R_{11}}}&{{R_{12}}} \\ {{R_{20}}}&{{R_{21}}}&{{R_{22}}} \end{array}} \right],{\rm{ }}{{t}}=({t_x},{t_y},{t_z}){\rm{ }}$

无法直接比较不同帧图像对应的旋转矩阵大小，所以将旋转矩阵转化为欧拉角来描述旋转，可以称为通过“偏航−俯仰−滚转 (yaw-pitch-roll)”3个角度描述一个旋转，求解3个旋转角：

$\left\{ {\begin{array}{*{20}{l}} {{\theta _{{\rm{yaw}}}}={\rm{arctan}}2({R_{10}},{R_{00}})} \\ {{\theta _{{\rm{pitch}}}}={\rm{arctan}}2( - {R_{20}},\sqrt {{R_{10}}^2 + {R_{10}}^2} )} \\ {{\theta _{{\rm{roll}}}}={\rm{arctan}}2({R_{21}},{R_{22}})} \end{array}} \right.$

将旋转矩阵和平移矩阵归一化处理：

$ d=\alpha \sqrt {{t_x}^2 + {t_y}^2 + {t_z}^2} + \beta \sqrt {{\theta _{{\rm{yaw}}}}^2 + {\theta _{{\rm{pitch}}}}^2 + {\theta _{{\rm{roll}}}}^2} ,{\rm{ }}\alpha {\rm{ + }}\beta {\rm{=1}} $

通过计算d的大小来度量参考帧与当前帧的运动幅值大小， $\alpha 、{\rm{ }}\beta$ 是相机运动时旋转矩阵和平移矩阵对于d的贡献权重。

2)在上述判断的基础上引入参考关键帧，将当前帧与参考关键帧的关键点匹配个数作为判断指标。如果匹配点对个数小于当前帧提取关键点个数的0.75倍，那么将当前帧作为候选关键帧传入下一线程处理。

4 实验设计与结果分析 4.1 实验平台与误差评价指标

视觉SLAM系统2个常用的评价指标是绝对轨迹误差(absolute trajectory error, ATE)和相对位姿误差(relative pose error, RPE)^[9]。本文实验基于PC端进行，系统为Ubuntu16，内存16 G，CPU i7。

4.2 实验结果轨迹分析

首先采用TUM公开数据集进行相关分析实验，因为是否存在回环对整个视觉SLAM系统的后端优化有很大影响，所以采用数据集中的fr2_large_with_loop带有回环的数据进行相关实验，实验结果的三维轨迹对比如图5所示。其中误差尺度在右侧用不同颜色进行区分，其中参考轨迹(reference)为数据集实际轨迹。可以看出图5(a)与图5(b)表示fr2_loop的轨迹误差图误差基本一致，证明本文在大幅提高ORB-SLAM2系统的基础上保持了良好鲁棒性。

4.3 定位实验结果分析

为了使对比实验有更加广泛的价值，在本文SLAM系统与ORB-SLAM2系统基础上，采用另一个视觉SLAM成熟方法RGBD-SLAM2系统进行相关对比实验。在TUM数据集进行相关对比实验，下面只显示fr1_room结果，其他对比实验结果在表1中具体记录。

对相对位姿误差的值进行统计绘图如图6所示。从图6可以看出3个SLAM系统在相对位姿误差分布上，本文SLAM系统与ORB-SLAM2系统在更小误差值上具有更多的分布数据，而误差值越小表明该SLAM系统的相对位姿误差越小，系统越稳定，本文SLAM系统和ORB-SLAM2系统、RGBD-SLAM2系统相比较，具有更好的鲁棒性。

一般数据的最大值(MAX)、最小值(MIN)、算术均值(Arthmetic MEAN)、中值(MEDIAN)、均方根误差(Root Mean Squard Error, RMSE)和标准差(Standard Deviation, STD)，这些数据对比如图7所示。

从图7可以看出，在上述几项误差评价指标中，本文SLAM系统都保持几乎所有误差项的最低值。

TUM数据集的各项定位实验数据如表1所示，所有数据集进行5次实验，取误差平均值进行计算。通过比较3个SLAM系统定位误差的RMSE值可以看出，本文系统在其中几项上表现好于ORB-SLAM2，其他几项上与ORB-SLAM2基本保持一致。

4.4 地图构建实验

地图构建是室内同步定位与建图要解决的另一个重要问题，因为环境地图的好坏直接影响导航结果与后续的处理过程，所以本文添加稠密点云地图构建线程^[10]，并为减少空间要求采用八叉树地图来存储地图，在满足导航要求基础上减少了地图体积^[11]。因为建图效果类似，下面只列出fr3_long_office的建图结果。

从图8可以看出，稠密点云地图对数据集中的桌面各种物体清晰度较高，可以清晰看出椅子、玩具等，而且根据本文方法将稠密点云地图转化为八叉树地图时，稠密点云地图200 M左右大小，而八叉树地图26 M左右。对TUM数据集其他的数据进行相同的建图实验，统计其稠密点云地图与八叉树地图大小如表2所示。

结合图8与表2可以看出，本文构建的八叉树地图在保留地图细节基础上大幅缩小了地图体积，并且可以进一步应用于机器人导航，对于室内机器人的计算和存储空间优化具有重要应用价值。

5 结论

本文首先针对ORB特征点提取耗时过多而影响系统速度问题，应用LK光流法来处理非关键帧的跟踪问题。通过基于旋转矩阵和平移矩阵的归一化处理及参考关键帧与当前帧的关键点匹配点阈值来判断新的关键帧。实验表明，本文方法在保持较高定位精度基础上数据处理速度约为40 f/s，是ORB-SLAM2系统的1.4倍左右。此外本文针对ORB-SLAM2系统只有稀疏点云地图的不足^[12]，添加了稠密点云的构建与八叉树地图的构建环节。

在下一步的工作中，将研究如何构建语义地图与利用语义地图进行定位的优化。