3. 后端优化基础

激光SLAM的前端（Front-end）通过帧间匹配（如ICP、NDT）或特征匹配，为我们提供了相邻帧之间的相对位姿变换。然而，这些局部估计不可避免地会累积漂移（Drift），导致全局地图出现“错位”或“重影”。后端优化（Back-end Optimization）的核心任务，就是利用所有历史观测数据（包括位姿约束、回环检测约束、传感器观测等），对机器人的轨迹和地图进行全局或局部的最优估计，以最小化累积误差。在众多优化方法中，基于图（Graph）的优化理论已成为现代激光SLAM系统（如Cartographer、LOAM系列、LIO-SAM）的主流。本章将深入剖析图优化的数学本质、位姿图（Pose Graph）与因子图（Factor Graph）的建模差异，并探讨其在实际工程中的应用与陷阱。

上图清晰地展示了后端优化的整体框架。无论是位姿图还是因子图，其底层数学原理均源于最大后验估计（MAP）与最小二乘问题。在激光SLAM中，我们通常将机器人不同时刻的位姿（位置与姿态）抽象为图中的“节点”，而将连接这些节点的“约束”（如里程计测量、回环检测结果）抽象为“边”。优化的过程，就是调整节点的状态变量，使得所有边的误差项（残差）的加权平方和最小。下面，我们将从图优化的数学本质出发，逐步拆解两种主流建模方式。

3.1 图优化理论：从最小二乘到稀疏求解

3.1.1 核心原理：将SLAM问题转化为最小二乘

图优化的核心思想是将状态估计问题转化为一个非线性最小二乘问题。假设我们有 n 个待优化的状态变量 x = [x₁, x₂, ..., xₙ]ᵀ（在激光SLAM中，xᵢ 通常表示第 i 帧的位姿，即 3x3 旋转矩阵 R 和 3x1 平移向量 t，或使用四元数表示）。同时，我们有 m 个约束，每个约束对应一个“边”，描述了两个状态变量之间的相对关系。例如，通过前端里程计，我们得到第 i 帧到第 i+1 帧的相对变换 ΔT，那么误差项可以定义为：

e_ij(x) = (ΔT_ij)⁻¹ · (x_i)⁻¹ · x_j

在数学上，我们通常使用李代数（Lie Algebra）将位姿映射到向量空间，从而将误差定义为向量形式。最终，我们的目标函数为：

F(x) = Σ e_ij(x)ᵀ · Ω_ij · e_ij(x)

其中，Ω_ij 是信息矩阵（Information Matrix），即协方差矩阵的逆，用于衡量该约束的置信度。信息矩阵越大，代表该约束越可靠，在优化中占据的权重越大。我们的目标是找到一组 x*，使得 F(x) 最小。

3.1.2 求解流程：高斯牛顿法与稀疏性利用

由于 F(x) 通常是非线性的，我们采用迭代法求解。最常用的方法是 高斯牛顿法（Gauss-Newton） 和 列文伯格-马夸尔特法（Levenberg-Marquardt）。其核心步骤为：

1. 线性化：在当前的估计值 x 附近，对误差项 e_ij 进行一阶泰勒展开。
2. 构建正规方程：将误差项代入目标函数，忽略高阶项，得到关于增量 Δx 的线性方程 H Δx = -b。其中 H 是海森矩阵（Hessian）的近似，b 是梯度向量。
3. 求解增量：求解上述线性方程组。这里的关键在于，SLAM问题的 H 矩阵具有天然的稀疏性（因为每个约束只连接少数几个节点），我们可以利用稀疏Cholesky分解或共轭梯度法等手段高效求解。
4. 更新状态：x ← x + Δx，并重复迭代，直到收敛（如增量小于阈值）。

3.2 位姿图（Pose Graph）：轻量级轨迹优化利器

3.2.1 核心原理：只优化轨迹，不优化地图

位姿图是图优化在SLAM中最直接的应用。在激光SLAM中，我们通常认为地图是由一系列激光点云拼接而成的，而拼接的准确性完全依赖于轨迹的准确性。因此，位姿图只将机器人不同时刻的位姿作为节点，而边则分为两类：

• 顺序边（Odometry Edge）：连接相邻帧，由前端里程计（如ICP匹配）提供，约束了相邻位姿的相对变换。
• 回环边（Loop Closure Edge）：当机器人回到曾经到过的位置时，通过回环检测（如Scan Context或基于特征的方法）建立当前帧与历史帧之间的约束。这是消除累积漂移的关键。

3.2.2 实战场景：2D激光SLAM的全局优化

