論文要約：層化安全性（Layered Safety）：多段階 CBF セーフティフィルタによる自律衝突回避の強化

この論文は、ロボットの自律衝突回避を強化するための**「層化されたセーフティフィルタ（Layered Safety Filters）」**の新しいエンドツーエンドフレームワークを提案しています。特に、局所的な知覚データ（点群）のみを用いて、動的な環境下で広範なアプリケーションに適用可能な、堅牢で信頼性の高い安全制御を実現する手法を提示しています。

以下に、問題定義、手法、主要な貢献、結果、および意義について詳細にまとめます。

---

1. 問題定義 (Problem)

従来の自律ロボット、特に足場型ロボット（四足歩行やヒューマノイド）における衝突回避には、以下の課題がありました。

• 知覚と制御の統合の難しさ: 動的な環境において、センサーノイズや部分的な観測条件下で、知覚データを信頼性の高い安全表現に変換し、安全な制御動作へ変換する必要がある。
• CBF（制御障壁関数）の限界: 既存の CBF ベースの手法は、即時の安全性を保証する計算効率の良い手法だが、予測計画を持たないため「近視眼的（myopic）」である。これにより、保守的な行動や、長期的な目標と即時の安全行動が衝突した際のタスク失敗を招くことがある。
• MPC（モデル予測制御）の限界: 将来の軌道を最適化できる MPC は予測能力を持つが、環境の不確実性や非凸最適化問題の難解さにより、計算負荷が高く、動的な障害物への反応が遅れる可能性がある。
• 高次元ダイナミクスの複雑さ: 足場型ロボットなどの高次元システムにおいて、フルオーダーのダイナミクスを用いたオンライン計画や安全制約の合成は計算的に困難であり、リアルタイム性を損なう。また、学習ベースの手法は形式的な安全性保証が得られない場合が多い。

2. 手法 (Methodology)

提案手法は、「知覚から制御まで」の統合アーキテクチャであり、以下の 3 つの主要な技術要素を多段階（マルチステージ）で組み合わせることで、予測能力と即時安全性を両立させています。

A. 知覚ベースの安全合成（ポアソン安全関数：PSF）

• ** occupancy マップの生成:** ロボット搭載センサーからの点群データを処理し、2 次元の占有マップ（Occupancy Map）を生成します。
• ポアソン安全関数 (PSF): 生成された占有マップから、ポアソン方程式を解くことで「ポアソン安全関数（PSF）」を合成します。
  • これにより、離散的な占有マップから、滑らかで微分可能な安全関数 $h_0$ が得られます。
  • この関数は、障害物表面を 0 レベルセットとし、自由空間で正の値をとるよう定義され、CBF として直接使用可能です。
  • 動的環境に対応するため、時間変化する安全関数としてパラメータ化されます。

B. 多段階セーフティフィルタ・アーキテクチャ

提案システムは、2 つの直列したフィルタリング段階で構成されます。

1. 予測セーフティ層（Predictive Safety Layer）:

   • モデル: 複雑なフルオーダーダイナミクスではなく、タスクレベルの主要なダイナミクスを捉える**低次元モデル（ROM: Reduced-Order Model、ここでは単積分器モデル）**を使用します。
   • 手法: 有限時間ホライズンにわたって、PSF を用いた離散時間 CBF（DCBF）制約を満たしつつ、目標速度からの偏差を最小化する MPC（モデル予測制御）問題を解きます。
   • 役割: 長期的な視点で最適な安全軌道（予測的な回避行動）を計画し、 nominal な安全コマンド $\mu_p$ を生成します。

2. リアルタイムセーフティ層（Real-Time Safety Layer）:

   • 手法: 予測層から出力されたコマンド $\mu_p$ を入力とし、より高頻度で動作する**CBF-QP（二次計画）**によるフィルタリングを行います。
   • 役割: 予測層の計画を最小限の変更で調整し、瞬時の安全性（Forward Invariance）を保証します。
   • ロバスト性: 低次元モデルとフルオーダーの実際のロボット間の追跡誤差を補償するためのロバスト性項（ISSf-CBF）を含めており、理論的にフルオーダーシステム全体の安全性を保証します。

