机器人路径规划（包括行人检测及动态避障总结）（长期更新）

本篇给予实际项目，作一个总结归纳

部分参考自　

（１）．http://blog.csdn.net/u013158492/article/details/50493676

（２）.  http://blog.csdn.net/heyijia0327/article/details/44983551

1.wiki

http://wiki.ros.org/people

主要用到的包：

leg_detector

---

输入：takes sensor_msgs/LaserScans as input and uses a machine-learning-trained classifier to detect groups of laser readings as possible legs

输出： publish people_msgs/PositionMeasurementArrays for the individual legs, and it can also attempt to pair the legs together and publish their average as an estimate of where the center of one person is as apeople_msgs/PositionMeasurement. 

订阅话题：

scan (sensor_msgs/LaserScan)

• The laser scan to detect from.
• 注意：在unseeded模式下，即只有一个激光传感器模式下，无其他检测源可以修正leg_detector检测到人腿的对与错，因此可能存在大量误检，但适用场景广

people_tracker_filter (people_msgs/PositionMeasurement)

• PositionMeasurements to use as the seeds (see above)
• 在seeded模式下，有其他传感器源，可提高行人检测准确性。一般来说，可加入摄像头，提供的message为people_msgs/PositionMeasurement

发布话题：

leg_tracker_measurements (people_msgs/PositionMeasurementArray)

• Estimates of where legs are

people_tracker_measurements (people_msgs/PositionMeasurementArray)

• Estimates of where people are

visualization_marker (visualization_msgs/Marker)

• Markers where legs and people are

---

people_velocity_tracker

---

订阅话题：

people_tracker_measurements (people_msgs/PositionMeasurementArray)

• Positions of people

发布话题：

/people (people_msgs/People)

• Positions and velocities of people

/visualization_marker (visualization_msgs/Marker)

• Markers

---

2. sending two types of sensor streams, sensor_msgs/LaserScan messages and sensor_msgs/PointCloud messages over ROS

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors

3.costmap_2d--ObstacleLayer

构造函数

ObstacleLayer() { costmap_ = NULL;}// this is the unsigned char* member of parent class Costmap2D.这里指明了costmap_指针保存了Obstacle这一层的地图数据

对于ObstacleLater，首先分析其需要实现的Layer层的方法：

  virtual void onInitialize();
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y);
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

  virtual void activate();
  virtual void deactivate();
  virtual void reset();

函数 onInitialize();： 
首先获取参数设定的值，然后新建observation buffer：

    // create an observation buffer
observation_buffers_.push_back(boost::shared_ptr < ObservationBuffer>
 (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,sensor_frame, transform_tolerance)));

   // check if we'll add this buffer to our marking observation buffers
if (marking)
      marking_buffers_.push_back(observation_buffers_.back());

    // check if we'll also add this buffer to our clearing observation buffers
if (clearing)
      clearing_buffers_.push_back(observation_buffers_.back());

然后分别对不同的sensor类型如LaserScan PointCloud PointCloud2，注册不同的回调函数。这里选LaserScan 分析其回调函数：

void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
                                      const boost::shared_ptr<ObservationBuffer>& buffer)
{
  // project the laser into a point cloud
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message->header;

  // project the scan into a point cloud
  try
  {
    projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
  }
  catch (tf::TransformException &ex)
  {
    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
             ex.what());
    projector_.projectLaser(*message, cloud);
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();
}

其中buffer->bufferCloud(cloud); 实际上是sensor_msgs::PointCloud2 >>>pcl::PCLPointCloud2 >>> pcl::PointCloud < pcl::PointXYZ > ; 然后才调用void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud) ：

void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
  Stamped < tf::Vector3 > global_origin;

  // create a new observation on the list to be populated
  observation_list_.push_front(Observation());

  // check whether the origin frame has been set explicitly or whether we should get it from the cloud
  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;

  try
  {
    // given these observations come from sensors... we'll need to store the origin pt of the sensor
    Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
                            pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);
    tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5));
    tf_.transformPoint(global_frame_, local_origin, global_origin);
    observation_list_.front().origin_.x = global_origin.getX();
    observation_list_.front().origin_.y = global_origin.getY();
    observation_list_.front().origin_.z = global_origin.getZ();

    // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
    observation_list_.front().raytrace_range_ = raytrace_range_;
    observation_list_.front().obstacle_range_ = obstacle_range_;

    pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;

    // transform the point cloud
    pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
    global_frame_cloud.header.stamp = cloud.header.stamp;
