SLAM学习笔记——基于滤波器的激光SLAM方法(Grid-based)

1 篇文章 0 订阅

         这一算法已经比较落后了, 因为滤波是用于估计当前的状态,如果我们之前的数据出现错误,是没有办法进行修正的,所以这个方法的整体效果并不是特别好。但是这个方法在整个激光SLAM的发展中还是占有很重要的地位的,而且这里蕴含的很多数学知识是非常重要的,所以我通过这篇文章详细说明一下吧。 

         这一方法生成的地图叫Grid-based,也就是栅格地图。还有另一种地图叫feature-based,也就是特征地图。一般依靠视觉生成的地图就是feature-based,激光雷达生成的地图一般是Grid-based,一般用激光(尤其是二维激光雷达)生成的Grid-based是可以用来导航的,但是用视觉生成的地图是很难用于导航的。基本介绍做完了就进入正片。

目录

贝叶斯滤波

1.1贝叶斯滤波特性

1.2贝叶斯滤波的推导

粒子滤波

2.1粒子滤波特性

2.2粒子滤波的流程

2.2.1状态传播

2.2.2权重评估

2.2.3重采样

2.3粒子滤波总结

2.3.1算法流程

2.3.2存在的问题

FastSLAM的原理及优化

3.1FastSLAM介绍

3.2算法流程

3.3存在的问题及优化

3.3.1问题一

3.3.2问题二

3.3.3进一步优化proposal分布

总结


贝叶斯滤波

        我们目前大部分状态估计用到的滤波基本都是基于贝叶斯滤波的。熟悉数理统计的应该知道数理统计领域分为两大流派:频率学派(古典学派)和贝叶斯学派。一般我们认为在实际生活中,贝叶斯学派更符合实际情况,所以我们生活中常见的各种估计其实是基于贝叶斯的。所以贝叶斯滤波其实是一大类方法的统称。 

         我们开始进入正题学习贝叶斯滤波,首先就是贝叶斯滤波的基本数学概念:

        上面这些都是比较基础的知识,就不做过多的解释,接下来我们看一下贝叶斯滤波是怎么一回事。

1.1贝叶斯滤波特性

  1. 估计的是概率分布,不是具体的数值。这一点就是和频率学派不同的地方之一,频率学派估计的典型方法是MLE(极大似然估计),估计的是具体的数值;而贝叶斯估计的典型方法是MAP(最大后验估计),估计的是概率分布。这里顺便说一下我对量大学派的理解,频率学派认为,有些参数它仅仅就是一些参数,我们要想办法估计这些参数;而对于贝叶斯来说,没有任何的参数,一切都是概率分布。
  2. 是一大类方法的统称。
  3. 是一个抽象的表达形式——对于不同问题有不同的实现方式(卡尔曼家族、粒子滤波)。
  4. 迭代估计形式。这个形式可以减少很多的计算量。

1.2贝叶斯滤波的推导

        所有的贝叶斯滤波最终都能推导成下图的形式:

         我们来具体分析一下怎么得到这个式子:

        我们用来表示一个量的后验概率分布,用来表示proposal分布,等于,表示一个预测的过程,意思就是我们已经知道xt-1的状态了,然后根据一个预测量来预测xt的状态得到一个新的分布,然后用(观测量)对这个新的分布进行校正。

        以上就是贝叶斯滤波的大致过程。

        回到上一条所说的第二个特性,凡是能推导成这种形式的滤波,都称为贝叶斯滤波。所以熟悉卡尔曼滤波的人可以看出来,卡尔曼滤波是贝叶斯滤波在线性高斯情况下的特例。

        现在开始正式对贝叶斯滤波进行推导。

        首先根据刚才分析的表达式,我们确定一个目标:已知、𝑢𝑡、𝑧t的情况下,得到的表达式。

        根据条件贝叶斯公式:

 其中:

 

 这个式子其实就是机器人的运动模型。

 

 令:

表示xt的后验概率分布;

表示xt的proposal概率分布。

 则:

