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,?,{\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?n)$	良	中	中
圖優化(batch)	$O(n^{1.5})$	優	差	高
iSAM2	$O(\log n)$	優	優	高

5. 未來發展方向

1. 概率推理新框架：結合變分推斷
2. 多傳感器融合：IMU+視覺+激光的聯合優化
3. 深度學習輔助：使用GNN學習約束權重
4. 分布式優化：多機器人協同SLAM

后端優化是SLAM精度與魯棒性的核心保障，需根據應用場景在計算效率與精度間權衡設計。