📚 课程目录第4章后端优化

4. 后端优化

在激光SLAM系统中，前端负责帧间匹配与局部里程计估计，后端则承担全局一致性维护的任务。当激光雷达在长走廊、隧道、地下车库等退化场景中运行时，前端不可避免会累积漂移，此时后端优化的质量直接决定了建图精度与定位鲁棒性。本节围绕图优化理论、位姿图构建、最小二乘建模、鲁棒核函数以及g2o/Ceres/GTSAM三大工具实践展开，帮助读者建立从理论到工程落地的完整认知。

一、图优化理论：从滤波到非线性优化的范式转变

传统EKF-SLAM将状态与协方差矩阵随观测递推更新，计算复杂度随路标数量呈平方增长，且线性化误差会随运行时间累积。图优化（Graph-based Optimization）则采用完全不同的思路：将SLAM问题建模为因子图（Factor Graph），把待估计的机器人位姿和路标视为顶点（Vertex/Node），把里程计约束、激光匹配约束、回环约束、GPS约束等视为边（Edge/Factor）。每条边代表一个观测方程，携带信息矩阵（协方差逆）作为权重。

图优化的核心思想是：所有观测共同构成一个超定方程组，通过最小化所有边的残差平方和来求解最优位姿配置。相比滤波方法，图优化支持反复线性化（Gauss-Newton或Levenberg-Marquardt迭代），能够利用回环约束重新分配全局误差，在大规模场景中精度显著更优。

核心公式：目标函数 F(x) = Σ eᵢ(x)ᵀ Ωᵢ eᵢ(x)，其中 eᵢ 为第 i 条边的残差，Ωᵢ 为信息矩阵。求解即寻找 x* = argmin F(x)。

二、位姿图构建：节点、边与信息矩阵

在激光SLAM中，最常用的图结构是位姿图（Pose Graph），即只保留机器人位姿节点，不显式维护路标。位姿图的构建流程如下：

节点生成：前端每输出一帧位姿，就在图中新增一个节点，记录时间戳、位姿初值（x, y, θ）以及该帧的点云或特征描述子。
里程计边：相邻节点之间添加相对位姿约束，来源于前端帧间匹配结果，信息矩阵根据匹配残差协方差设定。
回环边：当检测到当前帧与历史帧相似度超过阈值时，通过ICP/NDT精配准计算相对位姿，添加回环约束。回环边是消除累积漂移的关键。
全局约束：若存在GPS、RTK或已知地标，可作为一元边或二元边加入图中，锚定全局坐标系。

位姿图构建的质量取决于三个要素：节点初值是否合理、边约束是否准确、信息矩阵是否反映真实不确定性。若信息矩阵设置过大，优化会过度信任错误约束；设置过小，则约束形同虚设。

三、最小二乘与鲁棒核函数

图优化的求解本质是非线性最小二乘问题。给定残差函数 e(x)，目标是最小化 Σ eᵢᵀ Ωᵢ eᵢ。常用求解策略包括Gauss-Newton、Levenberg-Marquardt和Dog-Leg。Gauss-Newton在残差接近线性时收敛快，但Hessian矩阵可能奇异；LM通过引入阻尼因子λ提升稳定性，是SLAM后端最常用的方法。

然而，现实中的激光SLAM数据常包含外点（Outlier）：错误的回环检测、动态物体遮挡、退化场景中的错误匹配等。若直接使用平方误差，单个大残差会主导整个优化，导致位姿图被拉偏。鲁棒核函数（Robust Kernel）通过降低大残差的权重来抑制外点影响。

核函数	表达式	特点	适用场景
Huber	ρ(s) = s (s≤δ); 2δ√s - δ² (s>δ)	小残差二次，大残差线性	一般外点抑制，计算简单
Cauchy	ρ(s) = δ²/2 · ln(1 + s/δ²)	对大残差抑制更强	回环误检较多的场景
Geman-McClure	ρ(s) = s / (1 + s)	极强抑制，可能过度降权	外点比例高的恶劣环境
DCS	动态调整尺度参数	自适应，无需手动调参	外点比例未知的在线场景

