Whole-Body Safe Control of Robotic Systems with Koopman Neural Dynamics

この論文は、非線形ロボットの制御をデータ駆動で学習したクープマン線形モデルと安全集合アルゴリズムを統合し、追従と安全性の制約を単一の二次計画問題として解くことで、実時間での安全な全身制御を実現する手法を提案し、実機実験でその有効性を検証したものである。

要約

🤖 ロボットが抱える「3 つの悩み」

まず、ロボットが動くのが難しい理由を想像してください。

1. 動きが複雑すぎる（非線形性）:
   ロボットのアームは、人間の腕のように関節が絡み合っています。少し手を動かすだけで、他の部分も予想外に大きく動いたり、止まったりします。これを「数式で正確に予測して制御する」のは、**「暴れる子供を、完璧な数式でコントロールしようとする」**ようなもので、計算が重すぎてリアルタイム（瞬間瞬間）には処理しきれません。
2. 壁にぶつかる恐怖（安全性）:
   ロボットが目標地点へ向かう際、障害物（壁や人）にぶつからないようにする必要があります。でも、複雑な動きをするロボットにとって、「安全な道」を見つける計算は非常に難しく、**「迷路の出口が見えないまま、壁に激突しそうになる」**状態になりがちです。
3. シミュレーションと現実のズレ:
   コンピュータの中で練習させたロボットは、実際の機械になると「摩擦」や「モーターの遅れ」などで、思った通りに動きません。**「練習用シミュレーターでは完璧だったのに、本番で転んでしまう」**ようなものです。

---

💡 この論文の解決策：「コップマン・マジック」と「安全な道案内」

この研究は、上記の悩みを解決するために、**「コップマン（Koopman）」**という魔法のような道具を使います。

1. 「コップマン・マジック」：複雑な動きを「直線」に変える

通常、ロボットのアラームは複雑な曲線を描きます。しかし、コップマンという数学の道具を使うと、**「複雑な曲線の動きを、別の世界（高次元空間）では『直線』として見なせる」**という魔法をかけます。

• アナロジー:
  地球儀（球体）上の複雑な飛行経路を、平面の地図（直線）に書き換えて考えるようなものです。
  • 従来: 複雑な曲線を描きながら、毎回「次はどこへ曲がる？」と計算し続ける（大変！）。
  • この方法: 一度「直線」の世界に変換してしまえば、「まっすぐ進めばいい」という単純な計算で済みます。これにより、ロボットは**「脳みそ（計算能力）を節約して、素早く判断」**できるようになります。

2. 「安全な道案内」：失敗しないための「学習型フィルター」

ロボットを安全に動かすには、単に「直線」で進むだけでなく、「壁にぶつからないように」する必要があります。
これまでの方法は、「ロボットが危ない動きをしようとしたら、後から『止まれ！』と命令する（フィルター）」という二段階方式でした。しかし、これだとロボットが「止まりすぎて動けなくなる（死に筋）」という問題がありました。

この論文では、**「最初から安全な道筋を計算の中に組み込む」という新しい方法を採用しました。さらに、「敵対的（アドバーサリアル）な微調整」**というテクニックを使います。

• アナロジー:
  • 従来の方法: 運転中に「あぶない！」とブレーキを踏む。でも、急ブレーキで止まりすぎて目的地に着けない。
  • この方法: 運転する前に、**「もし壁が迫ってきたら、どうすれば一番安全に曲がれるか？」をシミュレーションで何千回も練習させます。そして、「この曲がり方だと壁にぶつかるぞ！」という「最悪のケース」**を見つけ出し、安全基準（どのくらい壁から離れるべきか）をロボットに最適化させます。
  • これにより、**「ブレーキを踏む必要がないほど、最初から安全なルート」**を自動的に見つけ出せるようになります。

3. 「現実への適応」：練習と本番のギャップを埋める

シミュレーション（練習）と現実（本番）では、ロボットの動きにズレが生じます。
この研究では、**「練習用モデルの『直線部分』だけを、本番のデータで少しだけ微調整（ファインチューニング）」**します。

