原问题：

能否简单并且易懂地介绍一下多个基于滤波方法的SLAM算法原理？

　　我怎么会写得那么长……如果您有兴趣可以和我一块把公式过一遍。
　　要讲清这个问题，得从状态估计理论来说。先摆上一句名言：

状态估计乃传感器之本质。（To understand the need for state estimation is to understand the nature of sensors.）

　　任何传感器，激光也好，视觉也好，整个SLAM系统也好，要解决的问题只有一个：如何通过数据来估计自身状态。每种传感器的测量模型不一样，它们的精度也不一样。换句话说，状态估计问题，也就是“如何最好地使用传感器数据”。可以说，SLAM是状态估计的一个特例。

1. 离散时间系统的状态估计

 

记机器人在各时刻的状态为，其中是离散时间下标。在SLAM中，我们通常要估计机器人的位置，那么系统的状态就指的是机器人的位姿。用两个方程来描述状态估计问题：

　　解释一下变量：

　　-运动方程
　　- 输入
　　- 输入噪声
　　- 观测方程
　　- 观测数据
　　- 观测噪声

　　运动方程描述了状态是怎么变到的，而观测方程描述的是从是怎么得到观察数据的。
　　请注意这是一种抽象的写法。当你有实际的机器人，实际的传感器时，方程的形式就会变得具体，也就是所谓的参数化。例如，当我们关心机器人空间位置时，可以取。进而，机器人携带了里程计，能够得到两个时间间隔中的相对运动，像这样，那么运动方程就变为：

　　同理，观测方程也随传感器的具体信息而变。例如激光传感器可以得到空间点离机器人的距离和角度，记为，那么观测方程为：
　　

其中是一个2D路标点。

　　举这几个例子是为了说明，运动方程和观测方程具体形式是会变化的。但是，我们想讨论更一般的问题：当我不限制传感器的具体形式时，能否设计一种方式，从已知的（输入和观测数据）从，估计出呢？

　　这就是最一般的状态估计问题。我们会根据是否线性，把它们分为线性/非线性系统。同时，对于噪声，根据它们是否为高斯分布，分为高斯/非高斯噪声系统。最一般的，也是最困难的问题，是非线性-非高斯(NLNG, Nonlinear-Non Gaussian)的状态估计。下面先说最简单的情况：线性高斯系统。

2. 线性高斯系统(LG，Linear Gaussian)

　　在线性高斯系统中，运动方程、观测方程是线性的，且两个噪声项服从零均值的高斯分布。这是最简单的情况。简单在哪里呢？主要是因为高斯分布经过线性变换之后仍为高斯分布。而对于一个高斯分布，只要计算出它的一阶和二阶矩，就可以描述它（高斯分布只有两个参数）。
　　线性系统形式如下：

　　其中是两个噪声项的协方差矩阵。为转移矩阵和观测矩阵。
　　对LG系统，可以用贝叶斯法则，计算的后验概率分布——这条路直接通向卡尔曼滤波器。卡尔曼是线性系统的递推形式（recursive，也就是从估计）的无偏最优估计。由于解释EKF和UKF都得用它，所以我们来推一推。如果读者不感兴趣，可以跳过公式推导环节。
　　符号：用表示的后验概率，用表示它的先验概率。因为系统是线性的，噪声是高斯的，所以状态也服从高斯分布，需要计算它的均值和协方差矩阵。记第时刻的状态服从：

　　我们希望得到状态变量的最大后验估计（MAP，Maximize a Posterior），于是计算：

第一行：给定观测y下的x最大概率)

第二行：是贝叶斯法则，

第三行：是第二行中分母和无关所以去掉。

　　第三行第一项即观测方程，有：

　　第二项即运动方程

　也很简单。
　　现在的问题是如何求解这个最大化问题。对于高斯分布，最大化问题可以变成最小化它的负对数。当我对一个高斯分布取负对数时，它的指数项变成了一个二次项，而前面的因子则变为一个无关的常数项，可以略掉（这部分我不敲了，有疑问的同学可以问）。于是，定义以下形式的最小化函数：

