一起自学SLAM算法：8.1 Gmapping算法

作者：在线问答5 | 2024-07-10 09:25:35

踩

gmapping算法

连载文章，长期更新，欢迎关注：

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

  8.1 Gmapping算法

        8.2 Cartographer算法

        8.3 LOAM算法

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

下面将从原理分析、源码解读和安装与运行这3个方面展开讲解Gmapping 算法。

8.1.1 Gmapping原理分析

首先要知道，Gmapping是一种基于粒子滤波的算法。在7.7.2节中已经提到过用RBPF（Rao-Blackwellization Particle Filter）这种粒子滤波器来求解SLAM问题，Fast-SLAM算法就是其典型实现之一。其中也有人基于RBPF来研究构建栅格地图（Grid Map）的SLAM算法，它就是ROS中大名鼎鼎的Gmapping算法。不过在Gmapping算法中，对RBPF的建议分布（proposal distribution）和重采样（resampling）进行了改进。下面就首先介绍RBPF的滤波过程，然后介绍对RBPF建议分布和重采样的改进，最后介绍使用改进RBPF滤波的过程，这些内容主要参考Gmapping算法的论文[1]。

1.RBPF的滤波过程

其实RBPF的思想就是将SLAM中的定位和建图问题分开来处理，如式（8-1）所示。也就是利用 $P(x_{1:t}|z_{1:t},u_{1:t-1})$ 首先估计出机器人的轨迹 $x_{1:k}$ ，然后在轨迹 $x_{1:t}$ 已知的情况下很容易估计出地图 $m$ 。

在给定机器人位姿的情况下，利用 $P(m|x_{1:t},z_{1:t})$ 进行建图很简单，可以参考文献[2]。所以，RBPF讨论的重点其实就是 $P(x_{1:t}|z_{1:t},u_{1:t-1})$ 定位问题的具体求解过程，一种流行的粒子滤波算法是SIR（sampling importance resampling）滤波器。那么，下面就来介绍基于SIR的RBPF滤波过程。

（1）采样

新的粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 由上个时刻粒子点集 $\left \{ x_{t-1}^{(i)} \right \}$ 在建议分布 $\pi$ 里采样得到。通常把机器人的概率运动模型做为建议分布 $\pi$ ，这样新的粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的生成过程就可以表示成 $x_{t}^{(i)}\sim P(x_{t}|x_{t-1}^{(i)},u_{t-1})$ 。

（2）重要性权重

上面只是介绍了生成当前时刻粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的过程，考虑整个运动过程，机器人每条可能的轨迹都可以用一个粒子点 $x_{1:t}^{(i)}$ 表示，那么每条轨迹对应粒子点 $x_{1:t}^{(i)}$ 的重要性权重可以定义成式（8-2）所示的形式。其中分子是目标分布，分母是建议分布，重要性权重反映了建议分布与目标分布的差异性。

（3）重采样

新生成的粒子点需要利用重要性权重进行替换，这就是重采样。由于粒子点总量保持不变，当权重比较小的粒子点被删除后，权重大的粒子点需要进行复制以保持粒子点总量不变。经过重采样后粒子点的权重都变成一样，接着进行下一轮的采样和重采样。

（4）地图估计

在每条轨迹对应粒子点 $x_{1:t}^{(i)}$ 条件下，都可以用 $P(m^{(i)}|x_{1:t}^{(i)},z_{1:t})$ 计算出一幅地图 $m^{(i)}$ ，然后将每个轨迹计算出的地图整合就得到最终的地图 $m$ 。

从式（8-2）中可以发现一个明显的问题，不管当前获取到的观测 $z_{t}$ 是否有效，都要计算一遍整个轨迹对应的权重。随着时间的推移，轨迹将变得很长，这样每次还是计算一遍整个轨迹对应的权重，计算量将越来越大。可以将式（8-2）进行适当变形，推导出权重的递归计算方法，如式（8-3）所示。其实就是用贝叶斯准则和全概率公式将分子展开，用全概率公式将分母展开，然后利用贝叶斯网络中的条件独立性进一步化简，最后就得到了权重的递归计算形式。

值得注意的是，式（8-2）中的建议分布 $\pi$ 以及利用权重重采样的策略还是一个开放性话题。其实，Gmapping算法主要就是对该RBPF的建议分布和重采样策略进行了改进，下面就来具体讨论这两个改进。

2.RBPF的建议分布改进

式（8-3）中建议分布 $\pi$ 最直观的形式就是采用运动模型来计算，那么当前时刻粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的生成及对应权重的计算方式就变为式（8-4）所示。

不过直接采用运动模型做为建议分布，显然有问题。如图8-1所示，当观测数据可靠性比较低时（即观测分布的区间 $L^{(i)}$ 比较大），利用运动模型 $x_{t}^{(i)}\sim P(x_{t}|x_{t-1}^{(i)},u_{t-1})$ 采样生成的新粒子落在区间 $L^{(i)}$ 内的数量比较多；而当观测数据可靠性比较高时（即观测分布的区间

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/在线问答5/article/detail/805557