Obstacle Avoidance

动态避障技能

使用 DWA、TEB、ORCA 等算法实现机器人动态避障

算法对比

DWA（动态窗口法）

ros2 launch nav2_bringup collision_detection_launch.py

关键参数

dwb_controller:
  plugin: dwb_local_planner/DwbLocalPlanner
  prm:
    # 速度限制
    max_vel_x: 0.5
    max_vel_theta: 1.0
    # 轨迹评估权重
    vx_scale: 1.0
    vtheta_scale: 1.0
    # 障碍物代价权重
    obstacle_scale: 1.0
    path_distance_bias: 1.0
    goal_distance_bias: 1.0

// DWA 速度采样
geometry_msgs::msg::Twist cmd;
double vx = std::clamp(target_vx, min_vx, max_vx);
double vtheta = std::clamp(target_vtheta, min_vtheta, max_vtheta);
cmd.linear.x = vx;
cmd.angular.z = vtheta;

TEB（时间弹性带）

ros2 launch teb_local_planner_test test_obstacle.launch.py

关键参数

TebLocalPlannerROS:
  odom_topic: /odom
  map_frame: map
  
  # 机器人参数
  max_vel_x: 0.5
  max_vel_x_backwards: 0.2
  max_vel_theta: 1.0
  acceleration_limits: [0.5, 0.5, 1.5]
  
  # 避障
  obstacles:
    min_obstacle_dist: 0.5
    inflation_dist: 0.6
    dynamic_obstacles_inflation_dist: 0.8
  
  # 轨迹优化
  dt_ref: 0.3
  dt_hysteresis: 0.1
  max_samples: 500

动态障碍物预测

// 基于卡尔曼滤波的障碍物轨迹预测
#include <Eigen/Dense>

void predictObstacle(const geometry_msgs::msg::Point& pos,
                     const Eigen::Vector2d& vel,
                     double dt,
                     geometry_msgs::msg::Point& predicted) {
  predicted.x = pos.x + vel(0) * dt;
  predicted.y = pos.y + vel(1) * dt;
}

// 速度估计（常用于视觉检测）
Eigen::Vector2d estimateVelocity(const std::vector<geometry_msgs::msg::Point>& history) {
  if (history.size() < 2) return Eigen::Vector2d::Zero();
  double dt = (history.back().header.stamp - history.front().header.stamp).seconds();
  if (dt < 1e-6) return Eigen::Vector2d::Zero();
  return Eigen::Vector2d(
    (history.back().x - history.front().x) / dt,
    (history.back().y - history.front().y) / dt
  );
}

规范

• 避障距离：高速 > 1.0m，低速 > 0.3m
• Costmap 更新频率 ≥ 10Hz
• 动态障碍物预测 horizon ≥ 2s
• 多机器人场景用 ORCA 或制度化区域分配

错误处理