　那么最大后验估计就等价于：

　　这个问题现在是二次项和的形式，写成矩阵形式会更加清晰。定义：


　　就得到矩阵形式的，类似最小二乘的问题：

　　于是令它的导数为零，得到：
  (*)
　　读者会问，这个问题和卡尔曼滤波有什么问题呢？事实上，卡尔曼滤波就是递推地求解(*)式的过程。所谓递推，就是只用来计算。对(*)进行Cholesky分解，就可以推出卡尔曼滤波器。详细过程限于篇幅就不推了，把卡尔曼的结论写一下：

　　前两个是预测，第三个是卡尔曼增益，四五是校正。
　　另一方面，能否直接求解(*)式，得到呢？答案是可以的，而且这就是优化方法（batch optimization）的思路：将所有的状态放在一个向量里，进行求解。与卡尔曼滤波不同的是，在估计前面时刻的状态（如）时，会用到后面时刻的信息（等）。从这点来说，优化方法和卡尔曼处理信息的方式是相当不同的。

3. 扩展卡尔曼滤波器

　　线性高斯系统当然性质很好啦，但许多现实世界中的系统都不是线性的，状态和噪声也不是高斯分布的。例如上面举的激光观测方程就不是线性的。当系统为非线性的时候，会发生什么呢？
　　一件悲剧的事情是：高斯分布经过非线性变换后，不再是高斯分布。而且，是个什么分布，基本说不上来。（摊手）
　　如果没有高斯分布，上面说的那些都不再成立了。于是EKF说，嘛，我们睁一只眼闭一只眼，用高斯分布去近似它，并且，在工作点附近对系统进行线性化。当然这个近似是很成问题的，有什么问题我们之后再说。
　　EKF的做法主要有两点。其一，在工作点附近，对系统进行线性近似化：

　　这里的几个偏导数，都在工作点处取值。于是呢，它就被活生生地当成了一个线性系统。
　　第二，在线性系统近似下，把噪声项和状态都当成了高斯分布。这样，只要估计它们的均值和协方差矩阵，就可以描述状态了。经过这样的近似之后呢，后续工作都和卡尔曼滤波是一样的了。所以EKF是卡尔曼滤波在NLNG系统下的直接扩展（所以叫扩展卡尔曼嘛）。EKF给出的公式和卡尔曼是一致的，用线性化之后的矩阵去代替卡尔曼滤波器里的转移矩阵和观测矩阵即可。
　　

其中

　　这样做听起来还是挺有道理的，实际上也是能用的，但是问题还是很多的。
　　考虑一个服从高斯分布的变量，现在，问服从什么分布？
　　我概率比较差，不过这个似乎是叫做卡尔方布。应该是下图中k=1那条线。

　　但是按照EKF的观点，我们要用一个高斯分布去近似。假设我们采样时得到了一个，那么就会近似成一个均值为0.25的高斯分布，然而卡方分布的期望应该是1。……但是各位真觉得k=1那条线像哪个高斯分布吗？
　　所以EKF面临的一个重要问题是，当一个高斯分布经过非线性变换后，如何用另一个高斯分布近似它？按照它现在的做法，存在以下的局限性：（注意是滤波器自己的局限性，还没谈在SLAM问题里的局限性）。

1. 即使是高斯分布，经过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能很差）。
2. 系统本身线性化过程中，丢掉了高阶项。
3. 线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值。于是，在这个工作点下计算的，也不是最好的。
4. 在估计非线性输出的均值时，EKF算的是的形式。这个结果几乎不会是输出分布的真正期望值。协方差也是同理。

那么，怎么克服以上的缺点呢？途径很多，主要看我们想不想维持EKF的假设。如果我们比较乖，希望维持高斯分布假设，可以这样子改：

