PythonRobotics 中的 Graph SLAM 公式化推导：从最大似然估计到稀疏最小二乘优化

导读

本文以 PythonRobotics 项目 docs/modules/4_slam/graph_slam/graphSLAM_formulation.rst 为核心，系统讲解 Graph SLAM 的数学公式化过程：如何把"根据一系列带噪测量（里程计、GPS、IMU、激光扫描匹配）估计机器人轨迹"的问题，建模成一张由位姿顶点与约束边组成的图，并最终转化为一个可迭代求解的加权最小二乘（χ² 最小化）问题。读完本文，你将掌握残差与信息矩阵的定义、流形上位姿的紧凑表示与 ⊖/⊞ 运算、最大似然到 χ² 的推导脉络，以及高斯-牛顿式迭代更新 Δx = −H⁻¹b 的完整推导，并能在 PythonRobotics 仓库中对照源码（SLAM/GraphBasedSLAM/）理解每一步公式的代码实现。

一、问题形式化：把 SLAM 写成概率模型

1.1 位姿序列与流形

设机器人在环境中运动，其轨迹由 N 个位姿的序列表示：

$$\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_N$$

每个位姿都位于某个流形 $\mathcal{M}$ 上（$\mathbf{p}_i \in \mathcal{M}$）。Graph SLAM 中常用的流形包括：

• 1 维、2 维、3 维空间：即 $\mathbb{R}$、$\mathbb{R}^2$、$\mathbb{R}^3$。这类环境是"直线型（rectilinear）"的，没有朝向（orientation）的概念；
• SE(2)：位姿由 $\mathbb{R}^2$ 中的位置与朝向角 $\theta$ 组成；
• SE(3)：位姿由 $\mathbb{R}^3$ 中的位置与朝向组成，朝向可以用欧拉角、四元数或 SO(3) 旋转矩阵表示。

在仓库源码中，SE(2) 位姿由 PoseSE2 类实现，其内部以 (x, y, theta) 三元组存储（构造函数将角度归一化到 $[-\pi, \pi)$），并提供了 to_array()、to_compact()、to_matrix() 与 from_matrix() 等表示转换方法，直接对应本文所述的流形表示需求。

1.2 测量、期望测量与残差

机器人在探索过程中采集到 M 个测量组成的集合 $\mathcal{Z} = \{\mathbf{z}_j\}$，例如里程计、GPS、IMU 数据。给定位姿序列后，可以计算第 j 个测量的期望值：

$$\hat{\mathbf{z}}_j(\mathbf{p}_1, \ldots, \mathbf{p}_N)$$

进而定义残差（residual）：

$$\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j)$$

残差的具体公式取决于测量类型。文档给出了一个里程计的例子：设 $\mathbf{z}_1$ 是机器人在 $\mathbf{p}_1$ 运动到 $\mathbf{p}_2$ 时采集到的里程计测量，则期望测量与残差为：

$$\hat{\mathbf{z}}_1(\mathbf{p}_1, \mathbf{p}_2) = \mathbf{p}_2 \ominus \mathbf{p}_1$$

$$\mathbf{e}_1(\mathbf{z}_1, \hat{\mathbf{z}}_1) = \mathbf{z}_1 \ominus \hat{\mathbf{z}}_1 = \mathbf{z}_1 \ominus (\mathbf{p}_2 \ominus \mathbf{p}_1)$$

其中 $\ominus$ 算子表示逆位姿合成（inverse pose composition）。

该算子与 $\oplus$（位姿合成）在 PoseSE2 中通过重载 __add__ / __sub__ 实现：p1 + p2 即 ${\mathbf{p}}_1\oplus {\mathbf{p}}_2$（把 p2 变换到 p1 的坐标系下），p1 - p2 即 ${\mathbf{p}}_1\ominus {\mathbf{p}}_2$。边（约束）的残差在 edge_odometry.py 中直接写成 estimate - (p2 - p1)，与文档公式逐字对应。

1.3 高斯噪声假设与信息矩阵

模型假设每个测量 $\mathbf{z}_j$ 带有独立、零均值、协方差为 $\Omega_j^{-1}$ 的高斯噪声，其中 $\Omega_j$ 被称为测量 j 的信息矩阵（information matrix）。于是：

$$p(\mathbf{z}_j \mid \mathbf{p}_1, \ldots, \mathbf{p}_N) = \eta_j \exp\left(-(\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^{\mathsf{T}} \Omega_j\, \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j)\right)$$