C. 理論的保証

• 定理 1（フルオーダーシステムの安全性）: ROM 上で生成された安全速度コマンドが、低次元モデルの追跡制御器によって正確に追跡されれば、フルオーダーの物理システム全体も安全（Forward Invariant）であることが証明されています。これにより、複雑なフルオーダーモデルを直接最適化することなく、モデルフリーな安全性保証が可能になります。

3. 主要な貢献 (Key Contributions)

1. 知覚ベースの安全合成: 点群データからリアルタイムで占有マップを生成し、ポアソン方程式を用いて幾何学的パラメータを持つ CBF 制約を合成するアルゴリズム。
2. 多段階モデルフリー制御アーキテクチャ: 予測 MPC 層とリアルタイム CBF-QP フィルタを直列に配置し、両方を単積分器 ROM 上で動作させる新しいパイプライン。フルオーダーの追跡保証により、理論的な安全性を保証します。
3. 性能分析（パレート最適性）: 単一ステージのフィルタ（予測のみ、またはリアルタイムのみ）との比較を通じて、タスクの最適性（効率性）と安全性のロバスト性のトレードオフをパレート分析で定量化し、層化アプローチの優位性を示しました。
4. ハードウェア検証: 四足歩行ロボット（Unitree Go2）とヒューマノイドロボット（Unitree G1）を用いた実環境での動的衝突回避実験。完全なオンボード知覚・計算による実証。

4. 実験結果 (Results)

実験は、Unitree Go2 四足歩行ロボットおよび Unitree G1 ヒューマノイドロボットで行われました。

• パレート最適性の検証:
  • 予測層のみ: 高い最適性（効率的な回避経路）を示すが、即時の安全マージンが低く、高速な障害物に対しては衝突する傾向がありました。
  • リアルタイム層のみ: 高いロバスト性（安全マージン）を示しますが、保守的な行動（直線的な後退など）となり、タスクの最適性が低下しました。
  • 多段階アーキテクチャ: 予測層の効率的な戦略を維持しつつ、リアルタイム層が安全性を補完することで、両者の中間的な望ましいトレードオフ（パレート最適解）を実現しました。
• 動的環境での性能:
  • 低速（1.0 m/s）のボール回避実験では、すべての手法が成功しましたが、多段階アーキテクチャが最適性とロバスト性のバランスが最も良いことを示しました。
  • 高速（1.25 m/s）のボール回避実験では、単一ステージのフィルタ（予測のみ、リアルタイムのみ）は複数の失敗（衝突）を記録しましたが、多段階アーキテクチャは 10 回中 10 回の成功を収めました。
• 汎用性:
  • 四足歩行ロボットが芝生の上で転がるボールや犬、人間を回避する様子。
  • ヒューマノイドロボットが人間を回避する様子。
  • これらは、異なるロボットプラットフォームや動的障害物に対して、同一のアーキテクチャが有効に機能することを示しています。

5. 意義と結論 (Significance & Conclusion)

この研究は、自律ロボットの安全制御において以下の点で重要な意義を持ちます。

• 予測と即時性の統合: 従来の「予測計画（MPC）」と「即時安全性（CBF）」のトレードオフを、多段階フィルタリングによって克服しました。これにより、保守的すぎず、かつ安全な動的行動が可能になります。
• 計算効率と安全性の両立: 高次元のフルオーダーモデルを直接扱うことなく、低次元モデル（ROM）上で計画を行い、追跡保証を通じてフルオーダーの安全性を裏付けることで、計算コストを抑えつつ形式的な安全性保証を実現しました。
• 実世界への適用: 複雑な知覚データ（点群）から直接安全制約を生成する PSF の手法と組み合わせることで、手動で設計された幾何学的モデルに依存せず、実環境の動的変化に適応できる汎用的なフレームワークを構築しました。

結論として、この層化されたセーフティフィルタは、制御された実験室環境から、不確実で動的な実世界環境へ自律ロボットを展開する際の、信頼性と効率性を高めるための強力な基盤技術となります。今後の課題として、高次元システムへの拡張や、意味論的なシーン理解との統合が挙げられています。