Pose Graph Optimization

• 又稱: PGO, pose-graph SLAM, pose graph optimisation
• 類別: State estimation & navigation › Localisation
• 標籤: slam, state-estimation, optimization, localization, mapping, factor-graphs

一種 SLAM 後端技術，透過調整由姿態構成的圖，讓姿態之間的里程計限制與迴圈閉合限制盡可能一致，藉此精修機器人的軌跡。

定義

姿態圖最佳化（Pose graph optimization）是藉由在一張圖上求解最小平方問題來估計機器人軌跡的過程。圖中的每個節點是一個機器人姿態（某一時刻的位置與方向），每條邊則是兩個姿態之間量測到的相對轉換及其不確定性，後者通常以資訊矩陣表示。邊來自相鄰姿態之間的里程計，以及迴圈閉合，也就是機器人認出自己曾經到過的地方。

由於每個量測都有雜訊，各條邊永遠不會完全一致：沿著一個迴圈把里程計邊串接起來，並不會讓機器人剛好回到迴圈閉合邊所指出的位置。姿態圖最佳化會找出讓所有邊上加權平方誤差總和最小的姿態集合，並依每個量測的確定程度，把修正量按比例分散到整條軌跡上。

由於旋轉無法以加法合成，這個問題是非線性的，因此要用 Gauss-Newton 或 Levenberg-Marquardt 等方法反覆求解，每一步都在目前的估計值附近線性化。所得到的線性系統是稀疏的，因為每個姿態只與少數幾個其他姿態相連，而稀疏求解器讓含有數以千計姿態的圖也能處理。g2o、GTSAM 與 Ceres Solver 等開源函式庫常用於這個步驟。

姿態圖最佳化是大多數以圖為基礎之 SLAM 系統的後端。它與光束法平差的差別在於不把地標保留為變數：只最佳化姿態，這樣比較快，但會捨棄部分資訊。它已知的弱點是對錯誤的迴圈閉合很敏感，這也是為什麼穩健成本函數與離群值剔除會是標準的附加做法。

為何重要

里程計與視覺里程計會隨時間無限制地漂移，而姿態圖最佳化是在找到迴圈閉合後消除這種累積漂移的標準方法。它把一組局部準確但全域不一致的量測，轉變成在整棟建築或整個場地範圍內都一致的軌跡與地圖。

主要應用

• 以光達（LiDAR）與相機建圖的圖式 SLAM 後端
• 消除輪式、視覺與視覺慣性里程計的累積漂移
• 多工作階段建圖，合併在不同時間記錄的地圖
• 多機器人建圖，把多台機器人的軌跡合併到同一個座標系中

實際應用

在典型的光達（LiDAR）或視覺 SLAM 架構中，前端會在機器人每移動一段設定的距離或角度時加入一個新節點，並以一條里程計邊與前一個節點相連。地點辨識模組會提出候選的迴圈閉合，這些候選在加入為邊之前會先經過幾何驗證（例如用 ICP 或特徵匹配）。接著後端執行 g2o 或 GTSAM 等最佳化器，可以在每次迴圈閉合後批次執行，也可以增量執行（如 iSAM2），修正後的姿態則用來重建地圖。大量的調整工作花在為每條邊指定的共變異數，以及 Huber 或 Cauchy 等穩健核函數上，因為單一個高信賴度的錯誤迴圈閉合就可能讓整張地圖彎曲變形。

常見誤解

• 姿態圖最佳化負責建立地圖 — 它只修正姿態；地圖是事後把感測器資料放到修正後的姿態上重新建立的
• 迴圈閉合越多越好 — 單一個錯誤的迴圈閉合就可能扭曲整條軌跡，因此迴圈閉合必須經過驗證，並以穩健成本降低權重
• 它與光束法平差相同 — 光束法平差也會最佳化地標位置，而姿態圖只保留姿態與相對限制

誰需要了解

• SLAM 與定位工程師
• 移動機器人與自動駕駛車輛開發者
• 從事視覺里程計與建圖的電腦視覺工程師
• 學習機率機器人學與狀態估計的學生

相關技術

• g2o
• GTSAM
• Ceres Solver
• 因子圖（Factor Graphs）
• 光束法平差（Bundle Adjustment）
• Levenberg-Marquardt 演算法（Levenberg-Marquardt Algorithm）

相關術語

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

相關專案

• LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
• oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
• Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO

影片

• Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB