crane_local_planner¶

概要¶

ロボット間の**リアルタイム経路計画と衝突回避**を担うパッケージです。RVO2（Reciprocal Velocity Obstacles）アルゴリズムを中核とし、マルチロボット環境での安全かつ効率的な経路生成を実現します。SSL競技の制約下でリアルタイム性能を重視した設計となっています。

主要機能¶

RVO2衝突回避: 複数ロボット間の相互衝突回避
リアルタイム経路計画: 10ms以下の高速経路生成
複数プランナー対応: 状況に応じたプランナー選択
動的障害物対応: 移動する敵ロボット・ボールの考慮
制約満足: ロボット動力学制約の考慮

アーキテクチャ上の役割¶

Craneシステムの**動作計画層**として、上位スキルからの目標位置・速度を受け取り、衝突回避を考慮した実現可能な制御コマンドを生成します。リアルタイム制約とマルチロボット協調の両立を実現する重要コンポーネントです。

プランナー構成¶

RVO2Planner（メインプランナー）¶

アルゴリズム: Reciprocal Velocity Obstacles 2.0
特徴:
- 分散的衝突回避（各ロボットが独立判断）
- 相互協調による最適解探索
- 実時間性能保証
適用場面: 通常のマルチロボット制御

GridmapPlanner（グリッドベース）¶

アルゴリズム: Grid-based A*経路探索
特徴:
- 静的障害物の詳細回避
- 事前計算による高速化
- 複雑な障害物配置対応
適用場面: 障害物密度が高い状況

SimplePlanner（基本プランナー）¶

アルゴリズム: 直線経路＋基本回避
特徴:
- 最小計算コスト
- 単純な経路生成
- フォールバック用途
適用場面: 計算資源制約時・緊急時

RVO2アルゴリズム詳細¶

基本原理¶

// 速度障害物の定義
struct VelocityObstacle {
  Vector2 apex;        // 障害物頂点
  Vector2 direction1;  // 境界方向1
  Vector2 direction2;  // 境界方向2
  double radius;        // 障害物半径
};

// 許可速度の計算
Vector2 computeAllowedVelocity(
  const Vector2& preferred_velocity,
  const std::vector<VelocityObstacle>& obstacles
) {
  // 1. 速度障害物群の統合
  // 2. 許可速度空間の計算
  // 3. 最適速度の選択
  return optimal_velocity;
}

マルチロボット協調¶

責任分担: 各ロボットが相手ロボットとの衝突回避責任を分担
相互予測: 他ロボットの意図を推定した協調行動
デッドロック回避: 膠着状態の自動解除機構

技術的特徴¶

動力学制約考慮¶

// ロボット制約の定義
struct RobotConstraints {
  double max_velocity;      // 最大速度
  double max_acceleration;  // 最大加速度
  double max_angular_vel;   // 最大角速度
  double radius;           // ロボット半径
};

// 制約満足解の生成
RobotCommand generateFeasibleCommand(
  const Vector2& target_position,
  const RobotConstraints& constraints
);

予測制御¶

短期予測: 0.5秒先までの軌道予測
衝突判定: 予測軌道での衝突可能性評価
動的調整: 予測結果に基づく速度調整

依存関係¶

アルゴリズム依存¶

rvo2_vendor: RVO2アルゴリズム実装
closest_point_vendor: 最近点計算ライブラリ

システム依存¶

crane_msg_wrappers: メッセージ変換
crane_msgs: システムメッセージ
diagnostic_updater: 診断機能

使用方法¶

基本的な使用例¶

#include "crane_local_planner/rvo2_planner.hpp"

// プランナー初期化
auto planner = std::make_shared<RVO2Planner>();
planner->setTimeStep(0.1);  // 100ms予測
planner->setAgentRadius(0.09);  // ロボット半径

// 経路計画実行
PlannerResult result = planner->plan(
  current_position,
  target_position,
  current_velocity,
  other_robots,
  obstacles
);

if (result.success) {
  // 制御コマンド適用
  sendVelocityCommand(result.velocity);
}

パラメータ設定¶

local_planner:
  ros__parameters:
    rvo2_planner:
      time_step: 0.1
      agent_radius: 0.09
      max_velocity: 3.0
      safety_margin: 0.02

    gridmap_planner:
      resolution: 0.05
      search_radius: 2.0

    planner_selection:
      primary: "rvo2"
      fallback: "simple"

動的プランナー切り替え¶

// 状況に応じた自動切り替え
PlannerType selectPlanner(const GameSituation& situation) {
  if (situation.robot_density > 0.8) {
    return PlannerType::GRIDMAP;  // 高密度時
  } else if (situation.computation_budget < 0.005) {
    return PlannerType::SIMPLE;   // 計算制約時
  } else {
    return PlannerType::RVO2;     // 通常時
  }
}

パフォーマンス最適化¶

計算効率化¶

空間分割: 近傍ロボットのみを考慮した計算削減
キャッシュ活用: 前回計算結果の再利用
並列処理: マルチスレッドによる高速化

メモリ効率¶

オブジェクトプール: 動的割り当て削減
データ構造最適化: キャッシュ効率の向上

パフォーマンス指標¶

リアルタイム性能¶

計算時間: <5ms（ロボット6台）
更新頻度: 100Hz
メモリ使用量: <50MB

衝突回避性能¶

衝突回避率: >99.5%
目標到達率: >95%
経路効率: 最短経路の1.2倍以内

将来展望¶

技術発展方向¶

学習ベース: 強化学習による経路最適化
予測精度: より長期の軌道予測
適応制御: 環境変化への自動適応

関連パッケージ: rvo2_vendor | crane_robot_skills | crane_planner_plugins