机器人导航dwa（局部避障）分析

前面部分引用http://blog.csdn.net/lqygame/article/details/72861439

（1）初始化：

在move_base节点中，通过类加载模块载入了BaseLocalPlanner（局部路径规划）的子类DWAPlannerROS的实例tc_，并调用其初始化函数，获取了一些初始状态信息比如机器人当前位置等，并创建了真正实现DWA算法的DWAPlanner类的实例dp_，最后设置了动态参数配置服务。dp_的构造函数做了一系列参数获取的操作，最重要的是将几种cost计算方法的实例加入一个名为critics的vector容器里。
（2）采样速度样本：
当move_base调用tc_的computeVelocityCommands方法后，tc_会调用dwaComputeVelocityCommands方法，并在其中调用dp_的findBestPath方法。findBestPath方法里调用SimpleTrajectoryGenerator类的实例generator_的initialise函数，这个函数就是主要负责速度采样的。
每个维度速度需要采样的养本数存放在vsamples_这个结构体内，vsamples_[0]是x方向样本数，vsamples_[1]是y方向样本数，vsamples_[2]是z方向样本数。首先计算各个方向的最大速度和最小速度，DWA算法只在第一步进行采样，所以最大速度为:
Max_vel=min(max_vel,vel+acc_lim*sim_period)
最小速度为:
Min_vel=max(min_vel,vel-acc_lim*sim_period)
其中max_vel，min_vel为人为设定的最大和最小速度，vel是当前速度，acc_lim是人为设定的最大加速度，sim_period是第一步的模拟时间，由人为设定的局部路径规划频率决定，默认为0.05。
当计算出各个维度的最大最小速度后，就创建三个VelocityIterator类的对象，并传入最大最小速度和样本数目，此对象的构造函数会生成同样数目的速度样本并放入samples_这个容器内。具体做法是先计算步长step_size：
step_size=(max-min)/(nums_samples-1)
max为最大速度，min为最小速度，nums_samples为样本数目。从最小速度每次多累加一次step_size即为一个速度样本，直到达到最大速度。将每个维度的速度样本取得后，再全部循环每个样本组里选择一个组合放入结构体vel_sample，最后将这些vel_sample放入sample_params_的容器里。至此，速度采样就完成了。
（3）样本评分
速度采样完成后，逐一循环对样本空间内的样本进行评分。对每一组速度调用scoreTrajectory函数计算其评分，而scoreTrajectory函数则对这一组速度调用所有critics容器里的costfunction计算每个cost从而累加算出总的cost。在计算过程中，一旦累加的cost大于当前最小的cost则抛弃这组速度。
之前说到的几种cost成本函数为下列所示：
ObstacleCostFunction
这个成本函数基于感知障碍物来评估轨迹。它或者由于轨迹通过障碍物而返回负值，或者0。
MapGridCostFunction
这个成本函数类基于轨迹离全局路径或者接近目标点有多近来评估轨迹。这个尝试利用距离预计算地图有相同距离的路径或者目标点的所有的规划，来优惠计算速度。
在 dwa_local_planner中，代价函数因为不同的目的，被多次实例化。保持轨迹接近于路径，使机器人朝局部目标前进，并且使机器人的前段点指向局部目标。代价函数是一个启发，可以带来坏的结果或者不合适的参数的失败。
OscillationCostFunction
震荡发生在X,Y,theta维度上，正/负值被连续的选择。为了阻止震荡，当机器人在任何方向移动时，与下一个循环相反的方向被标记为无效，直到机器人已经从所设置标记的位置移动而并且超过一定的距离。这个成本函数类帮助减少某些震荡，虽然这可以有效的阻止这些震荡，如果使用不合适的参数，但是有可能阻止良好的解。
PreferForwardCostFunction
考虑到好的激光扫描范围只在机器人的前面，这个成本函数类被设计在像PR2一样的机器人上。成本函数更喜欢正面向前运动，惩罚背面运用及扫射动作。在其他机器人上或者其他领域，这可能是非常不可取的行为。
（4）发布plan
通过上述几种评分机制，选取最优的一组速度样本，传递给move_base，并发布相应的local plan。move_base如果收到了可用的速度则发布给底盘，否则发布0速度，且如果寻找最优速度的时间超过了限制就会执行障碍物清理模式，state_会变为CLEARING。