粒子滤波

        粒子滤波其实是贝叶斯滤波的一种特例,换句话说,粒子滤波是贝叶斯滤波的一种实现方式。粒子滤波用的应用场景就很多了,比如说动态物体跟踪、机器人定位、SLAM等等。当在视觉中用滤波器做SLAM往往会出现很多问题,原因就是滤波器要同时估计机器人的位姿以及feature-based,随着运行feature-based会越来越大,但越慢。所以视觉中用滤波器做SLAM只适用于小环境。说到这里也许有人会问了,为什么激光雷是运用卡尔曼滤波法要对状态向量的立方求逆,随着feature-based的增大,状态向量也会越来越大,这就会导致程序运行越来达用滤波器做SLAM就可以了? 答案是激光雷达的SLAM算法里面,把估计位姿和地图的问题分成了两个问题单独讨论(RB Particle-filter)。但是由于之前提到的错误没办法修正的,所以这个方法也不适用于大的环境。所以随着环境的增大,出错的可能性也就越来越大,我们称主要造成这个情况的问题叫粒子耗散问题,很可惜这个问题是没办法解决的。

        综上所述,粒子滤波法因为粒子耗散问题的存在,也不适用于大环境的SLAM,但是它在小环境也是有不错的效果的。

        现在我们开始具体分析一下什么是粒子滤波。

2.1粒子滤波特性

  1. 是贝叶斯滤波的实现方式。
  2. 能处理非线性情况。
  3. 能处理多峰分布的情况。(多峰分布的典型就是全局定位)
  4. 用一系列的粒子(particle)近似概率分布。
  5. 非参滤波器。这个的意思就是粒子滤波是用一系列的粒子表示的,没有参数。

        示意图:

         这些特性非常适合用于机器人,比如说机器人所涉及的很多问题就是非线性情况,还有机器人的全局定位就是利用了多峰分布的特性。

2.2粒子滤波的流程

        以下面的式子说明:

 表示一个状态的假设——在定位中可以表示为机器人的位姿信息。

 表示假设的权重——在定位中可以表示为跟地图的匹配度。

粒子滤波工作的大致流程为:

  1.  用粒子进行状态传播。
  2. 评估每一个粒子的权重。
  3. 根据权重进行重采样——去除权重小的粒子。以的概率接受,权重清零。

示意图:

下面将对各个流程进行详细解释。

2.2.1状态传播

传播模型:

根据之前提到的

可以把t-1时刻的概率分布(粒子分布)表示成:

根据数据ut预测t-1时刻的概率分布(粒子分布):

因为这里是用粒子表示分布,那我们就可以对每个粒子进行单独的预测。所以这个传播模型就是对每个粒子进行传播,那这个传播模型就涉及到了机器人的运动模型的知识了。

运动学模型:

 从预测分布即为运动学模型中进行采样:

        设t-1时刻第i个粒子的位姿:

        设t-1时刻的

        设t时刻第i个粒子的位姿

        噪声为0均值的高斯分布的方差分别为Nx,Ny,Nz,则这个传播模型为:

        

如果用图像来表示的话:

可以看到随着传播 ,粒子扩散越明显,如果放任不管的话,这些粒子会逐渐趋向均匀分布,所以当我们有观测值的时候,我们就可以通过重采样把扩散的粒子(即权重小的粒子)除去,这样就可以使得粒子重新聚集。

2.2.2权重评估

        以定位为例,权重就是地图和机器人位姿的匹配程度。这里有几点需要知道:

  1. 我们无法知道机器人位姿的实际分布。
  2. 从机器人的预测分布进行采样,联合权重一起近似机器人的后验概率分布。
  3. 权重用来评估实际人的预测分布和实际分布的差,差越大,权重越小。
  4. 权重的定义:

对于某一个粒子:

因此权重为:

 ——这个是观测模型

——这个是不重采样的更新方式,每一次重采样过后都会进行一次权重更新。

下面用一组图片来说明一下:

图中的g表示proposal分布为先验概率分布,f表示后验概率分布进行权重评估后:

