A Sliding-Window Filter for Online Continuous-Time Continuum Robot State Estimation

技術概要：連続時間連続体ロボットのオンライン状態推定のためのスライディングウィンドウフィルタ

問題定義
連続体ロボット（CR）は、狭い空間を航行可能な可撓性マニピュレータであるが、その制御には正確な状態推定が依存する。既存の確率論的アプローチは二分される：

• フィルタリング手法（拡張カルマンフィルタなど）は計算効率が良くオンライン動作に適しているが、非線形システムのマルコフ構造を完全に捉えられない線形化近似により、精度が低下する傾向がある。
• バッチ平滑化手法は、すべての状態を共同最適化することで優れた精度を提供するが、すべての測定値への遡及アクセスを必要とし、リアルタイム制御への拡張性が低いため、通常はオフライン動作に限定される。
• 連続時間アプローチは CR にとって有望であるが、これまでオフラインのバッチ最適化に限定されていた。さらに、CR に対する既存のスライディングウィンドウ試行は、簡略化された非確率論的モデル（定曲率モデルや剛体リンク近似など）に依存するか、不確実性を定量化するための確率論的表現を欠いていた。

手法
著者は、CR のオンラインかつ連続時間状態推定のために設計された**スライディングウィンドウフィルタ（SWF）**を提案する。本手法は、最近のバッチ最適化技術から導かれたファクタグラフフレームワークに基づいている。

• 推定フレームワーク：システムは、ロボットのアーク長 $s$ および時間 $t$ にわたって、姿勢 $T(s,t)$、速度 $\varpi(s,t)$、ひずみ $\epsilon(s,t)$ を連続的に推定する。問題は、事前分布と測定ファクタの負対数尤度を最小化する最大事後確率（MAP）推定として定式化される。
  • 事前分布：「加速度における白色雑音」に基づく運動事前分布を用いた近似コッサールロッドモデルを利用する。
  • 測定値：チップ姿勢の測定値とジャイロスコープデータを組み込む。
• スライディングウィンドウ定式化：フィルタは、状態 $x_{a:k}$ を含む固定長の時間ウィンドウを維持する。
  • 因数分解：結合事後分布は、ウィンドウ以前の履歴をカプセル化する周辺化事前ファクタ、ロボットのアーク長に沿った空間ファクタ、ウィンドウ内の時間的運動・測定ファクタに分解される。
  • 周辺化：情報を伝播させながら固定ウィンドウサイズを維持するため、最古の時間ステップを情報形式のシュール補完を用いて周辺化する。これにより、新しいウィンドウに対して実効的な事前分布が生成され、厳密なマルコフ仮説に固有の誤差を緩和しつつマルコフ性を保持する。
• 実装詳細：
  • ウィンドウ拡張：新しい状態は、空間的事前分布と整合する推定値と、時間的事前分布と整合する推定値を平均化することで初期化される。
  • 状態抽出：利用可能なすべての測定値を活用するため、ウィンドウの後端から平均と共分散が抽出され、これにより小さな遅延（通常 $\leq 0.1$s）が生じる。
  • 共分散処理：再線形化によって引き起こされる不連続性に対処するため、隣接する時間ステップ間の結合共分散推定値は、状態がロックされる際に保存される。

主な貢献

1. CR 向け初の確率論的 SWF：本論文は、連続体ロボットのために設計された初の確率論的スライディングウィンドウフィルタを提示し、空間および時間両方にわたる可変曲率形状の推定を可能にする。
2. オンライン連続時間動作：本手法は、従来のバッチ連続時間手法のオフライン制限を克服し、オンラインでの連続時間状態推定を可能にする。
3. バランスの取れた性能：フィルタリングの計算効率とバッチ平滑化の精度の間の妥協点を実現し、確率論的不確実性表現を維持しながらリアルタイム以上の速度で動作する。
4. オープンソースの提供：さらなる研究を促進するため、コミュニティに対してオープンソース実装が提供される。

結果
本手法は、5 つの異なる軌道（自由空間運動、高速接触、衝撃相互作用を含む）を走行する実機の 3D プリント製腱駆動連続体ロボット上で検証された。

• 精度：SWF（0.1s ウィンドウ）は、チップ位置および回転の二乗平均平方根誤差（RMSE）において、フィルタ基準（0s ウィンドウ）を大幅に上回った。例えば、「高速接触」軌道において、SWF は位置 RMSE を 1.75% から 1.29% に低減した。
• バッチ手法との比較：SWF は、完全な 10s バッチ最適化基準と同等の精度を達成した（例：高速接触において 1.29% 対 1.29% の RMSE）が、計算コストは劇的に低減された。
• 計算効率：SWF は時間ステップあたり約 10ms（100Hz）で動作し、リアルタイム制約（33.3ms 閾値）を十分に満たしていたのに対し、バッチ手法はステップあたり約 1.3 秒を要した。
• ウィンドウサイズ感度：0.1s 以上のウィンドウサイズでは精度の改善が逓減することが示された。注目すべきは、非常に小さなウィンドウ（0.033s）でも、標準的なフィルタに対して有意な精度向上をもたらした点である。
• ロバスト性：フィルタは高速運動中のロボット追跡能力を示し、一時的なセンサー欠落からの迅速な回復を実現した。データ損失中に不確実性推定値が正しく拡大することも確認された。

意義と主張
著者は、本論文がフィルタリングと平滑化の間のギャップを埋めることで、この分野にとって「有望な方向」を表すと主張する。主な意義は、これまで利用できなかったオンライン、連続時間、確率論的状態推定を連続体ロボットに対して可能にする点にある。

論文は、完全なバッチ手法と比較してわずかな遅延とわずかに低下した滑らかさを導入するものの、元のフレームワークの連続時間特性を維持していると控えめに指摘している。著者は、フィルタのみの変種が >200Hz 相当で動作しているという残りの計算余力が、コントローラやプランナなどの下流アプリケーションを支援し得ると示唆している。また、パフォーマンスは異なるセンサー構成やノイズプロファイルによって変動し得ることを認め、より大きなウィンドウで特定の軌道（「範囲外」）において RMSE が予期せず増加したことは、境界におけるセンサー劣化によるものかもしれないとし、最適なウィンドウサイズは文脈に依存することを示唆している。