move_base.cpp

controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
blp_loader_.isClassAvailable(local_planner)
tc_ = blp_loader_.createInstance(local_planner);
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
tc_->setPlan(*controller_plan_)
tc_->computeVelocityCommands(cmd_vel)

dwa相关参数：
/move_base/DWAPlannerROS/acc_lim_th
/move_base/DWAPlannerROS/acc_lim_theta
/move_base/DWAPlannerROS/acc_lim_x
/move_base/DWAPlannerROS/acc_lim_y
/move_base/DWAPlannerROS/acc_limit_trans
/move_base/DWAPlannerROS/angular_sim_granularity
/move_base/DWAPlannerROS/forward_point_distance
/move_base/DWAPlannerROS/global_frame_id
/move_base/DWAPlannerROS/goal_distance_bias
/move_base/DWAPlannerROS/holonomic_robot
/move_base/DWAPlannerROS/latch_xy_goal_tolerance
/move_base/DWAPlannerROS/max_rot_vel
/move_base/DWAPlannerROS/max_scaling_factor
/move_base/DWAPlannerROS/max_trans_vel
/move_base/DWAPlannerROS/max_vel_x
/move_base/DWAPlannerROS/max_vel_y
/move_base/DWAPlannerROS/meter_scoring
/move_base/DWAPlannerROS/min_rot_vel
/move_base/DWAPlannerROS/min_trans_vel
/move_base/DWAPlannerROS/min_vel_x
/move_base/DWAPlannerROS/min_vel_y
/move_base/DWAPlannerROS/occdist_scale
/move_base/DWAPlannerROS/oscillation_reset_angle
/move_base/DWAPlannerROS/oscillation_reset_dist
/move_base/DWAPlannerROS/path_distance_bias
/move_base/DWAPlannerROS/penalize_negative_x
/move_base/DWAPlannerROS/prune_plan
/move_base/DWAPlannerROS/publish_cost_grid_pc
/move_base/DWAPlannerROS/publish_traj_pc
/move_base/DWAPlannerROS/restore_defaults
/move_base/DWAPlannerROS/rot_stopped_vel
/move_base/DWAPlannerROS/scaling_speed
/move_base/DWAPlannerROS/sim_granularity
/move_base/DWAPlannerROS/sim_time
/move_base/DWAPlannerROS/stop_time_buffer
/move_base/DWAPlannerROS/trans_stopped_vel
/move_base/DWAPlannerROS/use_dwa
/move_base/DWAPlannerROS/vth_samples
/move_base/DWAPlannerROS/vtheta_samples
/move_base/DWAPlannerROS/vx_samples
/move_base/DWAPlannerROS/vy_samples
/move_base/DWAPlannerROS/xy_goal_tolerance
/move_base/DWAPlannerROS/yaw_goal_tolerance

最佳速度的代价函数：
  DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
      planner_util_(planner_util),
      obstacle_costs_(planner_util->getCostmap()),
      path_costs_(planner_util->getCostmap()),
      goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
      goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
      alignment_costs_(planner_util->getCostmap())

    // set up all the cost functions that will be applied in order
    // (any function returning negative values will abort scoring, so the order can improve performance)
    std::vector<base_local_planner::TrajectoryCostFunction*> critics;
    critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
    critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
    critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
    critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
    critics.push_back(&path_costs_); // prefers trajectories on global path
    critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation

    oscillation_costs_.resetOscillationFlags();

    bool sum_scores;
    private_nh.param("sum_scores", sum_scores, false);
    obstacle_costs_.setSumScores(sum_scores);

    obstacle_costs_.setFootprint(footprint_spec);