可以看到权重大小的值跟真实的数值变化趋势很相似。但是大量的例子分布在了低权重区域,所以我们需要进行后续操作。

2.2.3重采样

  1. 到目前为止,新的粒子群是根据 proposal分布进行采样的,并且用观测模型计算权重,而最终的目的是用粒子群来近似后验概率分布。
  2. 对粒子群进行重采样,对于某一个粒子xi来说,以wi的概率接受这个粒子。
  3. 生成一个随机数,根据其落在的区间决定接受的粒子,重复N次。

我们可以借助下面的图像理解一下重采样的过程:

最终,权重更大的粒子会被多次接受,权重小的粒子很可能没有被接受。

所以重采样的目的就是用proposal分布的粒子和观测模型的权重,生成符合后验分布的粒子群。

那么上面的例子的重采样的效果图:

可以看到评估的效果已经很好了。

2.3粒子滤波总结

2.3.1算法流程

 

2.3.2存在的问题

  1. 粒子耗散问题,简单来说就是粒子多样性的散失,这是重采样我们复制了一些权重大粒子,丢掉了一些其他的粒子带来的问题。这个问题对定位的影响不大,但是对建图的影响是灾难性的。
  2. 维数灾难,这个就是粒子数会随着维数的增加呈指数增长,这也是在视觉SLAM中不会用到粒子滤波的原因之一。
  3. 当proposal比较差的时候,需要用很多的粒子才能较好的表示机器人的后验概率分布

FastSLAM的原理及优化

        FastSLAM算法是基于粒子滤波的,也是Gmapping算法的基本原理。

3.1FastSLAM介绍

  • SLAM:在给定传感器数据的情况下,同时估计机器人的位姿和环境地图:
  • SLAM可以分解成两个问题(RB Particle-filter):
  1. 机器人的定位。
  2. 基于已知机器人位姿的构图。因为激光雷达的测距很精准,所以这个是可以很好做出来的。可以认为这个问题是有close-form解的。

上面的这个分解用数学公式来表示:

其中为估计机器人的路径(位姿);为给定机器人位姿和传感器观测数据的情况,进行地图构建。

        所以这个算法就是用粒子滤波来估计机器人的位姿,然后分别为每一个粒子计算地图即可。因此一个粒子包含以下数据:机器人的轨迹和对应环境的地图。 

3.2算法流程

        我们回顾一下贝叶斯滤波:

描述:

        上式把对的估计转换成一个增量估计的问题。用粒子群表示,每个粒子用运动学模型进行传播。 对于传播之后的粒子,用观测模型进行权重计算,并且根据估计的位姿构建地图。

3.3存在的问题及优化

        因为FastSLAM算法是基于粒子滤波的,所以前面提到的粒子滤波存在的问题同样存在于FastSLAM算法中,我们现在研究一下怎么去优化。

3.3.1问题一

        问题:每一个粒子都包含自己的栅格地图。对于稍微大一点的环境来说,每一个粒子都会占用比较大的内存。如果机器人的里程计误差比较大,即proposal分布跟实际分布相差较大,则需要较多的粒子才能比较好的表示机器人位姿的后延概率分布,会造成内存爆炸

        目的:要保持粒子的数量在一个比较小的数值。

        方法:提升proposal分布采样的位姿质量,即:

这里加了一个scan-match的过程来减少粒子数。这个优化方式也同样运用在了Gmapping算法,后续可能会详细讲解。

3.3.2问题二

        问题:粒子耗散问题,因此每一次进行重采样都有一定的随机性。随着重采样次数的加多,粒子的多样性会耗散掉,即最终的所有粒子都来自同一个粒子或者少数的几个粒子的复制。

        目的:尽量缓解粒子耗散的问题。

        方法:减少重采样的次数,用一个量来表示当前估计和真实分布的差异性:

