PythonRobotics 中的 SLAM 算法实践：从 EKF SLAM、FastSLAM 到图优化 SLAM 的完整指南

导读

SLAM（Simultaneous Localization and Mapping，同时定位与建图）是移动机器人领域的核心问题之一：机器人需要在未知环境中一边估计自身位姿，一边构建环境地图。本指南以 PythonRobotics 仓库的 SLAM 模块文档 为骨架，完整梳理仓库中实现的五大 SLAM 示例——EKF SLAM、ICP 点云匹配、FastSLAM 1.0、FastSLAM 2.0 与基于图的 Graph SLAM——并结合 源码 与 测试用例，带你理解从贝叶斯滤波到图优化的算法演进、核心数学公式、参数含义与可运行代码，最终能够独立运行这些仿真并读懂其实现原理。

SLAM 问题：先有鸡还是先有蛋

SLAM（Simultaneous Localization and Mapping）指的是机器人同时估计自身位姿与周围环境地图的能力。其难点在于：定位需要地图，建图需要定位，二者相互依赖，因此 SLAM 常被称为"鸡生蛋、蛋生鸡"式的难题（chicken-and-egg problem）。

正如 slam_main.rst 所述，主流的 SLAM 求解方法包括扩展卡尔曼滤波（Extended Kalman Filter）、粒子滤波（Particle Filter）以及 FastSLAM 算法。该文档组织了一个完整的 SLAM 示例集合，共包含五个相互独立、可单独运行的子模块：

• iterative_closest_point_matching：基于 SVD 的二维 ICP 点云匹配
• ekf_slam：扩展卡尔曼滤波 SLAM
• FastSLAM1：FastSLAM 1.0
• FastSLAM2：FastSLAM 2.0
• graph_slam：基于图的 Graph SLAM（含真实 SE(2) 数据集优化）

下面按照"滤波类 SLAM → 点云匹配 → 粒子滤波 SLAM → 图优化 SLAM"的脉络逐一展开。

滤波类 SLAM：EKF SLAM 的完整推导与实现

算法概览与状态表示

EKF SLAM 文档 指出，EKF SLAM 将 SLAM 问题建模在单个扩展卡尔曼滤波器中，其状态向量同时包含机器人位姿 (x, y, θ) 和全部路标位置数组 [(x1, y1), (x2, y2), ..., (xn, yn)]：

X = [x, y, θ, x1, y1, x2, y2, ..., xn, yn]ᵀ

协方差矩阵 P 则跟踪各分量两两之间的不确定性，例如 σ_xy 表示对 x 与 y 估计之间的协方差，且 σ_xy = σ_yx。状态可更简洁地表示为位姿部分与地图部分的组合：

X = [x, m]ᵀ,   P = [[Σ_xx, Σ_xm], [Σ_mx, Σ_mm]]

其中 Σ_xx 是机器人位姿的不确定性、Σ_mm 是地图的不确定性、Σ_xm / Σ_mx 是位姿与地图之间的交叉不确定性。注意区分状态向量 X 与位姿 x。

仿真参数与噪声配置

文档给出了一套完整的仿真参数，在 ekf_slam.py 中可看到完全对应的实现：

参数	默认值	含义
Cx	diag([0.5, 0.5, deg2rad(30)])²	EKF 状态协方差（每步预测时叠加的位姿不确定性增量）
Q_sim	diag([0.2, deg2rad(1)])²	传感器噪声（距离 + 方位角）
R_sim	diag([1.0, deg2rad(10)])²	过程噪声（线速度 + 角速度）
DT	0.1 s	仿真时间步长
SIM_TIME	50.0 s	总仿真时长
MAX_RANGE	20.0 m	最大观测距离（超出则忽略该路标）
M_DIST_TH	2.0	马氏距离数据关联阈值
STATE_SIZE	3	状态维度 [x, y, yaw]
LM_SIZE	2	路标维度 [x, y]

核心循环：预测 + 更新

