基于滤波器的激光SLAM-2-Fast-SLAM

(Mark:有空把Fast-SLAM论文看一下)

原理

Fast-SLAM也是用粒子滤波（RBPF，就是PF加了RB分解，把地图m提出来了）。

之前我们已经推导过贝叶斯滤波即，给定观测和控制量，当前时刻状态量x 等于 

常数 * 观测 *（运动模型 * 上一时刻状态量）

那实际SLAM运用中我们还需要一个地图m，那就是求

 

（引入条件概率公式）

 

可以得到

地图m跟控制量无关

我们只需要把精力放在求机器人状态上面

因为有了状态之后，地图是有确定解的(close_form)。

 

跟之前的两篇一样，我们希望把这个状态量做成一个迭代解的形式。（把轨迹问题转变成增量问题）。

推导如下

（贝叶斯公式）

（条件概率公式）

（xt和观测无关，且知道xt-1就不需要u1:t-1，另有xt-1跟ut无关）

即，当前时刻的路径 等于 观测 * 运动学模型 * 当前时刻路径。

 

用粒子滤波的做法，我们把用粒子群表示。

每隔粒子用运动学模型进行传播。

传播之后用观测模型进行权重计算（并根据位姿构建地图）

 

存在的问题

占内存

1. 每个粒子带一幅地图，占内存
2. 如果里程计误差大，则需要更多的粒子才能比较好的代表分布，又需要内存。

 

改进

所以要提升proposal分布采样的质量。(GMAPPING)

比如我们以前从一个平滑的分布里面采样，就要很多样本，均匀的去采。

那如果我们引入一个scan-match，尽量在最匹配（可能是局部最优）的范围内采样，就可以减少采样数。

（好像是等于把一个分布隔断成一个一个的局部最优，每隔局部最优采一个样？）（用的是Ut,没有观测，怎么来的scan match？）

 

粒子耗散

重采样导致所有粒子来自一个或者少数几个粒子的复制。

改进

减少重采样次数，用一个量来表示当前估计和真实分布的差异。（GMAPPING）

 

进一步改进

进一步优化proposal

上面的优化方式：从proposal分布进行采样，然后用极大似然估计提升采样质量。

这里的优化方式：考虑最近一帧的观测，把proposal分布限制在一个狭小的有效区域，然后正常的对proposal分布进行采样。

（因为激光雷达的匹配比里程计的测量精确很多，从分布上来说，激光雷达匹配的方差要比里程计模型的方差小很多）

即原始proposal 被替换成

在L^i区域，要远大于，所以在这里我们令它为常数。

proposal分布从里程计观测模型变成了激光雷达观测模型

我们假设激光雷达观测模型符合高斯分布

其实是scan match（极大似然估计）得到一个局部极值（最大匹配值），

我们认为这个位姿在高斯分布的μ附近，那么就直接在这附近采样得到k个位姿。

对这K个分布求概率，以求解对应的高斯分布。

因此，我们的粒子传播又从运动学模型采样变成了从高斯分布采样。

Proposal变了之后，权重w计算方式也变了。

权重模型

（代入地图m）

又有（贝叶斯）

 

进一步优化方法的流程