激光SLAM从理论到实践学习——第五节（基于滤波的激光SLAM方法（Grid-based））

基于滤波的激光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时刻的后验分布。然后下一时刻来临时，迭代如上过程。

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

• 存在的问题

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

Gmapping原理：FastSLAM的原理及优化（先估计位姿再建图）

• 前言：是基于粒子滤波的，基于RBpf。就是gmapping的基本原理。

• FastSLAM介绍

  • SLAM:在给定传感器数据的情况下，同时估计机器人位姿和环境地图。

    他估计的是1到t时刻的，看公式可知，他在里程计数据u1:t和激光雷达数据z1:t已知的情况下，对机器人位姿x1:t和状态地图m的估计。他的概率分布就可以这样表示。

    

  • SLAM可以分解成两个问题（这个分解就是RB分解，所以叫RBpf）

    RB分解公式如下，对应刚分解的这两个问题，定位是a，构图是b，那么我们求解这个方程P(a,b),图中写成了p(a|b)，只需要知道P(a)即可，P(b|a)自然就知道了，所以说已知位姿，建图就很简单了。

    

    

    • 机器人的定位

    • 基于已知机器人位姿的构图

      这是个简单问题，因为我们已经知道机器人位姿了，激光雷达的数据是比较精准的，所以只需要把数据迭代上去，把图建出来。

    • 功能：用粒子滤波用来估计机器人位姿，知道一个粒子的路径之后，就可以然后分别为每一个粒子计算一个地图。

      x1:t如果已知，那么自然就可以求出p(m|x1:t,z1:t)，这个概率分布也就是建图了。所以只需要求解x1:t即可，也就是机器人的位姿（这里指的是1:t时刻，所以应该叫路径）

      所以一个粒子包含的数据有：机器人的轨迹x1:t，对应的环境地图。

• 算法流程

  那么怎么求位姿概率（在这里是1到t时刻的，所以是路径）分布呢？

  公式推导需要结合贝叶斯估计和粒子滤波。

  

  对x1:t的估计实际上就是估计1到t时刻的路径，所以他说转换为了一个增量估计的问题。

  p(x1:t-1|z1:t-1,u1:t-1)是一个粒子群，每个粒子都是用运动学模型进行传播都是根据前一时刻粒子的分布与ut对当前粒子分布进行估计，然后对传播后的粒子用观测模型进行权重计算，并根据估计的位姿构建地图。

• 存在的问题及优化

  • FastSLAM优化1

    • 问题：粒子过多导致内存问题 目的：保持粒子在一个相对较少的数值

    • 解决方法：

      在估计出粒子的分布之后，用当前雷达数据zt观测 当前粒子与地图，取最优粒子

  
  

  • FastSLAM优化2：粒子耗散问题
    • 问题：随着重采样次数的加多，粒子的多样性会减少，即最终所有的粒子都来自同一个粒子复制过来或者少数几个粒子复制过来的，对建图致命
    • 方法：减少重采样次数，设置一个阈值来确定是否重采样
      
      粒子多样性减少，因为重采样之前会计算每个粒子的权重来进行粒子的过滤，
      但是由于环境相似度高或噪声原因，粒子的权重可能被颠覆，导致权重高的粒子被删除，
      然后把剩下未删除的粒子进行复制（导致粒子多样性的减少）
      解决方案：选择性重采样：设置阈值来减少重采样次数。

  • FastSLAM优化3：进一步优化proposal分布，第三类优化（gmapping中没有的，本次作业会让我们修改gmapping源码）

    • 注：如果里程计很准，那么这个优化就没什么用。激光雷达的匹配比里程计的测量要精确很多，从分布上来说，激光雷达匹配的方差要比里程计模型的方差小很多。如下图可知，尖的地方是雷达的分布，扁平的是里程计的分布
    • 上面的优化方式：首先从proposal分布进行采样，然后进行极大似然估计提升采样的质量。
    • 本次优化方式：考虑最近一帧激光雷达的观测值

    

    • 如下图所示，激光匹配的方差比里程计要小得多，如果proposal分布用激光匹配来表示，则可以把采样范围限制在一个比较小的区域，因此可以用更少的粒子集覆盖机器人的概率分布（粒子会少用很多）。

      • 推导过程：首先我们在概率分布中加入雷达数据zt和地图m

        随后又可以进行贝叶斯分解

        

        因为里程计预测是一个常数，所以可以并到前面的n中。因此：

        

        这样就是说proposal分布从里程计观测模型就变成了激光雷达观测模型。

      

    • 因为激光雷达观测模型的方差很小，假设其服从高斯分布

      

      求解高斯分布：

      

    • 计算出高斯分布后，权重计算也发生变化

    

  • 换了一个预测分布，这个预测分布的方差小，所以需要的粒子数就小

  • 总结一下流程：

    我们已知t-1时刻的粒子群，激光雷达的观测数据，里程计数据（t时刻和t-1时刻差不多），要得到一个新的粒子群xt。

    scan-match和采样是对每个粒子都要进行的。

    来一个里程计数据，然后进行积分得到一个位姿，在这个位姿的基础上进行scan-match得到一个极值，然后在这个极值附近采K个位姿，默认这K个位姿是服从高斯分布的，然后计算高斯分布（高斯分布所用粒子数比里程计所用粒子数少，所以采用高斯分布）

    

    

    是与阈值进行判断