{
    double resolution = planner_util_->getCostmap()->getResolution();
    pdist_scale_ = config.path_distance_bias;
    // pdistscale used for both path and alignment, set  forward_point_distance to zero to discard alignment
    path_costs_.setScale(resolution * pdist_scale_ * 0.5);
    alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);

    gdist_scale_ = config.goal_distance_bias;
    goal_costs_.setScale(resolution * gdist_scale_ * 0.5);
    goal_front_costs_.setScale(resolution * gdist_scale_ * 0.5);

    occdist_scale_ = config.occdist_scale;
    obstacle_costs_.setScale(resolution * occdist_scale_);

    stop_time_buffer_ = config.stop_time_buffer;
    oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
    forward_point_distance_ = config.forward_point_distance;
    goal_front_costs_.setXShift(forward_point_distance_);
    alignment_costs_.setXShift(forward_point_distance_);
 
    // obstacle costs can vary due to scaling footprint feature
    obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);

}


 void DWAPlanner::updatePlanAndLocalCosts(
      tf::Stamped<tf::Pose> global_pose,
      const std::vector<geometry_msgs::PoseStamped>& new_plan) {
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i) {
      global_plan_[i] = new_plan[i];
    }

    // costs for going away from path
    path_costs_.setTargetPoses(global_plan_);

    // costs for not going towards the local goal as much as possible
    goal_costs_.setTargetPoses(global_plan_);

    // alignment costs
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();

    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    double sq_dist =
        (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
        (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);

    // we want the robot nose to be drawn to its final position
    // (before robot turns towards goal orientation), not the end of the
    // path for the robot center. Choosing the final position after
    // turning towards goal orientation causes instability when the
    // robot needs to make a 180 degree turn at the end
    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
      forward_point_distance_ * cos(angle_to_goal);
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
      sin(angle_to_goal);

    goal_front_costs_.setTargetPoses(front_global_plan);
    
    // keeping the nose on the path
    if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
      double resolution = planner_util_->getCostmap()->getResolution();
      alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
      // costs for robot being aligned with path (nose on path, not ju
      alignment_costs_.setTargetPoses(global_plan_);
    } else {
      // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
      alignment_costs_.setScale(0.0);
    }
  }


  bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {
    Trajectory loop_traj;
    Trajectory best_traj;
    double loop_traj_cost, best_traj_cost = -1;
    bool gen_success;
    int count, count_valid;
    for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {
      TrajectoryCostFunction* loop_critic_p = *loop_critic;
      if (loop_critic_p->prepare() == false) {
        ROS_WARN("A scoring function failed to prepare");
        return false;
      }
    }

    for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
      count = 0;
      count_valid = 0;
      TrajectorySampleGenerator* gen_ = *loop_gen;
      while (gen_->hasMoreTrajectories()) {
        gen_success = gen_->nextTrajectory(loop_traj);//生成一条轨迹（一组点）
        if (gen_success == false) {
          // TODO use this for debugging
          continue;
        }
        loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);//评分
        if (all_explored != NULL) {
          loop_traj.cost_ = loop_traj_cost;
          all_explored->push_back(loop_traj);
        }

        if (loop_traj_cost >= 0) {
          count_valid++;
          if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
            best_traj_cost = loop_traj_cost;
            best_traj = loop_traj;
          }
        }
        count++;
        if (max_samples_ > 0 && count >= max_samples_) {
          break;
        }        
      }
      if (best_traj_cost >= 0) {
        traj.xv_ = best_traj.xv_;
        traj.yv_ = best_traj.yv_;
        traj.thetav_ = best_traj.thetav_;
        traj.cost_ = best_traj_cost;
        traj.resetPoints();
        double px, py, pth;
        for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
          best_traj.getPoint(i, px, py, pth);
          traj.addPoint(px, py, pth);
        }
      }
      ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
      if (best_traj_cost >= 0) {
        // do not try fallback generators
        break;
      }
    }
    return best_traj_cost >= 0;
  }



ObstacleCostFunction:
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);
  double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);

class WorldModel{
      double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0)
}