Motion planning for hundreds of floating robots

技術要約：数百台の浮遊ロボットのための運動計画

問題提起
本論文は、平面上の多様体上で動作する、数百台規模の全方向移動型浮遊ロボット（水面ロボット）のための、衝突回避が可能で動的に実行可能な軌道を計画するという課題に取り組んでいる。垂直方向の回避動作を利用して高密度な遷移時の衝突を回避できる空中のドローンとは異なり、水面ロボットは2次元に制約されているため、調整と衝突回避が本質的に困難である。計画のコンテキストはインタラクティブなショーデザインであり、設計者が疎なキーフレーム（開始および終了構成）を指定し、数分間にわたる数千のタイムステップに及ぶ軌道を生成するために、即時的なフィードバック（数秒以内）を必要とする。

核心となる困難さは、衝突回避のスケーラビリティにある。フリートのサイズ（$N$）が増加するにつれ、エージェント間の結合が急速に増大し（しばしば二次関数的に増加）、ペアごとの衝突制約の数は $\binom{N}{2}K$ （$K$ はタイムステップ数）としてスケールする。大きな $N$ （例：500）および $K$ （例：1000）の場合、標準的な逐次凸計画法（SCP）の定式化は、何億もの非ゼロ要素を持つ二次計画問題（QP）となり、反復的な設計ループにおいて計算不能となる。

手法
著者らは、グローバルに結合された問題をより小さな独立したサブ問題へと分解する、スケーラブルで階層的な計画パイプラインを提案している。このパイプラインは、主に4つのステージで構成される。

1. 初期化と割り当て:

   • プロセスは、ロボットをターゲット位置にマッチングさせる最小コスト割当問題から始まる。「ラベル付き」ロボット（ユーザーが定義した特定のターゲットを持つもの）については、特定のマッチングを強制するためにコストを0または無限大とする。 「ラベルなし」ロボットについては、不要な入れ替えを減らすためにユークリッド距離を用い、総変位を最小化する。
   • 初期軌道は、開始位置と終了位置の間の線形補間によって生成される。この初期化段階では、一般に衝突回避はなされていない。

2. 衝突グラフの構築:

   • ノードが「ロボットと時刻のペア」を表す時空間グラフを構築する。
   • 空間エッジは、同じタイムステップにおいて安全半径 $R$ 以内にいるロボット同士を接続する。
   • 時間エッジは、衝突が発生する場合に、連続するタイムステップ間における同一のロボット自身を接続する。
   • このグラフの連結成分が「衝突クラスター」を定義する。著者らは、ペアごとの違反を効率的に検出するためにKD木データ構造を利用しており、素朴なペアワイズチェックと比較して約2000倍の高速化を実現している。

3. クラスタリングと分解:

   • 最適化アルゴリズムに回避経路を計画する自由度を与えるため、各クラスターの時間窓はハイパーパラメータ $B$ （3秒に設定）によって拡張（バッファリング）される。
   • 重複するバッファ付きクラスターは、union-find手続きを用いてマージされる。サブ問題が大きくなりすぎて並列化のメリットを打ち消すことを防ぐため、複雑性のキャップ（上限）が課される。マージによってQP行列の非ゼロ要素の閾値を超えた場合、結果として得られるサブ問題の最大複雑性を最小化するスコアリングメカニズムを用いて、グループは分割される。

4. サブ問題の解決とマージ:

   • 各クラスターは、SCPフレームワークを用いて独立かつ並列に解かれる。
   • 定式化の改善: 本論文では、決定ベクトルに位置と速度の両方（$\chi = [p, v]$）を含む「直接転写（direct transcription）」定式化を導入している。これにより、動力学を局所的な等式制約へと変換し、先行研究で使用されている「直接シューティング（direct shooting）」定式化と比較して、制約行列のスパース性を大幅に向上させた（複雑度を $O(N^2K^2)$ から $O(N^2K)$ へ削減）。
   • 堅牢性: 対称的な構成に対して初期値を摂動させるメカニズム、解が実行不可能になった場合に離散化を適応的に粗くするメカニズム、および軽量なポリッシング（磨き上げ）ステップが含まれる。
   • サイクル検出: クラスターのマージによって衝突パターンが振動する循環依存関係を検出する。検出された場合、競合するサブ問題は、後続のイテレーションのために単一の結合されたサブ問題へとロックされる。
   • 最終的に、局所解はグローバルな軌道へとマージされ、衝突のない解が見つかるまでループが繰り返される。

主な貢献

• スケーラブルな階層的パイプライン: 衝突グラフを構築し、問題を相互作用クラスターに分解して並列に解決することで、数百台のロボットを数秒で計画することを可能にする手法。
• 堅牢な分解メカニズム: 高速なグラフ更新、バッファリング戦略、サブ問題の爆発を防ぐための複雑性キャップ、および振動を防ぐためのサイクル検出を含む、一般的な病理的状況に対処する技術。
• 最適化の再定式化: クラスターレベルの軌道最適化において、QPのスパース性とソルバーの堅牢性を向上させる（特にラベル付きロボットの割合が高い構成において）直接転写定式化への移行。

結果と検証
本システムは、シミュレーションおよび2つの実世界での展開において検証された。

• シミュレーション: 本パイプラインは、最大500台のロボットに対し、1000タイムステップにわたる衝突のない軌道を生成することに成功した。500台のケーススタディでは、ラベル付きロボットが30%までの場合は100%の収束率を、それ以上の割合では99%の収束率を達成し、計算時間は通常数秒以内であった。
• 実世界展開 1 (チューリッヒ湖): 24台のロボットのフリートが、1〜2分間の遷移（400ステップ以上）のために振り付けられた。設計者はキーフレームを反復的に調整し、数秒以内に軌道を再生成することができた。
• 実世界展開 2 (ヴェネツィア・ビエンナーレ 2025): 8台のロボットのフリートが、2000ステップを超える遷移を行った。2491ステップの軌道が1.23秒で計算された。

意義
本論文は、このエンジニアリングに焦点を当てたパイプラインが、表面上におけるマルチロボット運動計画のスケーラビリティにおける核心的なボトルネックに対処していると主張している。問題を分解し、ソルバーの定式化を最適化することで、本アプローチは従来のモノリシックなSCPパイプラインよりも数桁高速である。この速度により、設計者が疎なキーフレームを反復的に調整し、衝突回避が可能で動的に実行可能な軌道に関する即時的なフィードバックを受け取ることができる、インタラクティブなユーザー体験が可能となる。これは、これまで計算上の制約により、2次元の表面環境では限定的であった能力である。