0 引言

在过去的十年里，室外定位在以卫星定位为基础的技术上获得了快速的发展和广泛的应用，如，全球定位系统 (Global Positioning System, GPS)。但是我们超过70%的时间都是在室内，因此室内定位技术具有巨大的研究价值和应用价值。由于室内环境复杂，需要从一系列的测量数据中估计移动节点的实时位置，所以到目前为止没有出现可以应用的解决方法^[1]。

在机器人的定位领域，基于激光和视觉的同时定位和地图构建 (Simultaneous Localization And Mapping, SLAM) 系统得到了快速的发展。SLAM是指搭载传感器的机器人在移动时利用传感器测量数据建立环境地图，同时估计机器人自身的位姿^[2]。SLAM同时包含定位与地图构建技术，被认为是实现机器人自主性的关键技术之一，对机器人的控制、导航、任务规划等领域有重要的研究意义^[3]。根据SLAM的求解方法可以将其分为扩展卡尔曼滤波类 (Extended Kalman Filter, EKF) SLAM、粒子滤波类 (Particle Filtering，PF) SLAM和图优化类 (Graph Optimization) SLAM。

EKF-SLAM最早由Smith等^[4]提出，在假设噪声为高斯噪声的基础上，通过运动模型和观测模型来估计机器人自身及路标点的位置。之后许多专家学者在其方法上进一步改进，针对EKF-SLAM的估计不一致性和非线性函数的线性化问题，Julier等^[5]提出无迹卡尔曼滤波 (Unscented Kalman Filter, UKF) 方法, 利用样本加权求和直接逼近随机分布，从而避免了对非线性函数雅各比矩阵的计算。张毅等^[6]提出了迭代扩展卡尔曼滤波 (Iterated Extended Kalman Filter, IEKF) 通过多次迭代更新获得更高的后验概率，降低了估计误差。此外，EKF-SLAM依赖于正确的数据关联，一旦数据关联出错则会导致构建地图失败；而且EKF-SLAM的计算成本高，计算量正比于地图规模的平方，因此只能对较小范围的环境构建地图。在EKF-SLAM框架上，一些研究人员在不同机器人系统上尝试了许多开发工作^[7]。

针对EKF-SLAM对正确数据关联十分敏感的问题，Sim等^[8]引入粒子滤波器 (PF) 来解决，其思路是将机器人和路标点的位置用粒子的分布来表达，由于没有了EKF-SLAM对噪声的高斯假设，同时粒子数量较多，对机器人和路标点位置具有多个假设，因此对正确的数据关联不敏感。但是PF-SLAM仍面临着由于粒子数量大导致计算成本高，以及由于重采样导致的粒子耗尽问题。为了减少粒子数量，Blanco等^[9]使用KL距离 (Kullback-Leibler Distance，KLD) 方法自适应确定粒子数量, 从而剔除了冗余的粒子。

除了滤波器求解SLAM问题之外，最近基于图优化SLAM成为研究的热点。其起源于基于捆集调整 (Bundle Adjustment，BA) 的运动构建 (Structure From Motion, SFM) 领域，SFM把位姿估计看作是一个全局优化问题，约束条件为相机的运动方程，目标函数为观测误差，最终通过最小化目标函数获得相机位姿。但是由于全局优化的计算量巨大，早期的SFM只能是离线的位姿估计和地图构建。后来研究者逐步认识到了SLAM求解问题的稀疏性，Sibley^[10]提出了把SLAM问题看作是一个图优化问题，图优化的节点描述了需要优化的变量，边则是与优化变量相关的约束条件，最终采用稀疏目标优化方法计算获得优化变量，即机器人的位姿和路标点位置。Klein等^[11]提出了平行跟踪和地图构建方法 (Parallel Tracking And Mapping, PTAM)，把运动跟踪和地图构建分为两个任务, 并在两个平行的线程同时进行处理，在线实时地实现了SLAM算法，虽然PTAM存在着缺少足够的回环检测、重定位不变性低、需要人工初始化地图等问题，但是其代表了视觉SLAM的一个重要突破。继PTAM之后，Mur-Artal等^[12]提出了ORB-SLAM (Oriented FAST and Rotated BRIEF-Simultaneous Localization And Mapping) 算法，该算法具有跟踪、地图构建、重定位三个平行线程，有效地解决了回环检测、重定位和地图初始化等问题，能够在小范围和大范围的未知环境中在线实时地实现高精度定位，但算法最终只构建基于特征点的稀疏地图，无法应用于机器人导航、路径规划等实际领域。

