机器人路径规划（包括行人检测及动态避障总结）（长期更新）
本篇给予实际项目，作一个总结归纳
部分参考自
（１）．http://blog.csdn.net/u013158492/article/details/50493676
（２）. http://blog.csdn.net/heyijia0327/article/details/44983551
1.wiki
主要用到的包：
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_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
构造函数
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对于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 width: 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）