//上面的操作都是针对 observation_list_.front()的一些meta数据作赋值,下面的操作是对(observation_list_.front().cloud_)作赋值操作，
    // now we need to remove observations from the cloud that are below or above our height thresholds
    pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
    unsigned int cloud_size = global_frame_cloud.points.size();
    observation_cloud.points.resize(cloud_size);
    unsigned int point_count = 0;

    // copy over the points that are within our height bounds
    for (unsigned int i = 0; i < cloud_size; ++i)
    {
      if (global_frame_cloud.points[i].z <= max_obstacle_height_
          && global_frame_cloud.points[i].z >= min_obstacle_height_)
      {
        observation_cloud.points[point_count++] = global_frame_cloud.points[i];
      }
    }

    // resize the cloud for the number of legal points
    observation_cloud.points.resize(point_count);
    observation_cloud.header.stamp = cloud.header.stamp;
    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
  }
  catch (TransformException& ex)
  {
    // if an exception occurs, we need to remove the empty observation from the list
    observation_list_.pop_front();
    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
              cloud.header.frame_id.c_str(), ex.what());
    return;
  }

  // if the update was successful, we want to update the last updated time
  last_updated_ = ros::Time::now();

  // we'll also remove any stale observations from the list
  //这个操作会将timestamp较早的点都移除出observation_list_
  purgeStaleObservations();
}

以下重点分析updateBounds：

void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,double* min_y, double* max_x, double* max_y)
{
  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  if (!enabled_)
    return;
  useExtraBounds(min_x, min_y, max_x, max_y);

  bool current = true;
  std::vector<Observation> observations, clearing_observations;

  // get the marking observations 
 current = current && getMarkingObservations(observations);
  // get the clearing observations
 current = current &&getClearingObservations(clearing_observations);

  // update the global current status
  current_ = current;

  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);//首先清理出传感器到被测物之间的区域，标记为FREE_SPACE
  }

  // place the new obstacles into a priority queue... each with a priority of zero to begin with
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;
    const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
    for (unsigned int i = 0; i < cloud.points.size(); ++i)
    {
      double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
      // if the obstacle is too high or too far away from the robot we won't add it
      if (pz > max_obstacle_height_)
      {
        ROS_DEBUG("The point is too high");
        continue;
      }
      // compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = 
(px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
 + (pz - obs.origin_.z) * (pz - obs.origin_.z);

      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
      {
        ROS_DEBUG("The point is too far away");
        continue;
      }
      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        ROS_DEBUG("Computing map coords failed");
        continue;
      }
      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

函数raytraceFreespace： 
会首先处理测量值越界的问题，然后调用:

MarkCell marker(costmap_, FREE_SPACE);
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);

最终raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 会将所有在(x0,y0)>>(x1,y1)之间的所有cell标记为FREE_SPACE。而updateRaytraceBounds 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）。 

updateBounds 在根据测量数据完成clear 操作之后，就开始了mark 操作，对每个测量到的点，标记为obstacle

 double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;

      // if the obstacle is too high or too far away from the robot we won't add it
      if (pz > max_obstacle_height_)
      {
        ROS_DEBUG("The point is too high");
        continue;
      }

      // compute the squared distance from the hitpoint to the pointcloud's origin
      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
          + (pz - obs.origin_.z) * (pz - obs.origin_.z);

      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
      {
        ROS_DEBUG("The point is too far away");
        continue;
      }

      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        ROS_DEBUG("Computing map coords failed");
        continue;
      }

      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }

函数 updateFootprint：

void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                    double* max_x, double* max_y)
{
    if (!footprint_clearing_enabled_) return;
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);//这里获得了在当前机器人位姿（robot_x, robot_y, robot_yaw）条件下，机器人轮廓点在global坐标系下的值

    for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
    {
      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);//再次保留或者扩张Bounds
    }
}

函数 updateCosts：

void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  if (footprint_clearing_enabled_)
  {
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);//设置机器人轮廓所在区域为FREE_SPACE
  }

  switch (combination_method_)
  {
    case 0:  // Overwrite调用的CostmapLayer提供的方法
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  // Maximum
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }
}