每个时间步的核心迭代由 ekf_slam 函数完成（源码）：先执行 Predict（预测），再执行 Update（更新）。

1. 预测阶段

运动模型仅依赖控制输入 (v, w)（线速度与角速度），其离散形式为：

x_{t+1} = F·x_t + B·u_t

其中 F 为单位矩阵，B 为 [[Δt·cosθ, 0], [Δt·sinθ, 0], [0, Δt]]。注意实际计算中控制输入带有误差项 σ_v、σ_w，即真实控制为 v_t + σ_v、w_t + σ_w。仿真中：

• 真值轨迹 X_true = F·X + B·U（使用无噪声控制）；
• 航位推算 X_DR = F·X + B·(U + R)（使用叠加过程噪声 R 的控制）。

协方差预测公式为 P = GᵀPG + Cx，其中 G 是运动模型雅可比（由 jacob_motion 计算，见 源码）。注意每步不确定性只随位姿增长，不会直接增加路标的不确定性。

2. 更新阶段

更新阶段利用观测到的路标来修正位姿估计，核心步骤包括：

• 数据关联：通过 search_correspond_landmark_id（源码）计算当前观测与已知路标的马氏距离，若所有已知路标的马氏距离都超过 M_DIST_TH，则判定为新路标（打印 New LM）并扩展状态与协方差矩阵。
• 计算新息（Innovation）：y = z_t - h(X)，即实际观测与"若机器人在预测位姿处本应观测到的值"之差（calc_innovation）。
• 卡尔曼增益：K = P̄_t·H_tᵀ (H_t·P̄_t·H_tᵀ + Q_t)⁻¹，H 是量测函数雅可比。直觉上它是"过程不确定性 /（过程不确定性 + 量测不确定性）"的多变量推广：若 Q_t << P̄_t（量测更可信），增益趋近 1，完全相信量测；若 Q_t >> P̄_t，增益趋近 0，相信预测。
• 状态与协方差更新：xUpdate = xEst + K·y，P_t = (I - K_t·H_t)·P̄_t。

更新完成后还需将航向角归一化到 [-π, π)（pi_2_pi）。

仿真运行方式

主循环（main）设置了 4 个 RFID 路标 [[10,-2],[15,10],[3,15],[-5,20]]，机器人以恒定的 v = 1.0 m/s、yaw_rate = 0.1 rad/s 运动 50 秒。运行：

python SLAM/EKFSLAM/ekf_slam.py

可视化结果中：蓝色线为真值轨迹、黑色线为航位推算、红色线为 EKF SLAM 估计轨迹、黑色星号为真实路标、绿色叉号为估计路标。文档的仿真输出 start!! / New LM / New LM / New LM 表明前三步分别发现了 3 个新路标。

对应的测试见 test_ekf_slam.py，验证了该仿真的可复现性。

点云配准基础：基于 SVD 的 ICP 匹配

在基于激光雷达的 SLAM 中，位姿估计常依赖相邻帧点云的配准。ICP 匹配文档 提供了一个轻量的 2D ICP 示例，它使用**奇异值分解（SVD）**来计算两组点之间的旋转矩阵与平移向量，同时支持 2D 与 3D 点集。

icp_matching.py 中 icp_matching(previous_points, current_points) 的核心迭代过程：

1. 最近邻关联（nearest_neighbor_association）：为当前帧每个点找到上一帧中的最近邻点，并计算残差误差；
2. SVD 运动估计（svd_motion_estimation）：对去中心化后的点积矩阵 W 做 SVD 分解，恢复旋转矩阵 R 与平移向量 t；
3. 更新点云：current_points = R·current_points + t，并累乘得到齐次变换矩阵 H；
4. 收敛判断：当误差变化 dError 小于 EPS = 0.0001 时判定收敛，超过 MAX_ITER = 100 次或误差反弹（dError < 0）则提前退出。