1. 为了克服第3条工作点的问题，我们以EKF估计的结果为工作点，重新计算一遍EKF，直到这个工作点变化够小。是为迭代EKF（Iterated EKF, IEKF）。
2. 为了克服第4条，我们除了计算，再计算其他几个精心挑选的采样点，然后用这几个点估计输出的高斯分布。是为Sigma Point KF（SPKF，或UKF）。

　　如果不那么乖，可以说：我们不要高斯分布假设，凭什么要用高斯去近似一个长得根本不高斯的分布呢？于是问题变为，丢掉高斯假设后，怎么描述输出函数的分布就成了一个问题。一种比较暴力的方式是：用足够多的采样点，来表达输出的分布。这种蒙特卡洛的方式，也就是粒子滤波的思路。
　　如果再进一步，可以丢弃滤波器思路，说：为什么要用前一个时刻的值来估计下一个时刻呢？我们可以把所有状态看成变量，把运动方程和观测方程看成变量间的约束，构造误差函数，然后最小化这个误差的二次型。这样就会得到非线性优化的方法，在SLAM里就走向图优化那条路上去了。不过，非线性优化也需要对误差函数不断地求梯度，并根据梯度方向迭代，因而局部线性化是不可避免的。
　　可以看到，在这个过程中，我们逐渐放宽了假设。

4. UKF 无迹卡尔曼

　　由于题主问题里没谈IEKF，我们就简单说说UKF和PF。
　　UKF主要解决一个高斯分布经过非线性变换后，怎么用另一个高斯分布近似它。假设，我们希望用近似。按照EKF，需要对做线性化。但在UKF里，不必做这个线性化。
　　UKF的做法是找一些叫做Sigma Point的点，把这些点用投影过去。然后，用投影之后的点做出一个高斯分布，如下图：
　　

这里选了三个点：。对于维数为N的分布，需要选2N+1个点。篇幅所限，这里就不解释这些点怎么选，以及为何要这样选了。总之UKF的好处就是：

• 不必线性化，也不必求导，对没有光滑性要求。
• 计算量随维数增长是线性的。

5. PF 粒子滤波 

　　UKF的一个问题是输出仍假设成高斯分布。然而，即使在很简单的情况下，高斯的非线性变换仍然不是高斯。并且，仅在很少的情况下，输出的分布有个名字（比如卡方），多数时候你都不知道他们是啥……更别提描述它们了。
　　因为描述很困难，所以粒子滤波器采用了一种暴力的，用大量采样点去描述这个分布的方法（老子就是无参的你来打我呀）。框架大概像下面这个样子，就是一个不断采样——算权重——重采样的过程：
　　

越符合观测的粒子拥有越大的权重，而权重越大就越容易在重采样时被采到。当然，每次采样数量、权重的计算策略，则是粒子滤波器里几个比较麻烦的问题，这里就不细讲了。
　　这种采样思路的最大问题是：采样所需的粒子数量，随分布是指数增长的。所以仅限于低维的问题，高维的基本就没办法了。

6. 非线性优化

　　非线性优化，计算的也是最大后验概率估计（MAP），但它的处理方式与滤波器不同。对于上面写的状态估计问题，可以简单地构造误差项：

然后最小化这些误差项的二次型：

这里仅用到了噪声项满足高斯分布的假设，再没有更多的了。当构建一个非线性优化问题之后，就可以从一个初始值出发，计算梯度（或二阶梯度），优化这个目标函数。常见的梯度下降策略有牛顿法、高斯-牛顿法、Levenberg-Marquardt方法，可以在许多讲数值优化的书里找到。
　　非线性优化方法现在已经成为视觉SLAM里的主流，尤其是在它的稀疏性质被人发现且利用起来之后。它与滤波器最大不同点在于， 一次可以考虑整条轨迹中的约束。它的线性化，即雅可比矩阵的计算，也是相对于整条轨迹的。相比之下，滤波器还是停留在马尔可夫的假设之下，只用上一次估计的状态计算当前的状态。可以用一个图来表达它们之间的关系：
　　

