1引言
同步定位与地图构建(SimultaneousLocalizationandMapping，SLAM)是机器人领域的技术热点与难点，其解决的定位和建图问题被认为是机器人实现自主导航的关键。该技术主要原理是通过机器人配备的多种传感器来感知周围环境，并计算出自己在当前环境下的位置。SLAM被提出后，先后经历了多个研究阶段。由于激光雷达精度高、范围广，早期的SLAM研究往往以激光雷达为主要的传感器。另外，早期的SLAM采用扩展卡尔曼滤波方法估计机器人的位姿[1]，但效果不好，对于某些强非线性系统，该方法会带来更多的截断误差，从而导致无法精准地实现定位与建图。而后基于粒子滤波的SLAM逐渐成为主流，粒子采样方法可有效避免非线性问题，但同样引出了随着粒子数增多，计算量增大的问题。
一直到2007年，Grisetti等[2]提出了基于改进粒子滤波的SLAM方法(Gmapping)，该方法通过改进的建议分布及自适应重采样技术有效地提高了定位精度并降低了计算量，是激光SLAM的一个里程碑。Konolige等[3]在2010年提出了KartoSLAM，该方法采用图优化代替粒子滤波，并采用稀疏点调整来解决非线性优化中矩阵直接求解难的问题。Kohlbrecher等[4]在2011年提出了HectorSLAM，该方法不需要里程计信息，通过利用高斯牛顿方法来解决扫描匹配问题，但对传感器要求较高，需要高精度的激光雷达才能运行。由谷歌于2016年提出的Cartographer[5]可以说是最新的激光SLAM方案。该方法将获得的每一帧激光数据，利用扫描匹配在最佳估计位置处插入子图(Submap)中，且扫描匹配只与当前子图有关。在生成一个子图后，会进行一次局部的回环(LoopClose)，而在所有子图完成后，利用分支定位和预先计算的网格进行全局的回环。相比于Gmapping和Hector，该方案具有累积误差较低，且不需要高成本设备的优点。视觉SLAM由于传感器成本低、图像信息丰富逐渐成为了SLAM的研究热点。
但与激光SLAM相比，视觉SLAM更加复杂。Davison等[6]在2007年首次提出的MonoSLAM被认为是众多视觉SLAM的“发源地”。该方法以扩展卡尔曼滤波为后端，追踪前端稀疏的特征点；利用概率密度函数来表示不确定性，从观测模型和递归的计算，最终获得后验概率分布的均值和方差。Sim等[7]利用粒子滤波实现了视觉SLAM，该方法避开了线性化的问题并且精度高，但需要使用大量的粒子，从而导致计算复杂度的提高。随后，为了减少计算复杂度，提取关键帧变得极为重要，其中最具代表性的是2007年Klein和Murray提出的PTAM[8]。该方法提出了简单有效地提取关键帧的技术，并且该方法最为关键的两点是：(1)实现了跟踪与建图的并行化，虽然跟踪部分需要实时响应图像数据，但后端优化却不需要实时计算。即后端优化可在后台慢慢运行，在需要的时候将两个线程同步即可。这也是首次提出了区分前后端的概念，引领了之后众多SLAM方法的架构设计。(2)第一次使用非线性优化，而不是传统的滤波器。从PTAM提出后，视觉SLAM研究逐渐转向了以非线性优化为主导的后端。
2016年Mur-Artal和Tardos[9]提出了现代SLAM中非常著名的第二代基于具有带方向性的加速分段测试特征(FAST)关键点以及带旋转不变性的二进制鲁棒独立基本特征(BRIEF)描述子的特征点(OrientedFASTandRotatedBRIEF，ORB)[10]的视觉SLAM系统ORB-SLAM2，是现代SLAM中做得十分完善并且易用的系统之一。该方法不仅支持单目、双目、深度摄像头三种模式，并且将定位、地图创建、闭环分为三个线程，且都使用ORB特征。ORB-SLAM2的闭环检测是一大亮点，该方法采用了词袋模型，有效地防止了累积误差，并且能在丢失之后迅速找回。但ORB-SLAM2的缺点也十分明显，图像特征提取与匹配以及后端的优化都需要计算资源，在嵌入式平台上实现实时运行存在一定的困难，并且构建的稀疏特征点地图虽然可以很好地满足定位，但无法提供导航、避障等功能。相比于提取图像中的特征点，根据图像像素灰度信息来计算相机运动的直接法从另一个方向实现了定位和建图。Stühmer等[11]提出的相机定位方法依赖图像的每个像素点，即用稠密的图像对准来进行自身定位，并构建出稠密的三维地图。Engel等[12]对当前图像构建半稠密深度地图，并使用稠密图像配准(DenseImageAlignment)法计算相机位姿。构建半稠密地图即估计图像中梯度较大的所有像素的深度值，该深度值被表示为高斯分布，且当新的图像到来时，该深度值被更新。Engel等[13]提出了LSD-SLAM算法，其核心是将直接法应用到半稠密的单目SLAM中，这在之前的直接法中很少见到。以往基于特征点只能构建稀疏地图，而稠密地图又需要RGB-D这种可以提供深度信息的相机。
Forster等[14]于2014年提出了半直接法单目视觉里程计(Semi-directMonocularVisualOdometry，SVO)，一种被称为“稀疏直接法”的方法，该方法将特征点与直接法混合使用，跟踪了一些关键点(如角点等)，然后按照直接法根据关键点周围的信息估计相机运动及位置。相比于其他方案，SVO既不用消耗大量资源去计算描述子，也不必处理过多的像素信息，因此该方法可广泛用于无人机、手持增强现实(AugmentedReality，AR)等设备上。Newcombe等[15]提出了Kinect融合的方法，该方法通过Kinect获取的深度图像对每帧图像中的每个像素进行最小化距离测量而获得相机位姿，且融合所有深度图像，从而获得全局地图信息。Gokhool等[16]使用图像像素点的光度信息和几何信息来构造误差函数，通过最小化误差函数而获得相机位姿，且地图问题被处理为位姿图表示。Kerl等[17]提出了较好的直接RGB-DSLAM方法，该方法结合像素点的强度误差与深度误差作为误差函数，通过最小化代价函数，从而求出最优相机位姿，该过程由g2o实现，并提出了基于熵的关键帧提取及闭环检测方法，从而大大降低了路径的误差。
在多传感器融合方面，目前有视觉传感器、激光雷达、惯性测量单元(InertialMeasurementUnit，IMU)及超声波传感器等多种传感器。目前，主要融合方向为激光雷达结合视觉传感器及IMU结合视觉传感器。Chen等[18]通过视觉传感器结合IMU来进行机器人的精准位姿估计，并在机器人上垂直安装二维激光雷达采集点云数据实现三维建图。Houben等[19]采用三维激光数据用于微型无人机定位，针对结构相似的环境激光定位会出现模糊的问题，提出在环境中加入视觉标记，提高定位精度，同时可用于微型无人机的重定位。王消为等[20]提出了一种双目视觉信息和激光雷达数据融合的SLAM方法，该方法基于改进的粒子滤波算法实现了在计算建议分布时观测数据，其中同时包含视觉信息和激光雷达数据。相对于里程计运动模型作为建议分布，该方法有效地提高了定位和建图的精度。张杰和周军[21]提出了一种激光雷达与视觉结合的SLAM方法，其中激光地图用于导航，视觉地图用于复原目标场景；并提出一种改进的迭代最近点法(IterativeClosestPoint，ICP)用于实现更快的点云拼接，同时采用图优化的方法降低了累积误差，保证地图精度。Shi等[22]在小型无人机上，利用视觉里程计为二维激光的ICP提供初值，在实时性和精确度方面都达到了较好的效果。
Qin等[23]提出了视觉与IMU的紧耦合方案，将视觉构造的残差项和IMU构造的残差项放在一起构成一个联合优化问题。Li等[24]和Lynen等[25]通过扩展卡尔曼滤波器对视觉和IMU进行融合以实时获取状态估计。相对而言，激光SLAM的效果仍然要优于视觉SLAM，但激光SLAM由于其本身激光数据的特性导致其无法有效地进行大范围的闭环检测。对于低价格的激光雷达，由于激光点不够密集，构建的地图常常会出现回到原来的位置地图，由此造成定位不准确而出现偏差的情况，这是由累积误差所导致的。同时在激光SLAM中，闭环检测一直是一大难点：由于获取的激光数据是二维点云数据，无明显特征且相互之间十分相似，所以基于激光数据的闭环检测往往效果不好。由于图像中包含了丰富的信息，故视觉SLAM在闭环检测方面存在着天然优势。ORB-SLAM2中提出的词袋模型，采用了ORB特征配合词袋的方法，具有很高的准确率和速度，是当前应用最广的闭环检测手段。针对低成本激光SLAM噪声大、精度低、难以闭环的问题，本文提出激光结合视觉进行联合优化的方法来提高定位建图精度，并通过视觉词袋模型有效地解决激光闭环检测难的问题。
2基于图优化的同步定位与地图构建框架
激光SLAM主要通过相邻帧激光的匹配来计算相邻帧位姿变换，但由于激光数据并非完全无噪声的数据，尤其对于低成本激光雷达，打出去的激光点较为稀疏，从而导致计算得到的相邻帧位姿变换存在误差，因此往往需要加入滤波或优化的方法来使定位更加精准。而视觉SLAM是通过特征点提取与匹配来反算位姿，但若其中一帧出现问题就会导致误差累积并
不断地增大。图优化是SLAM后端优化的热门方法，通过构造节点和约束边清晰地展示了一系列位姿和观测量的关系，再通过非线性优化的手段求出最优变量从而得到精准的位姿估计。该方法最早应用于视觉SLAM中，起初由于图像特征点数量过多，导致矩阵维数过大，求解困难，所以该方法一直未能成为主流方法。直到2011年海塞矩阵的稀疏性被发现，极大地提高了运算速度，使得非线性优化和图优化的思想成功应用于视觉SLAM和激光SLAM中。而根据位姿估计和优化的实时性要求不同，SLAM也被分为前端和后端两部分，当前主流的SLAM框架如图1所示。
前端主要通过传感器数据估计机器人的位姿，但无论是图像还是激光，观测到的数据都含有不同程度的噪声。相对而言，高精度激光雷达噪声会小很多但成本过高，而通过低成本的激光雷达及相机采集图像来进行位姿计算都会导致定位与实际真值有累积误差，并且累积误差会随着时间的增加而越来越大。后端优化的主要作用就是通过滤波或优化的方式提高定位及构建的地图精度，消除累积误差。本文采用图优化作为后端，通过非线性优化寻找下降梯度的方式来迭代实现误差最小化。简单来说，图优化是以图的形式来描绘优化问题。在SLAM中，图的节点表示位姿，边表示位姿之间以及位姿与观测量的约束关系。机器人在导航建图过程中，观测量为激光数据及通过摄像头不停地捕捉到的外部环境信息，生成大量机器人观测到的ORB特征点对应的三维空间点。将所有数据放入图的框架中，如图2所示。
其中，X表示关键帧位姿；O表示观测量，包含了特征点所对应的三维空间点坐标及二维激光数据。视觉误差由重投影误差(图3)来表示，重投影误差的计算需要给定相邻帧所对应的两个相机位姿、匹配好的特征点在两幅图像中的二维坐标及所对应的三维空间点的三维坐标。纯视觉SLAM往往通过特征点提取和匹配，再使用EPnP等方法来求取相邻帧位姿变换估计。但相比于相邻帧的激光匹配，该方法误差较大，因此本文使用激光扫描匹配得到的位姿估计作为后端优化的初始值。而对于相邻帧图像中通过特征点匹配得到的特征点对p1、p2，本文采用的深度摄像头可直接获取得到前一帧图像特征点p1所对应的三维空间点P的坐标，将点P重新投影到后一帧图像上形成图像中的特征点。由于位姿估计的误差及深度摄像头噪声的存在，与p2并非完全重合，两点之间的距离就是误差。
图3
图4联合优化整体框架
重投影坐标计算公式及过程如下：(1)通过世界坐标系到相机坐标系的变换关系(R,t)，计算出世界坐标系下点P所对应的相机坐标系下点
的三维坐标。
（1）
其中，R为旋转矩阵；t为平移矩阵；T表示转置。(2)将投至归一化平面，并将归为1得到归一化坐标Pc。
其中，(uc,vc)为归一化平面上的二维坐标。(3)根据相机内参模型，像素坐标系为归一化平面横轴缩放了fx倍，竖轴缩放了fy倍，并且原点平移了cx、cy个像素。因此，像素坐标(us,vs)计算公式如下：
则该点误差函数为
。其中，p2为直接通过图像得到的像素坐标；
为根据公式(1)～(3)计算得到的重投影坐标。将上述误差函 数扩展到相邻两帧之间，则最小化代价函数为
。通过非线性优化算法可以得到误差最小化时相应的位姿变换(R,t)和三维空间点坐标Pi。
相对于视觉误差，激光误差的获取更为简单。激光SLAM往往需要通过扫描匹配来实现相邻帧的位姿变换估计，而该估计值(R,t)无法保证前一帧所有激光数据经过该位姿变换与后一帧激光数据完全重合。因此，激光的误差定义如下：
(4)
之后，通过非线性优化来使误差函数最小化，求得的位姿再返回到前端作为下一帧的参考帧位姿。
3.后端优化与闭环检测
视觉SLAM构建的地图由特征点构成，特征地图最大的问题在于无法用于导航，仅适合用于定位。低成本激光雷达构建的栅格地图更适合用于导航，但存在着激光稀疏、噪声大的问题。因此，本文提出视觉激光融合的方法，在提高定位精度的同时确保地图更加精准，还解决了激光SLAM构建的栅格地图难以回环的问题。而二维栅格地图的构建主要依赖于一系列位姿和激光测量数据，根据栅格被占用的概率来增量式的建图。由于同一时刻观测到的视觉信息和激光数据并非完全相互独立，因此本文提出视觉激光联合优化来充分利用数据之间的约束。加入视觉信息的激光视觉联合优化的SLAM整体框架如图4所示。
3.1误差函数
传统视觉相邻帧误差函数已在第2节中给出具体形式，图3图4联合优化整体框架6263技术前沿技术前沿重投影坐标与三维空间点、上一帧图像特征点以及位姿变换的关系如下：
（5）
其中，K为相机内参；Z为三维点的深度值。则误差函数如下：
位姿变换(R,t)可写成对应的李代数形式，
李代数变换公式为：
（7）
其中，相邻帧共有m个匹配特征点，n个激光数据点。实际计算中可将m、n限制在一定数量内从而减少计算复杂度。单独的视觉SLAM有其自有的计算相邻帧位姿变换算法，但由于图像数据的特点，计算得到的(R,t)往往没有通过激光扫描匹配得到的位姿变换精准。因此，采用激光扫描匹配得到的位姿变换来估计误差函数中的位姿变换初始值。
3.2稀疏姿态调整
根据观测模型可以很容易判断出误差函数不是线性函数，所以本文采用非线性优化来求解误差函数最小值所对应的变量。由于三维特征点和激光数据量过多，且随着时间推移整体代价函数所包含的多项式会变得越来越多，故本文只优化误差函数中的位姿变量。同时，姿态图的优化也会随着顶点约束的增加而变慢，所以本文采用稀疏姿态调整法，利用矩阵的稀疏性来提高优化速率。由于机器人位姿是由变换矩阵不断计算得到且一一对应，即求得每两相邻帧之间的位姿变换，亦即可获得机器人的当前位姿。因此，将机器人位姿作为唯一变量并对其进行优化，视觉观测量和激光点云数据作为位姿之间的约束。设机器人位姿与位姿变换之间的关系为：
则误差函数可改写为关于位姿x的函数
。其中，x为位姿的集合，即待优化变量为：
（10）
其中，k为待优化位姿个数。相应地，是对整体自变量x的增量。因此，当加入增量后，目标函数为：
（11）
其中，J表示雅克比矩阵，为代价函数对自变量的偏导数；K为待优化位姿个数，相邻帧优化时为2，全局优化时为当前帧到回环帧之间的位姿个数。位姿优化可看作是最小二乘问题，而解决最小二乘问题的常用方法有梯度下降法、高斯牛顿法及LevenbergMarquadt(L-M)法。其中，L-M法是对梯度下降法和高斯牛顿法的综合运用，效果最佳，因此本文采用L-M法来求解上述最小二乘问题。通过加入拉格朗日乘子对误差函数进行改进：
（12）
其中，参数表示近似模型与实际模型相似程度，越接近1时，越小，高斯牛顿法的近似效果越好；越小时近似效果越差，优化方法更近似于梯度下降法。一般情况下，H矩阵维数很大，矩阵求逆的复杂度为O(n3)。但由于H矩阵内部包含着各个顶点之间的约束，而只有相邻顶点之间才具有直接约束，从而导致H矩阵大部分元素为0，具有稀疏性。因此，利用H矩阵特有的稀疏性可以大大提高运算速度。求解公式(16)可得，沿着梯度下降方向循环迭代，最终获得目标函数最小时所对应的自变量x，即机器人位姿。
3.3闭环检测
闭环检测是SLAM中的一个核心问题，通过对曾经走过的地点进行识别，能有效地减小累积误差，提高定位精度。基于激光雷达的SLAM算法往往因为数据单一而无法有效地进行闭环检测，而视觉图像丰富的纹理特征则刚好可以弥补激光雷达的这一缺陷。本文采用视觉SLAM中最常用的词袋模型(Bag-of-Words，BoW)，通过视觉特征来构建关键帧所对应的字典，在检测到回环后通过回环帧与当前帧的匹配来计算当前位姿，并将这一约束加入到后端中来进行回环帧与当前帧之间的全局优化，提高定位精度的同时，也防止了激光所构建的栅格地图常出现的无法闭合的情况。
表1位姿估计对比
由于构建室内地图所采集到的图像数量过多，且相邻图像之间具有很高的重复性，因此首先需要进行关键帧的提取。本文关键帧选取机制如下：(1)距离上一次全局重定位已经过去了15帧；(2)距离上一次插入关键帧已经过去了15帧；(3)关键帧必须已经跟踪到了至少50个三维特征点。其中，(1)、(2)是其独特性的基础，因为短时间视野内的特征不会发生明显变化；(3)保证了其鲁棒性，过少的地图点会导致计算误差的不均匀。相对而言，激光SLAM关键帧的选取要更为简单且稳定，但采用激光作为关键帧选取参量容易出现关键帧包含的图片特征点不够、图像不连续等问题。因此，本文采用基于视觉的关键帧选取机制，既保证相邻关键帧不会过于接近，又保证有足够的信息进行关键帧之间的匹配。常见的图像特征有尺度不变特征变换(ScaleInvariantFeatureTransform，SIFT)[26]、加速稳健特征(SpeededUpRobustFeatures，SURF)[27]和ORB等。其中，SIFT特征点具有旋转不变性和尺度不变性，并且稳定性高，不易受到光照及噪声的影响，是最理想的选择。然而，SIFT特征提取速度慢无法保证实时，故不适用于SLAM中。SURF效果与SIFT相近同样存在计算时间过长的问题。因此，本文选择ORB来构建词袋模型，ORB在保持了旋转不变性和尺度不变性的同时，速度要明显优于SIFT和SURF。
BoW通过比对图像所包含的特征将当前帧与每一个关键帧作相似度计算。首先当相似度表明当前帧与某一关键帧足够相似时，则认为机器人回到了该关键帧附近的位置，产生回环。然后，在回环后对当前帧和该关键帧进行激光点云的ICP匹配从而以该关键帧的位姿为基准，计算出当前机器人位姿，并将当前帧位姿、当前帧与回环帧之间的图像特征点、激光点云作为约束加入到图优化框架中，从而对检测到回环的关键帧与当前关键帧之间的一系列位姿进行全局优化。最后，根据优化后的位姿及每一帧位姿所携带的激光数据重新构建地图，消除地图中无法闭合的部分。
4实验
本文实验分为两部分：第一部分在小范围场景下进行定点定位精度的对比实验，分别对传统基于图优化的激光SLAM方法(即Karto)和本文提出的激光视觉相结合的方法进行定位数据采集；第二部分为闭环实验，验证本文提出的方法是否可以有效地解决激光SLAM可能出现的地图不闭合情况。
4.1实验平台和环境
本文实验在Turtlebot2上进行，配备笔记本电脑以及激光雷达和深度摄像头，电脑配置为IntelCorei5处理器、8G内存，运行ubuntu14.04＋ROSIndigo系统。二维激光雷达采用单线激光雷达RPLIDARA2，人为设置采样频率为5～15Hz，测量半径为8m。深度相机采用奥比中光公司出品的Astra深度相机。其中，该相机深度测距的有效范围为0.6～8m，精度达3mm；深度相机视角可以达到水平58°和垂直45.5°。
本文实验在中国科学院深圳先进技术研究院B栋办公楼进行，以机器人起始位置建立世界坐标系，在B栋5楼B、C区电梯之间选择5个标志性位置，分别用Karto和本文提出的方法进行位姿测量，实验过程如图5所示。从0点出发，以0点为世界坐标系原点，0到1方向为x轴方向，2到3方向为－y轴方向，依次沿1、2、3、4、5运动，各点实际坐标如表1所示。机器人实际位姿由时间和速度来控制。为保证视觉特征提取的稳定性，机器人线速度保持匀速0.2m/s，角速度30(°)/s，即机器人只有0.2m/s匀速前进、30(°)/s匀速右转及停止三个状态。其中，前进15s为前进3m；右转3s为右转90°。最终通过发布线速度和角速度指令并计时来控制机器人准确到达5个标志点。第二部分实验场景为中国科学院深圳先进技术研究院B栋4楼B区工位及C区3楼。其中，4楼工位为小范围闭环，C区3楼为大范围回环。
4.2实验结果分析
定位结果及误差对比如表1所示。表1中数据说明仅依靠激光进行SLAM时初始误差较小，但随着距离变远，测量值与实际位姿之间误差逐渐增大。而实验过程中使用的激光成本较低，光束本身不够密集，导致无法保证有足够的可用数据过滤掉测量过程中测量值本身的误差，从而产生累积误差。本文提出的激光视觉联合优化位姿的方法虽然同样存在累积误差，但加入了视觉信息约束可有效地缩小累积误差，可获得更高的定位精度。图6(a)～(d)分别是Karto[3]及本文提出的方法所构建的栅格地图；图6(e)是ORB-SLAM2所构建的C区3楼稀疏特征点地图。其中，机器人位姿由一系列红色箭头表示。图6(a)、(b)为工位环境的小范围闭环，从中可以看到，图6(a)白线圈处有不重合的现象，但由于场景较小，效果并不明显。
图6(c)、6(d)为C区大范围场景所构建的地图，由于场景较大，随着时间推移产生的累积误差会不断增大。图6(c)中可以明显看到白线圈出的部分地图并未重合，这是因为累积误差影响了机器人的定位效果，而由于定位偏差的加大，根据机器人位姿及激光数据所构建的地图会出现不闭合的现象。相对而言，图6(d)中机器人移动一圈回到起始位置时检测到了闭环，根据起始位置计算出了当前帧位姿，并将当前帧位姿作为约束，进行全局优化，再通过优化后的所有位姿及所携带的激光数据更新地图信息，消除无法闭合的情况。实验结果表明，本文提出的方法可以有效地检测回环，并优化两点之间的所有位姿，位姿优化之后再根据每一帧位姿所携带的激光数据重新构建地图，成功消除了无法闭合的地图部分。但由于加入了视觉信息，建图过程中对机器人速度限制变大，同时对动态障碍物的抗干扰能力变小，只能在无人时进行建图，且需要避免摄像头面向大面积白墙等无特征物体。
5.总结与展望
本文研究了SLAM中多传感器融合的问题，针对低成本激光雷达噪声大、视觉构建的稀疏特征地图不适用于导航的问题，提出了激光视觉相结合定位建图的方案。通过采用基于稀疏姿态调整的方法，将激光数据和图像信息进行联合优化，有效地提高了定位和建图的精度，并通过视觉词袋模型实现闭环检测，解决了激光闭环检测难实现的问题。但由于加入了视觉信息，系统鲁棒性变差，且室内环境光照变化往往较小，同时存在大量的动态障碍物，如摄像头捕捉到移动的行人时会导致视觉约束的错误，从而影响整体的定位建图效果。同时，当遇到白墙等物体时无法提取足够的特征，视觉约束失效从而导致建图失败。下一步工作将利用激光雷达和视觉各自的优点，提高整体的鲁棒性，保证建图的效果。