main 与 main_3d_points 两个演示分别生成 1000 个随机点，施加已知运动 [0.5, 2.0, -10°]（2D）或 [0.5, 2.0, -5, -10°]（3D）后调用 ICP 反解 R 与 T。由于 ICP 是许多激光 SLAM 前端（如 scan matching）的基础，graph_slam 的 SE(2) 数据集示例 中提到的"scan-matching edges"正是这类配准约束。

粒子滤波 SLAM：FastSLAM 1.0 三部曲

与 EKF 的本质区别

FastSLAM 1.0 文档 指出，FastSLAM 基于粒子滤波实现，属于概率 SLAM 方法家族，可配合特征地图或占据栅格地图使用。与 EKF 用单个高斯分布表示估计不同，粒子滤波用一组粒子表示机器人状态：每个粒子独立持有自己的位姿 (x, y, θ) 以及一组路标位置 [(x1, y1), ..., (xn, yn)]，即每个粒子维护一个确定性的位姿，并为每个路标维护一个独立的 2×2 EKF。

仿真可视化的颜色约定为：蓝线 = 真实轨迹、红线 = 估计轨迹、红点 = 粒子分布、黑线 = 航位推算、蓝叉 = 观测并估计的路标、黑叉 = 真实路标。

关键参数

fast_slam1.py 中与 EKF SLAM 不同的参数包括：

• Q = diag([3.0, deg2rad(10)])²：FastSLAM 路标 EKF 的量测协方差；
• R = diag([1.0, deg2rad(20)])²：粒子运动模型的过程噪声（R 越大，表示越不信任机器人精确执行了运动指令）；
• OFFSET_YAW_RATE_NOISE = 0.01：角速度偏置噪声；
• N_PARTICLE = 100：粒子数；
• NTH = N_PARTICLE / 1.5：有效粒子数阈值，低于它触发重采样。

三步迭代：Predict → Update → Resampling

fast_slam1 主函数（源码）每步执行三个操作：

1. 预测（Predict）：predict_particles 对每个粒子用带噪声的控制 ud = u + randn·R 套用与 EKF 相同的运动模型更新位姿，路标保持不变。此时粒子开始发散、不确定性增大。
2. 更新（Update）：update_with_observation 对每条观测，若是新路标（|lm| <= 0.01）则用 add_new_lm 初始化；若是已知路标，则先用下式计算粒子权重：

w_i = |2πQ|^(-1/2) · exp(-0.5·(z_t - ẑ_i)ᵀ·Q⁻¹·(z_t - ẑ_i))

其中 z_t 为实际量测、ẑ_i 为粒子 i 的预测量测。文档给出了直观的数值实验：单个粒子初始化后权重为 1.0，首次更新后约 0.023，而将粒子坐标故意设错（particles[0].x = -10）后权重骤降至约 7.95e-07——权重越低，重采样时被抽中的概率越低，粒子越可能消亡。路标本身的 EKF 更新由 update_KF_with_cholesky 通过 Cholesky 分解完成。 3. 重采样（Resampling）：resampling 先归一化权重，再计算有效粒子数 Neff = 1/(w·wᵀ)；当 Neff < NTH 时执行低方差重采样（low variance re-sampling）：按累积权重均匀抽取新粒子索引，权重恢复为 1/N_PARTICLE。文档用 100 个位置均匀分布在 [-0.5, 0.5]、权重呈高斯分布的粒子演示了重采样前后粒子向高权重区域聚集的过程。

运行方式：

python SLAM/FastSLAM1/fast_slam1.py

测试见 test_fast_slam1.py。

FastSLAM 2.0：粒子提议分布的改进

FastSLAM 2.0 文档 篇幅较短，说明其动画含义与 FastSLAM 1.0 一致。从算法演进的角度看，FastSLAM 1.0 在预测阶段只用运动模型（控制 u）推进粒子，量测 z 仅在更新阶段用于调整权重；而 FastSLAM 2.0 在提议分布（proposal distribution）中同时融合了运动模型与当前量测 z，因此采样的粒子更集中于量测似然高的区域，通常能用更少的粒子达到相近的估计质量。

