机器人同步定位和制图(simultaneous localization and mapping，SLAM)是实现机器人自主导航定位的关键技术，其主要解决的问题可简略描述为：机器人于未知的环境中移动，利用自身所携带的传感器对周围的环境进行感知，根据所感知到的信息描绘出周围环境大概的地图样貌并估计出自身在地图中所处的位置。根据所使用传感器的类型，SLAM技术可分为：基于激光的SLAM、基于声呐的SLAM、基于GPS的SLAM和基于视觉的SLAM^[1]。相较于其他的传感器，视觉传感器价格便宜，不太依赖于硬件性能，还能获取周围环境的深度信息，随着科技的进步和计算机技术的飞速发展，图像处理获得了极大的关注，目前基于视觉的SLAM技术越来越受到研究人员的青睐。

根据使用视觉传感器的数量和类型的不同，视觉SLAM系统主要可分为3类：单目视觉SLAM(MonoSLAM)、多目视觉SLAM和RGB-D SLAM。MonoSLAM虽然成本很低，但是无法测得实物的绝对深度，需要借助其他传感器的帮助估计机器人的轨迹和地图；相较于MonoSLAM，多目SLAM可以在运行过程中估计深度，但是计算量依然繁杂；RGB-D SLAM是一种新兴的相机，拥有3个“感官”：红外线投影仪、彩色摄像头、红外深度摄像头，它通过红外结构光原理可以直接获取图像中各像素实物与相机的真实距离。

SLAM本质上是一个机器人运行状态的估计问题，根据求解方法可将其分为两类：一类是基于滤波器^[2]的方法，另一类是基于图优化^[3]的方法。基于滤波器的方法将各个时刻机器人的姿态看作一系列独立的时间状态，是对最新时刻状态的估计和预测，是一种不断迭代的过程。基于图优化的方法和其存在本质的不同，它所关注的并不是当前时刻的某一状态，而是整个姿态图，需要考虑过去所有的信息，对全部时刻的姿态及各时刻间的约束关系进行优化。文献[4]提出一种实时SLAM算法，算法结合颜色信息和粒子采样步骤提高了定位精度，但是采用了点云配准ICP算法，导致该算法适用性不高；文献[5]引入了关键帧选取机制，借以提高地图成图效率，但是没有后端优化步骤，鲁棒性较低；文献[6]将RANSAC算法和ICP算法相结合，用以求取最优变换；文献[7]提出利用一种环境测量模型来验证运动估计的有效性，后端采用姿态图优化软件G2O^[8]来进行轨迹优化，但是由于算法限制导致环境适用性相对不高。

本文提出一种采用图像特征匹配的图优化SLAM方法，将整个过程描述为3个阶段：准备阶段、处理阶段和结束阶段。如图 1所示，准备阶段主要处理平台的搭建与硬件的校准问题，本文采用了通用的机器人平台，该平台主要包括底盘，RGB-D像机和支架3个部分，搭建完平台，将RGB摄像头和深度摄像头进行对准，分别对两个摄像头进行标定，确定摄像机的数学模型，记录标定参数用于下个阶段；处理阶段分为前端和后端，首先提取相邻两帧RGB图像的特征，计算其描述子并进行匹配，然后根据特征点的深度信息结合RANSAC^[9]算法思想进行相邻两帧图像间的运动估计^[10-11]，为了获得更好的鲁棒性和精度，本文将阈值的思想运用到了运动估计中，设置最小特征匹配数量等阈值，用于检验运动估计的有效性，后端主要通过关键帧选取机制检测回环，结合图优化方法解决轨迹漂移问题；结束阶段用以表达高精度的点云地图和轨迹。

1 算法原理及系统框架 1.1 RGB-D相机标定

由于深度图和彩色图的获取来源并不相同，因此RGB-D相机需要对红外摄像头和彩色摄像头分别标定^[12]。

根据模型求解需要，需要标定的参数有：焦距(f_x, f_y)、光心c_x, c_y及深度缩放因子d_s。参数之间的关系可以表达为相机内部标定参数矩阵

结合相机标定工具包将这些参数一一结算，进而确定出空间点到二维图像点的投影关系，对图像某一处像素坐标(u, v)，其深度值为depth(u, v)，则有

由式(2)即可确定该像素点的空间坐标

相机的外参表现为旋转平移关系

式中，R为旋转矩阵；t为平移矩阵。

1.2 视觉里程计

里程计是通过分析处理移动传感器所获取的信息进而推算出其位置姿态随着时间变化而改变的一种方法。在计算机领域中，将通过处理图像数据进行运动估计的里程计称为视觉里程计，基于图像特征的视觉里程计包括特征检测、特征匹配和运动估计3个部分。