４．Move Base 的配置文件分析

本文分析move base 的配置文件，从配置文件设置的角度，可以更清晰的把握move base对于costmap2D，global planner，local planner的调用关系。 
这里采用turtlebot_navigation package 为例：

<launch>
  <include file="$(find turtlebot_navigation)/launch/includes/velocity_smoother.launch.xml"/>
  <include file="$(find turtlebot_navigation)/launch/includes/safety_controller.launch.xml"/>

  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="map"/>
  <arg name="odom_topic" default="odom" />
  <arg name="laser_topic" default="scan" />
  <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/>
  <!-- 以下部分是move base调用，需要配置的文件：包括全局地图，局部地图，全局planner，局部planner -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
    <rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" />   
    <rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" />
    <!-- external params file that could be loaded into the move_base namespace -->
    <rosparam file="$(arg custom_param_file)" command="load" />

    <!-- reset frame_id parameters using user input data -->
    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

    <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <remap from="scan" to="$(arg laser_topic)"/>
  </node>
</launch>

从上面的配置上来可以看到以下内容，这些都是关于地图的配置：

 /param/costmap_common_params.yaml" command="load" ns="global_costmap"  
 /param/costmap_common_params.yaml" command="load" ns="local_costmap" 
 /param/local_costmap_params.yaml" command="load" 
 /param/global_costmap_params.yaml" command="load"  

地图配置，首先是采用了一个costmap_common_params.yaml文件配置了一些公共的参数：

max_obstacle_height: 0.60  # assume something like an arm is mounted on top of the robot

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
robot_radius: 0.20  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
# footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  # if the robot is not circular

map_type: voxel

obstacle_layer:
  enabled:              true
  max_obstacle_height:  0.6
  origin_z:             0.0
  z_resolution:         0.2
  z_voxels:             2
  unknown_threshold:    15
  mark_threshold:       0
  combination_method:   1
  track_unknown_space:  true    #true needed for disabling global path planning through unknown space
  obstacle_range: 2.5
  raytrace_range: 3.0
  origin_z: 0.0
  z_resolution: 0.2
  z_voxels: 2
  publish_voxel_map: false
  observation_sources:  scan bump
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.15
  # for debugging only, let's you see the entire voxel grid

#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  5.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.5  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true

然后分别设定global map 和local map：

global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 1.0
   publish_frequency: 0.5
   static_map: true
   transform_tolerance: 0.5
   <!-- global map引入了以下三层，经融合构成了master map，用于global planner-->
   plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

local_costmap:
   global_frame: odom
   robot_base_frame: /base_footprint
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
    4.0
   height: 4.0
   resolution: 0.05
   transform_tolerance: 0.5　／／local_planner 要求的实时性还是挺高的（特别是你把速度调高的时候），
而local_costmap 所依赖的全局坐标系一般是odom，绘制costmap的时候会反复询问odom-》base_link 坐标系的
信息，tf数据延迟要是大了会影响costmap，进而导致机器人planner实时性降低，机器人移动迟缓或者撞上障碍物。
所以有个参数transform_tolerance一定要慎重
   <!-- local map引入了以下两层，经融合构成了master map，用于局部planner-->
   plugins:
    - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

然后是planner的配置：

 /param/move_base_params.yaml" command="load" 

 /param/global_planner_params.yaml" command="load"  
 /param/navfn_global_planner_params.yaml" command="load"  

 /param/dwa_local_planner_params.yaml" command="load" 

文件move_base_params.yaml 内容：

shutdown_costmaps: false

controller_frequency: 5.0
controller_patience: 3.0


planner_frequency: 1.0
planner_patience: 5.0

oscillation_timeout: 10.0
oscillation_distance: 0.2

# local planner - default is trajectory rollout
base_local_planner: "dwa_local_planner/DWAPlannerROS"
<!--这里配置了local planner为dwa_local_planner -->
<!--这里配置了global planner为navfn/NavfnROS -->
base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner

对于global planner，可以采用以下三种实现之一：

"navfn/NavfnROS","global_planner/GlobalPlanner","carrot_planner/CarrotPlanner"

然后来看global planner的配置：

