SE(3)上的不變隨機濾波用於串行剛體機械臂的慣性-編碼器狀態估計

近日，一篇發表在arXiv上的論文（arXiv:2607.00026）提出了一種基於SE(3)李羣的不變擴展卡爾曼濾波器（IEKF），專門用於串行剛體機械臂的狀態估計。該濾波器的創新之處在於完全在SE(3)李羣框架內構建，能夠處理任意數量的連桿，並利用運動學方程的羣仿射性質，使得線性化誤差動力學變得自治。這意味着黎卡提方程可以直接控制真實的誤差協方差，而不依賴於局部近似，從而大幅提高了估計的準確性。

論文中設計了一種物理分離的噪聲模型，分別處理陀螺儀和加速度計的測量通道。加速度計通過重力補償積分提供平移旋量，其測量協方差隨採樣間隔變化，與過程噪聲離散化類似。陀螺儀噪聲則通過狀態相關的科里奧利項建模，該項在靜止時消失，並隨旋量幅度增大而增加，從而更準確地反映噪聲在非線性動力學中的傳播。

濾波器的結構是一個模塊化的每連桿IEKF鏈，其中每個連桿的預測協方差僅通過其前一個連桿的後驗協方差的伴隨變換來依賴。這種設計使得計算成本與連桿數成線性關係，非常適用於多連桿機械臂。此外，作者通過李代數上的李雅普諾夫函數建立了均方指數最終有界性，每個連桿的界通過伴隨算子範數鏈接，形成一個模塊化且可擴展至任意鏈長的穩定性證書。數值結果驗證了該設計的有效性和優越性。

論文作者S. Yaqubi等人提交的這項研究，不僅提出了理論框架，還通過數值實驗展示了其在典型機械臂模型中的表現。實驗結果表明，相比於傳統的局部線性化方法，該IEKF在估計精度和穩定性方面均有顯著提升。此外，模塊化的結構使得該濾波器易於集成到現有的機器人控制系統中，具有很高的實用價值。未來工作可能包括擴展到柔性連桿或考慮關節柔性的場景。