• アナロジー:
  練習用シミュレーターで「直進の感覚」は完璧に覚えたロボットが、本番の機械になると「タイヤの滑り」で少し曲がってしまいます。
  この研究では、「曲がり具合（直線部分の係数）」だけを、本番のデータに合わせて微調整します。これにより、**「複雑な動きを覚えるための脳（ニューラルネットワーク）」はそのまま使いながら、本番の環境に即した「足取り」**を身につけさせることができます。

---

🏆 結果：何ができたのか？

この方法を実際に**「Kinova Gen3（7 本指のロボットアーム）」と「Unitree Go2（四足歩行ロボット）」**でテストしました。

• 成果:
  • 複雑な障害物を避けながら、目標地点へ正確に移動できました。
  • 従来の複雑な計算方法に比べて、計算速度が圧倒的に速く（4 倍以上）、リアルタイムで安全を確保できました。
  • シミュレーションで練習したモデルを、ほとんど手直しせずに実際のロボットでも安全に動かすことができました。

📝 まとめ

この論文は、**「複雑な動きをするロボットを、魔法（コップマン）で『直線』の世界に変えて計算しやすくし、さらに『最悪のケース』を事前に学習させることで、安全かつ高速に動かす」**という画期的な方法を提案しています。

まるで、**「暴れん坊のロボットに、迷路を走るための『超高速ナビゲーター』と『安全運転の免許』を同時に与えた」**ようなものです。これにより、将来のロボットが工場や家庭で、より安全に、賢く活躍できる道が開かれました。

技術概要

論文「Koopman Neural Dynamics を用いたロボットシステムの全身安全制御」の技術的サマリー

本論文は、強非線形かつ高次元なダイナミクスを持つロボットシステム（マニピュレータや脚型ロボットなど）の、リアルタイムかつ安全な制御を可能にする新しいデータ駆動型フレームワークを提案しています。従来の非線形最適化に基づく安全制御が計算量的に困難であるという課題に対し、Koopman 演算子による線形化と**安全集合アルゴリズム（SSA）**を統合し、単一の二次計画（QP）問題として安全制約と追従制御を同時に解決する手法を提案しています。

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

---

1. 背景と問題定義

ロボット制御において、以下の 3 つの課題が安全な制御の実現を阻害しています。

1. 非線形性と計算コスト: 関節結合、接触相互作用、非線形アクチュエーションなどにより、ダイナミクスが強く非線形です。これを直接最適化問題に埋め込むと、凸性のない問題となり、リアルタイムでの求解が困難です。
2. 安全境界における実行可能性（Feasibility）: 安全領域の境界付近では、制御入力が安全を維持するために存在しなくなる（QP が解けない）事態が発生しやすくなります。
3. 学習モデルの誤差: 学習されたダイナミクスモデルの近似誤差が安全制約に伝播し、理論上は安全でも実際には衝突するリスクがあります。

既存の手法（CBF やセーフティフィルター）は、通常「 Nominal 制御」と「安全フィルター」を分離して設計しますが、これにより保守的な挙動やデッドロック、境界付近での実行可能性の崩壊を引き起こすことがあります。

2. 提案手法：Koopman 統合安全制御フレームワーク

提案手法は、Koopman 演算子理論を用いて非線形ダイナミクスを「リフトされた（lifted）」線形空間に変換し、その上で安全制御を統合的に設計します。

A. Koopman による線形リフト

• 非線形から線形へ: 状態 $x$ と入力 $u$ をニューラルネットワーク $\psi_\omega$ によって高次元の線形空間（リフト空間）$z$ に写像します。
• 線形ダイナミクス: リフト空間内では、システムが線形時不変（LTI）または線形時変（LTV）のダイナミクス $z_{k+1} = Az_k + Bu_k$ として近似されます。これにより、複雑な非線形制御問題を線形 MPC として定式化できます。
• 構造: 元の状態 $x$ とリフト状態 $z$ を保持し、情報損失を防ぎつつ、元の状態への復元 $x = Pz$ を可能にしています。

B. 敵対的微調整による安全指標の合成（Adversarial Fine Tuning）

学習されたモデルと硬い安全制約を結合する際、制約が満たせなくなる（実行不可能になる）リスクを低減するため、安全指標（Safety Index）の微調整を導入しました。