针对ORB-SLAM算法的不足，引入了在线快速构建未知环境三维稠密地图的方法，算法的整体步骤如图 1所示。

采用深度摄像头KINECT作为传感器设备，首先提出空间域上的方法提取关键帧，即当机器人的运动超过一定阈值时则把该帧选取为关键帧，并将它的点云拼接到已有的地图中；然后提出子地图法，将大规模的地图分解为一定规模的小地图，从而减少保存地图的运算内存和构建地图的时间，由此来提高算法的整体速度。实验表明，本文方法在标准RGBD数据集上能够取得较好的效果，在定位精度和快速性两方面都明显优于RGBD-SLAM^[13]算法，定位误差仅为RGBD-SLAM的40%，同时算法速度是RGBD-SLAM的2.6倍；在实际室内机器人实验中，本文方法能够快速准确地定位机器人位置，定位速度达到17帧/秒，能建立高精度的三维稠密地图。

1 ORB-SLAM概述

ORB-SLAM是一种基于图像特征和非线性优化的单目视觉SLAM系统。它包括基于ORB (Oriented FAST and Rotated BRIEF) 图像特征的词袋 (Bag of Words, BoW) 用于位置识别及回环检测^[14]、信息关联视图^[15]、G2o图优化通用框架^[16]。对于大范围的地图构建，应用了尺度感知的回环检测。该算法在全部的处理当中只用到了ORB来作特征检测和描述，这样一来提高了其在位置识别和回环检测的效果。文献[12]完整地介绍了ORB-SLAM算法，在此主要总结了它的三个主要步骤，包括跟踪、构建特征地图、回环检测，如图 2所示。

1.1 跟踪

这一部分的主要任务是通过KINECT获得视频帧，并用光流法^[17]来跟踪其位置。光流法是利用图像序列中像素在时间域上的变化，求解出空间运动物体在观察成像平面上的像素运动的瞬时速度s=(u, v)，从而计算出相邻帧之间物体的运动信息，估计上一帧跟当前帧之间存在的对应关系。假设像素的光流运动微小和亮度恒定，可以得到I(x, y, t)=I(x+dx, y+dy, t+dt)，由一阶泰勒展开得：

$I(x + dx,y + dy,t + dt) = I(x,y,t) + \frac{{\partial I}}{{\partial x}}dx + \frac{{\partial I}}{{\partial y}}dy + \frac{{\partial I}}{{\partial t}}dt$

(1)

即${I_x}dx + {I_y}dy + {I_t}dt = 0$，并令$u = \frac{{dx}}{{dt}},v = \frac{{dy}}{{dt}}$，那么则有：

$\left[ {{I_x}{\rm{ }}{I_y}} \right] \cdot \left[ \begin{array}{l} u\\ v \end{array} \right] = - {I_t}$

(2)

对于N个像素点的光流：

$\left[ {\begin{array}{*{20}{l}} {{I_{x1}}{I_{y1}}}\\ {{I_{x2}}{I_{y2}}}\\ { \vdots \vdots }\\ {{I_{xN}}{I_{yN}}} \end{array}} \right] \cdot \left[ {\begin{array}{*{20}{l}} u\\ v \end{array}} \right] = - \left[ {\begin{array}{*{20}{l}} {{I_{t1}}}\\ {{I_{t2}}}\\ \vdots \\ {{I_{tN}}} \end{array}} \right]$

