狀態估計中的對稱性：視覺慣性里程計等變濾波器之演進
在視覺慣性里程計（VIO）領域中，主流方法分成兩種：一邊是以非線性優化為基礎、以精度為優先的方案，另一邊則是以擴展卡爾曼濾波（EKF）為代表、以計算效率為重的濾波方案。濾波法長期以來受制於「線性化誤差」的累積，可能導致濾波器結果的不一致甚至發散。近年的一系列研究透過發掘並利用 VIO 問題內在的幾何結構，即「對稱性」，來緩解此一限制。從等變 VIO（EqVIO）(Goor and Mahony 2023)系統到多狀態約束等變濾波器（MSCEqF）(Fornasier et al. 2024)的發展，正是此一方法的代表。
這些先進濾波器背後的核心原理，是數學中的對稱性概念，其表現形式為「不變性」（invariance）或「等變性」（equivariance）。若一個系統的屬性在其狀態經過特定變換後仍維持不變，我們稱之為不變性。若該屬性以一種可預測且對應的方式隨之變換，則稱之為等變性。等變濾波器（EqF）框架，便是一種利用這些對稱性來建構更精準、更一致的觀測器而設計的狀態估計技術。兩年前我們介紹過，EqVIO 論文是此領域的一項基礎性工作，它為 VIO 問題設計了一個特定的李群（Lie group），即 VI-SLAM 群，以及與之關聯的等變濾波器。此一成果是透過辨識並利用系統中慣性與視覺元件各自的獨特對稱性而達成的。
EqVIO 框架的一大關鍵貢獻，在於其對慣性測量單元（IMU）動態的處理方式。基於 IMU 測量值來推算機器人的導航狀態（姿態、位置與速度）本質上是一個非線性過程。該研究的作者們證明了，一個理想的、無偏差的 IMU 動態系統具備一種可由擴展特殊歐氏群 SE_2(3) 所描述的特定對稱性。透過在此李群上表述系統的誤差狀態，誤差傳播的動態過程得以變得完全線性。這從根本上消除了傳統 EKF 在預測步驟中通常會累積的線性化誤差，從而實現了隨時間推移更為精確的狀態傳播。
在視覺方面，EqVIO 引入了一種基於延展正交群 SOT(3) 的創新地標參數化方法。此公式使得視覺量測函數——即將一個地標的 3D 位置映射到 2D 影像座標的函數——具備了等變性。此性質使得濾波器在量測更新步驟中，得以採用一種更高階的「等變輸出近似法」。不同於標準 EKF 更新依賴於對量測函數的一階泰勒級數近似，該技術利用實際的量測值來建構一個更精準的更新（innovation）模型，從而有效地消除了二階誤差項。其結果是更準確的修正步驟以及地標深度估計的更快收斂。
儘管 EqVIO 在速度與精度上展現了卓越的性能，但其作為一個完整的 SLAM 濾波器架構——將地標維護於狀態向量中——因其二次方的計算複雜度而帶來了可擴展性的挑戰。後續的 MSCEqF 研究則將焦點從純粹的精度需求轉向了現實世界應用中更為實際的「強韌性」需求。MSCEqF 將等變概念應用於計算效率更高的多狀態約束卡爾曼濾波（MSCKF）架構。在此方法中，狀態向量不包含地標；取而代之的是，它維護了一個由過去相機姿態組成的滑動窗口。這確保了濾波器的計算複雜度保持在一個有界的範圍內，與地標數量脫鉤。
除了架構上的轉變，MSCEqF 在理論上也做出了推進，即增強了系統的對稱群。它引入了一種更精密的半直接積群（semi-direct product group）結構，將 IMU 偏差與相機內參正式地整合進了等變框架中。相比於僅僅將這些校準狀態作為附加變量處理，這是一種在數學原理上更為嚴謹的方法，因為它改善了濾波器在從較大初始誤差收斂過程中的線性化誤差動態。這種精煉的數學模型轉化成了系統強韌性，賦予了濾波器即便在存在顯著初始校準誤差或其他未預期狀態擾動的情況下，依然能夠可靠收斂的能力——這正克服了許多 VIO 系統在真實世界部署中需要個別調整參數的問題。
EqVIO 首先奠定了基本原則，明確建模並利用系統對稱性，導出在精度和速度上均顯著優於傳統方法的濾波方案。而 MSCEqF 在此基礎上，將此概念應用於一個更經典的濾波架構，並深化了其理論框架，增加其強韌性。兩者共同展示了一種設計下一代狀態估計器的強大方法論，使其不僅性能卓越，更具備足夠的韌性以應對真實世界的挑戰。
聲明：此文章使用了AI進行論文概念討論與擬稿，再由筆者人工校對與修改。
作者：葉宸甫
參考論文：
1. Fornasier, Alessandro, Pieter van Goor, Eren Allak, Robert Mahony, and Stephan Weiss. (2024) “MSCEqF: A Multi State Constraint Equivariant Filter for Vision-Aided Inertial Navigation.” IEEE Robotics and Automation Letters 9 (1): 731–38.
2. van Goor, P., & Mahony, R. (2023). EqVIO: An Equivariant Filter for Visual-Inertial Odometry. IEEE Transactions on Robotics, 39(5), 3567-3585.
留言
張貼留言