機器人局部動态避障算法dwa解析

簡介

 　　dwa算法全稱叫動态視窗法（dynamic window approach），其算法過程主要分為仿真擷取機器人的運動軌迹、對軌迹進行評價選擇最優軌迹兩個主要過程，動态視窗表達的是仿真的運動軌迹數量有限，主要是因為機器人在較短的控制周期内隻能達到一定的速度。

一、機器人如何仿真擷取運動軌迹

1、擷取機器人速度樣本

  　　根據機器人目前速度以及運動特性，确定機器人可達的運動速度範圍。由于運動的最終目的是到達目标點，是以，在到達目标點附近時，需要降低機器人運動速度，也就是進一步限制機器人的速度範圍，以保證機器人能平穩的到達目标點。最後根據人為給定的樣本數，在限制的速度範圍内獲得樣本數個離散的速度樣本，包括線速度和角速度。

void SimpleTrajectoryGenerator::initialise(
    const Eigen::Vector3f& pos,　//機器人的位置
    const Eigen::Vector3f& vel,　//目前機器人速度
    const Eigen::Vector3f& goal,　//目标點
    base_local_planner::LocalPlannerLimits* limits,　//運動特性（加速度、最大最小速度......）
    const Eigen::Vector3f& vsamples,　//樣本
    bool discretize_by_time) {
    
   //給定機器人的最大最小運動速度
  double max_vel_th = limits->max_vel_theta;
  double min_vel_th = -1.0 * max_vel_th;
  discretize_by_time_ = discretize_by_time;
  Eigen::Vector3f acc_lim = limits->getAccLimits();
  pos_ = pos;
  vel_ = vel;
  limits_ = limits;
  next_sample_index_ = 0;
  sample_params_.clear();

  double min_vel_x = limits->min_vel_x;
  double max_vel_x = limits->max_vel_x;
  double min_vel_y = limits->min_vel_y;
  double max_vel_y = limits->max_vel_y;

  // if sampling number is zero in any dimension, we don't generate samples generically
  if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
    //compute the feasible velocity space based on the rate at which we run
    Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
    Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();

    if ( ! use_dwa_) {
      //根據機器人位置到目标點的距離，限制機器人的最大運動速度
      double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
      max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
      max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);

      // 根據控制周期和加速度特性，确定機器人可達的最大最小速度
      // 此處用的是sim_time_仿真時間，确定的是接下來一段時間内機器人可達的運動速度範圍，預設是1s
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
    } else {
      // 此處用的sim_period_是控制周期，也就是隻确定下一個控制周期機器人的運動速度範圍
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
    }

    //根據給定的速度樣本數，在速度空間内等間距的擷取速度樣本
    Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
    VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
    VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
    VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
    for(; !x_it.isFinished(); x_it++) {
      vel_samp[0] = x_it.getVelocity();
      for(; !y_it.isFinished(); y_it++) {
        vel_samp[1] = y_it.getVelocity();
        for(; !th_it.isFinished(); th_it++) {
          vel_samp[2] = th_it.getVelocity();
          //ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
          sample_params_.push_back(vel_samp);
        }
        th_it.reset();
      }
      y_it.reset();
    }
  }
}
           

2、生成運動軌迹

  根據速度樣本确定運動軌迹，是簡單運作學知識，主要注意的是用的仿真時間産生的樣本還是仿真周期産生的樣本，其中仿真時間指的是人為設定的一段仿真時間，預設１秒，而仿真周期指的是算法的實際控制周期，預設為0.1秒。

