SE(3)上の不変確率的フィルタリング：シリアル剛体マニピュレータの慣性・エンコーダ状態推定
本論文では、シリアル剛体マニピュレータの状態推定のための不変拡張カルマンフィルタ(IEKF)を、SE(3)リー群上で完全に定式化して開発した。運動学方程式の群アフィン性により、線形化誤差ダイナミクスが自律的となり、リッカチ方程式が真の誤差共分散を支配する。物理的に分離されたノイズモデルは、ジャイロスコープと加速度計のチャネルを独立に扱う。フィルタはモジュール式のリンクごとのIEKFチェーンとして構成され、計算コストはリンク数に線形である。リー代数リアプノフ関数により、平均二乗の指数最終有界性が確立される。
最近、arXivに投稿された論文（arXiv:2607.00026）は、シリアル剛体マニピュレータの状態推定のための不変拡張カルマンフィルタ（IEKF）をSE(3)リー群上で完全に定式化して提案した。このフィルタは、任意の数のリンクを持つマニピュレータに適用可能であり、運動学方程式の群アフィン性を活用することで、線形化誤差ダイナミクスを自律的にしている。これにより、リッカチ方程式が真の誤差共分散を直接制御でき、局所近似に依存しないため、推定精度が向上する。
論文では、物理的に分離されたノイズモデルが設計されている。加速度計は重力補償積分によって並進ツイストを提供し、その測定共分散はサンプル間隔に応じてスケールする。一方、ジャイロスコープのノイズは状態依存のコリオリ項でモデル化され、静止時にはゼロとなり、ツイストの大きさとともに増加する。これにより、非線形ダイナミクスを通じたノイズの伝搬をより正確に捉えられる。
フィルタの構造は、リンクごとのIEKFのモジュラーチェーンである。各リンクの予測共分散は、前のリンクの事後共分散の随伴変換のみに依存するため、計算コストはリンク数に線形となる。さらに、リー代数上のリアプノフ関数を用いて平均二乗指数最終有界性が確立され、各リンクの境界は随伴作用素ノルムを介して連鎖され、任意のチェーン長に拡張可能な安定性証明書を提供する。数値結果により、設計の有効性が確認された。
著者らは、数値実験を通じて、従来の局所線形化手法と比較して、推定精度と安定性において優れた性能を示した。モジュール構造により、既存のロボット制御システムへの統合も容易であり、実用的価値が高い。今後の課題として、フレキシブルリンクや関節の柔軟性を考慮した拡張が考えられる。