激光slam课程学习笔记－－第7课：基于滤波的SLAM方法（Ｇrid-based）

Posted 2022-11-28 鸿_H

前言：这系列笔记是学习曾书格老师的激光slam课程所得，这里分享只是个人理解，有误之处，望大佬们赐教。这节课主要介绍基于滤波的slam方法，重点学习其中的滤波数学理论，而非应用在slam。

［当前的实际使用的激光SLAM都不会选择滤波器类型的，因为其效果并不好．由于基于滤波方式在SLAＭ发展是有重要的意义的，所以也进行介绍一下．当然，滤波的知识在激光slam应用不多，但其背后的数学理论，贝叶斯估计＼粒子滤波，知识在其他领域很多使用］

１．贝叶斯滤波

［在状态估计里面，分为两派，一个是贝叶斯派，另外一个是频率派，其中状态估计大部分采用的是贝叶斯］

1.1　数学概念

［条件贝叶斯，条件部分后面不断添加条件ki,其对应的公式的条件部分，也添加ki也是成立的，如现在里面添加了一个条件ｚ］

1.2　贝叶斯滤波特性

１）估计的是一个概率分布，不是具体的数值
２）贝叶斯滤波是一大类方法的统称［并没有一个具体的概率分布的，所以也是一个抽象的表达形式］

３）是一个抽象的表达形式－－对于不同问题有不同的实现方式（卡尔曼家族（KF,EKF,VKF,PF），粒子滤波）
４）迭代估计形式［计算量比较小］

［与贝叶斯滤波不同派的频率派，其是MLE，采用的是极大似然估计（找到一个数值，使得当前发生的概率最大）］

［贝叶斯是最大后验估计（估计出数值的后验概率分布，从而找到概率最大值的地方）］

［两派区别在于，频率派认为有些参数只是数值而已，需要对参数进行估计；而贝叶斯派认为一切都是概率］

1.3　贝叶斯滤波的推导

［贝叶斯滤波公式，左侧bel(xt)表示后验概率；中间的bel(xt)顶－表示预测分布，专用术语称为proposal分布］
［右下＂功能＂部分，＂uz＂应该修改为”ut”的］

［凡是算法流程可以写成这种形式的，都可以称为贝叶斯估计（滤波）］
［卡尔曼滤波是贝叶斯滤波在线性高斯的特例］

［公式中，z1:t,u1:t可视为观测数据，比如分别表示雷达观测数据，里程计观测数据，然后进行估计机器人的位姿］

［ｘ理解为地图位姿，ｚ理解为里程计数据，ｕ为激光数据，注意数据前后帧是相互独立的，里程计数据和激光数据也是相互独立的］

[上图第左侧第一条公式使用的是条件贝叶斯公式，后面只是添加了一个公共项Z1:t-1]
[左侧其中，第一条公式，由于Zt跟Ｚ1:t-1,也跟u1:t都是没有关系的，所有根据贝叶斯公式，可以化简得到右侧式子；顺数第二条公式，使用了全概率公式,但是没有理解；顺数的第三个公式，根据相关性，化简得到的是运动模型；顺数第四个公式，右侧获得的是t-1时刻的概率分布]

［右侧从上往下第一条公式，bel(xt)表示后延概率分布；bel(xt)带杆，表示预测分布］
［右侧倒数第二条公式，右侧是一个矫正和预测概率分布相乘得到；该公式可表示一个迭代估计的过程］［只要知道初始位置，既可以一直进行迭代估计］
[个人感觉老师这里写错字了，后延概率应该写成后验概率，详细可参考维基百科]

［贝叶斯公式推导：https://www.cnblogs.com/ycwang16/p/5995702.html］

［公式中的p(xt|zt)写错了，应该为p(zt|xt)］
［注意红色的波峰，其会根据机器人的位置进行移动，而每次走到门前的所得到的观测概率都是顺数第二个波峰形态，而第三个波峰是由根据前一个波峰形态随机器人移动后，当前门情况观测波峰情况进行卷积得到的，最后就是得到了第四个波峰情况］［机器人有星星才表示有观测到了］

［最高峰位置，表示机器人的位置］
［随着移动，不断预测与观测，进而不断更新机器人位置概率分布；这是全局定位的过程］

[上面的内容出自《概率机器人》，可以google找一个pdf版本来看]

２．粒子滤波

［粒子滤波是贝叶斯滤波的一种特例，一种实现方式］［在物体跟踪，机器人定位，slam都可以使用］
[粒子滤波存在粒子耗散问题，当前没法解决]
［在视觉里面构建地图，使用粒子滤波方法，其地图更新就是在对状态向量求逆，随着状态向量的增大，其计算就会耗时越多，导致速度越来越慢，所以仅适合小环境里面使用；而在激光领域，实现了位姿估计和地图估计是分开，所以滤波方法在在稍微大一点的环境也是适用的］［但是，大一点环境，滤波方式都不适用，因为粒子耗散问题的出现］

