基于滤波的激光SLAM方法（Grid-based）

• 前言：本章介绍第一个SLAM系统。基于滤波的激光SLAM方法相对落后了，滤波是只估计当前时刻的状态，如果前一时刻出现错误，就无法进行修正了。
• 前言：Grid-based栅格地图，一般有两种地图，一个是栅格地图，还有一个是特征地图，视觉内用特征地图。

贝叶斯滤波

• 前言：状态估计大部分都是用贝叶斯的。

• 数学概念（概率里面的东西）

  

• 贝叶斯滤波特性

  • 1.估计的是概率分布，不是具体的数值（一切都是概率，典型贝叶斯方法是最大后验估计：估计出数值的后延概率分布，然后找到概率最大值的地方）

  • 2.是一大类方法的统称

  • 3.是一个抽象的表达形式，没有具体的概率分布形式。对于不同问题有不同的实现方法（卡尔曼家族、粒子滤波）

  • 4.迭代估计方式：计算量较小

    

• 贝叶斯滤波的推导

  • 理论部分

    最终是式子是：

    

    凡是可以写成这种形式的，都是贝叶斯估计。

    bel表示后验概率，bel上横线表示预测分布，就是积分中的一大段，里面表示的是，

    已知xt-1（可以说是t-1时刻机器人的位姿） 的状态

    

    得到一个控制量，预测xt（可以说是t时刻机器人的位姿）的状态，叫预测量

    

    就得到了一个新的分布，再用观测值对这个分布进行校正。

    看代码

    

    首先我们已知xt-1时刻的概率分布了，也已知ut和zt，对于我们机器人系统来说，ut就是里程计的测量值，zt就是激光雷达的测量值。首先我们就通过t-1时刻里程计的测量值ut，t-1时刻的位姿xt-1得到一个预测分布，预测t时刻机器人的位姿。

    然后在预测的基础上，用预测模型对观测模型进行校正，校正后就得到了一个新的分布，就是。

    所以他的功能就是：已知状态量t-1时刻的概率分布（已知t-1时刻机器人位姿），在给定t时刻的（机器人的）观测数据（zt，ut）的情况下估计状态量在t时刻的概率分布（估计t时刻机器人的位姿）。

    卡尔曼滤波是贝叶斯滤波在线性高斯的特殊情况，它符合贝叶斯滤波的流程。

  • 实际推导

    

    目标：在已知p(xt-1|z1:t-1,u1:t-1)、ut、zt的情况下，得到p(xt|z1:t,u1:t)的表达式。

    第一步和第二步过程可通过笔记中的贝叶斯公式转换得到。

    

    其中：

    因为激光雷达观测值zt跟前一时刻观测值zt-1是没有关系的，当我们知道xt的情况下，那么zt肯定和u1:t也没有关系，所以说zt只跟机器人的位置有关系。所以可以化简成如下方式。

    

    因为我们要化成迭代的方式，所以xt必须从xt-1里分解开来，所以他就变成了一个全概率公式。

    

    再根据笔记中的全概率公式，把又式一个积分号可变成两个，然后这一项的积分是等于1的（在全概率的公式中），所以就剩这一项了。

    显然xt是由xt-1来的，xt跟zt是没有关系的，xt跟前一时刻里程计数据u1-t也是没有关系的，也就是说，我们在知道xt-1的情况下，那么xt就只跟ut相关了。那么就可以化简成如下式。这其实就是机器人的运动模型，知道xt-1（上一时刻位姿），知道ut（当前时刻里程计数据），要求xt（当前时刻机器人位姿）。

    

    全概率公式中这一项积分等于1，我们知道xt-1这个时刻，跟ut是没关系的，所以把ut去了。

    

    令：

    看公式就知道，就是最后推导的式子，他表示xt的预测概率分布。

    则表示xt的后验概率分布。

    整个式子贝叶斯估计式子就可以写成：

    

    这个式子也就是说一个预测分布乘以一个观测，然后观测对预测分布进行校正就会得到我们的后验概率分布。

    其中我们的预测分布就是这个：