以经典的2D激光SLAM为例，假设机器人在一个走廊环境中运行了100米，由于轮式里程计的打滑，轨迹在转弯处出现了约0.5米的偏差。此时，位姿图优化可以发挥巨大作用：

1. 构建图：将100米轨迹按每0.5米一个关键帧，共建立200个节点。顺序边连接相邻节点，回环边则通过识别走廊尽头的特征（如墙角）建立。
2. 设置权重：顺序边的信息矩阵通常根据里程计的不确定性设定（如平移方差0.1m²，旋转方差0.01rad²）。回环边的信息矩阵应设置得更大（如平移方差0.01m²），以表示对回环检测结果的信任。
3. 执行优化：使用g2o或Ceres进行优化。优化后，轨迹的累积误差被“拉回”到正确位置，地图的错位现象消失。

3.2.3 常见误区：忽略回环边的“相对性”

一个常见的错误是，将回环检测得到的位姿（例如通过Scan-to-Map匹配得到的全局位姿）直接作为节点的“先验值”，而不是作为两个节点之间的“相对约束”。正确的做法是，将回环检测得到的当前帧位姿与历史帧位姿之间的相对变换作为边。如果错误地引入了约束，会破坏图的稀疏性，并可能导致优化结果向错误的坐标偏移。

3.3 因子图（Factor Graph）：多传感器融合的灵活框架

3.3.1 核心原理：概率图模型与增量推理

因子图是一种更一般化的概率图模型，它显式地建模了变量与观测之间的条件概率关系。在因子图中，节点仍然代表待估计的变量（可以是位姿、地图点、甚至传感器外参），而“因子”则代表一个函数，描述了与该因子相连的变量应满足的约束。因子图与位姿图最大的区别在于：

• 表达力更强：因子可以表达任意形式的观测模型，例如IMU预积分因子、激光点云匹配因子、GPS因子等。
• 支持增量优化：当新的观测到来时，因子图可以只更新受影响的部分变量，而不必重新优化整个图。这就是GTSAM库中著名的 iSAM2（Incremental Smoothing and Mapping）算法。

3.3.2 案例拆解：LIO-SAM中的因子图融合

LIO-SAM（Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping）是因子图在激光SLAM中的经典应用。其因子图结构如下表所示：

因子类型	连接变量	作用与来源
IMU预积分因子	相邻两帧位姿 (T_i, T_i+1)	约束两帧之间的相对运动，由IMU角速度和加速度积分得到，提供高频运动先验。
激光里程计因子	相邻两帧位姿 (T_i, T_i+1)	通过当前帧与局部地图匹配（特征提取+ICP）得到，提供低频但高精度的位姿变换。
回环检测因子	当前帧与历史帧 (T_i, T_j)	通过Scan Context或强度特征检测，提供全局约束，消除漂移。
GPS因子（可选）	单帧位姿 (T_i)	当GPS信号良好时，提供位置约束，抑制长距离漂移。

在LIO-SAM中，因子图不仅优化了位姿，还通过“边缘化”（Marginalization）技术将旧的激光帧和IMU状态从图中移除，从而保证了优化规模的恒定，实现了实时性。这种设计使得LIO-SAM在复杂环境下（如地下矿山、城市峡谷）依然能保持高精度和鲁棒性。

3.3.3 判断方法：何时选择位姿图，何时选择因子图？

这是一个工程决策问题。以下是一些参考标准：

• 传感器类型：如果仅使用激光雷达，且计算资源有限（如嵌入式平台），位姿图是更轻量的选择。
• 融合需求：如果需要融合IMU、GPS、相机等多种传感器，因子图提供了更自然的建模方式，因为每种传感器都可以定义为一个独立的因子。
• 实时性要求：因子图配合iSAM2可以实现增量更新，在长时间运行的大规模场景中，其计算效率远高于每次都进行全量优化的位姿图。

3.4 总结复盘：从理论到实践的桥梁

本章我们深入探讨了后端优化的三大基石。图优化理论提供了数学框架，位姿图是其在纯轨迹优化场景下的高效实现，而因子图则是面向多传感器融合的通用扩展。在实际工程中，选择哪种方案取决于你的传感器配置、计算平台和精度需求。无论选择哪种，理解其背后的“节点-边-信息矩阵”模型，以及稀疏求解的原理，都是你调试和优化SLAM系统的核心能力。

在下一阶段的课程中，我们将基于这些理论基础，深入探讨回环检测的具体实现方法（如Scan Context、强度描述子），以及如何将回环约束无缝地集成到本章所讲的图优化框架中，最终构建一个鲁棒、实时的激光SLAM系统。