式子中的w是权重估计之后的,不是重采样之后的。当𝑁eff较大时,说明差异性很小,不进行重采样;当𝑁eff较小于时,说明差异性很大,因此进行重采样。 这样可以极大的减少了重采样的次数,缓解了粒子耗散问题。所以粒子耗散问题只能缓解,不能去除。这个优化方式也同样运用在了Gmapping算法,后续可能会详细讲解。

3.3.3进一步优化proposal分布

        这个优化和前几个不一样,没有运用在Gmapping算法。但是我在尝试在Gmapping算法中加入,后续可能会写一篇关于Gmapping算法的文章来着重分析一下。

        之前的两个优化方式都是从proposal分布进行采样,然后进行极大似然估计提升采样的质量。而这一次,我们用的方法是考虑最近一帧的观测,把proposal分布限制在一个狭小的有效区域。然后在正常的对proposal分布进行采样。

        假设激光雷达的匹配比里程计的测量精确很多,从分布上来说,激光雷达匹配的方差要比里 程计模型的方差小很多。

如图所示,激光匹配的方差比里程计要小很多,如果proposal分布用激光匹配来表示,则可以把采样范围限制在一个比较小的区域,因此可以用更少的粒子即覆盖机器人的概率分布,所以我们可以认为这段数据中激光雷达占主导。

        proposal分布:

 在自己的区域占主导地位,此时的值可以看成常数,因此:即proposal分布从里程计观测模型变换到了激光雷达观测模型。

        因为激光雷达观测模型的方差较小,假设其服从高斯分布:

 对其求解:

极大似然估计得到局部极值:

我们认为 离高斯分布的均值比较近,因此在附近采样得到K个位姿:对这K个位姿进行打分,并认为这K个位姿服从高斯分布,即可求解得到高斯分布的表达式:

Proposal分布变为高斯分布由(𝜇, Σ)表示的高斯分布,因此粒子传播由从运动学模型采样修改为对该高斯分布进行采样。则权重的计算方式为:

最终算法流程:

已知数据:

Scan-match和采样:

计算高斯分布:

从高斯分布中采样并更新权重:

已知位姿的情况下进行地图更新:

已知位姿的情况下进行地图更新:

总结

        这篇文章到这里也算是结束了,写出这篇文章也是看了很多资料,后续可能会写一篇对Gmapping算法的解读和优化。

  • 14
    点赞
  • 46
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
粒子滤波SLAM matlab是一种使用粒子滤波算法来解决同时定位与地图构建(SLAM)问题的方法。在这种方法中,机器人使用传感器数据和运动模型来估计自身的位置,并同时构建出环境的地图。 在matlab中实现粒子滤波SLAM,可以使用一些开源的工具箱或者编写自己的代码。根据引用中提供的链接,可以找到一些粒子滤波SLAM的matlab实现示例。这些示例包含了粒子滤波算法的推导和应用。 在粒子滤波SLAM中,重采样步骤是一个非常重要的因素。在重采样过程中,低权值的粒子通常会被高权值的粒子所替代。重采样的目的是保留具有较高权值的粒子,并减少粒子退化的风险。为了减少重采样次数,可以使用有效粒子数来判断是否需要进行重采样。只有当有效粒子数降到一个阈值以下时,才执行重采样操作。 粒子滤波SLAM的关键是通过在高维空间中搜索解决方案来解决定位与地图估计之间的相互依赖关系。为了减少粒子数量,可以使用Rao-Blackwellized粒子滤波算法,它使用多个粒子来获取准确的地图信息。 总结来说,粒子滤波SLAM matlab是一种使用粒子滤波算法来解决同时定位与地图构建问题的方法。在matlab中实现粒子滤波SLAM可以借助开源工具箱或者编写自己的代码。重采样步骤和使用有效粒子数来判断重采样的时机是粒子滤波SLAM中的关键。使用Rao-Blackwellized粒子滤波算法可以减少粒子数量并提高地图估计的准确性。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* *2* *3* [基于粒子滤波SLAM(GMapping)算法分析](https://blog.csdn.net/tiancailx/article/details/78590809)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_1"}}] [.reference_item style="max-width: 100%"] [ .reference_list ]

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值