当然优化方式也存在它的问题。例如优化时间会随着节点数量增长——所以有人会提double window optimization这样的方式，以及可能落入局部极小。但是就目前而言，它比EKF还是优不少的。

7. 小结 

1. 卡尔曼滤波是递归的线性高斯系统最优估计。
2. EKF将NLNG系统在工作点附近近似为LG进行处理。
3. IEKF对工作点进行迭代。
4. UKF没有线性化近似，而是把sigma point进行非线性变换后再用高斯近似。
5. PF去掉高斯假设，以粒子作为采样点来描述分布。
6. 优化方式同时考虑所有帧间约束，迭代线性化求解。

　　呃好像题主还问了FastSLAM，有空再写吧……

注：
* 本文大量观点来自Timothy. Barfoot, "State estimation for Robotics: A Matrix Lei Group Approach", 2016. 图片若有侵权望告知。

PS：

　　SLAM中，状态变量经常是六自由度的位姿，由旋转矩阵和平移向量构成。然而问题是，旋转矩阵并不存在加法，只有对应到李代数上才可以清楚地定义它的运算。因此，当我们讨论这个位姿的噪声，说它服从高斯分布时，我们究竟在说什么，是一个很严重的问题。今后的博客将更深入地介绍李群李代数的知识。

 

 

1.SLAM与ROS的关系  转自https://zhuanlan.zhihu.com/p/68182016

1.1.关于SLAM

在了解SLAM之前，需要先对机器人有一个整体的认识。机器人是一个复杂的装置，涉及到执行机构、感知、决策等主要环节。机器人上的配备的常用执行机构有轮式运动底盘、机械手臂、音响和显示屏；机器人上的感知设备通常有激光雷达、声呐、摄像头、IMU、轮式里程计编码盘、麦克风、触摸感应；机器人的决策是机器人智能的体现，机器人通常借助感知装置持续跟外部环境进行交互，从而来获取机器人的状态和环境的状态，我们可以简单的把机器人获取自身状态的行为叫做自我认知，把机器人获取环境状态的行为叫做环境认知。机器人的自我认知和环境认知往往是相辅相成互相作用的，所以这里就不做区分了。由于目前的机器人智能还比较低级，所以这里讲到的机器人认知也是低级别的，例如人脸识别、语音识别、机器人定位、环境障碍物探测。有了认知，机器人就可以帮人类完成很多工作了，例如搬运货物、照看小孩、陪伴闲聊、帮忙管理家里的智能设备、查询天气交通新闻资讯等等。我们可以把机器人帮助人类完成的这些个工作叫做机器人的技能，机器人拥有的这些个技能我们可以简单的理解为机器人低级别的思想。机器人的躯壳+机器人的认知+机器人的思想，基本上就是机器人该有的模样了。

图1.1.1 一个典型机器人的模样

这样一看，机器人是一个超级复杂的东西，但是机器人上现在关键性的技术就那么几个。其实，学界和工业界热门的研究和开发也是围绕这几个关键技术展开的。关键技术1：机器人移动底盘，绝大部分机器人需要依靠移动底盘来行走，当然除了水下机器人和腿型机器人这些特殊形态外，电机的效率功率、电机控制电路、编码器检测、底盘运动模型等组成部分，机器人底盘是AGV智能车、家庭服务机器人、农业工业自动化移动平台必不可少的关键组件。关键技术2：机械手臂，机器人抓取物品、组装零件、写字画画都需要用到机械手臂，高性能的舵机、精密的机械关节、智能关节控制算法等几个组成部分，机械手臂在工业自动化领域已经得到广泛应用，比如汽车自动装配、手机零配件装配，不过应用在服务机器人和AGV智能车上的轻便微型机械手臂目前还不成熟或者说还不普及。关键技术3：SLAM导航，这里终于讲到了大名鼎鼎的SLAM技术了，学术上的SLAM是指同时定位和建图技术，但是平时大家说的SLAM通常是广义的机器人定位+环境建图+自主导航+动态避障，我们这里也讨论的是广义的SLAM，简单点说SLAM技术帮助机器人认识环境并在环境中自由行走到目的地。关键技术4：语音识别，语音交互是大部分机器人的标配，人类通过语音交互向机器人下达任务指令，语音交互包括语音识别、语音合成、自然语言处理等部分组成，目前基于语音交互做出来的小机器人也是遍地开花。关键技术5：物体识别，机器人借助摄像头对自己所处环境的各种事物进行识别，比如流行的人脸识别、车牌识别、手势识别等等。