贝叶斯估计的总结

整个过程可以这么理解：因为xt-1时刻的概率分布（t-1时刻的位姿）是已知的了，然后在xt-1时刻的概率分布上与ut对 xt进行预测，这就是预测分布。然后预测分布再对观测模型进行校正，得到后验概率分布。

然后贝叶斯滤波的特性就是估计出数值的后验概率分布，然后找到概率最大值的地方。

他是一个迭代估计的过程，也就是不断根据前一时刻的概率分布区估计当前时刻的概率分布。

上面的过程再说大白话一点就是：

我们已知xt-1时刻的位姿，然后通过当前里程计数据ut与上一时刻位姿对xt进行预测（看公式就知道，xt成立的条件是xt-1和ut），然后就得到了预测分布，随后使用当前激光雷达数据zt进行观测，，用预测模型对观测模型进行校正所以就得到了新的当前时刻的概率分布。就从t-1时刻的概率分布，估计出了t时刻的概率分布，然后只需要给定x0，也就是0时刻的概率分布，就可以一直迭代了，这就是贝叶斯估计。

贝叶斯滤波实例

一开始机器人是先做观测再做预测之后再观测，开始时机器人完全不知道自己在哪，也就是说他可以在机器人状态地图中的任何一个地方，所以在任何一个地方出现的概率都是等可能的，所以第一次是均匀分布的。然后开始走了，然后就开始按照我们的运动模型进行传播了，当他第一次走到门附近时，他认为我们的概率分布还是一个均匀分布（因为是第一次），假设机器人身上有个检测门的传感器，当他检测到门时，他们的概率分布是最高的，这个时候就不是均匀分布了，他会出现尖峰。然后继续运动，到了第二个门时，他此时的概率分布是尖的，但相比之前要扁平（因为里程计会有噪声），而且我们发现他卷积之前，每扇门的尖峰都在随着里程计的增加而增加，等到下一状态也就是找到第二扇门的时候，他就进行卷积，得到当前机器人的位姿，也就是最下方那张图那样的尖峰就出现了，这样就定位到了下一时刻机器人的位姿。

简述一下这个过程：当机器人在一个未知环境中时，他在地图中的位姿概率分布式一个均匀分布（因为是未知的，所以他可以在地图中任意一个地方出现），然后当他的传感器检测到第一扇门时，他确定了他的初始位姿，此时贝叶斯估计出来的概率分布图中会出现一个尖峰，此后的每扇门都会出现这样的尖峰。然后机器人开始继续运动，此时概率分布图会相对扁平（因为里程计运动会产生噪声），当他检测到第二扇门时，他开始进行贝叶斯估计，找到当前位姿。

走—》预测—》观测—》预测—》观测…

粒子滤波

• 前言：是贝叶斯滤波的一种特例或说实现方式，贝叶斯滤波是滤波的一种统称

• 示意图与特性：

  • 是贝叶斯估计器的一种实现方式
  • 能处理非线性情况
  • 能处理多峰分布的情况：用于全局定位
  • 用一系列的粒子近似概率分布：概率大的地方粒子就多
  • 非参滤波器
    