其中 $\eta_j$ 是归一化常数。

信息矩阵即协方差矩阵的逆。在 graph_based_slam.py 中，边的信息矩阵由两个位姿处观测噪声的协方差（经旋转矩阵变换后）求逆得到：edge.omega = np.linalg.inv(Rt1 @ sig1 @ Rt1.T + Rt2 @ sig2 @ Rt2.T)，这正是 $\Omega_j$ 的代码形态。

二、从贝叶斯最大后验到 χ² 最小化

2.1 目标：最大似然位姿集合

Graph SLAM 的目标是：给定测量 $\mathcal{Z}$，找出最大似然（maximum likelihood）的位姿集合：

$$\mathop{\mathrm{arg\,max}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p(\mathbf{p}_1, \ldots, \mathbf{p}_N \mid \mathcal{Z})$$

2.2 贝叶斯公式化简

利用贝叶斯规则：

$$p(\mathbf{p}_1, \ldots, \mathbf{p}_N \mid \mathcal{Z}) = \frac{p(\mathcal{Z} \mid \mathbf{p}_1, \ldots, \mathbf{p}_N)\, p(\mathbf{p}_1, \ldots, \mathbf{p}_N)}{p(\mathcal{Z})} \propto p(\mathcal{Z} \mid \mathbf{p}_1, \ldots, \mathbf{p}_N)$$

因为 $p(\mathcal{Z})$ 是（未知的）常数，且假设先验 $p(\mathbf{p}_1, \ldots, \mathbf{p}_N)$ 是均匀分布，所以最大后验等价于最大化似然 $p(\mathcal{Z} \mid \mathbf{p}_1, \ldots, \mathbf{p}_N)$。

2.3 推导为 χ² 最小化

结合测量独立性假设与高斯形式，逐步化简：

$$\begin{aligned} \mathop{\mathrm{arg\,max}}\ p(\mathbf{p}_1,\ldots,\mathbf{p}_N \mid \mathcal{Z}) &= \mathop{\mathrm{arg\,max}}\ p(\mathcal{Z} \mid \mathbf{p}_1,\ldots,\mathbf{p}_N) \\ &= \mathop{\mathrm{arg\,max}} \prod_{j=1}^{M} p(\mathbf{z}_j \mid \mathbf{p}_1,\ldots,\mathbf{p}_N) \\ &= \mathop{\mathrm{arg\,max}} \prod_{j=1}^{M} \exp\left(-(\mathbf{e}_j)^{\mathsf{T}}\Omega_j \mathbf{e}_j\right) \\ &= \mathop{\mathrm{arg\,min}} \sum_{j=1}^{M} (\mathbf{e}_j)^{\mathsf{T}}\Omega_j \mathbf{e}_j \end{aligned}$$

于是定义需要最小化的目标函数：

$$\chi^2 := \sum_{j=1}^{M} (\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^{\mathsf{T}}\Omega_j\, \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j)$$

这就是 Graph SLAM 的核心洞察：一个概率推理问题被等价地转化成了一个加权最小二乘问题——每个残差以其信息矩阵为权重，所有约束共同贡献一个标量代价 χ²。

在 1D 最小示例文档 graphSLAM_doc.rst 中，作者用一段 30 行左右的代码直观演示了这一思想：机器人在 1D 直线上运动（控制量 $u_t=1$），在 $x=3$ 处有一个路标，观测为机器人与路标的距离。通过枚举节点对构建"虚拟测量"约束，累加出系统信息矩阵 H 与信息向量 b。运行输出显示：未加约束时 The determinant of H: 0.0（H 奇异），加入锚定约束后行列式变为 18.75，5 次迭代后里程计估计从 [0, 1.5, 2.4] 被修正为 [0, 0.9, 1.9]，逼近真值 [0, 1.0, 2.0]。

三、维度分析与位姿紧凑表示

在深入算法之前，需要厘清问题的维度：

• N 个位姿 $\mathbf{p}_1, \ldots, \mathbf{p}_N$，每个位姿位于流形 $\mathcal{M}$ 上；
  • 每个位姿 $\mathbf{p}_i$ 表示为（某子集内的）$\mathbb{R}^d$ 向量：
    • SE(2) 位姿通常表示为 $(x, y, \theta)$，故 $d = 3$；
    • SE(3) 位姿通常表示为 $(x, y, z, q_x, q_y, q_z, q_w)$，其中 $(q_x,q_y,q_z,q_w)$ 是四元数，故 $d = 7$；
  • 同时还需要把位姿紧凑地表示为（某子集内的）$\mathbb{R}^c$ 向量：
    • SE(2) 位姿有三个自由度，$(x, y, \theta)$ 表示已经足够，故 $c = 3$；
    • SE(3) 位姿只有六个自由度，可紧凑表示为 $(x, y, z, q_x, q_y, q_z)$，故 $c = 6$（四元数冗余了一个维度，需归一化约束）。
• M 个测量 $\mathcal{Z} = \{\mathbf{z}_1, \ldots, \mathbf{z}_M\}$：
  • 每个测量的维度可以各不相同，文档用 $\bullet$ 表示"通配（wildcard）"变量；
  • 测量 $\mathbf{z}_j \in \mathbb{R}^{\bullet}$ 关联一个信息矩阵 $\Omega_j \in \mathbb{R}^{\bullet \times \bullet}$ 和残差函数 $\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j) \in \mathbb{R}^{\bullet}$；
  • 理论上一个测量可以约束 1 个到全部 N 个位姿，但实践中每个测量通常只约束 1 或 2 个位姿。

