BoundPlanner: 凸集合に基づく拘束付きマニピュレータ軌道計画手法の技術的サマリー

本論文は、ロボットマニピュレータのためのオンライン軌道計画フレームワーク「BoundPlanner」を提案しています。この手法は、複雑で動的な環境において、ロボットの運動学制約と衝突回避を同時に満たす高速かつ安全な軌道を生成することを目的としています。

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

1. 背景と問題定義

現代のロボットは、家事支援など非構造化された環境での自律動作が求められています。しかし、椅子や人間など、位置が常に変化する動的な障害物が存在する環境では、従来のオフライン計画や単純なオンライン計画では対応が困難です。

• 既存手法の限界:
  • オフライン計画: 環境変化への対応が遅く、リアルタイム性が不足している。
  • オンライン最適化ベースの計画: 複雑な制約条件下では計算コストが高く、失敗しやすい。
  • ポテンシャル関数ベース: 衝突回避を保証できず、細い障害物や多数の障害物（点群など）に対してスケーラビリティに問題がある。
  • 直交空間（カルテシアン空間）での計画: 運動学制約を無視すると、実行不可能な軌道になることが多い。
  • 関節空間での計画: 障害物を関節空間に変換するのは計算コストが高く、オンライン計画には不向きである。

本研究は、「カルテシアン空間での高速な経路探索」と「関節空間での運動学制約を考慮した軌道生成」を統合し、かつ衝突回避を保証する新しいアプローチを提案します。

2. 提案手法 (Methodology)

提案フレームワークは、2 つの主要なコンポーネントで構成されています。

A. BoundPlanner: 凸集合に基づくカルテシアン経路計画

エンドエフェクタの参照経路 $\pi(\phi)$ と、その周囲の安全な「境界（バウンド）」を生成する計画器です。

• 衝突フリーな凸集合の探索:
  • 作業空間（カルテシアン空間）内で、障害物を避ける大きな凸集合（Convex Sets）を計算します。
  • 凸集合の生成には、最大内接楕円体（MVIE）の体積最大化や、与えられた点群の凸包（Convex Hull）を用いた手法を採用しています。
  • これらの凸集合をノードとし、集合間の重なり（交差）をエッジとするグラフ $G$ を構築します。
• 経路探索と最適化:
  • ダイクストラ法を用いて、スタートからゴールまでの凸集合の列（経路）を探索します。
  • エッジコストには、集合の体積（大きい集合ほど好ましい）と距離を考慮した重み付けを行い、ロボットの運動学的余裕を確保しやすい経路を選択します。
  • 経路は、位置パス $\pi_p(\phi)$ と姿勢パス $\pi_o(\phi)$ から構成され、エンドエフェクタの形状（凸包）がすべての経路点で凸集合内に収まるように最適化されます。

B. BoundMPC: 凸集合境界を用いたモデル予測制御

BoundPlanner で生成された参照経路と凸集合の境界を用いて、オンラインで関節軌道 $T(t)$ を生成する制御器です。

• 経路からの逸脱許容:
  • 従来の MPC は経路への追従誤差を最小化しますが、BoundMPC は凸集合で定義された「安全な領域内」であれば経路から逸脱することを許容します。これにより、運動学制約（関節の可動範囲や速度制限）を満たしつつ、衝突を回避できます。
• ホライズン分割（Horizon Splitting）:
  • 経路が複数の凸集合にまたがる場合、制御ホライズンを分割し、各セグメントで適切な凸集合の制約を適用することで、制約の連続性を保ちます。
• マニピュレータ全体の衝突回避:
  • エンドエフェクタだけでなく、アームのリンク全体についても衝突回避を行います。
  • 障害物の数を直接考慮するのではなく、ロボット上の关键点（例：各関節軸）に対して、予測ホライズン内の位置に基づいて「衝突フリーな凸集合」を動的に生成する新しい定式化を採用しています。これにより、障害物数に依存しない効率的な計算が可能になります。

C. オンライン再計画（Replanning）

環境変化（目標位置の変更や障害物の移動）に対して、現在のロボット状態を初期条件として即座に再計画を行います。BoundPlanner の高速な計算能力により、リアルタイムでの対応が可能となります。

3. 主要な貢献 (Key Contributions)

1. BoundPlanner の提案: 衝突フリーなカルテシアン凸集合を計算し、マニピュレータの運動学を考慮した大きな領域を優先して探索する高速な経路計画器。
2. BoundMPC の拡張: 経路の逸脱を許容する境界として凸集合を利用し、ホライズン分割を用いて運動学制約を考慮したオンライン軌道生成を実現。
3. 新しい衝突回避定式化: 障害物の数に依存せず、マニピュレータのリンク全体（キーポイント）の予測軌道に基づいて凸集合を生成する手法。これにより、細い障害物や多数の障害物環境でも安全な軌道が保証されます。

4. 実験結果 (Results)

7 自由度の KUKA LBR iiwa 14 R820 ロボットを用いたシミュレーションと実機実験で評価されました。

• シナリオ 1（箱への到達）:
  • 箱の内部という狭い空間で、上部に障害物がある条件下で計画を行いました。
  • 比較対象: GCS（Graph of Convex Sets）、VP-STO、Bidirectional RRT+MPC。
  • 結果: BoundPlanner は、他の手法と比較して計画時間が最も短く（0.11 秒 vs 最大 233 秒）、失敗なしで目標に到達しました。RRT+MPC は局所最小に陥り失敗し、GCS は計算に時間がかかりました。
• シナリオ 2（棚からの把持と再計画）:
  • 把持対象物の位置が移動し、オンラインでの再計画が必要となるシナリオ。
  • 結果: 20 回のランダムな再計画イベントにおいて、BoundPlanner は平均 0.1 秒で再計画を完了し、衝突なしで成功しました。一方、RRT+MPC は追従誤差の最小化を優先した結果、2 回中 3 回の衝突が発生しました。

5. 意義と結論

• 高速性と安全性の両立: カルテシアン空間での凸集合探索の速さと、関節空間での MPC による運動学制約の厳密な遵守を両立させました。
• 動的環境への適応: 障害物の数や配置が変化しても、再計画が高速に行えるため、非構造化環境での実用性が高いです。
• 完全な衝突回避: エンドエフェクタだけでなく、アーム全体に対する衝突回避を、障害物数に依存しない効率的な手法で保証しています。

本研究は、複雑な環境におけるロボットマニピュレーションのオンライン計画において、計算効率と安全性のバランスを大幅に改善する重要なステップとなります。今後の課題として、長期的なタスク実行のための高レベル意思決定フレームワークとの統合や、境界付き経路に対して関節軌道が常に存在することを保証する理論的検討が挙げられています。