SLAM算法从传统滤波到现代粒子滤波方法的演进

概述

SLAM（即时定位与地图构建）领域从传统滤波方法（EKF-SLAM）演进到现代粒子滤波方法(GMapping），它们的演进关系体现了SLAM领域如何一步步解决计算瓶颈、提升鲁棒性、并最终走向实用化的过程。模块：RBPF (理论框架) → FastSLAM (第一个成功应用该理论的特征点SLAM算法) → GMapping (基于该理论的、优化的、针对激光雷达的网格地图SLAM方案)

用一个简单的类比和关系图来建立直观印象：

• 1.1 EKF-SLAM： 像一个严谨但保守的数学家。他假设所有不确定性（噪声）都是高斯的（钟形曲线），用一套复杂的方程同时估计自己的位置和地图上所有路标的位置。当地图和路标变得非常大时，他的计算量会爆炸式增长，而且一旦认错一个路标（数据关联错误），整个地图可能就全错了。
• 1.2 RBPF (Rao-Blackwellized Particle Filter)： 不是一个具体的SLAM算法，而是一个强大的数学框架。它说：“与其估计一个复杂的联合问题，不如把它拆开。我们用一群粒子（Particle）来估计机器人可能走过的路径（定位问题），然后给定每条路径，我们用更简单高效的方法（比如EKF）来估计地图。” 这个“先采样，后解析”的思想就是RBPF的核心。
• 1.3 FastSLAM： 是第一个成功将RBPF框架应用于SLAM问题的具体算法。它就是上面那个数学框架的第一个“实践者”。FastSLAM 1.0和2.0是它的两个主要版本，2.0更优。它明确展示了如何用粒子滤波处理路径，用EKF处理地标（特征点）地图。
• 1.4 GMapping： 是基于RBPF框架的一个具体、强大、且专门为激光雷达（LiDAR）设计的实现。它不是一个新算法，而是将RBPF for SLAM的理念工程化、优化到了极致，用于构建2D网格地图（Occupancy Grid Map）。可以认为GMapping是RBPF在激光SLAM上的“杀手级应用”。

1. EKF-SLAM (扩展卡尔曼滤波SLAM)

核心思想： 使用一个巨大的扩展卡尔曼滤波器（EKF）同时估计机器人的位姿（位置和姿态）和环境中所有路标点（landmarks）的坐标。它维护一个巨大的状态向量，这个向量包含机器人位姿和所有路标点的坐标。通过线性化非线性运动模型和观测模型，来递归地更新这个状态向量的均值和协方差（不确定性）。

解决了什么问题：

• 提供了SLAM问题第一个完整的概率估计框架，能够给出机器人位姿和地图路标的不确定性（协方差）。
• 在路标数量不多（<1000）且环境特征易于辨识的情况下，效果很好。

存在的局限性：

• 计算复杂度高： 协方差矩阵的大小是 (3+2N)^2（2D情况下，3是机器人状态，2N是N个路标的状态），更新计算量与N²成正比，当地图很大（N很大）时，计算无法实时进行。
• 数据关联脆弱： 如果错误地将一次观测关联到了错误的路标（即认错了路标），这个错误会通过巨大的协方差矩阵传播，导致整个估计结果崩溃，且难以恢复。
• 线性化误差： EKF只对非线性模型进行一阶线性化近似，在机器人运动模型或观测模型非线性程度高时（如快速旋转），会引入显著误差。
• 仅限于特征点地图： 生成的是稀疏的特征点地图，不适合需要稠密网格地图进行导航的应用。

2. RBPF (Rao-Blackwell Particle Filter) - 基于Rao-Blackwellized的粒子滤波器

核心思想： 这是一个算法框架，利用了Rao-Blackwell定理来分解复杂估计问题，对于SLAM，它的关键洞察是：完整SLAM后验概率可以分解为路径后验概率和给定路径下的地图后验概率的乘积，简单讲就是将定位和建图进行了分离，实现先实现自身定位，再基于定位实现建图。 论文中公式如下：

在这里插入图片描述

公式概述如下： P(路径, 地图 | 观测) = P(路径 | 观测) * P(地图 | 路径, 观测)

P(路径 | 观测)： 这个分布很复杂，用采样粒子滤波来近似，每个粒子代表一条机器人可能走过的路径。

P(地图 | 路径, 观测)： 给定一条路径，地图的估计就变得非常简单和高效，因为观测值在已知路径的条件下是独立的，对于特征点地图，可以用很多个独立的小EKF（每个路标一个）来估计；对于网格地图，可以用高效的栅格地图更新算法（如反传感器模型）。

解决了什么问题：

• 降低了计算复杂度： 将O(N²)的问题，通过分解，变成了管理一批粒子（路径）和每个粒子关联的一个易于计算的地图的问题。计算量从路标数量的平方级降到了线性级。
• 改善了数据关联： 每个粒子独立进行数据关联，如果某个粒子做出了错误的数据关联决策，导致它的地图很差，那么这个粒子的权重在重采样过程中就会变低，最终被淘汰。正确的关联会存活下来，这使得系统对数据关联错误更加鲁棒。
• 灵活性： 这个框架不限定地图的表示形式。既可以用它来做特征点地图（如FastSLAM），也可以用它来做网格地图（如GMapping）。

3. FastSLAM

核心思想： 是RBPF框架在SLAM问题上的第一个，也是最著名的具体实现，主要用于特征点地图。

工作原理：

• 粒子： 使用粒子滤波器来估计机器人的路径(轨迹) ，每个粒子包含一条机器人路径的估计，代表一条可能的机器人轨迹假设，并携带一个独立的地图估计。
• 粒子地图： 每个粒子都维护自己版本的地图，由于路径已知，地图估计被分解了，每个路标都有一个独立的小EKF来估计其位置，意味着更新一个路标不需要考虑其他路标，计算效率极高。
• 重要性权重： 粒子的权重由最新的观测数据与粒子自身地图的匹配程度（似然）来决定。
• 重采样： 定期根据权重重采样，淘汰差粒子，复制好粒子。

解决了什么问题：

• 它具体实现了RBPF的理论优势，成功解决了EKF-SLAM的计算复杂度过高和数据关联脆弱的问题，使得在大规模环境中进行基于特征点的SLAM成为可能。
• 能够有效处理非线性非高斯噪声系统，计算复杂度从EKF-SLAM的O(n²)降低到O(M·log(n))，其中M是粒子数，n是地图特征数

局限性：

• 粒子耗散问题：随着迭代进行，有效粒子数减少，导致估计精度下降
• 计算效率问题：当环境特征数量大时，计算量仍然较大

4. GMapping

核心思想： 这是一个基于RBPF框架的、专为激光雷达和2D网格地图优化的SLAM算法。

工作原理（相对于原始FastSLAM的优化）：

• 地图表示： 使用占用栅格地图（Grid Map），而不是特征点，这更符合激光雷达的数据特性，能生成用于导航的稠密地图。
• 提议分布优化： 这是GMapping性能卓越的关键，原始RBPF使用运动模型作为提议分布，由于里程计误差大导致模型结果非常不精确，会导致需要大量粒子。
• 扫描匹配（Scan Matching）：GMapping为每个粒子计算一个更优的局部位姿估计，然后用这个改进的估计来生成新的粒子，这大大减少了所需粒子数（通常只需几十个），极大提高了效率，即通过更高效的雷达数据的观测模型作为首选，代替FastSLAM基于里程计的运动模型。

在这里插入图片描述

• 选择性重采样： 避免每一次都重采样，频繁重采样会导致粒子多样性缺失（粒子耗散），GMapping通过计算有效粒子数的比例来判断是否真的需要重采样，即使重采样，从而保护了好的粒子不被淘汰。

解决了什么问题：

• 它解决了在室内和中小型环境中，利用低成本激光雷达和轮式里程计实现实时、高性能的2D网格地图构建的问题。
• 它提供了比原始FastSLAM高得多的效率和质量，使其成为基于激光的2D SLAM的事实标准算法长达十年之久，至今仍在广泛使用。