当位姿以紧凑形式参与运算时，使用 $\boxplus$ 算子表示位姿合成：输入可以是一个流形位姿与一个紧凑向量（或两个紧凑表示），输出可以是 $\mathcal{M}$ 中的位姿或 $\mathbb{R}^c$ 中的向量，视上下文而定。$\boxplus$ 正是后续迭代更新 $\mathbf{x}^{k+1} := \mathbf{x}^k \boxplus \Delta\mathbf{x}^k$ 的基础。

四、图结构：顶点与边

Graph SLAM 中的"Graph"指的是把问题看作一张图：

• 顶点集合 $\mathcal{V}$ 共 N 个，每个顶点 $v_i$ 关联一个位姿 $\mathbf{p}_i$；
• 边集合 $\mathcal{E}$ 共 M 条，每条边 $e_j$ 关联一个测量 $\mathbf{z}_j$。

实践中图中的边要么是一元边（unary，即自环），要么是二元边（binary）。需要注意区分两个记号：$e_j$ 指与测量 $\mathbf{z}_j$ 关联的图中的边，而 $\mathbf{e}_j$ 指与 $\mathbf{z}_j$ 关联的残差函数。

在 graph.py 中，Graph 类用两个列表存储顶点与边，并通过 _link_edges() 建立边的 vertex_ids 与顶点对象的双向索引映射；_Chi2GradientHessian 类则负责把每条边贡献的 χ²、梯度与 Hessian 累加汇总——这与文档中"χ² 对所有边求和"的公式完全一致。

五、迭代优化：线性化与 Δx = −H⁻¹b

5.1 待优化的目标与变量堆叠

在图上，目标函数写为：

$$\chi^2 = \sum_{e_j \in \mathcal{E}} \mathbf{e}_j^{\mathsf{T}}\Omega_j \mathbf{e}_j$$

设 $\mathbf{x}_i \in \mathbb{R}^c$ 是位姿 $\mathbf{p}_i \in \mathcal{M}$ 的紧凑表示，把所有位姿堆叠成一个大向量：

$$\mathbf{x} := \begin{bmatrix} \mathbf{x}_1 \\ \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_N \end{bmatrix} \in \mathbb{R}^{cN}$$

5.2 迭代更新与残差线性化

优化是迭代进行的。第 k 步的更新为：

$$\mathbf{x}^{k+1} := \mathbf{x}^k \boxplus \Delta\mathbf{x}^k$$

第 k+1 步的 χ² 误差为：

$$\chi_{k+1}^2 = \sum_{e_j \in \mathcal{E}} \left[\mathbf{e}_j(\mathbf{x}^{k+1})\right]^{\mathsf{T}} \Omega_j\, \mathbf{e}_j(\mathbf{x}^{k+1})$$

对残差在 $\Delta\mathbf{x}^k = \mathbf{0}$ 处做一阶泰勒线性化：

$$\mathbf{e}_j(\mathbf{x}^{k+1}) = \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta\mathbf{x}^k) \approx \mathbf{e}_j(\mathbf{x}^k) + \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta\mathbf{x}^k)}{\partial \Delta\mathbf{x}^k}\,\Delta\mathbf{x}^k$$