(3)

将式 (3) 记为A·s=b，那么：

$\mathit{\boldsymbol{s}}{\rm{ = }}\mathop {\arg \min }\limits_{\mathit{\boldsymbol{s}}} \left\| {{\mathit{\boldsymbol{A}}} \cdot {\mathit{\boldsymbol{s}}} - {\mathit{\boldsymbol{b}}}} \right\|$

(4)

假设地图点的三维位置可用，并且用ORB特征来描述。如果在当前图像帧中跟踪成功，则通过之前图像帧的运动模型平均值来估计当前KINECT的位姿，然后重映射地图点估计其在当前帧上的图像。用ORB特征描述的地图点与在预测点附近范围检测到的特征进行匹配，图像上最小汉明距离的特征点被选为匹配点。然后利用非线性优化最小化重投影误差来计算获得相机位姿。优化之后，匹配点根据合适的阈值被分为内点和外点。如果跟踪失败，首先把当前图像帧转换成图像词袋，检索图像数据库，为全局重定位查找关键帧。然后计算ORB特征和每个关键帧的地图云点的对应关系，最后对每个关键帧执行随机一致性检验 (RANdom Sample And Consensus，RANSAC) 迭代计算，用PnP (Perspective-n-Point) 算法估计KINECT位姿。一旦估计获得KINECT的位姿，就可以恢复跟踪模块。

1.2 构建局部地图

为了构建环境的三维地图，系统在KINECT获得的视频帧中提取合适的关键帧。利用跟踪模块获得的匹配特征点，估计关键帧之间的匹配关系。一旦匹配有效，就采用BA来优化估计三维地图点位置和关键帧位姿。算法在一个和跟踪线程平行的线程中以较低的频率连续计算匹配和迭代提高地图点的精度。BA最小化关键帧匹配点的重映射误差，T_{w, ci}为第i个关键帧的位姿，X_{w, j}为第j个三维地图点:

$\mathop {{\mathop{\rm argmin}\nolimits} }\limits_{{T_{w,{c_i}}},{X_{w,j}}} \sum\limits_{i,j} {{\rho _h}({\mathit{\boldsymbol{e}}}_{_{i,j}}^{\rm{T}}{\mathit{\boldsymbol{ \boldsymbol{\varOmega} }}}_{i,j}^{ - 1}{{\mathit{\boldsymbol{e}}}_{i,j}})} $

(5)

其中: ρ_h为Huber函数，Ω_{i, j}为协方差矩阵, e_{i, j}由式 (6)、(7) 计算获得：

${{\mathit{\boldsymbol{e}}}_{i,j}} = {{\mathit{\boldsymbol{u}}}_{i,j}} - {\rm{CamProj(}}{{\mathit{\boldsymbol{T}}}_{w,{c_i}}},{{\mathit{\boldsymbol{X}}}_{w,j}}{\rm{)}}$

(6)

$\frac{1}{{{Z_{{X_{w,j}}}}}}\left[ \begin{array}{l} 1{\rm{ 0 0}}\\ 0{\rm{ 1 0}} \end{array} \right]\left[ \begin{array}{l} {f_x}{\rm{ }}0{\rm{ }}{c_x}{\rm{ }}0\\ 0{\rm{ }}{f_y}{\rm{ }}{c_y}{\rm{ }}0\\ 0{\rm{ }}0{\rm{ }}1{\rm{ }}0 \end{array} \right]{{\mathit{\boldsymbol{T}}}_{w,{c_i}}}{{\mathit{\boldsymbol{X}}}_{w,j}}$

(7)

其中：u_{i, j}为第j个地图点在第i个关键帧上的观测；CamProj计算了地图点在已知相机位姿上的投影，其定义为式 (7)，其中f_x、f_y、c_x、c_y为相机内参。当KINECT到达一个新的区域，那么新的关键帧就会添加到地图中。添加关键帧之后，当前关键帧就会和之前的关键帧建立关联并初始化新的地图点。