2.1　粒子滤波器特性

特性：
１）贝叶斯估计器的一种实现方式
２）能处理非线性情况
３）能处理多峰分布的情况［多峰分布处理，可理解为全局定位］
４）能用一系列的粒子（particle）近似概率分布
５）非参滤波器

2.2　粒子滤波流程

［地图匹配度，里面存在一个重采样，其作用是去除权重小的部分］［流程中考虑的是单个粒子情况．］［这部分没理解］

2.3　粒子滤波状态传播

［上图中间公式得到是一个概率分布；最下面得到是单个粒子具体位置］

2.4 　权重评估原理

无法知道机器人位姿的实际分布；从机器人的预测分布进行采样，联合权重一起近似机器人的后验概率分布；权重用来评估实际的预测分布和实际分布的差，差越大，权重越小．

［注意右侧公式考虑的是一个粒子情况，所以没有积分号］
［权重w里面的ｐ，被称为观测模型］

2.5　权重评估

［左侧图ｆ表示真值分布，ｇ表示proposal（粒子数量）分布；右侧图ｆ表示权重大小分布，ｇ表示粒子密度］

2.6　重采样

［重采样，让权重大的地方，粒子多一些，权重小的地方粒子少一些］

［这一点也是可以从重采样的示意图（转盘指针）获得，权重大地方，同一个粒子，可多次重复选中，权重小的粒子，选中的概率会小一些甚至直接删除了］

［重采样，用预测分布的粒子和观测模型的权重生成符合后验概率分布的粒子群］

［重采样后，权重高的地方（左侧ｆ表示权重），粒子更密集（右侧下方粒子密度）了］
［ｆ是粒子后验分布情况］［重采样的目的是，使得粒子更符合后验分布］

2.7 　粒子滤波算法流程

［算法流程，对每个粒子都进行运动学模型传播，利用权重进行评估，后面进行重采样，使得粒子分布更符合粒子后验分布］

粒子滤波存在问题：

１）粒子耗散问题（粒子多样性的丧失）［对于建图来说极为致命］［主要是重采样产生；重采样是复制权重高粒子，删除权重低的粒子，但是保持粒子总量不变］

２）维数灾难［粒子滤波，如果粒子信息，维数增大，计算量指数级增长］

３）当proposal比较差的时候，需要用很多的粒子才能较好的表示机器人的后验概率分布［如扁平型（比较差的类型）的里程计，需要添加很多粒子才能表示模型后验分布样式］

３．　ＦastSLAM的原理及优化

［ＦastSLAM是基于粒子滤波的］

3.1 　fast-slam原理

［RB分解，形式如同条件概率p(ab) = p(b|a)*p(a),已知p(b|a)，估计p(a)就可以对p(a|b) 进行求解］

［公式展开时，视为条件概率展开即可；p(xt,m|ut,zt) = p(xt,m,ut,zt)/p(ut,zt)接着常规操作就行］

３.２　算法流程

［先预测，再矫正，将一个路径规划问题转换为迭代估计问题］

３.３　存在的问题及优化

［＂方法＂的作用是减少粒子数目］

3.4　粒子耗散优化

［当前粒子不好的粒子，后面可能是好的，所以重采样时，可能会把一些好的粒子替换掉了］［粒子耗散就是粒子多样性的缺失，上面＂问题＂最后一句已具体说明］
［这里介绍的方法，只能缓解粒子耗散问题，而不能根除］

3.5　 proposal分布优化

［进一步优化proposal分布，目的减少粒子数］

[优化原理是，利用激光的比较少的点数目，替换很多的imu粒子数目]

3.6 　proposal优化－－激光点符合高斯分布

［左侧第一个为加权平均值；第二个为方差］
［右侧倒数第二行公式，使用的是全概率展开的，个人目前不会推导］
［这里可以达到优化的目的，换了一种分布方式，其方差最小，保证了粒子数目最少］

3.7　算法流程

［scan-match是对每个粒子都进行的］
［右侧流程的伪代码加号，是假设没有噪声前提下直接进行的］

［除了右下的点点步骤外，其他三步是对每个粒子都进行的操作］
［gmapping 里面并没有进行fastslam优化的］
［个人对伪代码流程还没理解透］

４．　Ｇmapping的介绍

[这里介绍一个基于滤波方式的slam框架，gmapping的基本特性]

#####################
图片版权归原作者所有
致谢曾老师的付出

不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