Proprioceptive Invariant State Estimation for Humanoid Robots on Non-Inertial Ground

技術要約：非慣性地面におけるヒューマノイドロボットのための固有受容不変状態推定

問題提起
正確かつリアルタイムな状態推定は、脚式ロボットのモデルベースの計画および制御において不可欠である。既存の推定器（Invariant Extended Kalman Filter: InEKFなど）は、群アフィン力学と不変観測条件を利用することで静止した地形に対して高い性能を発揮してきたが、これらは支持地面が静止しているという仮定に依存している。この仮定は、非慣性環境（例：移動中の列車、船舶、航空機など）では成立しない。なぜなら、地面に起因する並進および回転運動が、システム力学と観測モデルに重大な非線形性を導入するためである。これらの運動を無視すると、モデリング誤差が生じ、推定精度を低下させる。

現在の非慣性状態推定のアプローチには、実用的な制限がある：

1. 絶対状態推定： ロボットの慣性世界フレームにおける絶対状態を推定する手法は、地面の絶対的なポーズや速度に関する直接的かつ正確な知識を必要とすることが多いが、これは外部の計測機器なしでは得られることが稀である。
2. 相対状態推定： 動的な地面フレームに対するロボットの状態を推定する最近の手法は、通常、地面に物理的に設置された外部IMUによってその運動を測定する必要がある。しかし、これは計測器の制約、通信遅延、および非構造化環境における信号消失のため、現実世界の展開においてはしばしば非現実的である。
3. 学習ベースの手法： 有望ではあるものの、学習ベースの推定器は膨大な訓練データを必要とし、非慣性環境への汎化性能については依然として不明確である。

外部センサや明示的な地面運動の測定に頼ることなく、動的な地面に対するロボットの運動を正確に追跡できる、完全な固有受容的（proprioceptive）な推定器が求められている。

手法
本論文では、オンボードセンサ（関節エンコーダおよび胴体と足部に搭載されたIMU）のみを使用して、動的な地面フレームに対するロボットのベースの相対的なポーズと速度を推定する、**固有受容的不変拡張カルマンフィルタ（Proprioceptive InEKF）**を提案する。

• 状態表現： フィルタは、動的な地面フレーム $\{D\}$ に対する相対的なベースの姿勢 ($R_B^D$)、位置 ($p_t^D$)、および速度 ($v_t^D$) を推定する。状態は、二重空間等長変換 $SE_3(3)$ と特殊直交群 $SO(3)$からなる行列リー群$G$ に埋め込まれている。
• プロセスモデリング：
  • フィルタは、相対的なベースの姿勢、速度、および位置に関する連続時間力学を導出する。
  • 極めて重要な点として、直接的な地面運動の測定（利用不可能）への依存を排除するために、スタンス足（接地足）における運動学的制約を利用する。足部に搭載されたIMUからの計測を利用することで、フィルタはプロセスモデルに必要な地面誘起の加速度と角速度を明示的に計算する。
  • モデルは地面の運動によって誘起される非線形効果を考慮し、足の滑りをホワイトガウスノイズとして扱う。
  • 足の着地やスタンスの切り替えなどの離散イベントは、ベースの状態の連続性を維持しながら、順運動学を用いてスタンス足のポーズを更新する離散伝播ステップを通じて処理される。
• 観測モデリング：
  • フィルタは、関節エンコーダから得られる脚オドメトリを観測源として使用する。
  • 観測モデルは、**右不変観測条件（right-invariant observation condition）**を満たすように定式化されている。この構造により、決定論的なケースにおいて誤差力学が線形かつ軌道に依存しないものとなり、大きな初期不確かさの下でも堅牢な収束を促進する。
• 可観測性解析：
  • 線形化されたログ誤差力学の局所的な可観測性解析により、相対的なベース速度は常に可観測であることが示されている。
  • 相対的なベース位置は、スタンス足のIMUが少なくとも2軸の回転を検出する場合、一般的な地面運動の下で可観測となる。
  • 解析の結果、標準的なセンシング構成（1つの胴体IMUと1つの足部IMU）ではベースの姿勢は不可観測であることが示された。しかし、複数の非共線的なIMUを胴体に増設することで、ベースの姿勢を再構成し、フルステートの可観測性を達成できることが示されている。

主な貢献

1. 完全固有受容的InEKFの定式化： 関節エンコーダと胴体/足部搭載IMUのみを使用して、非慣性地面に対するヒューマノイドロボットのベースポーズと速度を推定する、新しいInEKF。これにより、外部の地面センサを不要にする。
2. 明示的な非線形モデリング： 地面誘起の非線形性を明示的に考慮したプロセスおよび観測モデル。観測モデルは右不変観測条件を満たし、大きな初期誤差の下でも良好な誤差力学を実現する。
3. 可観測性の特性評価： 相対的なベースの位置と速度が可観測となる条件を確立する解析。さらに、ベースの姿勢の可観測性を回復するための、単純なセンサ拡張戦略（複数の胴体IMU）を提案する。
4. 実験による検証： 揺動およびピッチングを行うトレッドミル上での、Digitヒューマノイドロボットによる立位、スクワット、歩行タスクを用いたハードウェア実験。

実験結果
Digitロボットを用い、様々なロボットの動き（立位、スクワット、歩行）と地面の動き（揺動、ピッチング）を含む5つのテストケースで実験を行った。提案されたフィルタ（FRSと呼称）を、2つのベースライン、すなわちSRS（静止地面用の標準的なInEKF）およびDRS（外部の地面設置IMUを必要とする動的地面用のInEKF）と比較した。

• 収束速度： 提案されたFRSは、既存のInEKFと比較して96%の高速化を達成した。具体的には、位置推定において、DRSベースラインが3〜5秒を要したのに対し、FRSは0.1秒以内に真値へと収束した。
• 精度：
  • FRSは、既存のInEKFと比較して位置推定誤差を80%削減した。
  • 定常状態において、FRSは立位およびスクワットタスク（ケースT1–T4）で4 cm未満の位置誤差を達成した（DRSベースラインは約10 cm）。
  • ピッチングする地面での歩行中（ケースT5）においても、FRSは最大1メートルの初期誤差があるにもかかわらず、平均推定誤差を9 cm未満に維持した。
• 姿勢の回復： 3つの胴体搭載IMUを用いた実験により、ロール、ピッチ、ヨーの平均絶対誤差が0.4°未満であることを確認し、ベースの姿勢を再構成することに成功した。これは提案された拡張戦略の妥当性を裏付けるものである。
• リアルタイム性能： 提案手法を含むすべてのフィルタは、標準的なノートPC上で1サイクルあたり3 ms未満の計算時間を実現し、リアルタイム実装をサポートした。

意義と主張
本論文は、提案されたアプローチが、明示的な地面運動センシングを必要とせずに、非慣性環境におけるヒューマノイドロボットの正確かつリアルタイムな状態推定を可能にすることを主張している。足部IMUを利用して地面の力学を推論することで、本手法は外部計測器を必要とする実用的な制限を克服している。著者らは、このフィルタが、大きな初期不確かさや大きな地面運動の下でも、優れた収束性と精度を実現し、移動する環境に対して定義された計画および制御タスクのための堅牢なソリューションを提供すると断言している。本研究は、単純なセンサ拡張によってフルステートの可観測性が達成できることを検証しており、この手法が非構造化で動的な環境への展開に適していることを示している。