GlobalPlanner:                                  # Also see: http://wiki.ros.org/global_planner
  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true
  use_dijkstra: true                            # Use dijkstra's algorithm. Otherwise, A*, default true
  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false

  allow_unknown: true                           # Allow planner to plan through unknown space, default true
                                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
  planner_window_x: 0.0                         # default 0.0
  planner_window_y: 0.0                         # default 0.0
  default_tolerance: 0.0                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0

  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100
  planner_costmap_publish_frequency: 0.0        # default 0.0

  lethal_cost: 253                              # default 253
  neutral_cost: 50                              # default 50
  cost_factor: 3.0                              # Factor to multiply each cost from costmap by, default 3.0
  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true

对于local planner ，有以下两种选择：

"trajectory rollout","dwa_local_planner/DWAPlannerROS"

以下配置了DWAPlannerROS ：

DWAPlannerROS:

# Robot Configuration Parameters - Kobuki
  max_vel_x: 0.5  # 0.55
  min_vel_x: 0.0 

  max_vel_y: 0.0  # diff drive robot
  min_vel_y: 0.0  # diff drive robot

  max_trans_vel: 0.5 # choose slightly less than the base's capability
  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
  trans_stopped_vel: 0.1

  # Warning!
  #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
  #   are non-negligible and small in place rotational velocities will be created.

  max_rot_vel: 5.0  # choose slightly less than the base's capability
  min_rot_vel: 0.4  # this is the min angular velocity when there is negligible translational velocity
  rot_stopped_vel: 0.4

  acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 
  acc_lim_theta: 2.0
  acc_lim_y: 0.0      # diff drive robot

# Goal Tolerance Parameters
  yaw_goal_tolerance: 0.3  # 0.05
  xy_goal_tolerance: 0.15  # 0.10
  # latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 1.0       # 1.7
  vx_samples: 6       # 3
  vy_samples: 1       # diff drive robot, there is only one sample
  vtheta_samples: 20  # 20

# Trajectory Scoring Parameters
  path_distance_bias: 64.0      # 32.0   - weighting for how much it should stick to the global path plan
  goal_distance_bias: 24.0      # 24.0   - wighting for how much it should attempt to reach its goal
  occdist_scale: 0.5            # 0.01   - weighting for how much the controller should avoid obstacles
  forward_point_distance: 0.325 # 0.325  - how far along to place an additional scoring point
  stop_time_buffer: 0.2         # 0.2    - amount of time a robot must stop in before colliding for a valid traj.
  scaling_speed: 0.25           # 0.25   - absolute velocity at which to start scaling the robot's footprint
  max_scaling_factor: 0.2       # 0.2    - how much to scale the robot's footprint when at speed.

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true
  global_frame_id: odom


# Differential-drive robot configuration - necessary?
#  holonomic_robot: false

５．机器人局部避障的动态窗口法(dynamic window approach)

 

首先在V_m∩V_d的范围内采样速度：  
allowable_v = generateWindow(robotV, robotModel)  
allowable_w  = generateWindow(robotW, robotModel)  
然后根据能否及时刹车剔除不安全的速度：  
    for each v in allowable_v  
       for each w in allowable_w  
       dist = find_dist(v,w,laserscan,robotModel)  
       breakDist = calculateBreakingDistance(v)//刹车距离  
       if (dist > breakDist)  //如果能够及时刹车，该对速度可接收  
    如果这组速度可接受，接下来利用评价函数对其评价，找到最优的速度组  

来源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html  
BEGIN DWA(robotPose,robotGoal,robotModel)  
   laserscan = readScanner()  
   allowable_v = generateWindow(robotV, robotModel)  
   allowable_w  = generateWindow(robotW, robotModel)  
   for each v in allowable_v  
      for each w in allowable_w  
      dist = find_dist(v,w,laserscan,robotModel)  
      breakDist = calculateBreakingDistance(v)  
      if (dist > breakDist)  //can stop in time  
         heading = hDiff(robotPose,goalPose, v,w)   
          //clearance与原论文稍不一样  
         clearance = (dist-breakDist)/(dmax - breakDist)   
         cost = costFunction(heading,clearance, abs(desired_v - v))  
         if (cost > optimal)  
            best_v = v  
            best_w = w  
            optimal = cost  
    set robot trajectory to best_v, best_w  
END  

那么如何得到（v,w）呢？见：航迹推演（Odometry）