【第23话：定位建图】SLAM后端优化方法详解

SLAM 后端优化方法详解

SLAM（Simultaneous Localization and Mapping）后端优化是SLAM系统中的关键环节，负责对前端输出的传感器数据进行全局一致性优化，消除累积误差。后端通常基于图优化框架（如g2o、GTSAM），将机器人的位姿和地图特征点表示为图中的节点，将传感器观测数据转换为边（约束），通过非线性最小二乘法（如Levenberg-Marquardt算法）最小化误差函数，得到最优的位姿和地图估计。

后端优化分为增量式和批量式。增量式优化（如iSAM2）适用于实时性要求高的场景，通过逐步更新图结构实现高效优化；批量式优化则对所有历史数据进行全局调整，精度更高但计算量较大。现代SLAM系统常采用因子图（Factor Graph）建模，结合鲁棒核函数（如Huber核）处理异常观测，并融合闭环检测（Loop Closure）以修正长期漂移。

后端优化的核心目标是通过概率推断（如最大后验估计，MAP）实现全局一致的地图与轨迹，其性能直接影响SLAM系统的鲁棒性和精度。

1. 后端优化核心目标

在SLAM（同时定位与建图）中，后端优化负责解决两个关键问题：

• 全局一致性：消除里程计累积误差
• 闭环校正：修正回环检测时的位姿漂移
  数学表达为最小化代价函数：
  $$\underset{\mathbf{x}}{\min }\sum _{(i,j)\in \mathcal{E}}\rho \left({\Vert {\mathbf{z}}_{ij}-h_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)\Vert }_{{\Sigma }_{ij}}^2\right)$$
  其中：
• $\mathbf{x}$为状态变量（位姿+地图点）
• $\rho$为鲁棒核函数（如Huber）
• ${\Sigma }_{ij}$为协方差矩阵

---

2. 主流优化方法

(1) 基于滤波的方法

• 扩展卡尔曼滤波 (EKF)

  • 状态向量：${\mathbf{x}}_k=[{\mathbf{x}}_v,{\mathbf{m}}_1,\cdots ,{\mathbf{m}}_n{]}^T$
  • 更新方程：
    $$\begin{array}{rl}{\hat{\mathbf{x}}}_{k|k-1}& =f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k)\\ {\mathbf{P}}_{k|k-1}& ={\mathbf{F}}_k{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$
  • 缺点：计算复杂度$O(n^2)$，线性化误差累积

• 粒子滤波 (FastSLAM)

  • 使用Rao-Blackwellized粒子滤波
  • 每个粒子独立维护地图：
    $$p({\mathbf{x}}_{1:t},\mathbf{m}|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})=\sum _{i=1}^N{w}_t^{(i)}{\delta }_{{\mathbf{x}}_{1:t}^{(i)}}({\mathbf{x}}_{1:t})p(\mathbf{m}|{\mathbf{x}}_{1:t}^{(i)},{\mathbf{z}}_{1:t})$$

(2) 基于图优化的方法

• 因子图模型

• 优化流程：

  1. 构建图：节点=位姿/路标，边=约束
  2. 线性化误差函数：${\mathbf{e}}_{ij}\approx {\mathbf{e}}_{ij}^0+{\mathbf{J}}_{ij}\Delta \mathbf{x}$
  3. 求解正规方程：$\mathbf{H}\Delta \mathbf{x}=-\mathbf{b}$
     • $\mathbf{H}=\sum {\mathbf{J}}^T{\Sigma }^{-1}\mathbf{J}$
     • $\mathbf{b}=\sum {\mathbf{J}}^T{\Sigma }^{-1}\mathbf{e}$

• 关键算法：

  • Gauss-Newton：$\Delta \mathbf{x}=-({\mathbf{J}}^T\mathbf{J}{)}^{-1}{\mathbf{J}}^T\mathbf{e}$
  • Levenberg-Marquardt：$({\mathbf{J}}^T\mathbf{J}+\lambda \mathbf{I})\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{e}$

(3) 增量式优化

• iSAM2 (Incremental Smoothing and Mapping)
  • 使用贝叶斯树维护因子图
  • 关键步骤：

    def update(isam, new_factors):# 增量更新isam.update(new_factors)# 部分重新线性化isam.relinearize(affected_vars)# 优化result = isam.optimize()
    

  • 优势：实时性高，复杂度$O(\log n)$

---

3. 关键技术挑战

• 稀疏性利用：使用Schur补加速求解
  $$\left[\begin{array}{cc}{\mathbf{H}}_{pp}& {\mathbf{H}}_{pm}\\ {\mathbf{H}}_{mp}& {\mathbf{H}}_{mm}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_p\\ \Delta {\mathbf{x}}_m\end{array}\right]=\left[\begin{array}{c}{\mathbf{b}}_p\\ {\mathbf{b}}_m\end{array}\right]$$
  通过消元路标点：
  $$({\mathbf{H}}_{pp}-{\mathbf{H}}_{pm}{\mathbf{H}}_{mm}^{-1}{\mathbf{H}}_{mp})\Delta {\mathbf{x}}_p={\mathbf{b}}_p-{\mathbf{H}}_{pm}{\mathbf{H}}_{mm}^{-1}{\mathbf{b}}_m$$

• 鲁棒性处理：

  • 使用Huber核函数：
    $$\rho (s)=\{\begin{array}{ll}s& \text{if }s\le \delta \\ 2\sqrt{\delta s}-\delta & \text{otherwise}\end{array}$$
  • RANSAC预处理异常值

• 大规模场景优化：

  • 采用分级优化：局部子图→全局优化
  • 使用位姿图（Pose Graph）压缩：

    # 创建位姿图
    pose_graph = PoseGraph()
    # 添加节点（位姿）
    node1 = pose_graph.add_node(pose1)
    # 添加约束（相对位姿变换）
    pose_graph.add_edge(node1, node2, relative_pose, covariance)
    

---

4. 性能对比

方法	计算复杂度	全局一致性	实时性	实现难度
EKF	$O(n^2)$	中	差	低
粒子滤波	$O(N\cdot n)$	良	中	中
图优化(batch)	$O(n^{1.5})$	优	差	高
iSAM2	$O(\log n)$	优	优	高

5. 未来发展方向

1. 概率推理新框架：结合变分推断
2. 多传感器融合：IMU+视觉+激光的联合优化
3. 深度学习辅助：使用GNN学习约束权重
4. 分布式优化：多机器人协同SLAM

后端优化是SLAM精度与鲁棒性的核心保障，需根据应用场景在计算效率与精度间权衡设计。