Invariant Stochastic Filtering on SE(3) for Inertial-Encoder State Estimation of Serial Rigid Manipulators

技術要約：直列剛体マニピュレータの慣性・エンコーダ状態推定における $SE(3)$ 上の不変確率フィルタリング

問題提起
直列剛体マニピュレータの正確な状態推定には、幾何学的な一貫性と確率論的な厳密さの同時達成が必要である。各リンクの構成はリー群 $SE(3)$ 内に存在するが、古典的なカルマンフィルタは平坦なベクトル空間上で動作するため、大きな角度における線形化誤差や、オイラー角やクォータニオンの定式化に特有のジンバルロック、正規化制約といった問題を引き起こす。さらに、既存の幾何学的フィルタは、単一の物体またはチェーン全体をモノリシックな状態として扱うことがあり、リンク数に対して3次（cubic）の計算コストを要する。また、慣性計測装置（IMU）のノイズモデリングにも重要な欠落が存在する。標準的な手法では、加速度計のノイズ（積分によるサンプル間隔のスケーリング）と、ジャイロスコープのノイズ（非線形コリオリ力による状態依存の伝播）という、物理的な発生源の違いを考慮できていない。

手法
著者らは、任意の数のリンクを持つマニピュレータに対し、完全にリー群 $SE(3)$ 内で定式化された不変拡張カルマンフィルタ（IEKF）を開発した。本手法は、以下の4つのコアコンポーネントに基づいている。

1. 幾何学的誤差力学： フィルタは、推定値 $(\hat{g}_i, \hat{V}_i)$ とリー代数誤差ベクトル $\tilde{\xi}_i$ を厳密に分離して保持する。運動学方程式（$\dot{g}_i = g_i V_i$）の**群アフィン特性（group-affine property）**を利用することで、線形化された誤差力学が自律的であることを示している。これにより、リッカチ方程式が局所的な近似ではなく、真の誤差共分散を制御することが保証される。
2. 物理的に分離されたノイズモデル： ジャイロスコープと加速度計のチャネルを独立して扱う新しいノイズモデルを導入している。
   • 加速度計： 比力（specific force）の計測値は、並進ツイストへと積分される。その結果、計測共分散はプロセスノイズの離散化と同様に、サンプル間隔 $\Delta t$ に比例してスケーリングする。
   • ジャイロスコープとコリオリ力： ジャイロスコープのノイズは回転チャネルに直接入るだけでなく、非線形ニュートン・オイラー動力学を通じてコリオリ項を介して伝播する。これにより、静止時には消失し、ツイストの大きさに応じて増大する、状態依存のプロセスノイズ寄与が生じる。
3. モジュール型チェーン合成： フィルタは、リンクごとのIEKFの連鎖として構成されている。リンク $i$ の予測共分散は、随伴変換（Adjoint-transformed）された共分散を通じてのみ、リンク $i-1$ の事後分布に依存する。この伝播メカニズムにより、全状態を一つのモノリシックな状態として推定する場合の3次のコストを回避し、総計算コストをリンク数に対して線形（$O(n)$）に抑えることができる。
4. 安定性解析： 著者らは、平均二乗の意味における**指数的最終有界性（Exponential Ultimate Boundedness: EUB）**を確立した。リー代数のリャプノフ関数を用い、リンクごとの境界を導出し、それらを随伴作用素のノルムを用いてマニピュレータ全体に連鎖させた。極めて重要な点として、本解析は、随伴写像（Adjoint map）が標準的なユークリッド内積の下では等長写像（isometry）ではないことを考慮することで、ナイーブな安定性議論を修正している。つまり、不確実性の増幅はリンクの幾何学的形状（長さ）に依存する。

主な貢献
本論文は、幾何学的確率推定に対する4つの具体的な貢献を主張している。

• (C1) ノイズモデリング： 状態依存のコリオリノイズ項と、加速度計のサンプル間隔スケーリングを組み込んだ、物理的に分離されたIMUノイズモデル。
• (C2) モジュール型アーキテクチャ： 随伴共分散伝播によって実現された、線形計算コストを持つ、リンクごとのIEKFの連鎖。
• (C3) 完全な導出： 混合ポーズ・ツイスト観測ヤコビアンを含む、IEKFの完全な導出。
• (C4) 安定性証明： リー代数リャプノフ関数から導出された、連鎖的な指数的最終有界性証明。これは、任意のチェーン長に対してスケーラブルなモジュール型の安定性保証を提供する。

結果
設計の妥当性は、大振幅の3次元運動を実行する3自由度2リンク剛体直列マニピュレータの数値シミュレーションを通じて検証された。提案されたIEKFを、座標ベースのEKF（CEKF）および生のエンコーダ・ベースラインと比較した。

• 精度： IEKFは、全関節において生のエンコーダ・ベースラインに対して33%のRMSE減少、CEKFに対しては24%の減少を達成した。
• 堅牢性： CEKFの誤差は座標の線形化誤差により運動の振幅とともに増大したが、IEKFの誤差は安定しており、$SE(3)$ 幾何学的誤差モデルの振幅不変性を実証した。
• 整合性： 正規化推定誤差二乗（NEES）解析により、フィルタの統計的一貫性が確認された。平均値は95%のカイ二乗受理帯内に収まった。
• 不確実性伝播： 結果は、随伴写像を通じて上流の推定不確実性を取り込むことで、下流のリンク（例：リンク2）の共分散バンドが正しく拡大することを裏付けた。

意義
本論文は、幾何学的な直感と確率論的な厳密さの間の溝を埋める、状態推定のための厳密なフレームワークとして位置づけられる。$SE(3)$ 上でネイティブに動作するようにすることで、平坦な空間の手法における特異点や線形化の不整合を回避している。モジュール構造により、膨大な計算コストをかけることなく、複雑で長いチェーンのマニピュレータへと適用を拡張できる。さらに、状態依存のノイズを明示的に扱い、幾何学的増幅係数を考慮した安定性証明を導出したことは、既存の幾何学的フィルタと比較して、柔軟な、あるいは剛体マニピュレータの制御に対して、より物理的に正確で理論的に健全な基盤を提供する。著者らは、このフレームワークが、学習ベースの推定器に対する幾何学的に一貫した事前分布として機能し、学習による補正の下でもリー群の一貫性を維持するという課題に対処できることを示唆している。