FastSLAM总结

他的目标就是在里程计数据u1:t和激光雷达数据z1:t已知的情况下，对机器人位姿x1:t和状态地图m的估计，目标函数为，然后我们只需要估计当前机器人位姿xt即可知道地图，因为地图是根据位姿xt和激光雷达的数据（激光雷达数据是比较精准的，所以只需要把数据迭代上去，把图建出来）算出来的概率分布。

问题就是如何求解求位姿概率（在这里是1到t时刻的，所以是路径）分布呢？

可以通过公式推导（需要结合贝叶斯估计和粒子滤波）计算得到。

对x1:t的估计实际上就是估计1到t时刻的路径，所以他说转换为了一个增量估计的问题。

p(x1:t-1|z1:t-1,u1:t-1)是一个粒子群，每个粒子都是用运动学模型进行传播都是根据前一时刻粒子的分布与ut对当前粒子分布进行估计，然后对传播后的粒子用观测模型进行权重计算，并根据估计的位姿构建地图。

三种优化方式总结

• 前两种在运用在了Gmapping中，第三种是我们要加上的。

• FastSLAM优化1：粒子过多

  • 问题：粒子过多导致内存问题 目的：保持粒子在一个相对较少的数值

  • 解决方法：

    在估计出粒子的分布之后，用当前雷达数据zt观测当前粒子与地图，取最优粒子

    

• FastSLAM优化2：粒子耗散问题

  • 问题：随着重采样次数的加多，粒子的多样性会减少，即最终所有的粒子都来自同一个粒子复制过来或者少数几个粒子复制过来的，对建图致命
  • 方法：减少重采样次数，设置一个阈值来确定是否重采样

• FastSLAM优化3

  我们已知t-1时刻的粒子群，激光雷达的观测数据，里程计数据（t时刻和t-1时刻差不多），要得到一个新的粒子群xt。scan-match和采样是对每个粒子都要进行的。

  然后来一个里程计数据，然后进行积分得到一个位姿，在这个位姿的基础上进行scan-match得到一个极值，然后在这个极值附近采K个位姿，默认这K个位姿是服从高斯分布的，然后计算高斯分布（高斯分布所用粒子数比里程计所用粒子数少，所以采用高斯分布）

Gmapping的介绍

• 是目前使用最广泛的2D激光SLAM算法

• 在较小环境中能实现较好的建图效果

• 以FastSLAM为基本原理（粒子滤波）

• 在FastSLAM的基础上进行了优化1和优化2（上面讲过）

• 没有使用优化3（作业要实现优化3，FastSLAM中介绍过，参考文献中有优化3的完整流程）

• 

•