• 流程

  • 粒子滤波使用一系列粒子来表示一个概率分布。以定位问题为准：

    X为t时刻（当前时刻）粒子集的概率分布。

    

    如图，我们看到图中尖的地方粒子就密集，平的地方粒子就稀疏。

    权重、重采样（去除权重小的粒子），重采样后权重清零。

    在机器人定位中，我们是用一个假设的位姿与机器人地图匹配度来评估粒子的权重。

  • 粒子滤波的流程就是：

    • 1.首先用粒子进行状态传播

      对每个粒子使用贝叶斯估计得到一个预测分布之后，根据数据ut预测t-1时刻的概率分布（粒子分布）得到的是一个具体的位姿。如何进行传播的？用机器人的运动模型进行传播。

      • 传播的数学模型是预测概率分布，我们已知t-1时刻的粒子分布，根据当前里程计数据ut预测t-1时刻的粒子分布。

      
      错误：图中根据ut预测t-1时刻的概率分布（粒子分布），t-1改成t
      预测分布即为从运动学模型中进行采样。我们没有观测值的时候进行状态传播时，传播会越来越扩散，越来越大。

      状态传播的公式就是P(xt|xt-1,ut)，公式的意思就是通过t-1时刻的粒子分布于当前t时刻里程计数据估计当前t时刻的粒子分布。而前一时刻t-1时刻的粒子概率分布是从0时刻均匀分布后不断通过传播模型（贝叶斯滤波中的预测分布）传播得到的概率分布。然后传播后我们就得到了一个预测值，当下一时刻来临，此时刻就变成t-1，根据里程计数据ut与前一时刻（t-1时刻）的粒子分布预测t时刻的粒子分布，他其实就是贝叶斯滤波的一种，在贝叶斯中把位姿分布概率理解成粒子分布概率即可。 在代码中就是通过运动学模型公式进行实现的。看图就知道我们进行状态传播之后得到的是一个粒子云的分布。

      

    • 2.计算每个粒子的权重（粒子与地图匹配程度），进行观测校正。权重评估（机器人位姿与地图的匹配程度也就是P(zt|xt)的概率）。

      • 因为无法知道机器人位姿的实际分布。

      • 从机器人的预测分布进行采样，联合权重一起近似机器人的后验概率分布。

      • 权重用来评估实际机器人的预测分布和实际分布的差，差越大，权重越小。

      • 对于某一个粒子，得预测概率分布和后验概率分布。

      • 得到权重为当前粒子的后验分布/预测分布

        

        

    • 3.根据权重进行重采样（去除低权重的粒子，删除，还能减少计算量）

      • 到目前为止，新的粒子群是根据proposal分布进行采样的，并且用观测模型计算权重，而最终的目的是用粒子群来近似后验概率分布。
      • 对粒子群进行重采样，对于某一个粒子xi来说，以wi的概率接收这个粒子。
      • 生成一个随机数，根据其落在的区间确定接收的粒子，重复N次。权重大的粒子就会被多次选中，权重小的粒子就会被删除。
      • 重采样的过程：用proposal分布的粒子和观测模型的权重，生成符合后验概率分布的粒子群。
        

  • 粒子滤波算法流程代码形式（他和贝叶斯滤波的目的都是估计状态用）

粒子滤波总结

粒子滤波的算法流程如下：首先确定一个t-1时刻的粒子集，且我们知道t时刻的观测数据（里程计ut，激光雷达zt），我们对于每一个粒子都进行运动学模型传播，状态传播的公式就是P(xt|xt-1,ut)，公式的意思就是通过t-1时刻的粒子分布于当前t时刻里程计数据估计当前t时刻的粒子分布。而前一时刻t-1时刻的粒子概率分布是从0时刻均匀分布后不断通过传播模型（贝叶斯滤波中的预测分布）传播得到的概率分布。然后传播后我们就得到了一个预测值，当下一时刻来临，此时刻就变成t-1，根据里程计数据ut与前一时刻（t-1时刻）的粒子分布预测t时刻的粒子分布，他其实就是贝叶斯滤波的一种，在贝叶斯中把位姿分布概率理解成粒子分布概率即可。然后用我们算出来的观测模型（此时刻的粒子的概率分布）进行权重评估（粒子的图匹配程度）给每个粒子评分，然后再进行重采样，丢弃无用粒子，让粒子更加符合我们的后验分布，然后返回粒子集就得到了我们t时刻的后验分布。

• 跟贝叶斯估计是很像的。
  

• 存在的问题

  • 它存在粒子耗散的问题，粒子多样性的丧失在建图中是致命的，他主要原因是重采样带来的。
  • 维数灾难，因为可能环境出现六维，那么粒子数会呈指数增长
  • 里程计差

FastSLAM的原理及优化

• 前言：是基于粒子滤波的

Gmapping的介绍