• パラメータ化された安全指標: 安全条件 $\phi(x) \le 0$ を、学習可能なパラメータ $\Gamma = (n, \beta)$ を持つ関数 $\phi_\Gamma$ として定義します。
• Learner-Critic アーキテクチャ:
  • Critic: 安全境界付近で制約違反（実行不可能）を引き起こす「カウンター例（状態と制御入力）」を探索します。
  • Learner: 見つかったカウンター例に対して、安全指標のパラメータ $\Gamma$ を更新し、制約が実行可能になるように調整します。
• これにより、モデルの近似誤差を考慮した上で、安全集合内での前方不変性（Forward Invariance）を確保しつつ、過剰な保守性を避けた安全指標を学習します。

C. 統合された安全 MPC

学習された Koopman モデルと調整済みの安全指標を用いて、追従コストと安全制約を**単一の二次計画（QP）**として定式化します。

• 従来の「制御→フィルタリング」という 2 段階アプローチではなく、安全制約を最適化問題の内部に直接埋め込むため、一貫した最適化が行われ、実行可能性が保証されます。
• 制約は、リフト空間の線形ダイナミクスと安全指標の時間微分（線形化）を用いて線形不等式として記述されます。

3. 主要な貢献

1. Koopman 線形化による安全制御の統合: 非線形ロボットの全身制御において、安全制約を事後処理（フィルタリング）ではなく、Nominal 制御器と統合して単一の QP で解く手法を提案しました。これにより、保守性と計算効率のトレードオフを解消しています。
2. 学習ダイナミクス向け安全指標の敵対的微調整: 学習モデルの誤差を考慮し、安全境界での実行不可能性を回避するための安全指標の適応的学習手法を提案しました。
3. シミュレーションから実機への適応（Sim-to-Real）: 学習された埋め込み関数は固定したまま、リフト空間の線形行列（A, B）のみをハードウェアデータで微調整することで、最小限の再学習で実機（Kinova Gen3）への展開に成功しました。

4. 実験結果

対象ロボット: Kinova Gen3 7 自由度マニピュレータ、Unitree Go2 四足歩行ロボット。

• 予測精度:
  • 従来の線形モデル（LTI, LTV）やニューラルネットワークダイナミクスモデル（NNDM）と比較し、Koopman モデル（KDM）は長期的な予測誤差が最も小さく、特に長ホライズンでの精度が優れていることを示しました（Fig. 4）。
• 安全制御性能（2D/3D 空間）:
  • 障害物回避: 複数の障害物がある環境において、Koopman-MPC（KMPC）は目標軌道の追従と障害物回避を両立しました。
  • 実行可能性: 敵対的微調整を適用した結果、QP の実行不可能なケースが大幅に減少しました（Table I: 単一障害物で 108 回→42 回、複数障害物で 632 回→113 回）。
  • 性能比較: 非線形 MPC（NMPC）と比較して、計算時間が 4.2 倍以上高速であり、かつ NMPC が衝突するケースでも KMPC は安全に回避しました（Table II）。
• 実機実験（Sim-to-Real）:
  • 実機 Kinova Gen3 において、微調整後のモデルは関節角度の予測誤差を平均 0.140 rad、エンドエフェクタ位置誤差を平均 0.031 m まで低減しました（Fig. 6, 7）。
  • 再学習なしで、学習済みの埋め込み構造を維持しつつ、実機のダイナミクス誤差（摩擦、遅延など）を線形部分で吸収することに成功しました。

5. 意義と将来展望

• 意義: 本論文は、強非線形・高次元ロボットシステムにおいて、「学習モデルの精度」と「数学的に保証された安全性」、そして**「リアルタイム性」**を同時に達成する実用的な枠組みを提供しました。特に、安全フィルターを分離せず統合制御を行うことで、過剰な保守性やデッドロックを回避できる点が画期的です。
• 将来展望:
  • 位置情報に基づく 1 次安全指標から、速度や曲率を含む高次安全指標への拡張（より動的な環境への対応）。
  • ヒューマノイドロボットなど、さらに高次元のシステムへのスケーリング。

結論として、Koopman 演算子に基づく安全制御は、モデルフリーな手法に代わる、スケーラブルで解釈可能性が高く、安全クリティカルなロボット応用に向けた有力な解決策となります。