最初，地图点和关键帧是冗余的，那么需要进一步更为严苛地筛选它们。出现以下情况的地图点就会被剔除：1) 这些点在接下来的图像帧中无法跟踪和匹配; 2) 投影光线通过三角化计算点处于低视差; 3) 三角测量点会产生的重投影误差较大。这个严格地图点筛选过程保证了构建地图的鲁棒性。为了保证重构地图的简洁性，降低BA过程中的复杂度，检测冗余关键帧并删除它们，在关键帧集中，如果一个关键帧的90%的地图点至少在其他的三个关键帧中被检测到，那么则剔除该关键帧。

1.3 回环检测

为了降低视觉里程计过程中的积累误差，利用各关键帧的相互关联性进行有效的闭合回环，在本质视图上优化位姿。这样就可以将累计的误差分散到位姿图中，并通过相似变换矫正尺度偏移。通过回环检测优化位姿之后，根据优化后的关键帧矫正地图点云。首先计算关键帧K_i的词袋和其数据关联视图附近关键帧的相似度，并提出相似度较低的关键帧，同时删除和K_i直接连接的关键帧，最终获得闭合回环。然后当闭合回环达到一定程度时，利用通用图优化框架 (General Graph optimization, G2o) 优化回环位姿图，如图 3所示。最后更新地图点，融合重复的地图点。位姿图优化公式如下：

$\mathop {{\mathop{\rm argmin}\nolimits} }\limits_{{X_{w,i}},{X_{w,j}}} \sum\limits_{i,j} {({\mathit{\boldsymbol{e}}}_{_{i,j}}^{\rm{T}}{\Delta _{i,j}}{{\mathit{\boldsymbol{e}}}_{i,j}})} $

(8)

其中e_{i, j}由式 (9) 计算获得:

${{\mathit{\boldsymbol{e}}}_{i,j}} = {\log _{{\mathop{\rm sim}\nolimits} (3)}}({{\mathit{\boldsymbol{X}}}_{i,j}}{{\mathit{\boldsymbol{X}}}_{w,j}}{\mathit{\boldsymbol{X}}}_{w,i}^{ - 1})$

(9)

其中:X_{i, j}是位姿X_{w, j}到位姿X_{w, i}的相似变换，log_{sim (3)} 将转移矩阵的位姿误差映射到7维欧氏空间R⁷，Δ_{i, j}为边的信息矩阵。

2 三维稠密地图快速构建

在ORB-SLAM系统中，其构建的是三维稀疏特征地图，分辨率极低，无法在实际的机器人领域中应用，如机器人导航、路径规划等。为了克服这一不足之处，本文提出了利用深度摄像头KINECT作为传感器设备来构建三维稠密地图。如果将每一帧的点云都融合到地图中，那么地图的容量会很大，从而降低了系统实时性能。由于机器人在运动过程中相邻图像帧具有连续性，即相邻帧的位姿在空间上变化较小，提出了空间域上的关键帧提取方法来筛选合适的图像帧，并引入子地图法进一步减少构建地图的时间，由此来提高算法的速度。

KINECT摄像头由彩色相机和深度传感器组成，其中深度传感器由红外发射器和红外接收器两部分构成，如图 4所示。彩色相机可以获得每个像素点的RGB值即彩色图像，而深度传感器则可以测量到像素点的距离信息即深度图像，根据KINECT成像原理，如图 5所示，融合彩色图和深度图像计算获得三维点云。

从二维彩色图像和深度图像计算三维点云的公式如下：