bool SimpleTrajectoryGenerator::generateTrajectory(
      Eigen::Vector3f pos,  //機器人的位姿
      Eigen::Vector3f vel,  //運動速度
      Eigen::Vector3f sample_target_vel,  //樣本速度
      base_local_planner::Trajectory& traj) {  //需要生成的軌迹
  double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
  double eps = 1e-4;
  traj.cost_   = -1.0; // placed here in case we return early
  //trajectory might be reused so we'll make sure to reset it
  traj.resetPoints();

  //确定樣本是否超過設定的最大移動速度
  // make sure that the robot would at least be moving with one of
  // the required minimum velocities for translation and rotation (if set)
  if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) &&
      (limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_vel_theta)) {
    return false;
  }
  // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
  if (limits_->max_vel_trans >=0 && vmag - eps > limits_->max_vel_trans) {
    return false;
  }

  //确定仿真使用的控制周期數
  int num_steps;
  if (discretize_by_time_) {
    num_steps = ceil(sim_time_ / sim_granularity_);
  } else {
    //compute the number of steps we must take along this trajectory to be "safe"
    double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
    double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
    num_steps =
        ceil(std::max(sim_time_distance / sim_granularity_,
            sim_time_angle    / angular_sim_granularity_));
  }

  if (num_steps == 0) {
    return false;
  }

  //确定生成軌迹的時間間隔（僅對利用仿真時間進行速度采樣的情況）
  //compute a timestep
  double dt = sim_time_ / num_steps;
  traj.time_delta_ = dt;
  
  Eigen::Vector3f loop_vel;
  //連續加速意味着用的是仿真時間進行的速度采樣，不是單個控制周期能達到的運動速度。是以，需要根據機器人的運動特性确定接下來的控制周期内機器人能達到的運動速度
  if (continued_acceleration_) {
    // assuming the velocity of the first cycle is the one we want to store in the trajectory object
    loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
    traj.xv_     = loop_vel[0];
    traj.yv_     = loop_vel[1];
    traj.thetav_ = loop_vel[2];
  } else {
    //否則用的就是仿真周期進行的采樣，直接将采樣速度作為生成軌迹的速度
    // assuming sample_vel is our target velocity within acc limits for one timestep
    loop_vel = sample_target_vel;
    traj.xv_     = sample_target_vel[0];
    traj.yv_     = sample_target_vel[1];
    traj.thetav_ = sample_target_vel[2];
  }

  //根據仿真的周期數，生成仿真軌迹
  for (int i = 0; i < num_steps; ++i) {

    //add the point to the trajectory so we can draw it later if we want
    traj.addPoint(pos[0], pos[1], pos[2]);

    //如果用的是仿真時間進行的速度采樣，在每個仿真控制周期内，速度需要根據加減速特性确定
    if (continued_acceleration_) {
      //calculate velocities
      loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
      //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
    }

    //根據速度和時間，确定運動軌迹上的下一個點
    //update the position of the robot using the velocities passed in
    pos = computeNewPositions(pos, loop_vel, dt);

  } // end for simulation steps

  return true; // trajectory has at least one point
}
           

二、如何對軌迹進行評價并選取最優軌迹

１、代價函數

oscillation_costs_ //振蕩代價函數，一旦速度發生振蕩，直接丢棄速度樣本

obstacle_costs_ //障礙物代價函數，當軌迹進入障礙物區，直接丢棄目前軌迹樣本

goal_costs_ //目标代價函數，優先選擇距離局部目标點近的軌迹

path_costs_ //路徑代價函數，優先選擇貼近全局路徑的軌迹

goal_front_costs_ //與goal_costs基本一緻，不太了解注釋中的nose是指什麼？

alignment_costs_ //與path_costs基本一緻，不太了解注釋中的nose是指什麼？

  注：為了能适應不同場景的需求，可以對這些代價函數配置不同的權重，進而能實作不同場景對這些代價函數的重視程度

2、主要評價函數的實作

2.1、障礙物代價函數

  　　通過仿真軌迹将機器人輪廓映射到全局坐标系下，并對輪廓邊上的代價值進行分析，選擇最大的代價值作為障礙物代價，進而确定機器人是否會撞到障礙物