注意这里的链式法则分成两部分：残差对（合成后的）位姿的偏导 × （合成后的）位姿对 $\Delta\mathbf{x}^k$ 的偏导。由于 SE(2)/SE(3) 的流形结构，$\boxplus$ 起到"在流形切空间上做加性更新"的作用，这正是流形优化的核心技巧。

5.3 二次型近似：梯度 b 与 Hessian H

把线性化残差代回 χ² 表达式并展开（忽略高阶项），可以得到标准的二次型近似：

$$\chi_{k+1}^2 \approx \chi_k^2 + 2\,\mathbf{b}^{\mathsf{T}}\Delta\mathbf{x}^k + (\Delta\mathbf{x}^k)^{\mathsf{T}} H\, \Delta\mathbf{x}^k$$

其中：

$$\mathbf{b}^{\mathsf{T}} = \sum_{e_j \in \mathcal{E}} [\mathbf{e}_j(\mathbf{x}^k)]^{\mathsf{T}} \Omega_j\, J_j, \qquad H = \sum_{e_j \in \mathcal{E}} J_j^{\mathsf{T}} \Omega_j\, J_j$$

这里的 $J_j$ 是残差关于 $\Delta\mathbf{x}^k$ 的雅可比矩阵（即上一步链式法则的结果，维度为 $\bullet \times dN$ 与 $dN \times cN$ 的乘积）。

源码佐证：在 graph_based_slam.py 的 fill_H_and_b 函数中，每条二元边按其两端顶点的块索引（id1 = edge.id1 * STATE_SIZE、id2 = edge.id2 * STATE_SIZE）把 A.T @ omega @ A、A.T @ omega @ B、B.T @ omega @ A、B.T @ omega @ B 累加到 H 的四个分块中，并把 A.T @ omega @ e、B.T @ omega @ e 累加到 b 中——与公式逐项对应。解析雅可比由 calc_jacobian 给出，对 SE(2) 位姿 $(x, y, \theta)$ 为 3×3 矩阵。

5.4 最优更新与收敛

对二次型求极小，令其对 $\Delta\mathbf{x}^k$ 的导数为零，得到最优更新：

$$\Delta\mathbf{x}^k = -H^{-1}\mathbf{b}$$

将该更新通过 $\mathbf{x}^{k+1} := \mathbf{x}^k \boxplus \Delta\mathbf{x}^k$ 施加到位姿上，重复迭代直到收敛。

锚定约束（固定原点）：由 $\boxplus$ 更新得到的线性系统 H 本身是奇异的（机器人整体平移/旋转不改变 χ²，即规范自由度（gauge freedom）问题）。因此源码中在求解前固定第一个位姿：

• 在 graph_based_slam.py 中：H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)；
• 在 graph.py 的 Graph.optimize() 中，若 fix_first_pose=True，则将 Hessian 前 dim 行/列清零并在对角块上加单位阵，同时把梯度前 dim 项清零；
• 配套文档 graphSLAM_doc.rst 中也特别强调："锚定约束是必需的，否则信息矩阵将是奇异的"，并用行列式 0 → 18.75（1D 示例）与 0 → 716.2（2D 示例）的对比直观验证了这一点。

5.5 收敛判据

两个实现采用略有差异但等价的收敛判据：

• graph_based_slam.py：计算更新量内积 diff = (dx.T @ dx)[0, 0]，当 diff < 1.0e-5 时提前终止（MAX_ITR = 20 为最大迭代上限）；
• graph.py 的 Graph.optimize(tol=1e-4, max_iter=20, fix_first_pose=True)：比较相邻两次迭代 χ² 的相对下降量，若 χ² 下降且相对变化小于 tol 则停止；求解环节用 scipy.sparse.linalg.spsolve 解稀疏线性系统（H 以 lil_matrix 稀疏格式存储），这正是文档强调的"每条边通常只约束 1～2 个位姿、H 具有稀疏结构"这一事实的直接受益点。

六、两类残差实现对比：解析雅可比与数值雅可比

文档的公式推导给出了通用框架，而仓库中恰好提供了两种风格的实现可供对照学习：

实现	文件	雅可比方式	适用场景
仿真示例	graph_based_slam.py	解析雅可比（calc_jacobian 返回 3×3 的 A、B 矩阵）	自包含仿真，无需外部数据
求解器包	graphslam/	数值微分（edge_odometry.py 以 EPSILON=1e-6 有限差分逼近雅可比）	通用 .g2o 数据集，避免手推雅可比

