Gmapping 原理之目标分布与提议分布

1. 概述

简单讲一下Gmapping的原理中的提议分布与目标分布，并指出论文中的公式错误，如果阅读此文章，需要先熟悉原始论文，如果本文有错误，欢迎指出讨论，同时此文章供自己以后参考(已经写了很久了，一直没有发出来)。

2. 目标分布与提议分布

2.0 概率复习

可能用到的概率知识
条件概率公式

贝叶斯公式

全概率公式

组合拳

以上图片均来自百度图片搜索结果，感谢！

2.1 目标分布

目标分布也称为真实分布，如果考虑机器人的运动，那么机器人在t时刻位置的真实分布就是目标分布，但是这是不能得到的，只是理论上的分布，我们需要只能通过其他分布来得到该分布，评价的标准就是权重f/g，f表示目标分布，g表示提议分布。

2.2 提议分布

因为目标分布是得不到的，因此需要使用提议分布来得到目标分布的真实情况。一般的，使用里程计模型来作为提议分布。但是改进的提议分布发现激光具有单峰的特性，方差更小，更适合作为提议分布，因此考虑激光和里程计一起作为改进的提议分布。
简单列出一下权重计算不同，后面会推导权重计算公式。
只使用里程计模型，那么得到的权重计算公式为

如果考虑激光测量的单峰特性，得到的权重计算公式为(公式13少了一个m_t-1)

2.3 改进提议分布

对于机器人位于x_t-1位姿时，输入的里程计测量信息为u_t-1，观测值是z_t-1。根据里程计运动模型使用里程计测量信息u_t-1和位姿x_t-1可以得到x’_t，但是在t时刻具有观测z_t可以对位姿x’_t进行评价或者修正。
根据概率论知识，SLAM工作可以划分为定位和建图两部分，一般是通过观测值z_1:t和里程计信息u_1:t-1进行定位得到x_1:t，然后根据位姿x_1:t以及观测信息进行建图得到m。

使用里程计模型可以通过位姿x_t-1和输入u_t-1得到位姿x_t。由于里程计模型的似然分布相比激光似然分布较为分散，因此可以使用激光对提议分布进行改善。有更加明显尖峰的就是激光的似然分布。

根据输入以及观测对位姿估计的公式，可以进行分解为（u_t应该是u_t-1，论文中标注错了）

激光和里程计的似然分布在L区域里差异较大，可以认为里程计的似然为常数k，激光似然值占据了主导作用。L表示一部分较小的区域，这部分区域能够获得Z的概率较大。

进一步可以得到L区域里（里程计似然为常数，可以抵消掉）

这说明位姿估计取决于扫描匹配的结果，在x_t位姿和m_t-1地图下得到z_t的似然值越大，机器人位于x_t的位姿概率就越大。
进一步将似然函数近似为高斯分布，可以得到

高斯分布的均值和方差估计为

归一化因子

而采样点x_j是根据里程计运动模型得到的

2.4 权重更新

每个粒子的权重计算公式如下，表明了目标分布和提议分布的近似程度，理论上近似程度越高，得到的效果就越好。

将提议分布进行分解得到递推的公式。可以在后面进一步得到权重的递推公式。

因此权重公式可以进一步化简如下，这几个公式采用理解和推导的方式可以得到。（公式5到公式6表示拥有了机器人位姿x_1:t-1和观测z_1:t-1就可以生成地图m_t-1，实际Gmapping中的程序也是这样的）

归一化因子为

如果使用里程计模型作为提议分布pi，也就是使用里程计模型来估计位姿x_t，那么上面的等式会变成

但是之前说过激光的似然分布比较集中，进行提议分布改进得到效果会更好，因此可以使用下面作为真正的提议分布（机器人位姿x_1:t-1和观测z_1:t-1就可以生成地图m_t-1）。

进一步可以得到权重更新公式，（公式13处出现了错误，少了m_t-1，这个错误很明显，之前已经多次分析过了）

根据13可以看到这是一个积分表达式，表示在所有位姿x中计算对应的概率然后积分，但是在实际应用中只需要考虑有效区域L中即可，这个之前也进行了推导，L区域为

在区域L中计算高斯分布，均值和方差为

再回到之前的权重公式，只考虑区域L，那么进一步有（与公式13对应的，这里就已经加上了m_t-1）

也就是说每个粒子不仅携带一个地图、位姿轨迹树、还会携带在位姿和地图下对应的改进高斯提议分布。

然而在目前开源的Gmapping源代码中并没有实际运用这个方法。。

3. 参考链接

1. Improved_Techniques_for_Grid_Mapping_With_Rao-Blackwellized_Particle_Filters

大家根据自己实际情况下载，欢迎支持，尊重知识。
researchgate公开免费链接 researchgate Grid_Mapping
CSDN链接，需要1积分 CSDN Grid_Mapping

Gmapping

Gmapping 之安装与配置
Gmapping Dropped 100.00% of messages so far 解决办法
Gmapping 原理之目标分布与提议分布
Gmapping 原理之源代码框架