关键技术1：机器人移动底盘

关键技术2：机械手臂

关键技术3：SLAM导航

关键技术4：语音交互

关键技术5：物体识别

本人在这里总结归纳了机器人上的5大关键技术，由于个人能力和篇幅限制原因，接下来的内容将主要涉足机器人SLAM导航领域的相关技术。

SLAM导航技术，主要用来解决机器人的定位、环境建图、自主导航、动态避障等问题。SLAM这项技术其实已经有几十年的历史了，SLAM最早是出现在军事应用中，比如勇气号火星探测车，在不能实时遥控的未知环境行星上的探测车为了执行任务，需要借助SLAM技术来导航和避障。后来慢慢的SLAM技术就从军用转民用了，有了我们现在看到的小到家里的扫地机器人大到无人驾驶汽车的各种SLAM应用，还有各种AR和VR应用很多也用到了SLAM技术。

其实单独的定位技术我们日常生活中能见到很多，比如最常用的GPS定位技术、wifi定位技术、磁条导轨定位技术。单独的环境建图技术也有很多成熟的应用，比如医学中的CT对人体的全方位扫描技术、电影制作中对某个三维物体的扫描建模、隧道勘探测绘等。但是面对机器人这样一个复杂的应用，单独的定位技术和单独的环境建图技术都不能很好的解决问题，于是结合了定位与建图的SLAM技术就出现了。接下来，简单的梳理一下定位、建图、SLAM同时定位于建图、SLAM导航技术的理论发展过程。

（1）机器人中的不确定性

机器人所处的环境存在大量不可预测性，机器人中传感器的测量存在噪声和干扰，电机等执行机构也存在一定程度的不可靠性，还有一些不确定性是由机器人的软件导致的。要研究机器人的感知和行为就必须对这些不确定性进行建模，利用概率理论可以明确的表示这种不确定性，并且可以利用概率算法来对概率分布进行推理计算，以数学的方式合理的表示机器人的模糊性和置信度。简单点说，就是通过对机器人的概率建模，可以对机器人的不确定性进行明确的可计算性的表示。

（2）机器人中的状态估计

处理机器人中的这种不确定性的概率技术我们称之为概率机器人技术，其核心是用传感器数据来估计状态的思路。

图1.1.2 机器人与环境交互

机器人通过传感器对机器人自身状态和环境状态进行测量，这里的机器人状态包括机器人位姿、运动里程计信息、线速度和角速度信息等，而环境状态包括环境中物体的位置、物体的特征等。机器人同时利用控制动作来调用执行机构和环境进行交互，这里的控制动作包括机器人运动、物体操纵等。这样，机器人在通过传感器测量和控制动作与环境进行交互的过程中，对环境模型和机器人位姿进行估计。对环境模型的迭代估计就是我们常说的环境建图，对机器人位姿的迭代估计就是我们常说的机器人定位。

利用传感器测量和控制动作对环境模型和机器人位姿进行估计由概率法则迭代计算来完成。控制动作的概率模型我们称之为运动概率模型，传感器测量的概率模型我们称之为测量概率模型，这里的概率法则迭代计算过程就是大名鼎鼎的贝叶斯迭代网络。

图1.1.3 贝叶斯迭代网络

（3）贝叶斯滤波及其各种实现算法

