第13卷第1期 智能系统学报 Vol.13 No.I 2018年2月 CAAI Transactions on Intelligent Systems Feb.2018 D0:10.11992/tis.201703006 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.TP.20180131.0858.002.html 视觉同时定位与地图创建综述 周彦，李雅芳，王冬丽，裴廷睿 (湘潭大学信息工程学院.湖南湘潭411105) 摘要：同时定位与地图创建(simultaneous localization and mapping,SLAM)自1986年提出以来一直是机器人领域 的热点问题，被认为是实现真正全自主移动机器人的关键。其目的是让机器人在未知环境下实现自身定位同时创建 出环境地图。视觉SLAM(visual simultaneous localization and mapping,VSLAM)是仅用相机作为传感器的定位与制 图。随着计算机视觉和机器人技术的发展，VSLAM已成为无人系统领域的研究焦点。本文对VSLAM的最新研究 现状进行总结，阐述了VSLAM中的主要问题，分别介绍了VSLAM基于滤波和图优化的实现方法，并探讨了 VSLAM的研究与发展方向。 关键词：计算机视觉：同时定位与地图创建：VSLAM:机器人：滤波：图优化：综述：深度学习 中图分类号：TP24文献标志码：A文章编号：1673-4785(2018)01-0097-10 中文引用格式：周彦，李雅芳，王冬丽，等.视觉同时定位与地图创建综述J.智能系统学报，2018,13(1)：97-106. 英文引用格式：ZHOU Yan,LI Yafang,WANG Dongli,.etal.A survey of VSLAM[J.CAAI transactions on intelligent systems, 2018,13(1:97-106. A survey of VSLAM ZHOU Yan,LI Yafang,WANG Dongli,PEI Tingrui (College of Information Engineering,Xiangtan University,Xiangtan 411105,China) Abstract:Simultaneous localization and mapping(SLAM),an essential task for an autonomy robot,has been a hot top- ic in the field of robotics since the concept first proposed in 1986.The purpose is to make a robot locate itself in an un- known environment while simultaneously construct a map of the environment.Visual SLAM(VSLAM)refers to that one using a camera or cameras as the sole sensor.With the development of computer vision and robotics,VSLAM has become the focus in the field of unmanned systems.In this paper,we survey the recent progress of VSLAM.After identifying the main problems in the development of VSLAM.we introduce the VSLAM methods based on both filter and graph optimizations.Finally,the further study and development directions of VSLAM are given. Keywords:computer vision;simultaneous localization and mapping;VSLAM;robot;filter,graph optimization;survey; deep learning 移动机器人为实现自主导航，面临着在哪里、了机器人领域的热点研究问题。目前，已经有了 到哪里、怎么去3个需要解决的关键问题。“在哪 很多有效方法来解决已知环境中（有环境先验信 里”是机器人对自身的定位，后两个问题即机器人需 息)机器人自主定位与已知机器人位置情况下的地 要解决的路径规划问题。对自主移动机器人来说， 图创建问题。然而在很多环境中，机器人无法利 定位是重中之重，是路径规划的基石。在定位中， 用全局定位系统进行定位，而且事先获取环境先验 机器人首当其冲的任务便是感知周围的环境，并对 信息很困难，甚至是不可能的1，此情此景下，机器 之加以描述。移动机器人的定位和地图创建已成为 人需要在没有环境先验信息的情况下，在移动过程 收稿日期：2017-03-03.网络出版日期：2018-01-31. 基金项目：国家自然科学基金项目(61773330,61372049,61100140， 中一边计算自身位置，一边构建环境地图，于是移 61104210):湖南省自然科学基金项目(2017JJ2253):湖 南省教育厅优秀青年基金项目(17B259), 动机器人的同时定位与地图创建(SLAM0问题 通信作者：周彦.E-mail:yanzhou@xtu.edu.cn. 应运而生
DOI: 10.11992/tis.201703006 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.TP.20180131.0858.002.html 视觉同时定位与地图创建综述 周彦，李雅芳，王冬丽，裴廷睿 （湘潭大学 信息工程学院，湖南 湘潭 411105） 摘 要：同时定位与地图创建 (simultaneous localization and mapping，SLAM) 自 1986 年提出以来一直是机器人领域 的热点问题，被认为是实现真正全自主移动机器人的关键。其目的是让机器人在未知环境下实现自身定位同时创建 出环境地图。视觉 SLAM (visual simultaneous localization and mapping，VSLAM) 是仅用相机作为传感器的定位与制 图。随着计算机视觉和机器人技术的发展，VSLAM 已成为无人系统领域的研究焦点。本文对 VSLAM 的最新研究 现状进行总结，阐述了 VSLAM 中的主要问题，分别介绍了 VSLAM 基于滤波和图优化的实现方法，并探讨了 VSLAM 的研究与发展方向。 关键词：计算机视觉；同时定位与地图创建；VSLAM；机器人；滤波；图优化；综述；深度学习 中图分类号：TP24 文献标志码：A 文章编号：1673−4785(2018)01−0097−10 中文引用格式：周彦, 李雅芳, 王冬丽, 等. 视觉同时定位与地图创建综述[J]. 智能系统学报, 2018, 13(1): 97–106. 英文引用格式：ZHOU Yan, LI Yafang, WANG Dongli, et al. A survey of VSLAM[J]. CAAI transactions on intelligent systems, 2018, 13(1): 97–106. A survey of VSLAM ZHOU Yan，LI Yafang，WANG Dongli，PEI Tingrui (College of Information Engineering, Xiangtan University, Xiangtan 411105, China) Abstract: Simultaneous localization and mapping (SLAM), an essential task for an autonomy robot, has been a hot topic in the field of robotics since the concept first proposed in 1986. The purpose is to make a robot locate itself in an unknown environment while simultaneously construct a map of the environment. Visual SLAM (VSLAM) refers to that one using a camera or cameras as the sole sensor. With the development of computer vision and robotics, VSLAM has become the focus in the field of unmanned systems. In this paper, we survey the recent progress of VSLAM. After identifying the main problems in the development of VSLAM, we introduce the VSLAM methods based on both filter and graph optimizations. Finally, the further study and development directions of VSLAM are given. Keywords: computer vision; simultaneous localization and mapping; VSLAM; robot; filter; graph optimization; survey; deep learning 移动机器人为实现自主导航，面临着在哪里、 到哪里、怎么去 3 个需要解决的关键问题。“在哪 里”是机器人对自身的定位，后两个问题即机器人需 要解决的路径规划问题。对自主移动机器人来说， 定位是重中之重，是路径规划的基石。在定位中， 机器人首当其冲的任务便是感知周围的环境，并对 之加以描述。移动机器人的定位和地图创建已成为 了机器人领域的热点研究问题[1-2]。目前，已经有了 很多有效方法来解决已知环境中（有环境先验信 息）机器人自主定位与已知机器人位置情况下的地 图创建问题[1]。然而在很多环境中，机器人无法利 用全局定位系统进行定位，而且事先获取环境先验 信息很困难，甚至是不可能的[3] ，此情此景下，机器 人需要在没有环境先验信息的情况下，在移动过程 中一边计算自身位置，一边构建环境地图，于是移 动机器人的同时定位与地图创建 (SLAM) 问题 [4-5] 应运而生。 收稿日期：2017−03−03. 网络出版日期：2018−01−31. 基金项目：国家自然科学基金项目 (61773330, 61372049, 61100140, 61104210)；湖南省自然科学基金项目 (2017JJ2253)；湖 南省教育厅优秀青年基金项目 (17B259). 通信作者：周彦. E-mail：yanzhou@xtu.edu.cn. 第 13 卷第 1 期 智 能 系 统 学 报 Vol.13 No.1 2018 年 2 月 CAAI Transactions on Intelligent Systems Feb. 2018
·98· 智能系统学报 第13卷 SLAM也称为CML(concurrent mapping and 似性越高。对于浮点型描述子采用欧氏距离，对于 localization).最先由Smith Self和Cheeseman于 二进制字符型描述子使用汉明距离(Hamming dis- 1986年提出5-6。这一理论是实现真正全自主移动 tance),汉明距离指两个描述子（二进制串）不同位 机器人的关键已经成为共识7-)。SLAM以传感器 数的个数。当特征点数量非常大时，快速近似最近 作为划分标准，主要分为激光、视觉两大类。其中， 邻(FLANN)算法I能够满足SLAM的实时性需求。 激光SLAM研究较早，理论和工程均比较成熟，视 常用的特征提取和匹配算法有SIFT算法、 觉SLAM尚处于实验室研究阶段。SLAM早期研 SURF算法和ORB算法。SIFT算法中使用斑检 究侧重于使用滤波理论来最小化运动物体的位姿和 测方法和浮点型特征描述子，在建立高斯差分空间 地图路标点的噪声。自21世纪以来，学者们借鉴运 金字塔的基础上提取出具有尺度不变性的特征点， 动恢复结构SfM(structure from motion)中的方式例， 然后对特征点邻域内的点的梯度方向进行直方图统 以优化理论为基础求解SLAM问题，该方法通常以 计。特征点的主方向就是直方图中比重最大的方 位姿图的形式描述机器人各时刻的状态，又称为基 向，必要时可选一个辅方向。SFT特征集旋转不变 于图优化的SLAM,在VSLAM领域中取得了主导 性、尺度不变性、对图像变形和光照鲁棒等优点于 地位10-1 一身，不足之处是计算量大，计算速度慢，需要在 1 VSLAM存在的问题分析 GPU加速的情况下才可满足SLAM的实时性需 求。SURF1算法是对SIFT算法的改进，使用基于 1.1特征点提取、描述与匹配 DoH的斑点特征检测方法；在特征点的描述上， 图像的特征一般可划分为点特征、直线特征以 SURF算法通过积分图，利用两个方向上的Harr小 及边缘、轮廓特征，其中线、边缘、轮廓等特征在高 波模板进行梯度计算，然后对邻域内点的梯度方向 维空间进行处理，计算量大；点特征对遮挡相对鲁 以扇形的方式进行统计，得到特征点的主方向。SURF 棒、提取速度快并且识别性好，所以应用较多。局 算法速度快且稳定性好，应用也较为广泛。Ethan- 部特征点不仅能够保留图像重要特征信息，而且也 Rublee在2011年提出的ORBI1算法使用改进的 使得信息的数据量减少，使计算速度和匹配速度都 FAST特征点检测算法，ORB的特征描述子采用改 加快，因此基于特征的VSLAM普遍采用点特征。图1 进后的二进制字符串特征描述子BRIEF。由于采 标出了可作为图像特征的部分。 用速度极快的二进制描述子，ORB使得整个图像特 角点 征提取的环节大大加速。 边缘 1.2特征点深度获取 单日相机无法直接获取深度信息，深度信息通 过反深度法(inverse depth)、三角测量（三角化）、粒 子滤波法等来获取。Civera等提出了反深度法， 该方法旨在减少深度分布非高斯性的影响。反深度 图1可作为图像特征的部分：角点、边缘、斑点 法为获得较好的线性效果，在EKF系统里使用深度 Fig.1 Parts that can be used as image features:corner, edge,blob 的倒数进行更新。三角测量最早由高斯提出，是指 斑点和角点是局部特征点中比较流行的两种。 通过在两个不同地点观察同一个点的夹角，确定出 斑点的重要特征是与周围区域有颜色和灰度上的差 该点的距离（深度）。使用关键帧与稀疏捆集调整 (sparse bundle adjustment,SBA)框架的VSLAM系 别。斑点检测方法应用最广泛的是利用高斯拉普拉 统，如文献[18-21]，均采用了该方法。Davison等2 斯算子检测的方法(LOG),以及利用像素点海森矩 采用的Particle Filter方法会在特征所在的深度方向 阵（二阶微分）及其行列式值的方法(DOH)。角点 上生成多个粒子，通过粒子的匹配、更新来得到特 描述的是两条边的交点，其检测方法常用Harris角 征点深度的概率分布，不足之处是容易增加系统的 点检测算法和FAST角点检测算法。对特征点的描 不一致性，致使最后概率估计发散。 述有浮点型特征描述子和二进制字符串特征描述 双目相机一般由左和右两个水平放置的相机组 子。提取特征点后需对两幅图像进行特征匹配，特 成，通过同步采集到的左右相机的图像，计算图像 征匹配采用计算描述子间距离的方法，距离越小相 之间的视差，来估计每一个像素的深度
SLAM 也称为 CML (concurrent mapping and localization)，最先由 Smith Self 和 Cheeseman 于 1986 年提出[5-6]。这一理论是实现真正全自主移动 机器人的关键已经成为共识[7-8]。SLAM 以传感器 作为划分标准，主要分为激光、视觉两大类。其中， 激光 SLAM 研究较早，理论和工程均比较成熟，视 觉 SLAM 尚处于实验室研究阶段[1]。SLAM 早期研 究侧重于使用滤波理论来最小化运动物体的位姿和 地图路标点的噪声。自 21 世纪以来，学者们借鉴运 动恢复结构 SfM(structure from motion) 中的方式[9] ， 以优化理论为基础求解 SLAM 问题，该方法通常以 位姿图的形式描述机器人各时刻的状态，又称为基 于图优化的 SLAM，在 VSLAM 领域中取得了主导 地位[10-11]。 1 VSLAM 存在的问题分析 1.1 特征点提取、描述与匹配 图像的特征一般可划分为点特征、直线特征以 及边缘、轮廓特征，其中线、边缘、轮廓等特征在高 维空间进行处理，计算量大；点特征对遮挡相对鲁 棒、提取速度快并且识别性好，所以应用较多。局 部特征点不仅能够保留图像重要特征信息，而且也 使得信息的数据量减少，使计算速度和匹配速度都 加快，因此基于特征的 VSLAM 普遍采用点特征。图 1 标出了可作为图像特征的部分。 斑点和角点是局部特征点中比较流行的两种。 斑点的重要特征是与周围区域有颜色和灰度上的差 别。斑点检测方法应用最广泛的是利用高斯拉普拉 斯算子检测的方法（LOG），以及利用像素点海森矩 阵（二阶微分）及其行列式值的方法（DOH）。角点 描述的是两条边的交点，其检测方法常用 Harris 角 点检测算法和 FAST 角点检测算法。对特征点的描 述有浮点型特征描述子和二进制字符串特征描述 子。提取特征点后需对两幅图像进行特征匹配，特 征匹配采用计算描述子间距离的方法，距离越小相 似性越高。对于浮点型描述子采用欧氏距离，对于 二进制字符型描述子使用汉明距离（Hamming distance），汉明距离指两个描述子（二进制串）不同位 数的个数。当特征点数量非常大时，快速近似最近 邻（FLANN）算法[12]能够满足 SLAM 的实时性需求。 常用的特征提取和匹配算法有 SIFT 算法、 SURF 算法和 ORB 算法。SIFT[13]算法中使用斑检 测方法和浮点型特征描述子，在建立高斯差分空间 金字塔的基础上提取出具有尺度不变性的特征点， 然后对特征点邻域内的点的梯度方向进行直方图统 计。特征点的主方向就是直方图中比重最大的方 向，必要时可选一个辅方向。SIFT 特征集旋转不变 性、尺度不变性、对图像变形和光照鲁棒等优点于 一身，不足之处是计算量大，计算速度慢，需要在 GPU 加速的情况下才可满足 SLAM 的实时性需 求。SURF[14]算法是对 SIFT 算法的改进，使用基于 DoH 的斑点特征检测方法；在特征点的描述上， SURF 算法通过积分图，利用两个方向上的 Harr 小 波模板进行梯度计算，然后对邻域内点的梯度方向 以扇形的方式进行统计，得到特征点的主方向。SURF 算法速度快且稳定性好，应用也较为广泛。EthanRublee 在 2011 年提出的 ORB[15]算法使用改进的 FAST 特征点检测算法，ORB 的特征描述子采用改 进后的二进制字符串特征描述子 BRIEF[16]。由于采 用速度极快的二进制描述子，ORB 使得整个图像特 征提取的环节大大加速。 1.2 特征点深度获取 单目相机无法直接获取深度信息，深度信息通 过反深度法（inverse depth）、三角测量（三角化）、粒 子滤波法等来获取。Civera 等 [17]提出了反深度法， 该方法旨在减少深度分布非高斯性的影响。反深度 法为获得较好的线性效果，在 EKF 系统里使用深度 的倒数进行更新。三角测量最早由高斯提出，是指 通过在两个不同地点观察同一个点的夹角，确定出 该点的距离（深度）。使用关键帧与稀疏捆集调整 （sparse bundle adjustment，SBA）框架的 VSLAM 系 统，如文献[18-21]，均采用了该方法。Davison 等 [22] 采用的 Particle Filter 方法会在特征所在的深度方向 上生成多个粒子, 通过粒子的匹配、更新来得到特 征点深度的概率分布，不足之处是容易增加系统的 不一致性，致使最后概率估计发散。 双目相机一般由左和右两个水平放置的相机组 成，通过同步采集到的左右相机的图像，计算图像 之间的视差，来估计每一个像素的深度。 㻾◥ 䓥㑄 ᪽◥ 图 1 可作为图像特征的部分：角点、边缘、斑点 Fig. 1 Parts that can be used as image features: corner, edge, blob ·98· 智 能 系 统 学 报 第 13 卷
第1期 周彦，等：视觉同时定位与地图创建综述 ·99· 图2中，O、OR为左右相机的光圈中心，黑色 境中的同一物体。在大方向上，特征匹配解决了 框为成像平面，∫为焦距，M、为成像的平面坐标， SLAM中的数据关联问题，但这个过程中带有误 R为负数。根据几何关系，由相似三角形P-P- 差，所以对图像特征匹配的结果优化是必要的，主 P和P-O-OR得 要方法有固定区域匹配、Active Matching、l-Point z-f b-u+ug RANAC、几何约束等。 b PTAM(parallel tracking and mapping)W及其改 整理得 进算法主要使用固定区域匹配的方法。PTAM假定 fb Z=- -,d =u-ug 前后两帧图像中像素距离在一个阈值内，超出这个 式中d为P在左眼相机图像和右眼相机图像中的 阈值就认为是错误匹配，该法适用于特征点距离相 横坐标之差，叫做视差。根据视差就可以估计一个 机稍远、深度变化不大的场合，不适用于相机快速 像素离相机的距离。 运动的场合。 基于EKF滤波的VSLAM系统多采用Davis-. on提出的Active Matching2a方法。Active Match- ing方法中，在使用EKF系统运动模型获得系统状 态预测的基础上，估计环境中的特征点在相机中的 投影位置，再进一步处理即可得到图像中特征点的 左眼像素 右眼像素 分布区域。此方法对相机的绝大部分运动情况鲁 01 09 棒，但如果出现相机姿态估计协方差较大的情况， 52 左眼相机 右眼相机 几何模型 易产生大的特征匹配估计区域，可能匹配错误。 图2双目相机模型 为去除Active Matching中的错误匹配，Civera Fig.2 Binocular camera model Grasa等提出1-Point RANSAC2方法。该方法用 深度相机主动测量每个像素的深度直接获取深 随机选择的一个匹配点的匹配信息来更新相机姿 度信息P。目前的RGB-D相机按原理可分为两大 态，之后计算其他匹配点与估计图像位置的距离， 类，即通过红外结构光(structured light)来测量像素 并判断这个距离是否在一定的阈值范围内，若不 距离和通过飞行时间法(time of flight,.ToF)测量像 在，被认为是外点并剔除它，最后利用得到的内点 素距离。在结构光原理中，相机向探测目标发射一 集来更新整个滤波器状态。该方法主要应用在基 于EKF滤波的SLAM系统中，由于频繁地更新系 束光线（通常是红外光），根据返回的结构光图案， 统状态，运算时间代价比较大。 计算像素离自身的距离。在ToF中，相机向目标发 射脉冲光，然后根据发送到返回之间的光束飞行时 几何约束方法利用PNP(perspective N points)P阿 对极几何2等剔除误匹配点。该方法因利用几何求 间，确定物体离自身的距离。在测量深度之后， 解，不需要频繁更新系统状态，故而能获得较好的 RGB-D相机完成深度与彩色图像像素之间的配对， 系统运行速度。但是对于不同的情况该方法需要具 输出一一对应的彩色图和深度图。图3是RGB-D 体问题具体分析，使用相对应的几何约束条件，相 相机的原理图。 应地增加了系统的复杂性。 结构光原理 飞行时间原理 1.4累积误差 SLAM中的误差来源主要为里程计误差、观测 发射 返回 发射 返回 误差和错误的数据关联带来的误差3个方面。在 VSLAM中，环境的先验信息和机器人的位置都是 时间差 未知的，位置误差（视觉里程计误差）不能根据环境 结构光发射器结构光接收器 脉冲光发射器 脉冲光接收器 先验信息得到有效纠正，故而随着机器人运动距离 的增大位置误差也逐渐累积。位置误差的增大会造 图3RGB-D相机原理图 成错误的数据关联，相应的特征标志的误差也跟着 Fig.3 Schematic of RGB-D camera 增大：反过来，机器人的位置误差因为参考了有误 1.3数据关联的优化问题 差的特征也会增大。因此，里程计误差与特征标志 SLAM中数据关联是对两个路标(VSLAM中 之间相互影响使整个VSLAM系统产生累积误差， 路标指图像特征)进行匹配，确定它们是否对应环 无法保证地图和轨迹的全局一致性。图4中，累积误
图 2 中，OL、OR 为左右相机的光圈中心，黑色 框为成像平面，f 为焦距，uL、uR 为成像的平面坐标， uR 为负数。根据几何关系，由相似三角形 P-PLPR 和 P-OL-OR, 得 z− f z = b−uL +uR b 整理得 z = f b d ,d = uL −uR 式中 d 为 P 在左眼相机图像和右眼相机图像中的 横坐标之差，叫做视差。根据视差就可以估计一个 像素离相机的距离 z。 深度相机主动测量每个像素的深度直接获取深 度信息[23]。目前的 RGB-D 相机按原理可分为两大 类，即通过红外结构光（structured light）来测量像素 距离和通过飞行时间法（time of flight, ToF）测量像 素距离。在结构光原理中，相机向探测目标发射一 束光线（通常是红外光），根据返回的结构光图案， 计算像素离自身的距离。在 ToF 中，相机向目标发 射脉冲光，然后根据发送到返回之间的光束飞行时 间，确定物体离自身的距离。在测量深度之后， RGB-D 相机完成深度与彩色图像像素之间的配对， 输出一一对应的彩色图和深度图。图 3 是 RGB-D 相机的原理图。 1.3 数据关联的优化问题 SLAM 中数据关联是对两个路标（VSLAM 中 路标指图像特征）进行匹配，确定它们是否对应环 境中的同一物体。在大方向上，特征匹配解决了 SLAM 中的数据关联问题，但这个过程中带有误 差，所以对图像特征匹配的结果优化是必要的，主 要方法有固定区域匹配、Active Matching、1-Point RANAC、几何约束等。 PTAM(parallel tracking and mapping) [18] 及其改 进算法主要使用固定区域匹配的方法。PTAM 假定 前后两帧图像中像素距离在一个阈值内，超出这个 阈值就认为是错误匹配，该法适用于特征点距离相 机稍远、深度变化不大的场合，不适用于相机快速 运动的场合。 基于 EKF 滤波的 VSLAM 系统多采用 Davison 提出的 Active Matching[24]方法。Active Matching 方法中，在使用 EKF 系统运动模型获得系统状 态预测的基础上，估计环境中的特征点在相机中的 投影位置，再进一步处理即可得到图像中特征点的 分布区域。此方法对相机的绝大部分运动情况鲁 棒，但如果出现相机姿态估计协方差较大的情况， 易产生大的特征匹配估计区域，可能匹配错误。 为去除 Active Matching 中的错误匹配，Civera、 Grasa 等提出 1-Point RANSAC[25-26]方法。该方法用 随机选择的一个匹配点的匹配信息来更新相机姿 态，之后计算其他匹配点与估计图像位置的距离， 并判断这个距离是否在一定的阈值范围内，若不 在，被认为是外点并剔除它，最后利用得到的内点 集来更新整个滤波器状态。该方法主要应用在基 于 EKF 滤波的 SLAM 系统中，由于频繁地更新系 统状态，运算时间代价比较大。 几何约束方法利用 PNP（perspective N points） [27] 、 对极几何[28]等剔除误匹配点。该方法因利用几何求 解，不需要频繁更新系统状态，故而能获得较好的 系统运行速度。但是对于不同的情况该方法需要具 体问题具体分析，使用相对应的几何约束条件，相 应地增加了系统的复杂性。 1.4 累积误差 SLAM 中的误差来源主要为里程计误差、观测 误差和错误的数据关联带来的误差 3 个方面。在 VSLAM 中，环境的先验信息和机器人的位置都是 未知的，位置误差（视觉里程计误差）不能根据环境 先验信息得到有效纠正，故而随着机器人运动距离 的增大位置误差也逐渐累积。位置误差的增大会造 成错误的数据关联，相应的特征标志的误差也跟着 增大；反过来，机器人的位置误差因为参考了有误 差的特征也会增大。因此，里程计误差与特征标志 之间相互影响使整个 VSLAM 系统产生累积误差， 无法保证地图和轨迹的全局一致性。图 4 中，累积误 P ጒⱨ㉌ टⱨ㉌ ദ㏫ z P PL b PR f OL OR uL −uR ጒⱨⰤᱦ टⱨⰤᱦ ܌ҁὍಷ 图 2 双目相机模型 Fig. 2 Binocular camera model ⤲࣋上㵸ᬢ䬠⤲ ࣋اٴᲰgal ࣽᄰ 䔀ఊ ࣽᄰ 䔀ఊ ㏿Ჰࣽاٴᄰஔ ㏿Ჰاٴᣑᩢஔ 㘵۞ࣽاٴᄰஔ 㘵۞اٴᣑᩢஔ ᬢ䬠ጚ 图 3 RGB-D 相机原理图 Fig. 3 Schematic of RGB-D camera 第 1 期 周彦，等：视觉同时定位与地图创建综述 ·99·
·100· 智能系统学报 第13卷 差使得估计轨迹和真实轨迹相差很大。当前VSLAM 2.1基于滤波器的实现方法 系统多采用回环检测的方式减小这一误差。回环检 2.l.1基于扩展卡尔曼滤波器(extended kalman 测是指机器人识别出曾经到达过的场景的能力，当 filter,EKF)EKF-VSLAM 机器人看到两张相似图片时，计算图像数据的相似 21世纪之前，SLAM中的状态估计主要使用 性，如果回环检测成功，可以显著地减小累积误 滤波的方法。在SLAM中，系统的状态由机器人的 差。回环检测在VSLAM中意义重大，既关系到估 位姿和地图信息（路标）组成。用卡尔曼滤波器 计的地图和轨迹在长时间下的正确性，也可在跟丢 (KF)实现SLAM必须遵循运动方程和观测方程都 时进行重定位，大大增强了系统的鲁棒性。 符合线性高斯模型、系统的状态服从高斯分布这两 个假设。基于KF的SLAM由系统状态预测和更新 两步组成，与此同时，对地图进行加入新路标、删除 旧路标等操作。KF中，假设系统都是线性的，但是 现实中，机器人的运动模型与观测模型往往都是非 线性的。对此，通常采用一阶泰勒展开来近似表示 (a)真实轨迹 (b)出现累积误差的轨迹 非线性模型的扩展卡尔曼滤波器(extended Kalman 图4真实轨迹与出现累积误差的轨迹 filter,EKF)方法来实现SLAM。 Fig.4 Real track and track with accumulated error 卡尔曼滤波器是实现SLAM的基本方法之一网 2 VSLAM实现方法 其协方差矩阵描述了机器人的位置和地图的不确定 信息。当机器人连续观测到环境中的特征标志时， VSLAM的实现方法分为基于滤波器的方法和 所有协方差矩阵子阵的行列式呈单调递减。每一时 基于图优化的方法。其中，基于滤波器的方法只估 刻机器人能观测到路标不会很多，只有少数几个。 计当前时刻的位姿，是一种增量式算法：基于图优 基于卡尔曼滤波器的SLAM的时间复杂度为 化的方法根据所有观测到的信息，对整个机器人运 O(n,n表示地图中的特征标志数B0。为了达到降 动轨迹进行估计。前者又称为在线SLAM,后者又 低SLAM的时间复杂度的目的，Leonard等提出 称为全SLAM(FULL SLAM)。表1给出了常用的 了DSM(decoupled stochastic mapping)方法， 开源VSLAM方案，其中有使用滤波方法的，也有 DSM中机器人位置估计被各子地图分别保存，当机 使用优化方法的，本文2.1和2.2节将对典型方案 器人从1个子地图运动到另1个子地图时，将前 详述。 1个子地图的信息以EKF的方式传送给后1个子 表1常用开源VSLAM方案 地图。Williams等提出的基于CLSF(constrained Table 1 Commonly used open source VSLAM solutions local submap filter)的SLAM方法涉及全局坐标已 方案名称 传感器形式 地址 知的子地图，首先构建出这些子地图，然后机器人 MonoSLAM 单目 https://github.com/hanmekim/Sc 运动过程中只利用观测信息更新自身位置和局部子 eneLib2 地图中的特征标志，并且在时效范围内向全局地图 PTAM 单目 http://www.robots.ox.ac.uk/-gk/ 传递局部子地图信息。Guivant等提出了1种没 PTAM/ ORB-SLAM 单目为主htp://webdiis.unizar..es/-raulmu 有任何信息丢失的SLAM优化算法CEKF(com- r/orbslam/ pressed extended Kalman filter))。在CEKF中，已观 LSD-SLAM 单目为主http:/vision.in.tum.de/research/ 测到的地图路标一分为二分成A与B两部分，比较 vslam/Isdslam 特别的是，用A来记录活动子地图（机器人当前位 SVO 单目 https://github.com/uzh- rpg/rpg_svo 置的邻域)。当机器人在A中运动时，机器人的位 DTAM RGBD https://github.com/anuranbaka/ 置与地图A通过观测信息得到实时更新，与此同 OpenDTAM 时，地图B受到子观测信息的影响被递归地记录； DVO RGBD https://github.com/tum- vision/dvo slam 当机器人运动到A的区域之外时，观测信息被传送 RTAB-MAP 双目/RGBD https:/github.com/introlab/tab 给子地图B,地图B进行一次性更新，新的活动子 map 地图同时被创建。 RGBD-SLAM-V2 RGBD https://github.com/felixendres/rg bdslam v2 为了降低SLAM的时间复杂度，Thrun等也 Elastic Fusion 单目 https://github.com/mp3guy/Elast 提出去相关的方法，即基于稀疏信息滤波器(sparse icFusion extended information filter,.SElF)的SLAM方法，该
差使得估计轨迹和真实轨迹相差很大。当前 VSLAM 系统多采用回环检测的方式减小这一误差。回环检 测是指机器人识别出曾经到达过的场景的能力，当 机器人看到两张相似图片时，计算图像数据的相似 性，如果回环检测成功，可以显著地减小累积误 差。回环检测在 VSLAM 中意义重大，既关系到估 计的地图和轨迹在长时间下的正确性，也可在跟丢 时进行重定位，大大增强了系统的鲁棒性。 2 VSLAM 实现方法 VSLAM 的实现方法分为基于滤波器的方法和 基于图优化的方法。其中，基于滤波器的方法只估 计当前时刻的位姿，是一种增量式算法；基于图优 化的方法根据所有观测到的信息，对整个机器人运 动轨迹进行估计。前者又称为在线 SLAM，后者又 称为全 SLAM（FULL SLAM）。表 1 给出了常用的 开源 VSLAM 方案，其中有使用滤波方法的，也有 使用优化方法的，本文 2.1 和 2.2 节将对典型方案 详述。 2.1 基于滤波器的实现方法 2.1.1 基于扩展卡尔曼滤波器（extended kalman filter, EKF）的 EKF-VSLAM 21 世纪之前， SLAM 中的状态估计主要使用 滤波的方法。在 SLAM 中，系统的状态由机器人的 位姿和地图信息 (路标) 组成。用卡尔曼滤波器 (KF) 实现 SLAM 必须遵循运动方程和观测方程都 符合线性高斯模型、系统的状态服从高斯分布这两 个假设。基于 KF 的 SLAM 由系统状态预测和更新 两步组成，与此同时，对地图进行加入新路标、删除 旧路标等操作。KF 中，假设系统都是线性的，但是 现实中，机器人的运动模型与观测模型往往都是非 线性的。对此，通常采用一阶泰勒展开来近似表示 非线性模型的扩展卡尔曼滤波器 (extended Kalman filter，EKF) 方法来实现 SLAM。 卡尔曼滤波器是实现 SLAM 的基本方法之一[29]。 其协方差矩阵描述了机器人的位置和地图的不确定 信息。当机器人连续观测到环境中的特征标志时， 所有协方差矩阵子阵的行列式呈单调递减。每一时 刻机器人能观测到路标不会很多，只有少数几个。 基于卡尔曼滤波器的 SLAM 的时间复杂度为 O(n 2 )，n 表示地图中的特征标志数[30]。为了达到降 低 SLAM 的时间复杂度的目的，Leonard 等 [31]提出 了 DSM (decoupled stochastic mapping) 方法。 DSM 中机器人位置估计被各子地图分别保存，当机 器人从 1 个子地图运动到另 1 个子地图时，将前 1 个子地图的信息以 EKF 的方式传送给后 1 个子 地图。Williams 等 [32]提出的基于 CLSF (constrained local submap filter) 的 SLAM 方法涉及全局坐标已 知的子地图，首先构建出这些子地图，然后机器人 运动过程中只利用观测信息更新自身位置和局部子 地图中的特征标志，并且在时效范围内向全局地图 传递局部子地图信息。Guivant 等 [33]提出了 1 种没 有任何信息丢失的 SLAM 优化算法 CEKF ( compressed extended Kalman filter)。在 CEKF 中，已观 测到的地图路标一分为二分成 A 与 B 两部分，比较 特别的是，用 A 来记录活动子地图 (机器人当前位 置的邻域)。当机器人在 A 中运动时，机器人的位 置与地图 A 通过观测信息得到实时更新，与此同 时，地图 B 受到子观测信息的影响被递归地记录； 当机器人运动到 A 的区域之外时，观测信息被传送 给子地图 B，地图 B 进行一次性更新，新的活动子 地图同时被创建。 为了降低 SLAM 的时间复杂度，Thrun 等 [34]也 提出去相关的方法，即基于稀疏信息滤波器 (sparse extended information filter，SEIF) 的 SLAM 方法，该 表 1 常用开源 VSLAM 方案 Table 1 Commonly used open source VSLAM solutions 方案名称 传感器形式 地址 MonoSLAM 单目 https://github.com/hanmekim/Sc eneLib2 PTAM 单目 http://www.robots.ox.ac.uk/~gk/ PTAM/ ORB-SLAM 单目为主 http://webdiis.unizar.es/~raulmu r/orbslam/ LSD-SLAM 单目为主 http://vision.in.tum.de/research/ vslam/lsdslam SVO 单目 https://github.com/uzhrpg/rpg_svo DTAM RGBD https://github.com/anuranbaka/ OpenDTAM DVO RGBD https://github.com/tumvision/dvo_slam RTAB-MAP 双目/RGBD https://github.com/introlab/rtab map RGBD-SLAM-V2 RGBD https://github.com/felixendres/rg bdslam_v2 Elastic Fusion 单目 https://github.com/mp3guy/Elast icFusion (a) ⱋ䒔䔥 (b) ܦ⣜㉛⼛䄛ጚ⮰䒔䔥 图 4 真实轨迹与出现累积误差的轨迹 Fig. 4 Real track and track with accumulated error ·100· 智 能 系 统 学 报 第 13 卷
第1期 周彦，等：视觉同时定位与地图创建综述 ·101· 方法中，只对约束关系进行局部更新，这种局部更 线性优化的方法（现代SLAM系统）可以取得更好 新使得信息矩阵近似于系数矩阵，有效降低 的效果1o SLAM的时间复杂度。 另外，在2011年滤波方面出现了基于RFS Davisonl3s于2007年提出的MonoSLAM,是第 (random finite set)的方法B。RFS是滤波中新兴的 一个基于EKF方法实时的单目VSLAM系统，虽然 潮流B8),RFS是以集合为元素的集合，此集合中的 初步解决了实时的问题，能够在线创建稀疏地图， 元素及元素个数都是随机变量。文献38]对环境地 漂移多少仍然不能确定，目前已经停止对其的开发。 图和传感器观测信息用RFS建模，构造联合目标状 图S是基于EKF的单目VSLAM流程图。 态变量的RFS。依据贝叶斯滤波框架，利用概率假 设密度滤波(probability hypothesis density,.PHD)B9 传感器信息 实现对机器人位姿和环境地图同时估计。该算法避 免了数据关联的问题，相对于EKF和P℉能更有效 地表达SLAM问题。 开始/ 运动模型 EKF 待征 新特 2.2现代SLAM系统：基于非线性优化的方法 EKF预测 更新 转换 征点 现代S1AM系统分为两个部分：前端和后端。 前端提取传感器数据构建模型用于状态估计，后 数据 测量 6维特征 端根据前端提供的数据进行优化。这个架构如图6 关联 模型 到3维特征 所示。 前端 后端 视 特征点 传感器数据 图像 初始化 特征提取 致据关联 优化 图5基于EKF的单目VSLAM流程图 短期（跟踪 Fig.5 Flowchart of EKF-based monocular VSLAM 长期（回环 2.1.2基于粒子滤波器的FastSLAM M.Montemerlo等B6-3提出了1种基于粒子滤 波器(particle filter,PF)的FastSLAM方法。Fast- 图6典型SLAM系统 Fig.6 Typical SLAM system SLAM包含了机器人定位和特征标志位置估计两个 当前SLAM事实标准形成来源于Lu和Milios 过程。粒子滤波器法中机器人可能的运动路径用粒 它是Gutmann和Konoligel研究的后续。典型的 子表示，1个粒子对应着1种可能，每条路径的好坏 SLAM系统如图6所示，前端进行特征提取、数据 由利用观测信息计算得到的粒子权重来评价。对于 关联和初值优化。前端的数据关联模块包括1个短 每个粒子来说机器人的运动路径是确定的，故特征 标志之间相互独立且其观测信息只与机器人的位姿 期（局部）数据关联模块和1个长期（回环）数据关 联模块。通常意义下的数据关联问题在SLAM中 有关。FastSLAM的时间复杂度为O(kn),其中 k为粒子个数0。用树形数据结构优化后的时间复 是指递增定位与建图过程中如何确定当前连续的传 杂度可以达到O(k log n)B0。FastSLAM能够比较 感器观测之间或者当前时刻的观测与最近所创建的 好地表示机器人的非线性、非高斯运动模型。 局部地图中特征间的关联关系，这也称为短期（局 EKF存在非线性误差，且需要存储、维护和更 部)数据关联；回环检测中的数据关联研究机器人 新状态量的均值和方差。如果把路标也加入状态的 沿不同的路径回到某一循环的起点时，如何确定当 话，由于V SLAM中路标数量很大，这个存储量是 前创建的局部地图中的特征与以前所创建的循环起 相当大的，且与状态量呈平方增长（因为要存储协 点处地图中的特征间的关联关系，这称为长期（回 方差矩阵)。因此，EKF普遍被认为不适用于大型 环)数据关联。短期的数据关联模块负责关联传感 场景。P℉采样所需的粒子数量，随维度的增加呈指 器中连续的观测值对应的特征：得到1帧图像数据 数增长，所以仅限于低维的问题，对高维问题不适 后，对其进行预处理，筛选出关键帧，对图像进行特 用。除此之外，滤波器方法在一定程度上假设了马 征提取、匹配以及运动求解并得到局部地图，也就 尔可夫性，如果当前帧与很久之前的帧有关（例如 是视觉里程计(visual odometry,VO);长期的数据 回环)，那么滤波器就会难以处理这种情况。因为 关联负责将新的观测值关联到旧的路标上，也就是 滤波这些明显的缺点，在同等计算量的情况下，非 回环(loop closure)
方法中，只对约束关系进行局部更新，这种局部更 新使得信息矩阵近似于系数矩阵，有效降 低 SLAM 的时间复杂度。 Davison[35]于 2007 年提出的 MonoSLAM，是第 一个基于 EKF 方法实时的单目 VSLAM 系统，虽然 初步解决了实时的问题，能够在线创建稀疏地图， 漂移多少仍然不能确定，目前已经停止对其的开发。 图 5 是基于 EKF 的单目 VSLAM 流程图。 2.1.2 基于粒子滤波器的 FastSLAM M. Montemerlo 等 [36-37]提出了 1 种基于粒子滤 波器 (particle filter, PF) 的 FastSLAM 方法。 FastSLAM 包含了机器人定位和特征标志位置估计两个 过程。粒子滤波器法中机器人可能的运动路径用粒 子表示，1 个粒子对应着 1 种可能，每条路径的好坏 由利用观测信息计算得到的粒子权重来评价。对于 每个粒子来说机器人的运动路径是确定的，故特征 标志之间相互独立且其观测信息只与机器人的位姿 有关。FastSLAM 的时间复杂度为 O (kn) ，其中 k 为粒子个数[30]。用树形数据结构优化后的时间复 杂度可以达到 O (k log n) [30]。 FastSLAM 能够比较 好地表示机器人的非线性 、非高斯运动模型。 EKF 存在非线性误差，且需要存储、维护和更 新状态量的均值和方差。如果把路标也加入状态的 话，由于 V SLAM 中路标数量很大，这个存储量是 相当大的，且与状态量呈平方增长（因为要存储协 方差矩阵）。因此，EKF 普遍被认为不适用于大型 场景。PF 采样所需的粒子数量，随维度的增加呈指 数增长，所以仅限于低维的问题，对高维问题不适 用。除此之外，滤波器方法在一定程度上假设了马 尔可夫性，如果当前帧与很久之前的帧有关（例如 回环），那么滤波器就会难以处理这种情况。因为 滤波这些明显的缺点，在同等计算量的情况下，非 线性优化的方法（现代 SLAM 系统）可以取得更好 的效果[10]。 另外，在 2011 年滤波方面出现了基于 RFS (random finite set) 的方法[38]。RFS 是滤波中新兴的 潮流[38] ，RFS 是以集合为元素的集合，此集合中的 元素及元素个数都是随机变量。文献[38]对环境地 图和传感器观测信息用 RFS 建模，构造联合目标状 态变量的 RFS。依据贝叶斯滤波框架，利用概率假 设密度滤波（probability hypothesis density, PHD） [39] 实现对机器人位姿和环境地图同时估计。该算法避 免了数据关联的问题，相对于 EKF 和 PF 能更有效 地表达 SLAM 问题。 2.2 现代 SLAM 系统：基于非线性优化的方法 现代 SlAM 系统分为两个部分：前端和后端[1]。 前端提取传感器数据构建模型用于状态估计，后 端根据前端提供的数据进行优化。这个架构如图 6 所示。 当前 SLAM 事实标准形成来源于 Lu 和 Milios[40] ， 它是 Gutmann 和 Konolige[41]研究的后续。典型的 SLAM 系统如图 6 所示，前端进行特征提取、数据 关联和初值优化。前端的数据关联模块包括 1 个短 期（局部）数据关联模块和 1 个长期 (回环) 数据关 联模块。通常意义下的数据关联问题在 SLAM 中 是指递增定位与建图过程中如何确定当前连续的传 感器观测之间或者当前时刻的观测与最近所创建的 局部地图中特征间的关联关系，这也称为短期（局 部）数据关联；回环检测中的数据关联研究机器人 沿不同的路径回到某一循环的起点时，如何确定当 前创建的局部地图中的特征与以前所创建的循环起 点处地图中的特征间的关联关系，这称为长期（回 环）数据关联。短期的数据关联模块负责关联传感 器中连续的观测值对应的特征：得到 1 帧图像数据 后，对其进行预处理，筛选出关键帧，对图像进行特 征提取、匹配以及运动求解并得到局部地图，也就 是视觉里程计（visual odometry，VO）；长期的数据 关联负责将新的观测值关联到旧的路标上，也就是 回环（loop closure）。 ᐬ/ ֈ₎ ьᙋஔԍᖛ 䓼ߔὍಷ EKF 䶰≷ ᢚ 㖀ڟ EKF ᰠ ≷䛻 Ὅಷ 6 㐠➥ᒭ ݜ3 㐠➥ᒭ ➥ᒭ◥ ࡂ݉ ➥ᒭ 䒘ᢎ ➥ ᒭ◥ 㻲䶽 ప N N Y Y 图 5 基于 EKF 的单目 VSLAM 流程图 Fig. 5 Flowchart of EKF-based monocular VSLAM ьᙋஔᢚ ࡂф 〚ऺ 〚ݹ ➥ᒭं ᢚڟ㖀 ⴙ(䌋䍖) 䪫(ఊ⣛) 图 6 典型 SLAM 系统 Fig. 6 Typical SLAM system 第 1 期 周彦，等：视觉同时定位与地图创建综述 ·101·