$\left\{ \begin{align} & z={d}/{s}\; \\ & x=(u-{{c}_{x}})\cdot {z}/{{{f}_{x}}}\; \\ & y=(v-{{c}_{y}})\cdot {z}/{{{f}_{y}}}\; \\ \end{align} \right.$

(10)

其中:f_x, f_y, c_x, c_y为相机内参，(u, v) 为图像坐标，(x, y, z) 为图像坐标系坐标，d深度相机测得像素点的距离，单位毫米 (mm)，s为实际距离和测得距离d的比例系数，这里取1000。

从相机坐标系点云到全局坐标系点云的变换公式如下：

${{\mathit{\boldsymbol{X}}}_{w,j}} = {{\mathit{\boldsymbol{T}}}_{w,{c_i}}}{{\mathit{\boldsymbol{X}}}_{{c_i},j}}$

(11)

其中:T_{w, ci}为第i个关键帧的位姿，X_{ci, j}为在第i个关键帧坐标系上的点云，X_{w, j}是变换后获得在全局坐标系上的点云。

为了避免三维点云的冗余，造成不必要的计算量，本文采用基于空间域上的方法来提取关键帧，其思想是只有当相机在相邻帧运动了一定大小时才把该帧视为关键帧，并把它的点云叠加到现有的地图中去。其计算公式如下：

$\begin{array}{l} \min \_norm \le \left\| {\Delta \mathit{\boldsymbol{t}}} \right\| + \min (2\pi - \left\| r \right\|,\left\| r \right\|) \le \\ \quad \quad \quad \quad \quad \max \_norm \end{array}$

(12)

其中:Δt为相机在相邻帧之间的位移矢量; r为相机在相邻帧之间的旋转角度，用它们的范数和来描述相机运动大小; min_norm为相机的最小运动，即当相机在相邻帧运动大于min_norm时则把该帧提取为关键帧; max_norm为相机的最大运动，即当其运动大于max_norm时，可认为是相机位姿估计错误，剔除该帧。在本文中根据实验经验，min_norm和max_norm分别取值为0.4、5。

在大范围环境中建立三维稠密地图时，整个地图会相当大，占用大量的计算机内存，从而降低了系统的整体速度。本文提出了子地图法，将整个大地图分成多个包含一定关键帧的子地图。当地图规模达到了一定的程度即一定关键帧数量时，将该子地图从计算内存中释放出来，保存到计算机硬盘中。待系统将要关闭，则将多个子地图整合为完整的地图。本文的子地图规模为10个关键帧。

3 实验结果和分析

为了验证本文算法的有效性，进行了两组实验，包括：

1) 对比实验。利用RGBD-SLAM提供的标准数据集进行相机定位和地图构建实验，并和RGBD-SLAM进行对比。

2) 实际环境实验。利用Turtlebot2机器人在实验室楼道进行机器人定位和稠密地图构建实验。

下面分别是这两组实验的过程和结果分析。

3.1 对比RGBD-SLAM

本节主要从算法的准确性和快速性两方面，将本文算法和RGBD-SLAM算法进行对比。

文献[13]提供了标准的RGB-D数据集，包括由KINECT获取的彩色图和深度图，由高精度动作捕捉系统获得的KINECT运动的真实位姿以及获取各个数据集的相机内参。除此之外，该文献还提供了计算估计位姿和真实位姿的均方根误差 (Root Mean Square Error, RMSE) 工具，这样可以有效地评估算法的定位精度。

本文主要利用RGB-D数据集中FR1组数据进行实验，估计KINECT相机运行轨迹并构建三维稠密地图。实验对比结果如表 1所示，其中RGBD-SLAM实验结果来自文献[13]，图 6和图 7分别是本文算法在FR1数据集上三维地图局部图、估计轨迹和实际轨迹对比图。

从表 1可以知道:1) 在定位精度方面，本文方法明显高于RGBD-SLAM算法。RGBD-SLAM在FR1 room数据包上的定位误差最大，达到了0.219 m，但是本文方法在这个数据包上的定位误差只有0.045 m。RGBD-SLAM在FR1数据集上平均定位误差是0.097 m，而本文方法的平均定位误差为0.038 m，仅为RGBD-SLAM的40%；2) 在快速性方面，本文方法也明显优于RGBD-SLAM算法。RGBD-SLAM在FR1数据集上平均每帧的处理时间是0.349 s，而本文方法仅用了0.132 s，速度约是RGBD-SLAM的2.6倍。