通过对机器人中状态估计的了解，我们知道了贝叶斯概率法则起着重要的作用，而且大部分计算置信度的通用算法都是由贝叶斯算法给出的。这里说的贝叶斯算法也称作贝叶斯滤波，该算法根据测量和控制数据计算置信度分布bel()。贝叶斯滤波是一种递归算法，也就是说t时刻的置信度由t-1时刻的置信度来计算。

贝叶斯滤波算法具有两个基本步骤，即预测和更新。根据运动概率模型，可以对初步的预测置信度分布；然后利用测量概率模型，对上一步预测得到的置信度分布做进一步的更新计算。贝叶斯滤波的具体实现算法有多种，根据置信度表示方式的不同，贝叶斯滤波的具体实现算法可以分为参数化实现算法和非参数化实现算法两种。参数化实现算法，就是将置信度分布用多元高斯分布的参数进行表示。根据采用不同高斯分布的参数，又可以分为卡尔曼滤波和信息滤波。非参数化实现算法，就是将置信度分布用有限数量的具体值来近似表示。根据采用不同具体值近似的方法，又可以分为直方图滤波和粒子滤波。

图1.1.4 贝叶斯滤波及各种具体实现算法

由于贝叶斯滤波理论及各种具体实现算法涉及到大量深奥的数学知识，由于篇幅和个人能力受限，就不展开了，有兴趣的朋友可以参阅《概率机器人》这本书，这里面有细致的讲解和推导。为了加深大家在阅读和编写代码时的理解，这里贴出了贝叶斯滤波的伪代码实现，和机器人中广泛应用的卡尔曼滤波和粒子滤波的展示实例。

图1.1.5 基础贝叶斯滤波的伪代码实现

图1.1.6 卡尔曼滤波的伪代码实现

图1.1.7 卡尔曼滤波对置信度分布的更新过程

图1.1.8 粒子滤波的伪代码实现

图1.1.9 粒子滤波对置信度分布的更新过程

（4）机器人中的运动与测量概率模型

了解了机器人中的状态估计及各种滤波概率算法后，我们发现机器人的运动和测量概率模型对算法中的预测和更新步骤起着至关重要的作用。

运动模型我们都知道，就是机器人一个地方运动到另一个地方，位姿变化与运动控制量之间的关系。那什么叫运动概率模型呢？由于控制量作用在机器人上使机器人的位姿变化的过程是一个存在误差的不确定过程，所以控制量作用后产生的结果需要用一个概率分布来描述，这就是运动概率模型。根据运动控制量的不同，可以分成速度运动模型和里程计运动模型。速度运动模型中，控制量是线速度和角速度；里程计运动模型中，控制量是机器人的前后时刻的相对位姿。速度运动模型中，线速度和角速度会受到机器人底盘构造的约束。实验表明，里程计运动模型虽然仍存在误差，但比速度运动模型要更精确。

图1.1.10 机器人的运动概率模型

同样的道理，机器人上的传感器在测量过程中存在误差，所以测量结果需要用一个概率分布来描述。主要讨论当下最流行的机器人测距仪，即激光扫描测距仪。由测距仪的近似物理模型，可以沿着一个波束测量到附近物体的距离，也就是波束模型。另一种测距仪模型不依赖任何传感器的物理模型，而是将传感器扫描到的终点值映射到地图的全局坐标空间，这就是似然域模型。估计一个测量和地图之间的相关性中，测距仪用到了地图匹配模型。还有一种是从传感器原始测量中提取特征，提取出来的特征我们可以称之为路标，这就是路标模型。

图1.1.11 机器人的测量概率模型

（5）移动机器人定位与建图

图1.1.12 移动机器人定位与建图

在了解了机器人中的状态估计、状态估计的各种概率实现算法、机器人运动与测量概率模型，我们再来重新理解上面这样一个经典的贝叶斯迭代网络，就很好理解了，这个里面包含了机器人定位和建图两大问题。机器人的定位其实就是位姿估计问题，位姿估计的概率实现算法可以有上面提到的EKF、AMCL等算法来实现，由于这两种算法在机器人位姿估计中都很流行，所以就不多展开了。机器人的建图其实就是环境特征估计问题，同样也可以用概率的方法就行估计。

