从零理解Gmapping的SLAM算法

论文下载：百度网盘：https://pan.baidu.com/s/1fQIcUYFRNXdheZYzg9mFow?pwd=zhan

Gmapping算法是机器人领域一个非常经典和实用的2D-SLAM解决方案，Gmapping作为基于滤波SLAM的经典代表，其通过RBPF分解问题，提出改进提议分布和选择性重采样等核心思想，使其在中小型室内环境中表现出色，且在ROS中集成度高，易于上手。
虽然在大场景和回环处理方面存在局限，并依赖里程计，但理解其原理和实现，不仅能便于掌握一种实用的SLAM工具，更为后续学习更复杂的基于图优化的SLAM算法（如Cartographer）打下基础，本文从基本原理、核心改进、优缺点等方面展开解析。

1. GMapping算法概述

Gmapping是基于滤波SLAM框架的开源算法，跟FastSLAM一样采用了RB粒子滤波器（RBPF），将复杂的SLAM问题（同时定位与建图）分解为定位和建图两个相对独立的过程，先进行定位（估计机器人轨迹），在已知轨迹的基础上再进行建图，即以概率基础：
P(位姿, 地图 | 观测, 控制) = P(位姿 | 观测, 控制) * P(地图 | 位姿, 观测)。

简单来说，Gmapping通过粒子滤波来估计机器人的运动轨迹（定位），而每个粒子都独立维护一张地图（建图），最终的全局地图由权重最高的那个粒子（最优粒子）所维护的地图产生。

2. 核心原理：RBPF粒子滤波

Gmapping的基石是RBPF粒子滤波，每个粒子都是一个对机器人整个运动路径（位姿历史）和环境的完整假设。

每个粒子包含的信息：

• 一条机器人路径的历史假设：$s^{[k]}=s_1,s_2,...,s_t$ （[k]是粒子索引）。

• 一张基于该路径假设的地图：通常为栅格地图（每个栅格有占用、空闲、未知三种状态）。

一个权重：$w^{[k]}$，代表该粒子（即该路径和地图假设）正确的可能性。

算法循环（处理一帧激光数据）：

• 采样（运动更新）： 对于每个粒子，根据运动模型（通常基于里程计信息 $u_t$）和上一时刻的位姿，预测当前时刻的位姿，并加上一定的随机噪声（采样），$s_t^{[k]}=p(s_t|s_{t-1}^{[k]},u_t)$。这一步之后，粒子会散开，因为噪声使得大家对当前位置的猜测变得不同。

• 观测更新（扫描匹配）： 这是Gmapping的第一个关键改进，它不仅依赖运动模型，还利用了当前的激光观测数据 $z_t$。通过扫描匹配技术，在当前粒子预测位姿的一个小邻域内，寻找一个使得当前激光数据与粒子自身地图匹配得最好的位姿，并用这个更优的位姿更新该粒子的当前位姿假设，这极大地提高了提议分布的准确性，减少了对粒子数量的需求。

• 计算重要性权重： 更新每个粒子的权重 $w_t^{[k]}\propto p(z_t|s_t^{[k]},m^{[k]})$，权重反映了在当前粒子的位姿假设下，观察到实际激光数据 $z_t$ 的似然概率，简单说，就是当前激光数据与粒子地图的匹配程度越高，权重越大。

• 选择性重采样： 这是Gmapping的第二个关键改进，重采样会淘汰权重低的粒子，复制权重高的粒子，但频繁重采样会导致粒子退化，通过引入有效粒子数（Neff） 作为触发条件：$Neff=\frac{1}{\Sigma (w_i^2)}$。只有当 Neff 小于设定阈值（表明粒子权重差异很大，分布集中）时，才进行重采样，这有效减少了不必要的重采样次数，保持了粒子多样性。

以下是Gmapping算法核心流程的步骤概览：

3. 优势与局限

与其他经典激光SLAM算法的简单对比如下：