技術的サマリー：カスケード最適化による接触豊か操作のための同時接触選択と計画

1. 問題定義

接触豊か操作では、ロボットが対象物を所望の状態へ駆動するために、意図的に接触を形成・解除する必要があります。これには、接近軌道、接触位置、接触力、および接触モード（接触、分離、滑り、付着）間の遷移のシーケンスを計画することが含まれます。

既存のアプローチは重大な限界に直面しています：

• 階層的および混合整数法： 初期の研究では、連続状態の計画と離散的な接触モードの探索を分離するか、混合整数計画を用いていました。これらは、接触軌道の組み合わせ爆発によりスケーラビリティが著しく低下し、操作戦略とロバスト性を制限する傾向があります。
• 学習ベースの方法： オフライン強化学習（RL）を通じて非滑らかなダイナミクスを処理できる一方で、多様な物体タイプや新しいタスク設定における汎化能力に欠けることが多いです。
• 接触非明示モデル予測制御（CIMPC）： CIMPC などの最近のモデルベース手法は、オンラインハイブリッド計画を可能にします。しかし、これらは通常能動的な接触選択を欠いています。手作業で設計されたコスト関数や与えられた参照軌道に依存するため、操作行動の分布が狭く制限されます。これにより、特にロボットの初期推定が不適切な場合や複雑な幾何学形状を持つ物体に直面した場合、局所最適解に陥りやすくなります。さらに、接触ダイナミクスに内在する相補性は非滑らかさと疎な勾配をもたらすため、接触選択と計画の統合設計を困難にしています。

ここで扱われる核心的な課題は、不正確なダイナミクスや知覚ノイズの下でも、リアルタイムで多様な接触位置シーケンスと操作軌道を自律的に推論・生成できる統合フレームワークの欠如です。

2. 手法：SCSP フレームワーク

著者は、**接触選択最適化（CSO）と接触計画最適化（CPO）の 2 つのモジュールからなる、新しいカスケード最適化フレームワークである同時接触選択と計画（SCSP）**を提案します。

A. 接触選択最適化（CSO）

CSO は、ハイブリッド計画に内在する勾配の疎さと非滑らかさの問題に対処するため、物体表面における最適な接触位置のグローバル探索を行うように設計されています。

1. サンプリングと交換による離散化： 物体幾何学の非滑らかさを処理するため、CSO は物体表面上で候補接触点をサンプリングします。
   • 近似凸分解（ACD）： 物体幾何学を凸多角形に分解し、局所的な詳細からの干渉を軽減するとともに、到達可能性と安定性を向上させます。
   • 最遠点サンプリング（FPS）： 凸近似面上で点を均一にサンプリングします。
   • 有効領域の選択： 到達不能、遮蔽、または意味的に不適切な領域をオンラインでフィルタリングし、有効な候補セットを形成する手順です。
2. 代理接触モデル（SCM）： 元の接触ダイナミクスの計算複雑性と相補性制約を解決するため、著者は SCM を導入します。
   • 仮定： このモデルは、短い時間間隔内で接触セットが固定され、多接触相互作用を独立したバネ・ダンパーシステムに分解すると仮定します。
   • 定式化： これにより、環境接触力をロボット接触力の明示的な区分的線形関数として表現することが可能になります。
   • 最適化： 問題は**混合整数二次計画（MIQP）**問題として再定式化されます。内部ループ（接触力最適化）はサブミリ秒スケールで解ける二次計画（QP）であり、外部ループ（接触位置選択）も効率的に解かれます。
3. 目的： CSO の目的は、最終姿勢誤差を最小化する前に、静的に安定した姿勢の集合（$X_g$）への到達を優先し、不安定な接触事前分布の生成を防ぐことです。

B. 接触計画最適化（CPO）

CPO は、CSO によって予測された最適な接触位置を参照として受け取り、実行可能な接近および接触軌道をオンラインで生成します。

1. ランキング戦略： ロバスト性を高めるため、CPO は CSO の出力を盲目的に追従しません。現在のロボット構成に対する最近点（$p_{near}$）を、CSO によって予測された最適点（$\hat{p}^*_r$）と比較して評価します。
   • 改善率（$\rho$）を計算し、$p_{near}$ が十分に良い準最適参照かどうかを判定します。
   • スライディングウィンドウ戦略により、$\hat{p}^*_r$ と $p_{near}$ 間の切り替えを制御し、頻繁で不安定な接触変化を回避します。