3.2 实际环境实验

为了进一步分析本文方法在实际室内机器人定位和三维稠密地图构建的效果，利用开源机器人平台Turtlebot2在实验室楼道进行了分析实验。Turtlebot2机器人主要由KOBUKI移动底座、KINECT深度摄像头和笔记本组成，其结构如图 8所示。本文的SLAM算法是在i5-3210M，2.5 GHz CPU，8 GB RAM，ubuntu14.04操作系统的笔记本上用C++实现的。

实验室楼道由四段直线道组成，如图 9所示，楼道存在大量的相似物体，如门、瓷砖等，并且瓷砖的角点数量较少，这些给特征匹配和回环检测带来了巨大的挑战。Turtlebot2机器人上配备了KINETCT摄像头，并使得机器人坐标系和KINECT摄像头坐标系重合。实验过程中，用XBOX遥控手柄控制机器人以0.4 m/s速度水平运动，本文系统在线定位机器人位姿并构建三维稠密地图，最终获得机器人水平二维轨迹图 (图 10) 和三维地图 (图 11、图 12)。

由于缺少运动捕抓系统，无法确定机器人的真实位置，为了分析机器人的定位精度，把每段路道中的一点和终点选为测量点。由于机器人是水平运动，因此测量点的真实位置可以用二维坐标点 (x, z) 来描述。本文采用卷尺测量获得5个测量点的真实位置，最后用测量点的真实位置和算法估计位置的RMSE来度量定位精度。均方根误差计算方法$RMSE = \sqrt {(\sum\limits_{i = 1}^n {} {{({t_i} - \overline {{t_i}} )}^2})/n\;} $，其中$\overline {{t_i}} $、t_i分别为测量点的真实位置和估计位置，在本文中n=5。五个测量点的真实位置、估计位置以及RMSE见表 2。机器人在楼道运动过程中位姿估计存在误差累计，5个测量点的均方根误差为1.04 m，机器人整个运动轨迹的距离为50 m，即存在2%的误差。

从图 11和图 12中可以看到，本文系统即使在大范围的环境下依然能够取得较好的构图效果，三维地图和实际三维环境相符，没有出现地图重叠和变形等情况，并且地图中楼道的反光瓷砖、地面和杂物都清晰可见。

在整个实验中，系统共处理了1548张视频帧，关键帧为130帧，整体速度为11帧/s, 其中定位速度为17帧/s。结合上文对定位精度和三维稠密地图的效果分析，系统可以满足机器人同时定位和地图构建的精度和速度上的要求。

4 结语

为了建立高精度定位、大范围和快速的室内机器人定位和三维稠密地图构建系统，本文利用ORB-SLAM算法估计机器人三维位姿，然后采用深度摄像头KINECT获得每一帧的稠密点云，为了剔除冗余的视频帧引入了空间域上的关键帧提取方法，并采用子地图法进一步减少地图构建的时间以提高整体速度。利用机器人Turtlebot2在实验室楼道进行了分析实验，结果表明，本文的方法能够在大范围环境中准确地定位机器人位置，在运动轨迹为50 m的大范围中，机器人的均方根误差为1.04 m，即误差为2%;同时系统的整体速度为11帧/秒，其中定位速度达到17帧/秒，可以满足室内机器人在精度、大范围和快速性的要求。但系统仍存在不足，由于ORB-SLAM算法是基于ORB角点特征来实现位置识别和回环检测的，所以在角点特征较少的环境中机器人的定位效果较差，容易定位失败。今后将会结合基于图像像素的直接SLAM进一步提高ORB-SLAM方法的定位效果。