运行方式：

python SLAM/FastSLAM2/fast_slam2.py

对应的测试见 test_fast_slam2.py。两个版本的核心参数与数据结构（Particle 类、N_PARTICLE、Q、R 等）基本一致，可对照学习。

图优化 SLAM：Graph SLAM 从公式到真实数据集

与滤波方法的本质区别

Graph SLAM 文档 及 graphSLAM_doc.rst 指出：与 EKF、UKF、粒子滤波等概率递推方法不同，图技术将 SLAM 形式化为一个优化问题，主要面向离线求解完整 SLAM（轨迹走完后统一优化所有位姿），也有变体支持在线估计或求解位姿子集。GraphSLAM 利用运动信息与环境观测构造一个最小二乘问题，用标准优化技术求解。

数学形式：信息矩阵与信息向量

graphSLAM_formulation.rst 给出了严谨的推导。设机器人轨迹为 N 个位姿 p₁...p_N，收集 M 个量测 Z = {z_j}，每个量测被建模为带零均值高斯噪声、协方差为 Ω_j⁻¹ 的独立量测，其中 Ω_j 称为该量测的信息矩阵。残差定义为 e_j = z_j ⊖ ẑ_j（⊖ 为逆位姿合成运算）。目标是求量测下的最大后验位姿：

argmax p(p₁...p_N | Z) = argmin Σ_j e_jᵀ·Ω_j·e_j =: argmin χ²

采用高斯-牛顿式的迭代线性化：x^(k+1) = x^k ⊞ Δx^k，对残差做一阶泰勒展开后，χ² 化为 χ_k² + 2bᵀΔx + ΔxᵀHΔx 的二次型，最优更新为：

Δx = -H⁻¹·b

其中 H（信息矩阵，亦称 Ω）与 b（信息向量，亦称 ξ）由所有边（约束）累加而成，迭代至收敛。文档还讨论了流形与维度：SE(2) 位姿用 (x, y, θ) 表示（d = 3, c = 3），SE(3) 位姿用 (x, y, z, qx, qy, qz, qw) 表示（d = 7, c = 6）。

最小示例：1D 机器人与单路标

graphSLAM_doc.rst 用一个 1D 机器人演示核心思想：机器人以 u_t = 1 前进，但里程计有噪声；环境中只有位于 x=3 的一个路标，观测为机器人与路标的距离。通过虚拟量测（virtual measurement）技巧——把"两个时刻观测到同一路标"转化为两个节点之间的相对约束——构造 H 与 b。求解前 det(H) = 0（信息矩阵奇异，因为缺乏绝对参考），加上锚定约束 H[0,0] += 1 后 det(H) = 18.75，迭代 5 次后里程计从 [0, 1.5, 2.4] 优化为 [0, 0.9, 1.9]，逼近真值 [0, 1, 2]。

文档特别强调三点重要结论：

1. 信息矩阵与信息向量的贡献是累加的（各边叠加到既有值上）；
2. 2D 机器人场景下约束是非线性的，累加 H、b 时需要残差对各状态的雅可比（A = ∂e_ij/∂x_i，B = ∂e_ij/∂x_j）；
3. 锚定约束必不可少，否则信息矩阵奇异、无法求逆。

平面 SE(2) 示例与源码对应

文档随后给出了 3 自由度 [x, y, θ]ᵀ 的平面示例。其虚拟量测误差方程为：

e_ij^x = x_j + d_j·cos(ψ_j + θ_j) - x_i - d_i·cos(ψ_i + θ_i)
e_ij^y = y_j + d_j·sin(ψ_j + θ_j) - y_i - d_i·sin(ψ_i + θ_i)
e_ij^ψ = ψ_j + θ_j - ψ_i - θ_i