2. 事前分布ガイド型ハイブリッド計画： 軌道最適化は、統合された目的関数内で 3 つの段階に分解されます。
   • リフティングフェーズ： 反発ポテンシャルと引力ポテンシャルを用いて、エンドエフェクタを物体から離し、目標接触位置上の中間経由点へと誘導します。
   • プレースイングフェーズ： エンドエフェクタを参照接触位置（$p_{ref}$）へ駆動して接触を確立します。
   • スイッチング： フェーズ間の遷移は、ランキングトリガ変数 $\gamma$ によって制御されます。
3. ソルバ： このフレームワークは、ポイント接触計画のためのオペレーショナルスペース制御（OSC）や、フルオーダー冗長ロボット計画のためのサンプリングベース MPC（例：MPPI）など、異なるソルバをサポートします。

3. 主要な貢献

1. カスケード最適化フレームワーク： 能動的な接触選択（CSO）と接触計画（CPO）を統合する新しい SCSP フレームワークにより、モデルの不正確さの下でも多様な操作戦略とロバスト性を可能にします。
2. 効率的な接触選択アルゴリズム： メッシュサンプリングと代理接触モデルを用いて、非滑らかな接触選択を離散 - 連続問題に変換し、問題を高速なグローバル探索のための扱いやすい MIQP に還元する手法です。
3. 事前分布ガイド型接触計画： 接触事前分布を評価するためのランキング戦略を用いた定式化と、ハイブリッド計画（接近、接触、スイッチング）のための統合目的関数であり、様々なロボット構成に適用可能です。
4. 実験的検証： 広範なシミュレーションおよび実世界実験により、SCSP がマーカーやモーションキャプチャなしで視覚知覚のみを用いて、任意に複雑な物体幾何学の 6 次元非把持操作を達成する初のモデルベース手法であることを実証しました。

4. 実験結果

このフレームワークは、指先および Franka Emika Panda ロボットを用いた非把持操作タスク（地上での回転と反転）で検証されました。

• シミュレーション：
  • 成功率： SCSP は、I-MPC、CF-MPC、サンプリング手法などのベースラインと比較して、著しく高い成功率を達成しました（例：指先回転で 97%、ロボット回転で 95%）。
  • ロバスト性： 複雑な幾何学形状の処理や局所最小値の回避において、学習ベースの方法（DyWA）をほとんどのシナリオで凌駕しました。
  • アブレーション： ランキング戦略を除去した場合（w/o RS）、成功率が著しく低下し、接触事前分布の評価の必要性が確認されました。
• 実世界：
  • SCSP は、単一視点カメラ（FoundationPose）のみを状態推定に使用して、滑らかおよび粗い表面上で不規則な物体（アヒル、ウサギ、手、Xbox コントローラー）の操作に成功しました。
  • 不正確なダイナミクス（粗い質量/摩擦推定）および知覚ノイズにもかかわらず、高い成功率（回転で最大 100%）を維持しました。
  • ベースライン手法（DyWA、サンプリング）は、接触滑り、不安定な接触選択、または実行誤差への適応 inability により頻繁に失敗しました。
• 汎化性： このフレームワークは、多接触タスク（サッカーのドリブル、傾けた押し操作）および長視野タスク（引き出しや電子レンジ扉の開閉）に成功裏に拡張され、オンライン物理推論を行う能力を実証しました。

5. 意義と主張

本論文は、SCSP が能動的な接触選択を可能にすることで、既存の接触非明示手法の理論的限界を克服すると主張しています。最近の RL アプローチは成功を示していますが、SCSP は多様かつロバストな接触計画をより解釈可能にすることを目指しています。

著者は、世界モデル（CSO）の精度よりも、それがプランナとどのようにインターフェースするかの方が重要であることを強調しています。CPO におけるランキング戦略により、システムは最適ではないが実行可能な接触位置を利用でき、CSO におけるモデルエラーに起因する不安定性を軽減します。これは、正確な知覚や事前に定義された接触シーケンスに依存することなく、複雑な実世界操作タスクに対して、同時接触選択と計画の統合フレームワークが実現可能かつ効果的であることを示唆しています。

この研究は、任意に複雑な物体幾何学、多様なロボット構成、および様々な接触豊かタスクを処理できる汎用フレームワークとして SCSP を位置づけ、モデルベースの効率性と複雑な操作に必要な適応性の間のギャップを埋めています。