1.2.1 特征检测与匹配

常用的特征检测方法有SIFT、SURF^[13]、ORB^[14]等算法，对于SIFT算法，虽然其鲁棒性较高，但是计算复杂，耗时过多，不适合系统的实时运行需求，ORB算法虽然速度快但是鲁棒性较低，不符合高精度的成果需求，而SURF算法的鲁棒性虽稍次于SIFT，但是速度上却有明显的提升。本文遵循保证一定程度上的高效性、鲁棒性及准确性的原则，暂先采用SURF算法。

提取出特征点后，根据特征点周围的信息生成特征描述子进而完成特征匹配，根据特征相似性建立起两帧图像间特征点的对应关系。

1.2.2 运动估计

传统ICP(迭代最近点)算法对于初值的选择非常敏感，当初值选取不能满足求解需要时，算法就会陷入局部最优解的缺陷，为了提高算法的适用性及鲁棒性，本文提出采用RANSAC(随机采样一致性)算法来求取两帧图像间运动转换的最优解。

首先在所输入的观测数据集中随机提取出部分数据作为假设inliners(内点)，根据这些inliners所预测出的模型筛选出在一定阈值范围内适用于该模型的所有inliners，假如所得到的inliners足够多，则使用所有的inliners来优化该模型，否则重复执行第一步，直到得到inliners足够多的最优模型。具体步骤如图 2所示。

1.3 闭环检测与图优化 1.3.1 闭环检测

在视觉里程计^[15]的执行过程中，机器人的运动估计是一个不断递推的过程，由于每一次运动估计都是相互独立的，并且估计过程在一定程度上存在着噪声，对于缺少特征的估计，噪声就更为明显，这就形成了误差的传递累积，而闭环检测则可以极大地减少累计误差，缓和轨迹漂移。

为了提高算法鲁棒性，提升成果质量，本文提出了一种闭环检测方案，首先检测小闭环，对x帧图像执行运动估计，根据姿态图的结构建立一定深度的最小生成树，为了避免重复，先剔除最邻近的x帧图像，随机采样y帧偏向于较早时间的图像帧用于检测闭环，若检测到闭环，则在闭环附近采样z帧再次检测闭环，然后检测大回环，创立关键帧机制，将无法匹配上一关键帧的一帧图像加入关键帧序列，随机采样k帧关键帧与当前帧匹配，若成功匹配则检测到一个闭环。试验表明，通过对大小闭环的检测减少了轨迹的漂移，在一定程度上提升了算法的适用性。

1.3.2 图优化

通过运动估计及闭环检测完成姿态图的构建后，SLAM问题就转化为图优化的问题，由于估计误差的存在，姿态图并不具备全局一致性轨迹，针对该问题，本文在G2O框架下进行姿态图优化。为了计算全局一致性轨迹，将估计误差函数F(X)最小化，其误差公式为

式中，X=(x₁^T, x₂^T, …, x_n^T)^T为姿态向量；z_ij为约束均值矩阵；Ω_ij为约束信息矩阵；e(x_i, x_j, z_ij)为矢量误差函数，代表了满足该约束条件的程度。

3 仿真试验

为了验证算法的有效性，文本使用了TUM数据中的rgbd_dataset_freiburg1_d Esk数据集进行仿真试验，数据集中提供了机器人的真实轨迹，为了能更形象地验证算法的可行性，本文计算了一个均方根函数RMSE来表达绝对轨迹误差ATE，其具体函数公式为

式中，为估计轨迹；X={x_i, …, x_n}为其所对应的真实轨迹，仿真结果如图 3所示。

从图 3(b)中可以明显看出，本文所提出的算法进行的轨迹估算与真实轨迹基本保持一致，其绝对轨迹误差值ATE稳定在0.024 m左右，绝对轨迹误差均方差RMSE稳定在0.042 m左右，数据表明本文所提出算法实用性较高。

3 结论

本文提出了一种采用图像特征匹配技术的RGB-D SLAM算法。本算法提取出RGB图像的特征，计算其描述子并进行匹配，根据特征点的深度信息结合RANSAC算法求取出机器人的转换矩阵，利用相邻图像间的转换矩阵进行机器人的运动估计并得到机器人的位姿信息，对于所得到的转换估计，采用了基于阈值的算法思想来验证转换的有效性，进而剔除错误的估计结果得到最优的估测位姿，后端提出了一种创新的闭环估计策略，极大地提高了算法的适用性，最后结合全局约束信息，在G2O的框架下进行整个姿态图的优化，生成全局一致性轨迹，试验证明本文所提出的算法具有较高的鲁棒性，实用性较强, 为进一步提高算法的适应性，将来会以传感器的多元融合作为主要研究方向。