其中数值雅可比通过"对紧凑位姿的每个维度加微小扰动 ε、重算残差、差分求导"实现：

jacobian[:, d] = (self.calc_error() - err) / EPSILON

它牺牲了一点精度换取了实现上的通用性——这正是 graphSLAM_SE2_example.rst 中"为简单起见，使用数值微分替代解析雅可比"一语的来源。测试 test_graph_based_slam.py 通过把仿真时间缩短为 20 秒并关闭动画来快速验证整条仿真链路可以正常收敛运行。

七、在真实 SE(2) 数据集上验证公式

公式推导的最终检验来自真实数据。graphSLAM_SE2_example.rst 展示了如何用上述框架求解真实世界的 SE(2) 数据集（数据文件 data/input_INTEL.g2o）：

• 数据集包含 1228 个顶点、1483 条边；
• 边分为两类：
  1. 里程计边（odometry edges）：约束两个连续顶点，测量直接来自里程计数据（共 1227 条）；
  2. 扫描匹配边（scan-matching edges）：约束两个非连续顶点，通常由 2D LiDAR 或路标匹配得到，即文献中的回环闭合（loop closure）（共 256 条）；
• 初始状态下，里程计边贡献的 χ² 误差仅为 0.232，而扫描匹配边贡献高达 7,191,686——说明里程计小误差随时间累积成轨迹的大偏差，而回环约束"拉回"了漂移；
• 经 g.optimize() 迭代 6 次后，总 χ² 从 7,191,686 降至 215.84（odometry 边 142.189，scan-matching 边 73.652），优化前后对比印证了公式推导的有效性。

加载与保存 .g2o 文件的解析逻辑位于 load.py 与 Graph.to_g2o()（graph.py）：VERTEX_SE2 行解析为顶点，EDGE_SE2 行解析为边（测量 + 上三角信息矩阵展开），这正是 Graph SLAM 社区通用的 g2o 文件格式。

八、运行示例

在仓库根目录下，可以直接运行仿真示例（需要 numpy、scipy、matplotlib）：

python SLAM/GraphBasedSLAM/graph_based_slam.py

运行输出会依次打印每轮优化前的 cost 与边数、每次迭代的 iteration 与 diff（更新量大小）；仿真中蓝线为真实轨迹、黑线为航位推算（dead reckoning）、红线为 Graph SLAM 优化后的估计轨迹，黑色星号为用于生成图边的路标。相关依赖可参考 requirements/requirements.txt，导航到 docs/modules/4_slam/graph_slam/graph_slam_main.rst 可查看该模块在项目文档中的总入口。

九、总结与延伸阅读

回顾全文，Graph SLAM 的公式化可以概括为一条清晰的链条：

1. 建模：轨迹 = N 个流形上的位姿；测量 = 带高斯噪声的约束；
2. 残差：$\mathbf{e}_j = \mathbf{z}_j \ominus \hat{\mathbf{z}}_j$，以信息矩阵 $\Omega_j$ 加权；
3. 目标：贝叶斯最大后验 ⟺ 最小化 $\chi^2 = \sum \mathbf{e}_j^{\mathsf{T}}\Omega_j \mathbf{e}_j$；
4. 求解：在流形上用 $\boxplus$ 迭代更新，线性化残差得到 $\Delta\mathbf{x}^k = -H^{-1}\mathbf{b}$，直至收敛；
5. 实现要点：稀疏累加 H 与 b、锚定约束消除规范自由度、解析或数值雅可比任选。

仓库内与该主题直接相关的进一步阅读材料包括：

• 公式推导的配套最小示例与 2D 平面示例：graphSLAM_doc.rst
• 真实 SE(2) 数据集的完整优化过程：graphSLAM_SE2_example.rst
• 模块总览：graph_slam_main.rst
• 仿真实现源码：graph_based_slam.py
• 通用求解器包：graphslam/graph.py、graphslam/edge/edge_odometry.py、graphslam/pose/se2.py
• 数据集：data/input_INTEL.g2o

注：本文公式化部分的原始推导由 Jeff Irion 撰写，其求解器实现源自 python-graphslam 项目（已并入本仓库的 graphslam 目录）；文中引用的图 graphSLAM_doc_2_0.png 展示了 1D 最小示例中机器人位置、路标与观测的几何关系，是该公式化过程的直观演示。