工程建议：量产项目中推荐优先使用Huber或Cauchy核函数，δ参数通常设为残差标准差的1.5~3倍。若回环检测模块已具备较强的几何验证能力，可适当放宽核函数强度，避免过度降权导致真实约束被削弱。

四、g2o/Ceres/GTSAM工具实践

三大优化库各有侧重，量产选型需结合团队技术栈与场景需求。

g2o是经典图优化库，以超图（HyperGraph）为核心数据结构，内置丰富的顶点和边类型。其优势在于与ORB-SLAM系列生态兼容性好，自定义边和顶点灵活。以下是一个位姿图优化片段：

// g2o 位姿图优化核心步骤
g2o::SparseOptimizer optimizer;
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
    std::make_unique<g2o::BlockSolver_6_3>(
        std::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>()));
optimizer.setAlgorithm(solver);

// 添加位姿顶点
for (size_t i = 0; i < poses.size(); ++i) {
    auto* v = new g2o::VertexSE3();
    v->setId(i);
    v->setEstimate(poses[i]);
    optimizer.addVertex(v);
}

// 添加里程计边（信息矩阵 = 协方差逆）
auto* edge = new g2o::EdgeSE3();
edge->setVertex(0, optimizer.vertex(i));
edge->setVertex(1, optimizer.vertex(i+1));
edge->setMeasurement(relative_pose);
edge->setInformation(info_matrix);
optimizer.addEdge(edge);

optimizer.initializeOptimization();
optimizer.optimize(50);

Ceres Solver由Google开发，以自动微分和丰富的损失函数著称。其Problem/ResidualBlock接口简洁，适合快速原型开发。Ceres内置Huber、Cauchy等核函数，通过LossFunction直接指定。在激光SLAM中，Ceres常用于NDT配准和位姿图联合优化。

GTSAM（Georgia Tech Smoothing and Mapping）基于因子图理论，提供iSAM2增量式优化器，特别适合在线SLAM。其优势在于：支持增量更新，无需每次全图重优化；内置ISAM2、Levenberg-Marquardt等求解器；因子类型丰富，包括BetweenFactor、PriorFactor、RobustNoiseModel等。在量产激光SLAM中，GTSAM的iSAM2常用于回环后的快速全局校正。

常见误区：许多工程师在回环优化后直接使用优化结果覆盖当前位姿，导致定位跳变。正确做法是：优化完成后，通过插值或滑动窗口平滑过渡，避免输出位姿突变影响下游控制模块。此外，信息矩阵的尺度需与实际残差量纲匹配，否则优化器会隐性偏向某些约束。

五、案例复盘：地下车库回环优化

某量产项目在地下车库场景中，前端激光里程计运行约300米后累积漂移达1.2米。回环检测模块在车辆回到起点附近时触发，通过NDT精配准得到相对位姿，残差约0.15米。将该回环边加入GTSAM因子图，使用Huber核函数（δ=0.3），iSAM2增量优化后全局位姿误差降至0.08米。关键调参记录如下：

# GTSAM 回环因子配置示例
noise_model = gtsam.noiseModel.Robust.Create(
    gtsam.noiseModel.mEstimator.Huber.Create(0.3),
    gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.02]))
)
loop_factor = gtsam.BetweenFactorPose2(
    key_from, key_to, relative_pose, noise_model
)
graph.push_back(loop_factor)
result = isam.update(graph, initial_estimate)

复盘发现两个关键点：其一，回环边的信息矩阵不能直接沿用前端里程计的协方差，需根据精配准的残差分布重新标定；其二，Huber核函数的δ值若设置过小（如0.1），会将真实回环约束也降权，导致优化后仍有残余漂移。最终δ=0.3在抑制外点与保留真实约束之间取得了平衡。

综上，后端优化是激光SLAM从“能跑”到“跑得准、跑得稳”的核心环节。理解图优化的数学本质、掌握位姿图构建方法、合理使用鲁棒核函数、并根据场景选择合适的工具库，是量产定位工程师必须具备的工程能力。