即"若运动与量测都完美，从节点 i 与节点 j 分别投影出的路标位置应当重合"。在 graph_based_slam.py 中，Edge 类封装误差向量 e 与信息矩阵 omega；calc_edge 计算虚拟量测误差并用旋转矩阵构造 omega = (R1·σ·R1ᵀ + R2·σ·R2ᵀ)⁻¹；calc_edges 遍历所有节点组合 itertools.combinations，仅当两节点观测到同一路标 ID（z_list[t1][iz1,3] == z_list[t2][iz2,3]）时才生成边。仿真参数：DT = 2.0s、SIM_TIME = 100s、MAX_RANGE = 30m、C_SIGMA1/2 = 0.1、C_SIGMA3 = deg2rad(1)、MAX_ITR = 20。

优化过程可概括为：构造边 → 线性化（计算雅可比 A、B）→ 累加 H 与 b → 固定原点（H[0:3,0:3] += I）→ 求解 dx = -H⁻¹b → 更新位姿，重复至收敛。文档示例中，单次迭代后 graphSLAM 定位误差为 0.0107，优于里程计误差 0.0004（该示例里程计本身噪声很小，随着节点与路标增多，图优化的优势会更明显）。

真实世界 SE(2) 数据集：INTEL 数据集

graphSLAM_SE2_example.rst 展示了用 graphslam 子包对真实世界数据集（INTEL 数据集，数据位于 data 目录）做迭代优化的完整流程：

from graphslam.graph import Graph
from graphslam.load import load_g2o_se2

g = load_g2o_se2("data/input_INTEL.g2o")

该数据集包含 1228 个顶点、1483 条边，边分两类：

1. 里程计边（odometry edges）：约束相邻两个顶点（共 1227 条），量测来自里程计数据；
2. 扫描匹配边（scan-matching edges）：约束非相邻顶点（共 256 条），即回环闭合（loop closure），可由 2D LiDAR 或路标配准得到。

初始状态：里程计边 χ² ≈ 0.232（与位姿一致），而扫描匹配边 χ² ≈ 7191686.15（误差巨大）——这正是里程计误差随时间累积、轨迹发生漂移的体现。调用 g.optimize() 后，χ² 从 7191686 快速下降到 215.8，其中扫描匹配边 χ² 降至 73.65，说明回环约束成功把漂移的轨迹"拉回"正确位置。这一收敛过程清晰展示了 Graph SLAM 将多源数据（里程计 + 扫描匹配）统一纳入一个优化问题的能力。

运行方式：

python SLAM/GraphBasedSLAM/graph_based_slam.py

测试见 test_graph_based_slam.py。

总结与选型视角

PythonRobotics 的 SLAM 模块用可运行的最小化仿真覆盖了 SLAM 领域三种主流技术路线：

算法	思想	特点	主要文件
EKF SLAM	单高斯扩展卡尔曼滤波	状态含位姿 + 全部路标，协方差跟踪不确定性，计算量随路标数平方增长	SLAM/EKFSLAM/ekf_slam.py
ICP	最近邻 + SVD 配准	计算帧间旋转/平移，是激光 SLAM 前端的基础	SLAM/ICPMatching/icp_matching.py
FastSLAM 1.0	粒子滤波 + 每路标独立 EKF	每个粒子维护位姿与 n 个 2×2 EKF，权重驱动重采样	SLAM/FastSLAM1/fast_slam1.py
FastSLAM 2.0	改进提议分布	采样时融合量测，粒子效率更高	SLAM/FastSLAM2/fast_slam2.py
Graph SLAM	图优化最小二乘	离线批量优化，含锚定约束与回环闭合，可处理大规模真实数据	SLAM/GraphBasedSLAM/graph_based_slam.py

这些示例共享相同的仿真骨架（observation、motion_model、pi_2_pi 等辅助函数），便于横向对比不同算法的表现。全部模块都有对应的 测试文件 与基于 requirements.txt 的依赖环境，读者可以直接在仓库中运行上述命令，观察动画并修改参数（如噪声大小、粒子数、路标分布）来加深对每个算法的理解。