（6）SLAM同时定位与建图

上面已经单独的提出了机器人定位与机器人建图的方法，但是独立的定位问题是建立在地图已知的情况下的，单独的建图问题也是建立在定位已知的情况下的。当机器人不能得到环境地图，也不知道自身位姿的时候，SLAM问题就出现了。也就是说SLAM要同时的进行机器人定位和建图，这个问题比单独的定位和单独的建图都要难得多。

在讨论具体的SLAM算法之前，我们先要了解SLAM的两种主要形式：在线SLAM问题和全SLAM问题。

图1.1.13 在线SLAM问题和全SLAM问题

在线SLAM算法的代表是EKF SLAM，历史上最早并可能是最有影响力的SLAM算法，可以说是SLAM研究的元老级算法。EKF SLAM中的地图是基于特征的，地图由点地标组成。除了估计机器人当前的位姿，EKF SLAM算法还估计路径上遇到的所有地标的坐标，也就是机器人位姿和地图地标点包含进联合状态矢量里，算法对该联合状态矢量进行估计。

图1.1.14 EKF SLAM伪代码实现

全SLAM算法的代表是GraphSLAM，该算法将机器人的运动路径和测量组件成一个软约束的图，利用图论中的优化算法对整个图中的轨迹点和测量点进行估计。由于图的稀疏性特点，可以大大加快计算。

图1.1.15 GraphSLAM伪代码实现

EKF SLAM和GraphSLAM是两个极端。EKF SLAM需要取得每一时刻的信息，把信息分解为概率分布，因此每一步的计算代价都非常昂贵。而GraphSLAM刚好相反，只是简单的积累每一时刻的信息，也就是简单的将收到的信息存储下来，然后可以离线的进行推理的步骤，加之存储下来的信息的稀疏性，因此GraphSLAM计算的开销是比较小的，但是随着地图规模扩大算法会消耗越来越多的内存直至崩溃。面对这两个极端问题，当然就会有介于EKF SLAM和GraphSLAM是两个极端之间的折中的方法，就是SEIF SLAM。SEIF SLAM算法继承了EKF SLAM信息表示的高效性，也保留了GraphSLAM计算代价小的优点，可以说SEIF SLAM是高效和可实现的SLAM算法。还有另外一种高效和可实现的SLAM算法，就是FastSLAM，该算法使用粒子滤波估计机器人的路径，我们都知道粒子滤波和众多基于参数化的滤波算法相比存在计算开销小和便于处理非线性模型的优势，基于FastSLAM的多个变种算法在机器人已经得到广泛的应用了，比如gmapping等等，由于篇幅限制就不展开了。

（7）现今主流的SLAM算法

现今在机器人上使用最广泛的应该算激光SLAM了，在扫地机器人、服务机器人、AGV智能车上普遍搭载了单线激光雷达SLAM算法，像无人驾驶汽车、户外机器人则普遍搭载了多线激光雷达SLAM。另一种热门的研究是视觉SLAM，视觉SLAM有配备单目、双目、深度相机的多种形态，并且根据采用视觉特征点的区别还有直接法、半直接法、稀疏法之分。然后还有就是各种复合式的SLAM算法，比如激光与视觉融合的SLAM、融合了IMU的视觉SLAM。最后，就是一些最新颖的SLAM算法，比如用深度学习来做的端到端的SLAM、基于物体识别的语义SLAM。由于本文的重点不是SLAM综述，所以具体的算法性能比较就不展开了，有兴趣的朋友可以参阅相关SLAM综述文章。

图1.1.16现今主流的SLAM算法

（8）机器人自主导航与动态避障

