大牛讲堂｜SLAM第一篇：基础知识 - 推酷
大牛讲堂｜SLAM第一篇：基础知识
雷锋网 (搜索“雷锋网”公众号关注) 注：本文作者高翔博士。Simultaneous Localization and Mapping，即时定位与地图构建技术。无论在室内、野外、空中还是水下，SLAM是机器人进入未知环境遇到的第一个问题。本期将给大家介绍SLAM的基础知识：传感器与视觉SLAM框架。
传感器与视觉SLAM框架
近来年，智能机器人技术在世界范围内得到了大力发展。人们致力于把机器人用于实际场景：从室内的移动机器人，到野外的自动驾驶汽车、空中的无人机、水下环境的探测机器人等等，均得到了广泛的关注。
在大多数场合中，我们研究机器人会碰到一个基础性的困难，那就是定位和建图，也就是所谓的SLAM技术。没有准确的定位与地图，扫地机就无法在房间自主地移动，只能随机乱碰；家用机器人就无法按照指令准确到达某个房间。此外，在虚拟现实（Virtual Reality）和增强现实技术（Argument Reality）中，没有SLAM提供的定位，用户就无法在场景中漫游。在这几个应用领域中，人们需要SLAM向应用层提供空间定位的信息，并利用SLAM的地图完成地图的构建或场景的生成。
当我们谈论SLAM时，最先问到的就是传感器。 SLAM的实现方式与难度和传感器的形式与安装方式密切相关。 传感器分为激光和视觉两大类 ，视觉下面又分三小方向。下面就带你认识这个庞大家族中每个成员的特性。
1. 传感器之激光雷达
激光雷达是最古老，研究也最多的SLAM传感器。它们提供机器人本体与周围环境障碍物间的距离信息。常见的激光雷达，例如SICK、Velodyne还有我们国产的rplidar等，都可以拿来做SLAM。激光雷达能以很高精度测出机器人周围障碍点的角度和距离，从而很方便地实现SLAM、避障等功能。
主流的2D激光传感器扫描一个平面内的障碍物，适用于平面运动的机器人（如扫地机等）进行定位，并建立2D的栅格地图。这种地图在机器人导航中很实用，因为多数机器人还不能在空中飞行或走上台阶，仍限于地面。在SLAM研究史上，早期SLAM研究几乎全使用激光传感器进行建图，且多数使用滤波器方法，例如卡尔曼滤波器与粒子滤波器等。
激光的优点是精度很高，速度快，计算量也不大，容易做成实时SLAM。 缺点是价格昂贵 ，一台激光动辄上万元，会大幅提高一个机器人的成本。因此激光的研究主要集中于如何降低传感器的成本上。对应于激光的EKF-SLAM理论方面，因为研究较早，现在已经非常成熟。与此同时，人们也对EKF-SLAM的缺点也有较清楚的认识，例如不易表示回环、线性化误差严重、必须维护路标点的协方差矩阵，导致一定的空间与时间的开销，等等。
2. 传感器之视觉SLAM
视觉SLAM是21世纪SLAM研究热点之一，一方面是因为视觉十分直观，不免令人觉得：为何人能通过眼睛认路，机器人就不行呢？另一方面，由于CPU、GPU处理速度的增长，使得许多以前被认为无法实时化的视觉算法，得以在10 Hz以上的速度运行。硬件的提高也促进了视觉SLAM的发展。
以传感器而论，视觉SLAM研究主要分为三大类：单目、双目（或多目）、RGBD。其余还有鱼眼、全景等特殊相机，但是在研究和产品中都属于少数。此外，结合惯性测量器件（Inertial Measurement Unit，IMU）的视觉SLAM也是现在研究热点之一。 就实现难度而言，我们可以大致将这三类方法排序为：单目视觉&双目视觉&RGBD。
单目相机SLAM简称MonoSLAM，即只用一支摄像头就可以完成SLAM。这样做的好处是传感器特别的简单、成本特别的低，所以单目SLAM非常受研究者关注。相比别的视觉传感器， 单目有个最大的问题，就是没法确切地得到深度。这是一把双刃剑。
一方面，由于绝对深度未知，单目SLAM没法得到机器人运动轨迹以及地图的真实大小。直观地说，如果把轨迹和房间同时放大两倍，单目看到的像是一样的。因此，单目SLAM只能估计一个相对深度，在相似变换空间Sim(3)中求解，而非传统的欧氏空间SE(3)。如果我们必须要在SE(3)中求解，则需要用一些外部的手段，例如GPS、IMU等传感器，确定轨迹与地图的尺度（Scale）。
另一方面，单目相机无法依靠一张图像获得图像中物体离自己的相对距离。为了估计这个相对深度，单目SLAM要靠运动中的三角测量，来求解相机运动并估计像素的空间位置。即是说，它的轨迹和地图，只有在相机运动之后才能收敛，如果相机不进行运动时，就无法得知像素的位置。同时，相机运动还不能是纯粹的旋转，这就给单目SLAM的应用带来了一些麻烦，好在日常使用SLAM时，相机都会发生旋转和平移。不过，无法确定深度同时也有一个好处：它使得单目SLAM不受环境大小的影响，因此既可以用于室内，又可以用于室外。
相比于单目，双目相机通过多个相机之间的基线，估计空间点的位置。与单目不同的是，立体视觉既可以在运动时估计深度，亦可在静止时估计，消除了单目视觉的许多麻烦。不过，双目或多目相机配置与标定均较为复杂，其深度量程也随双目的基线与分辨率限制。 通过双目图像计算像素距离，是一件非常消耗计算量的事情，现在多用FPGA来完成。
RGBD相机是2010年左右开始兴起的一种相机，它最大的特点是可以通过红外结构光或Time-of-Flight原理，直接测出图像中各像素离相机的距离。因此，它比传统相机能够提供更丰富的信息，也不必像单目或双目那样费时费力地计算深度。目前常用的RGBD相机包括Kinect/Kinect V2、Xtion等。不过，现在多数RGBD相机还存在测量范围窄、噪声大、视野小等诸多问题。出于量程的限制，主要用于室内SLAM。
视觉SLAM框架
视觉SLAM几乎都有一个基本的框架&。 一个SLAM系统分为四个模块（除去传感器数据读取）：VO、后端、建图、回环检测。 这里我们简要介绍各模块的涵义，之后再详细介绍其使用方法。
SLAM框架之视觉里程计
Visual Odometry，即视觉里程计。它估计两个时刻机器人的相对运动（Ego-motion）。在激光SLAM中，我们可以将当前的观测与全局地图进行匹配，用ICP求解相对运动。而对于相机，它在欧氏空间里运动，我们经常需要估计一个三维空间的变换矩阵——SE3或Sim3（单目情形）。 求解这个矩阵是VO的核心问题，而求解的思路，则分为基于特征的思路和不使用特征的直接方法。
基于特征的方法是目前VO的主流方式。对于两幅图像，首先提取图像中的特征，然后根据两幅图的特征匹配，计算相机的变换矩阵。最常用的是点特征，例如Harris角点、SIFT、SURF、ORB。如果使用RGBD相机，利用已知深度的特征点，就可以直接估计相机的运动。给定一组特征点以及它们之间的配对关系，求解相机的姿态，该问题被称为PnP问题（Perspective-N-Point）。PnP可以用非线性优化来求解，得到两个帧之间的位置关系。
不使用特征进行VO的方法称为直接法。它直接把图像中所有像素写进一个位姿估计方程，求出帧间相对运动。例如，在RGBD SLAM中，可以用ICP（Iterative Closest Point，迭代最近邻）求解两个点云之间的变换矩阵。对于单目SLAM，我们可以匹配两个图像间的像素，或者像图像与一个全局的模型相匹配。直接法的典型例子是SVO和LSD-SLAM。它们在单目SLAM中使用直接法，取得了较好的效果。目前看来，直接法比特征VO需要更多的计算量，而且对相机的图像采集速率也有较高的要求。
SLAM框架之后端
在VO估计帧间运动之后，理论上就可以得到机器人的轨迹了。然而视觉里程计和普通的里程计一样，存在累积误差的问题（Drift）。直观地说，在t1和t2时刻，估计的转角比真实转角少1度，那么之后的轨迹就全部少掉了这1度。时间一长，建出的房间可能由方形变成了多边形，估计出的轨迹亦会有严重的漂移。所以在SLAM中，还会把帧间相对运动放到一个称之为后端的程序中进行加工处理。
早期的SLAM后端使用滤波器方式。由于那时还未形成前后端的概念，有时人们也称研究滤波器的工作为研究SLAM。SLAM最早的提出者R. Smith等人就把SLAM建构成了一个EKF（Extended Kalman Filter，扩展卡尔曼滤波）问题。他们按照EKF的形式，把SLAM写成了一个运动方程和观测方式，以最小化这两个方程中的噪声项为目的，使用典型的滤波器思路来解决SLAM问题。
当一个帧到达时，我们能（通过码盘或IMU）测出该帧与上一帧的相对运动，但是存在噪声，是为运动方程。同时，通过传感器对路标的观测，我们测出了机器人与路标间的位姿关系，同样也带有噪声，是为观测方程。通过这两者信息，我们可以预测出机器人在当前时刻的位置。同样，根据以往记录的路标点，我们又能计算出一个卡尔曼增益，以补偿噪声的影响。于是，对当前帧和路标的估计，即是这个预测与更新的不断迭代的过程。
21世纪之后，SLAM研究者开始借鉴SfM（Structure from Motion）问题中的方法，把捆集优化（Bundle Adjustment）引入到SLAM中来。优化方法和滤波器方法有根本上的不同。它并不是一个迭代的过程，而是考虑过去所有帧中的信息。通过优化，把误差平均分到每一次观测当中。在SLAM中的Bundle Adjustment常常以图的形式给出，所以研究者亦称之为图优化方法（Graph Optimization）。图优化可以直观地表示优化问题，可利用稀疏代数进行快速的求解，表达回环也十分的方便，因而成为现今视觉SLAM中主流的优化方法。
SLAM框架之回环检测
回环检测，又称闭环检测（Loop closure detection），是指机器人识别曾到达场景的能力。如果检测成功，可以显著地减小累积误差。 回环检测实质上是一种检测观测数据相似性的算法。 对于视觉SLAM，多数系统采用目前较为成熟的词袋模型（Bag-of-Words, BoW）。词袋模型把图像中的视觉特征（SIFT, SURF等）聚类，然后建立词典，进而寻找每个图中含有哪些“单词”（word）。也有研究者使用传统模式识别的方法，把回环检测建构成一个分类问题，训练分类器进行分类。
回环检测的难点在于，错误的检测结果可能使地图变得很糟糕。这些 错误分为两类：1.假阳性 （False Positive），又称感知偏差（Perceptual Aliasing），指事实上不同的场景被当成了同一个； 2.假阴性 （False Negative），又称感知变异（Perceptual Variability），指事实上同一个场景被当成了两个。感知偏差会严重地影响地图的结果，通常是希望避免的。一个好的回环检测算法应该能检测出尽量多的真实回环。研究者常常用准确率-召回率曲线来评价一个检测算法的好坏。
下期我们将具体地介绍这几个模块
敬请期待！
雷锋网注：本文由大牛讲堂授权雷锋网发布，如需转载请联系原作者并注明作者和出处，不得删减内容。
已发表评论数()
请填写推刊名
描述不能大于100个字符!
权限设置： 公开
仅自己可见
正文不准确
标题不准确
排版有问题
主题不准确
没有分页内容
图片无法显示
视频无法显示
与原文不一致您所在位置： &
&nbsp&&nbsp&nbsp&&nbsp
一种改进的高斯混合概率假设密度SLAM算法.pdf7页
本文档一共被下载：
次 ,您可全文免费在线阅读后下载本文档。
文档加载中...广告还剩秒
需要金币：100 &&
一种改进的高斯混合概率假设密度SLAM算法.pdf
你可能关注的文档：
··········
··········
第29 卷 第11 期 控 制 与 决 策 2014 年 11 月
Vol. 29 No. 11 Control and
Decision Nov.
2014 文章编号:
9-07 DOI: 10.13195/j.kzyjc. 一种改进的高斯混合概率假设密度SLAM 算法 1,2 1 1 1 闫德立 , 宋永端 , 宋 宇 ,
康轶非 1. 北京交通大学电子信息工程学院，北京. 石家庄铁道大学电气与电子工程学院，石家庄050043 摘 要:
针对高斯混合概率假设密度SLAM GMPHD-SLAM
算法存在的估计精度低和计算代价高的问题, 提出一 种无迹高斯混合概率假设密度SLAM 算法 unscented-GMPHD-SLAM . 其主要特点在于: 将无迹卡尔曼滤波器应用 于机器人位姿粒子权重计算及概率假设密度更新过程中, 可提高算法整体估计性能; 将更新的高斯项按照传感器视 域分类, 有效降低了算法计算量. 通过仿真实验, 将所提出算法与传统PHD-SLAM 算法进行比较, 结果表明该算法在 提高估计精度和降低计算负担方面是十分有效的. 关键词:
移动机器人；同时定位与地图构建；高斯混合概率假设密度；无迹卡尔曼滤波器 中图分类号:
TP273 文献标志码: A An improved Gaussian mixture PHD SLAM algorithm 1,2 1 1 1 YAN De-li
SONG Yong-duan
KANG Yi-fei 1. College of Electronic and Information Engineering，Beijing Jiaotong University ，Beijing 100044 ，China ；2. College of Electric and Electronic Engineering，Shijiazhuang Tiedao University，Shijiazhuang 050043，China ．Correspondent: SONG Yong-duan，E-mail ：ydsong@ Abstract: For
probability
hypothesis
SLAM GMPHD-SLAM
of low estimation accuracy and high computational cost, the GMPHD-SLAM algorithm based on unscented transform, called unscented-GMPHD-SLAM,
proposed. The
contribution
the calculation of particle’s
正在加载中，请稍后...12197人阅读
ML/DL/AI/VR/DM（33）
什么是SLAM?机器人在未知环境中，要实现智能化需要完成三个任务，第一个是定位(Localization)，第二个是建图(Mapping)，第三个则是随后的路径规划(Navigation)。
　　之前地平线的高翔博士用这样一句话概括SLAM的释义。不过实际生活中的SLAM都是和激光雷达或者单目/双目摄像头结合的形式出现在我们面前的，有时甚至跟更多的传感器融合，这让很多人觉得这个技术不是一两句话能概括清楚的，但当我们熟悉完它在每一个领域的应用啊，就会明白，它，SLAM，原来是这样一个东西，而且会比我们所想象的要重要的多。
　　两年前，扫地机就是它的代言人
　　若是两年前有人跟我们提SLAM技术的话，很多人的印象是那个是用在室内移动机器人上的技术，如果非要找个刚需场景的话，扫地机那边画图边扫地的样子简直是最好像向陌生人解释这东西有多牛掰的神器。
　　但是，谁也没料到，两年后，因为计算机视觉技术的快速发展，摄像头和激光雷达成本的降低，SLAM正在AR、机器人、无人机、无人驾驶等领域火速入侵，用夸张点的说法，就像移动互联网时代的手机地图定位一样，只要是跟位置相关的生意，谁抛弃了这张地图，谁将抛弃了自己的未来。
　　SLAM的世纪意义
　　至于SLAM技术为什么这么重要，用SLAM技术发烧友Guang Ling的话说。
　　一个机器人也好，无人汽车也好，其中最核心，最根本的问题有四个，定位技术(Localization), 跟踪技术(Tracking), 路径规划技术(Path Planning), 还有就是控制技术(Controlling)。 而这四个问题中的前三个，SLAM都扮演了最核心的功能， 其意义，打个比方，就像移动互联网时代中，wifi和3/4G数据传输技术对手机的意义，是一样的。扮演一个最核心，也是最不可或缺的角色。
　　若是手机离开了WIFI和数据网络，就像无人车和机器人，离开了SLAM一样。但对于AR和AI来说，因为这两个品类非常宽泛，所以，SLAM对其影响，就相对比较有限。
　　它正在入侵这些领域
　　一个技术重要的最好证据，就是看见它正在在各个领域深入应用，并且各个领域都视它为未来，下面我们从这四个时下最热的领域一窥它的火爆。
　　1)室内机器人
　　扫地机要算机器人里最早用到SLAM技术这一批了，国内的科沃斯、塔米扫地机通过用SLAM算法结合激光雷达或者摄像头的方法，让扫地机可以高效绘制室内地图，智能分析和规划扫地环境，从而成功让自己步入了智能导航的阵列。
　　不过有意思的是，科沃斯引领时尚还没多久，一大帮懂Slam算法的扫地机厂商就开始陆陆续续地推出自己的智能导航，直到昨天雷锋网还看到一款智能扫地机新鲜出炉，而这追逐背后的核心，大家都知道就是SLAM技术的应用。
　　而另一个跟SLAM息息相关的室内移动机器人，因为目前市场定位和需求并不明确，我们目前只能在商场导购室内机器人和Buddy那样的demo视频里才能看到，国内Watchhhh Slam和Slam Tech两家公司都是做这方面方案提供的，以现实的观点看，现在室内移动机器人市场定位和需求没落地的时候，由方案商公司推动，商用室内移动机器人先行，这反而是一种曲线救国的发展方式。
　　目前基于SLAM技术开发的代表性产品有微软的Hololens，谷歌的Project Tango以及同样有名的Magic Leap，后者4月20号公布它的新一代水母版demo后，国内的AR公司更加看到了这个趋势，比如进化动力近期就公布了他们的SLAM demo, 用一个小摄像头实现VR头显空间定位，而易瞳去年10月雷锋网去试用新品的时候，就发现已经整合SLAM技术了，国内其他公司虽然没有正式公布，但我们可以肯定，他们都在暗暗研发这项技术，只等一个成熟的时机就会展现给大家。
　　进化动力CTO聂崇岭向雷锋网(搜索“雷锋网”公众号关注)表示，如果用一个准确的说法
　　很多VR应用需要用到SLAM技术，定位只是一个feature，路径记录、3D重构、地图构建都可以是SLAM技术的输出。
　　3)无人机
　　国外的话，原来做 Google X Project Wing 无人机的创始人 MIT 机器人大牛 Nicholas Roy 的学生 Adam Bry 创办的 Skydio，挖来了 Georgia Tech 的 Slam 大牛教授 Frank Dellaert 做他们的首席科学家。
　　国内大家非常熟悉的大疆精灵四避障用的双目视觉+超声波，一位大疆工程师徐枭涵在百度百家的撰文里坦率承认
　　“P4里面呈现的主动避障功能就是一种非常非常典型的Slam的弱应用，无人机只需要知道障碍物在哪，就可以进行 Planning，并且绕开障碍物。当然Slam能做的事情远远不止这些，包括灾区救援，包括探洞，包括人机配合甚至集群，所有的关于无人机的梦想都建立在Slam之上，这是无人机能飞(具有定位，姿态确定以后)的时代以后，无人机最核心的技术。”
　　而近期另一个号称刷爆美国朋友圈的hover camera无人机，因为其创始人的的计算机视觉背景，正式把SLAM技术应用进来了，在介绍他们无人机的主要产品技术时，提到了
　　●SLAM(即时定位与地图构建)：通过感知自身周围环境来构建3D增量式地图，从而实现自主定位和导航。
　　4)无人驾驶
　　因为Google无人驾驶车的科普，很多人都知道了基于激光雷达技术的Lidar Slam。Lidar Slam是指利用激光雷达作为外部传感器，获取地图数据，使机器人实现同步定位与地图构建。虽然成本高昂，但目前为止是最稳定、最可靠、高性能的SLAM方式。
　　另外，2011 年，牛津大学Mobile Robotics Group 首次向公众展示他们的第一辆无人驾驶汽车野猫(Wildcat)，这是一辆由 Bowler Wildcat 4X4 改装而成的车。汽车头顶的相机和激光能够搜集信息然后即时分析导航，已经成功通过了测试。2014 年，他们改装的一辆 Nissan 的 Leaf 也成功路测。
　　Mobile Robotics Group主要研究领域是大规模的导航和对自然场景理解。据称，团队所拥有的技术非常牛逼，其复杂和先进性远远超过一般的同步定位与地图构建(SLAM)算法。
　　可圈可点的是，对于无人驾驶技术，他们并没有使用 GPS 或者是嵌入式的基础设施(信标之类的)，而是使用算法来导航，包括机器学习和概率推理来建立周围的地图等。
　　SLAM技术在AR上的应用，跟机器人不一样吗?
　　既然SLAM技术如文章SLAM的世纪意义所说，它对于机器人是个通用技术，要更深入的了解它，你只能通过它在不同领域应用的异同来比较，你才会知道它到底是个什么东西。
　　* 关于SLAM技术在AR行业的应用，跟在机器人上使用时有什么不同的地方。Watchhhh Slam创始人刘博士在接受arinchina采访的时候曾给出了很好的答案，雷锋网这里就不重复了：
　　1)精度上，AR一般更关注于局部精度，要求恢复的相机运动避免出现漂移、抖动，这样叠加的虚拟物体才能看起来与现实场景真实地融合在一起;机器人一般更关注全局精度，需要恢复的整条运动轨迹误差累积不能太大，循环回路要能闭合，而在某个局部的漂移、 抖动等问题往往对机器人应用来说影响不大。
　　2)效率上，AR需要在有限的计算资源下实时求解，人眼的刷新率为24帧，所以AR的计算效率通常需要到达30帧以上; 机器人本身运动就很慢，可以把帧率降低，所以对算法效率的要求相对较低。
　　3)配置上，AR对硬件的体积、功率、成本等问题比机器人更敏感，比如机器人上可以配置鱼眼、双目或深度摄像头、高性能CPU等硬件来降低SLAM的难度，而AR应用更倾向于采用更为高效、鲁邦的算法达到需求。
　　* 关于SLAM技术在AR行业的应用，跟在无人机行业的应用有什么不同的地方。易瞳CTO艾韬表示
　　无人机范围大，精度要求没那么高，有其他的光流、超声波传感器的辅助;而应用在AR的话，AR的帧率要求高，定位要求更高，对体验也要求更高。
　　* 关于SLAM技术在机器人行业的应用，跟在无人车行业的应用有什么不同的地方。Velodyne激光雷达的老翁表示
　　SLAM在无人驾驶中的应用和移动机器人中的应用，从原理上讲，没有任何区别，无人驾驶汽车，就是移动机器人中的一种。sabastian教授的《Aritificial Inteligence for Robot》教程中，直接把无人驾驶汽车，叫做移动机器人。
　　但实际应用场景的差别，会让传感器和所用激光雷达的成本很不一样。
　　* 关于SLAM技术在无人机行业的应用，跟在无人车行业的应用有什么不同的地方。Velodyne激光雷达的老翁接着表示
　　关于无人车和无人机中，Slam的区别，最大的区别是二维和三维。无人车是二维，无人机是三维。SLAM在二维世界中，有三个量，x轴，y轴，和方向角。三维世界中，会复杂很多，有6个量，x，y，z，roll，yaw，pitch。就是说，在确定位置时，无人机要多确定三个量，相应的计算量会大很多。但是原理仍然是一样的。
　　SLAM在同一个行业的应用，用摄像头和激光雷达的区别在哪里
　　很多人注意到，同一个行业的SLAM应用，有人用相对传统的激光雷达技术，有人用号称必定是未来趋势的计算机视觉技术(单目+双目摄像头)，以室内机器人为例，它们的区别在哪里呢?
　　速腾聚创CEO邱纯鑫表示
　　区别应该从传感器分，再从算法上分，他们精度上目前能做到差不多，激光相对要稳定一些，而帧数，视觉要快很多，比如比30针(人眼上限)要快很多，毕竟每秒出数不同，配置的话，现在激光雷达也有平价华的方案了，配置要求都不是那么高。
　　而以无人机为例，不管精灵四还是Hover camera的SLAM，各家都要在SLAM技术上结合额外的超声波或者光流技术定位，雷锋网问邱纯鑫这是不是表明纯SLAM技术有硬伤，邱纯鑫表示
　　纯SLAM算法是有些问题的，比如基于已经地图的SLAM还是基于未知环境的SLAM，后者就有难度。
　　小结：
　　从上面SLAM在各大领域的小荷才露尖尖角，我们已经嗅到了SLAM将在各个领域全面入侵的苗头。
　　有了它，将让真正的智能机器人触手可及。
　　有了它，无人机将不再依赖于鸡肋的色块识别。
　　有了它，Magic Leap 这样的AR神话将变成现实。
　　如果仅仅是一个无人机行业，借用大疆那位工程师的经典点评“所有的关于无人机的梦想都建立在SLAM之上”，那其它行业SLAM将带给我们怎样的震撼，你能预见吗?
Kinect视觉SLAM技术介绍
摘要：本文介绍SLAM的历史、理论以及实现的方式，且主要介绍基于视觉（Kinect）的实现方式
本文介绍SLAM的历史、理论以及实现的方式，且主要介绍基于视觉（Kinect）的实现方式。
1.&&&&前言
开始做SLAM（机器人同时定位与建图）研究已经近一年了。从一年级开始对这个方向产生兴趣，到现在为止，也算是对这个领域有了大致的了解。然而越了解，越觉得这个方向难度很大。总体来讲有以下几个原因：
1）入门资料很少。虽然国内也有不少人在做，但这方面现在没有太好的入门教程。《SLAM for dummies》可以算是一篇。中文资料几乎没有。
2）SLAM研究已进行了三十多年，从上世纪的九十年代开始。其中又有若干历史分枝和争论，要把握它的走向就很费工夫。
3）难以实现。SLAM是一个完整的系统，由许多个分支模块组成。现在经典的方案是“图像前端，优化后端，闭环检测”的三部曲，很多文献看完了自己实现不出来。
4）自己动手编程需要学习大量的先决知识。首先你要会C和C++，网上很多代码还用了11标准的C++。第二要会用Linux。第三要会 cmake，vim/emacs及一些编程工具。第四要会用openCV, PCL, Eigen等第三方库。只有学会了这些东西之后，你才能真正上手编一个SLAM系统。如果你要跑实际机器人，还要会ROS。
当然，困难多意味着收获也多，坎坷的道路才能锻炼人（比如说走着走着才发现Linux和C++才是我的真爱之类的。）鉴于目前网上关于视觉SLAM的资料极少，我于是想把自己这一年多的经验与大家分享一下。说的不对的地方请大家批评指正。
这篇文章关注视觉SLAM，专指用摄像机，Kinect等深度像机来做导航和探索，且主要关心室内部分。到目前为止，室内的视觉SLAM仍处于研究阶段， 远未到实际应用的程度。一方面，编写和使用视觉SLAM需要大量的专业知识，算法的实时性未达到实用要求；另一方面，视觉SLAM生成的地图（多数是点 云）还不能用来做机器人的路径规划，需要科研人员进一步的探索和研究。以下，我会介绍SLAM的历史、理论以及实现的方式，且主要介绍视觉 （Kinect）的实现方式。
2.&&&&SLAM问题
SLAM，全称叫做Simultaneous Localization and Mapping，中文叫做同时定位与建图。啊不行，这么讲下去，这篇文章肯定没有人读，所以我们换一个讲法。
3.&&&&小萝卜的故事
从前，有一个机器人叫“小萝卜”。它长着一双乌黑发亮的大眼睛，叫做Kinect。有一天，它被邪恶的科学家关进了一间空屋子，里面放满了杂七杂八的东西。
小萝卜感到很害怕，因为这个地方他从来没来过，一点儿也不了解。让他感到害怕的主要是三个问题：
1） &自己在哪里？
2） &这是什么地方？
3）怎么离开这个地方？
在SLAM理论中，第一个问题称为定位&(Localization)，第二个称为建图&(Mapping)，第三个则是随后的路径规划。我们希望借助Kinect工具，帮小萝卜解决这个难题。各位同学有什么思路呢？
4.&&&&Kinect数据
要打败敌人，首先要了解你的武器。不错，我们先介绍一下Kinect。众所周知这是一款深度相机，你或许还听说过别的牌子，但Kinect的价格 便宜，测量范围在3m-12m之间，精度约3cm，较适合于小萝卜这样的室内机器人。它采到的图像是这个样子的（从左往右依次为rgb图，深度图与点云 图）：
Kinect的一大优势在于能比较廉价地获得每个像素的深度值，不管是从时间上还是从经济上来说。OK，有了这些信息，小萝卜事实上可以知道它采集到的图片中，每一个点的3d位置。只要我们事先标定了Kinect，或者采用出厂的标定值。
我们把坐标系设成这个样子，这也是openCV中采用的默认坐标系。
o’-uv是图片坐标系，o-xyz是Kinect的坐标系。假设图片中的点为(u,v)，对应的三维点位置在(x,y,z)，那么它们之间的转换关系是这样的：
或者更简单的：
后一个公式给出了计算三维点的方法。先从深度图中读取深度数据（Kinect给的是16位无符号整数），除掉z方向的缩放因子，这样你就把一个整 数变到了以米为单位的数据。然后，x,y用上面的公式算出。一点都不难，就是一个中心点位置和一个焦距而已。f代表焦距，c代表中心。如果你没有自己标定 你的Kinect，也可以采用默认的值：s=5000, cx = 320, cy=240, fx=fy=525。实际值会有一点偏差，但不会太大。
5.&&&&定位问题
知道了Kinect中每个点的位置后，接下来我们要做的，就是根据两帧图像间的差别计算小萝卜的位移。比如下面两张图，后一张是在前一张之后1秒采集到的：
你肯定可以看出，小萝卜往右转过了一定的角度。但究竟转过多少度呢？这就要靠计算机来求解了。这个问题称为相机相对姿态估计，经典的算法是 ICP（Iterative Closest Point，迭代最近点）。这个算法要求知道这两个图像间的一组匹配点，说的通俗点，就是左边图像哪些点和右边是一样的。你当然看见那块黑白相间的板子同 时出现在两张图像中。在小萝卜看来，这里牵涉到两个简单的问题：特征点的提取和匹配。
如果你熟悉计算机视觉，那你应该听说过SIFT, SURF之类的特征。不错，要解决定位问题，首先要得到两张图像的一个匹配。匹配的基础是图像的特征，下图就是SIFT提取的关键点与匹配结果：
对实现代码感兴趣的同学请Google“opencv&匹配”即可，在openCV的教程上也有很明白的例子。上面的例子可以看出，我们找到了一 些匹配，但其中有些是对的（基本平等的匹配线），有些是错的。这是由于图像中存在周期性出现的纹理（黑白块），所以容易搞错。但这并不是问题，在接下来的 处理中我们会将这些影响消去。
得到了一组匹配点后，我们就可以计算两个图像间的转换关系，也叫PnP问题。它的模型是这样的：
R为相机的姿态，C为相机的标定矩阵。R是不断运动的，而C则是随着相机做死的。ICP的模型稍有不同，但原理上也是计算相机的姿态矩阵。原则 上，只要有四组匹配点，就可以算这个矩阵。你可以调用openCV的SolvePnPRANSAC函数或者PCL的ICP算法来求解。openCV提供的 算法是RANSAC（Random Sample Consensus，随机采样一致性）架构，可以剔除错误匹配。所以代码实际运行时，可以很好地找到匹配点。以下是一个结果的示例。
上面两张图转过了16.63度，位移几乎没有。
有同学会说，那只要不断匹配下去，定位问题不就解决了吗？表面上看来，的确是这样的，只要我们引入一个关键帧的结构（发现位移超过一个固定值时， 定义成一个关键帧）。然后，把新的图像与关键帧比较就行了。至于建图，就是把这些关键帧的点云拼起来，看着还有模有样，煞有介事的：
1－200帧的匹配结果
然而，如果事情真这么简单，SLAM理论就不用那么多人研究三十多年了（它是从上世纪90年代开始研究的）（上面讲的那些东西简直随便哪里找个小硕士就能做出来……）。那么，问题难在什么地方呢？
6.&&&&SLAM端优化理论
最麻烦的问题，就是“噪声”。这种渐近式的匹配方式，和那些惯性测量设备一样，存在着累积噪声。因为我们在不断地更新关键帧，把新图像与最近的关 键帧比较，从而获得机器人的位移信息。但是你要想到，如果有一个关键帧出现了偏移，那么剩下的位移估计都会多出一个误差。这个误差还会累积，因为后面的估 计都基于前面的机器人位置……哇！这后果简直不堪设想啊（例如，你的机器人往右转了30度，再往左转了30度回到原来的位置。然而由于误差，你算成了向右 转29度，再向左转31度，这样你构建的地图中，会出现初始位置的两个“重影”）。我们能不能想办法消除这个该死的误差呢？
朋友们，这才是SLAM的研究，前面的可以说是“图像前端”的处理方法。我们的解决思路是：如果你和最近的关键帧相比，会导致累计误差。那么，我们最好是和更前面的关键帧相比，而且多比较几个帧，不要只比较一次。
我们用数学来描述这个问题。设：
不要怕，只有借助数学才能把这个问题讲清楚。上面的公式中，xp是机器人小萝卜的位置，我们假定由n个帧组成。xL则是路标，在我们的图像处理过 程中就是指SIFT提出来的关键点。如果你做2D SLAM，那么机器人位置就是x, y加一个转角theta。如果是3D SLAM，就是x,y,z加一个四元数姿态（或者rpy姿态）。这个过程叫做参数化（Parameterization）。
不管你用哪种参数，后面两个方程你都需要知道。前一个叫运动方程，描述机器人怎样运动。u是机器人的输入，w是噪声。这个方程最简单的形式，就是 你能通过什么方式（码盘等）获得两帧间的位移差，那么这个方程就直接是上一帧与u相加即得。另外，你也可以完全不用惯性测量设备，这样我们就只依靠图像设 备来估计，这也是可以的。
后一个方程叫观测方程，描述那些路标是怎么来的。你在第i帧看到了第j个路标，产生了一个测量值，就是图像中的横纵坐标。最后一项是噪声。偷偷告诉你，这个方程形式上和上一页的那个方程是一模一样的。
在求解SLAM问题前，我们要看到，我们拥有的数据是什么？在上面的模型里，我们知道的是运动信息u以及观测z。用示意图表示出来是这样的：
我们要求解的，就是根据这些u和z，确定所有的xp和xL。这就是SLAM问题的理论。从SLAM诞生开始科学家们就一直在解决这个问题。最初， 我们用Kalman滤波器，所以上面的模型（运动方程和观测方程）被建成这个样子。直到21世纪初，卡尔曼滤波器仍在SLAM系统占据最主要的地 位，Davison经典的单目SLAM就是用EKF做的。但是后来，出现了基于图优化的SLAM方法，渐渐有取而代之的地位[1]。我们在这里不介绍卡尔 曼滤波器，有兴趣的同学可以在wiki上找卡尔曼滤波器，另有一篇中文的《卡尔曼滤波器介绍》也很棒。由于滤波器方法存储n个路标要消耗n平方的空间，在
计算量上有点对不住大家。尽管08年有人提出分治法的滤波器能把复杂度弄到O(n) [2]，但实现手段比较复杂。我们要介绍那种新兴的方法: Graph-based SLAM。
图优化方法把SLAM问题做成了一个优化问题。学过运筹学的同学应该明白，优化问题对我们有多么重要。我们不是要求解机器人的位置和路标位置吗？ 我们可以先做一个猜测，猜想它们大概在什么地方。这其实是不难的。然后呢，将猜测值与运动模型／观测模型给出的值相比较，可以算出误差：
通俗一点地讲，例如，我猜机器人第一帧在(0,0,0)，第二帧在(0,0,1)。但是u1告诉我机器人往z方向（前方）走了0.9米，那么运动 方程就出现了0.1m的误差。同时，第一帧中机器人发现了路标1，它在该机器人图像的正中间；第二帧却发现它在中间偏右的位置。这时我们猜测机器人只是往 前走，也是存在误差的。至于这个误差是多少，可以根据观测方程算出来。
我们得到了一堆误差，把这些误差平方后加起来（因为单纯的误差有正有负，然而平方误差可以改成其他的范数，只是平方更常用），就得到了平方误差和。我们把这个和记作phi，就是我们优化问题的目标函数。而优化变量就是那些个xp, xL。
改变优化变量，误差平方和（目标函数）就会相应地变大或变小，我们可以用数值方法求它们的梯度和二阶梯度矩阵，然后用梯度下降法求最优值。这些东西学过优化的同学都懂的。
注意到，一次机器人SLAM过程中，往往会有成千上万帧。而每一帧我们都有几百个关键点，一乘就是几百万个优化变量。这个规模的优化问题放到小萝 卜的机载小破本上可解吗？是的，过去的同学都以为，Graph-based SLAM是无法计算的。但就在21世纪06，07年后，有些同学发现了，这个问题规模没有想象的那么大。上面的J和H两个矩阵是“稀疏矩阵”，于是呢，我 们可以用稀疏代数的方法来解这个问题。“稀疏”的原因，在于每一个路标，往往不可能出现在所有运动过程中，通常只出现在一小部分图像里。正是这个稀疏性， 使得优化思路成为了现实。
优化方法利用了所有可以用到的信息（称为full-SLAM, global SLAM），其精确度要比我们一开始讲的帧间匹配高很多。当然计算量也要高一些。
由于优化的稀疏性，人们喜欢用“图”来表达这个问题。所谓图，就是由节点和边组成的东西。我写成G={V,E}，大家就明白了。V是优化变量节点，E表示运动/观测方程的约束。什么，更糊涂了吗？那我就上一张图，来自[3]。
图有点模糊，而且数学符号和我用的不太一样，我用它来给大家一个图优化的直观形象。上图中，p是机器人位置，l是路标，z是观测，t是位移。其中 呢，p, l是优化变量，而z,t是优化的约束。看起来是不是像一些弹簧连接了一些质点呢？因为每个路标不可能出现在每一帧中，所以这个图是蛮稀疏的。不过，“图” 优化只是优化问题的一个表达形式，并不影响优化的含义。实际解起来时还是要用数值法找梯度的。这种思路在计算机视觉里，也叫做Bundle Adjustment。它的具体方法请参见一篇经典文章[4]。
不过，BA的实现方法太复杂，不太建议同学们拿C来写。好在2010年的ICRA上，其他的同学们提供了一个通用的开发包：g2o [5]。它是有图优化通用求解器，很好用，我改天再详细介绍这个软件包。总之，我们只要把观测和运动信息丢到求解器里就行。这个优化器会为我们求出机器人 的轨迹和路标位置。如下图，红点是路标，蓝色箭头是机器人的位置和转角（2D SLAM）。细心的同学会发现它往右偏转了一些。：
7.&&&&闭环检测
上面提到，仅用帧间匹配最大的问题在于误差累积，图优化的方法可以有效地减少累计误差。然而，如果把所有测量都丢进g2o，计算量还是有点儿大 的。根据我自己测试，约10000多条边，g2o跑起来就有些吃力了。这样，就有同学说，能把这个图构造地简洁一些吗？我们用不着所有的信息，只需要把有 用的拿出来就行了。
事实上，小萝卜在探索房间时，经常会左转一下，右转一下。如果在某个时刻他回到了以前去过的地方，我们就直接与那时候采集的关键帧做比较，可以吗？我们说，可以，而且那是最好的方法。这个问题叫做闭环检测。
闭环检测是说，新来一张图像时，如何判断它以前是否在图像序列中出现过？有两种思路：一是根据我们估计的机器人位置，看是否与以前某个位置邻近； 二是根据图像的外观，看它是否和以前关键帧相似。目前主流方法是后一种，因为很多科学家认为前一种依靠有噪声的位置来减少位置的噪声，有点循环论证的意 思。后一种方法呢，本质上是个模式识别问题（非监督聚类，分类），常用的是Bag-of-Words (BOW)。但是BOW需要事先对字典进行训练，因此SLAM研究者仍在探讨有没有更合适的方法。
在Kinect SLAM经典大作中[6]，作者采用了比较简单的闭环方法：在前面n个关键帧中随机采k个，与当前帧两两匹配。匹配上后认为出现闭环。这个真是相当的简单实用，效率也过得去。
高效的闭环检测是SLAM精确求解的基础。这方面还有很多工作可以做。
8.&&&&小结
本文我们介绍了SLAM的基本概念，重点介绍了图优化解决SLAM问题的思路。我最近正在编写SLAM程序，它是一个Linux下基于cmake 的工程。目前仍在开发当中。欢迎感兴趣的同学来交流研究心得，我的邮件是：gaoxiang12@mails.。
[1] Visual SLAM: Why filter? Strasdat et. al., Image and Vision Computing, 2012.
[2] Divide and Conquer: EKF SLAM in O(n), Paz Lina M et al., IEEE Transaction on Robotics, 2008
[3] Relative bundle adjustment, Sibley, Gabe, 2009
[4] Bundle adjustment – a Modern Synthesis. Triggs B et. el., Springer, 2000
[5] g2o: A General Framework for Graph Optimization, Kummerle Rainer, et. al., ICRA, 2011
[6] 3-D Mapping with an RGB-D Camera, IEEE Transaction on Robotics, Endres et al., 2014
&&相关文章推荐
参考知识库
* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
访问：198262次
积分：3712
积分：3712
排名：第7985名
原创：179篇
转载：30篇
评论：24条
(3)(8)(15)(16)(10)(8)(7)(14)(19)(6)(10)(5)(6)(15)(3)(5)(2)(2)(3)(6)(5)(2)(1)(4)(9)(1)(2)(3)(2)(7)(8)}