double CostmapModel::footprintCost(const geometry_msgs::Point& position,   //機器人在全局坐标系下的位置
  const std::vector<geometry_msgs::Point>& footprint,    //機器人輪廓
   double inscribed_radius, double circumscribed_radius)   //内切圓、外接圓半徑
   {
    //used to put things into grid coordinates
    unsigned int cell_x, cell_y;

    //get the cell coord of the center point of the robot
    //獲得機器人在地圖坐标系下的坐标
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -1.0;

    //if number of points in the footprint is less than 3, we'll just assume a circular robot
    //當輪廓上的點數少于３時，認為機器人是個圓形機器人，并且隻判斷機器人中心是否在不可走區域
    if(footprint.size() < 3){
      unsigned char cost = costmap_.getCost(cell_x, cell_y);
      //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
        return -1.0;
      return cost;
    }

    //now we really have to lay down the footprint in the costmap grid
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0;
    double footprint_cost = 0.0;

    //footprint是一個多邊形，判斷該多邊形是否與障礙物發生碰撞的方法是：計算多邊形的所有邊的最大代價值，進而确定是否與障礙物相撞
    //we need to rasterize each line in the footprint
    for(unsigned int i = 0; i < footprint.size() - 1; ++i){
      //get the cell coord of the first point
      //獲得地圖中機器人輪廓上的一個點的坐标
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        return -1.0;
      
      //獲得地圖中相鄰輪廓點的坐标
      //get the cell coord of the second point
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
        return -1.0;

      //确定目前輪廓點與相鄰輪廓點構成的邊的最大代價值
      line_cost = lineCost(x0, x1, y0, y1);
      //選取所有邊的最大代價值
      footprint_cost = std::max(line_cost, footprint_cost);

      //if there is an obstacle that hits the line... we know that we can return false right away 
      if(line_cost < 0)
        return -1.0;
    }

    //擷取第一個輪廓點與最後一個輪廓點構成的邊的最大代價值
    //we also need to connect the first point in the footprint to the last point
    //get the cell coord of the last point
    if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
      return -1.0;

    //get the cell coord of the first point
    if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
      return -1.0;

    line_cost = lineCost(x0, x1, y0, y1);
    //确定所有邊的最大代價值
    footprint_cost = std::max(line_cost, footprint_cost);

    if(line_cost < 0)
      return -1.0;

    //if all line costs are legal... then we can return that the footprint is legal
    return footprint_cost;

  }
           

2.2、目标代價函數

  首先根據全局路徑與局部代價地圖的邊界确定局部目标點，然後依據局部目标點生成栅格地圖，每個栅格處的值代表目前栅格到目标點的距離，對于障礙物栅格，直接置為到目标點的距離無窮遠。最後再根據軌迹末端點處栅格的位置，直接通過索引在地圖中擷取該位置距目标點的距離，作為距目标點的代價。是以，目标代價函數的主要任務是生成栅格地圖。

//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
  void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,　//局部代價地圖
      const std::vector<geometry_msgs::PoseStamped>& global_plan) 　//全局路徑
    {
    sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

    int local_goal_x = -1;
    int local_goal_y = -1;
    bool started_path = false;

    //調整全局路徑分辨率與地圖分辨率一緻
    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
    adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());

    // 将全局路徑與局部代價地圖邊界的交點作為局部目标點
    for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
      double g_x = adjusted_global_plan[i].pose.position.x;
      double g_y = adjusted_global_plan[i].pose.position.y;
      unsigned int map_x, map_y;
      if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
        local_goal_x = map_x;
        local_goal_y = map_y;
        started_path = true;
      } else {
        if (started_path) {
          break;
        }// else we might have a non pruned path, so we just continue
      }
    }
    if (!started_path) {
      ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
      return;
    }

   	//建構距離優先隊列，并添加局部目标點作為隊列的第一個點
    queue<MapCell*> path_dist_queue;
    if (local_goal_x >= 0 && local_goal_y >= 0) {
      MapCell& current = getCell(local_goal_x, local_goal_y);
      costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
      current.target_dist = 0.0;
      current.target_mark = true;
      path_dist_queue.push(&current);
    }
	
	//按優先隊列的順序，從局部目标點開始以單個栅格為步長向外膨脹，進而直接确定出每個栅格距離局部目标點的距離
    computeTargetDistance(path_dist_queue, costmap);
  }
           

2.3、路徑代價函數

  該代價函數的實作原理與目标代價函數類似，隻是該函數是以局部路徑上的所有點作為起點向外膨脹來建構栅格地圖，并且是以仿真軌迹上的所有點的距離值的和的平均值作為軌迹的代價值。