机器人用SLAM构建出了环境的地图，在已知了环境地图的情况下，可以用SLAM的重定位功能或者单独的基于已知地图的定位算法比如AMCL来进行机器人的定位。环境地图和机器人位姿都有了，就可以开始来做自主导航和避障了。机器人自主导航可以分成两个实现部分，第一个部分就是路径规划，第二个部分就是控制策略。路径规划利用地图信息寻找一条能到达目标的全局路径，全局路径在机器人导航过程中起到全局战略性的指导。理想情况是，机器人完全按照全局路径移动到目标，但是实际环境往往是多变和复杂的，而且机器人实际控制也会存在偏差，所以机器人的实际运动控制需要有一套控制策略来最终实现。控制策略需要尽量逼近全局路径、尽量远离障碍物、最快时间到达目标等因素，这些因素可以用一个回报函数来评价，寻找最佳控制策略的过程中递归的计算每一次行动的回报函数值。这样控制策略在回报函数的指引下，就可以给出最佳的控制策略，控制策略控制机器人完成实际的移动。

图1.1.17 主流路径规划算法

1.2.关于ROS

ROS机器人操作系统在机器人应用领域很流行，依托代码开源和模块间协作等特性，给机器人开发者带来了很大的方便。

ROS是一个适用于机器人的开源的元操作系统。其实它并不是一个真正的操作系统，其底层的任务调度、编译、寻址等任务还是由Linux操作系统完成，也就是说ROS实际上是运行在Linux上的次级操作系统。但是ROS提供了操作系统应用的各种服务（如：硬件抽象、底层设备控制、常用函数实现、进程间消息传递、软件包管理等），也提供了用于获取、编译、跨平台运行代码的工具和函数。ROS主要采用松耦合点对点进程网络通信，目前主要还是支持Ubuntu系统，windows和Mac OS目前支持的还不好，所以推荐在Ubuntu系统上安装使用ROS。

总结起来就是，使用ROS能够方便迅速的搭建机器人原型。ROS使用了BSD许可证，这是一个很宽松的开放许可证，允许在商业和闭源产品中使用，这一点对开发产品的创业公司很重要。ROS当前的代码统计量，总行数超过1400万，作者超过2477名。代码语言以C++为主，63.98%的代码是用C++编写的，排名第二的是python，占13.57%，可以说ROS基本上都是使用这两种语言，来实现大部分的功能。

图1.2.1 ROS的图标

大家一听到ROS机器人操作系统，就被操作系统几个字给吓到了。其实，ROS就是一个分布式的通信机制，帮助程序进程之间更方便的通信。由于机器人是一个多部件的设备，四肢、视觉、听觉等部位都有配套的程序进行控制，那么要协调这些部位怎么办呢？就需要让这些分散的部位能够互相的通信，这正是ROS被设计的最初目的。随着越来越多的人参与到ROS的开发完善，ROS才发展成今天很完善很系统的样子来，涵盖了大量的第三方工具和大量的实用的开源算法软件包。搞懂了ROS的通信机制后，机器人的各种算法的开发还是基于我们常见的C++和Python的。

节点是主要的计算执行进程，功能包中创建的每个可执行程序在被启动加载到系统进程中后，该进程就是一个ROS节点，node1、node2、node3等都是节点（node）。节点都是各自独立的可执行文件，能够通过主题（topic）、服务（server）或参数服务器（parameter server）与其他节点通信。ROS通过使用节点将代码和功能解耦，提高了系统的容错力和可维护性。所以你最好让每一个节点都具有特定的单一的功能，而不是创建一个包罗万象的大节点。节点如果用c++进行编写，需要用到ROS提供的库roscpp；节点如果用python进行编写，需要用到ROS提供的库rospy。

图1.2.2 ROS网络通信的架构

如果你是刚刚接手ROS方面的开发或项目，你肯定会觉得ROS中的各种概念非常奇怪，但是当你对ROS的使用熟练之后，你就觉得这些概念很好理解了。与其他操作系统相似，一个ROS程序的不同组件要被放在不同的文件夹下，这些文件夹是根据不同的功能来对文件进行组织的。

图1.2.3 ROS程序的组织形式