RoboCup Rescue Summer School Navigation Tutorial

Transcription

1 RoboCup Rescue Summer School 2012 Institute for Software Technology, Graz University of Technology, Austria 1

2 Literature Choset, Lynch, Hutchinson, Kantor, Burgard, Kavraki and Thrun. Principle of Robot Motion Theory, Algorithms and Implementation. MIT Press

3 Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter Fox MIT Press Literature 3

4 Motivation challenges in the real world physical laws, e.g. inertia, acceleration uncertainty, e.g. maps, observations, locomotion geometric constraints, e.g. shape of a robot dynamic environment, e.g. a moving crowd complexity, e.g. tractability of problems 4

5 ROS Navigation ROS provides a full navigation stack Solves the localization problem does global planning does local planning supports differential drive and omni-directional robots works with or without global localization and map can work with 2d and/or 3d maps and sensors drawbacks works only for flat office-like environments optimized for the Willow Garage PR2 very compact design 5

6 map server map ROS Navigation Overview movebase_simple/goal global planner global costmap acml recovery behavior local planner local costmap move_base base controller laser node 6

7 Particle-Filter Localization 7

8 Localization in ROS ROS provides a ready-to use localization stack the Adaptive Monte Carlo Localization (amcl) package laser-based localization particle filter localization (MCL) KLD sampling to control the sample size augmented MCL to recover from localization errors sensor model beam range finder model likelihood field model motion model sample-based odometry model (differential drive) sample-based odometry model (omni-directional drive) 8

9 Local Planner fff t i+1 t i 9

10 Dynamic Window Approach the approach assumes a differential drive robot moving along arcs considers aspects of the dynamic works in the velocity space (v,ω) the approach uses the actual velocity of the robot a dynamic window of reachable velocities in the next cycle given by the dynamic of the robot calculate admissible velocities allow to stop before hitting an obstacle maximize an objective function O( v, ω) = a heading( v, ω) + b velocity( v, ω) + c dist( v, ω) 10

11 Institute for Software Technology 11 Dynamic Window Approach ]}, [ ], [ ), {( t t vt v vt v v v v a a a a d ω ω ω ω ω ω + + = heading(v,ω): π- Θ Target distance(v,ω): closest obstacle velocity(v,ω): proportional to v } ), ( 2 ), ( 2 ), {( b b a v dist v v dist v v v ω ω ω ω ω =

12 Cost Maps cost maps represents cost for being at a particular cell cost maps are 2d grid maps global cost map initialized by the global map updated by sensor information map coordinate frame local cost map rolling window centered around the robot cares about local obstacles updated by sensor information odometry coordinate frame 12

13 sensor update Cost Maps changes map according to sensor data, i.e. range data marking: range readings occupies cells clearing: free cells closer than range reading representation free: cell is definitive free occupied: cell is definitive occupied unknown: don t know inflation obstacles are inflated by robot s footprint to maintain distance to obstacles 13

14 LPN Algorithm a method to find minimum cost paths based on linear programming work well on grid maps, extenion of the wave-front algorithm given path as points in sample space P={p 1,p 2,,p n } costs for traversing P navigation function represents the steepest gradient for a path P k starting at point k goal F( P) = N a path P with minimum costs k k i= 1 I( p i ) + k 1 i= 1 = min F( P P k k A( p, p ) i i+ 1 ) 14

15 LPN Algorithm initially assign 0 to goal and to all other points obstacles contribute to I(p) goal point is active point expand path to neighbors p and calculate F (p) if F (p) < F(p) update F(p) and set p as active point continue for all active points until no points remain 15

16 Configuration map server amcl move_base robot 16

17 Running Example - Stage stage is a 2d robot simulator used for a step by step practical tutorial trough ROS navigation 17

18 robot base controller Configuration - Robot run the appropriate controller of your robot receives Twist (6d velocity) messages on /cmd_vel publishes Odometry messages on /odom sensors start the appropriate services for laser scanners and/or Kinect provide the right transformations all transformations between the robot base and its sensors kinect_base_link odom base_link base_laser_link 18

19 Configuration - Map use a pre-recorded map (or generate one) maps are simple grid maps stored as image file: occupied (black), free (white), unknown (grey) start the map server provides a given map on /map or as a pull service specify map file as command line parameter plus resolution or use a configuration file image: world/willow-full.pgm resolution: 0.1 origin: [0.0, 0.0, 0.0] occupied_thresh: 0.65 free_thresh: negate: 0 19

20 Configuration - AMCL start the adaptive monte carlo localization (amcl) package automatically subscribes to map, odom, scan example configurations available: amcl_diff.launch resp. amcl_omni.launch initial pose can be easily set with rviz many parameters to shape the localization process important parameter: odom_model_type: diff or omni model parameter: odom_alpha1.. odom_alpha6 laser_model_type: beam or laser_likelihood_max_dist numer of minimal and maximal particles odom_frame_id: odom 20

21 Configuration Cost Maps configuration split in 3 separated files common parameter ranges for obstacle handling robot footprint sources for sensor inputs local cost map frame ids (odometry, robot) frequency for update and publishing static or rotating mode size global cost map frame ids (map, robot) static or dynamic 21

22 Configuration Planners configuration of the local planner depends on the kinematics of your robot important parameter velocity constraints of the robot acceleration constraints of the robot holonomic kinematics 22

23 Run it at once to run the whole thing easily combine it into one launch file <launch> <master auto="start"/> <node name="map_server" pkg="map_server" type="map_server" args="$(find navigation_summer_school)/map.yaml"/> <include file="$(find amcl)/examples/amcl_diff.launch" /> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <rosparam file="$(find navigation_summer_school)/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find navigation_summer_school)/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find navigation_summer_school)/local_costmap_params.yaml" command="load" /> <rosparam file="$(find navigation_summer_school)/global_costmap_params.yaml" command="load" /> <rosparam file="$(find navigation_summer_school)/base_local_planner_params.yaml" command="load" /> </node> </launch> 23

24 Sending Goals C++ Example move_base accepts goals on topic or by is service simply send a goal message 24

25 #include "ros/ros.h" #include "geometry_msgs/posestamped.h" int main(int argc, char **argv) { ros::init(argc, argv, "goal_publisher"); ros::nodehandle n; ros::publisher goal_pub = n.advertise<geometry_msgs::posestamped>("/move_base_simple/goal", 1); ros::rate loop_rate(1); geometry_msgs::posestamped msg; if (argc!= 3) { } ROS_INFO("Please provide x and y of the goal!"); msg.header.frame_id = "/map"; msg.pose.position.x = atol(argv[1]); msg.pose.position.y = atol(argv[2]); msg.pose.position.z = 0.0; msg.pose.orientation.x = 0.0; msg.pose.orientation.y = 0.0; msg.pose.orientation.z = 0.0; msg.pose.orientation.w = 1.0; ROS_INFO("Sending Robot to %f, %f", msg.pose.position.x, msg.pose.position.y); goal_pub.publish(msg); ros::spinonce(); loop_rate.sleep(); return 0; } 25

26 Frontier-Based Exploration very simple and popular approach by Yamauchi basis is occupancy grid with three classes of cells open: p(m[x,y])<prior probability unknown: p(m[x,y])=prior probability occupied: p(m[x,y])>prior probability find frontier cells that are at the border of open and unknown areas, an open cell is adjacent to an unknown cell group adjacent to frontier regions (blob detection region growing) select large enough regions as frontier navigate to a frontier 26

27 Example Frontier-Based Exploration 27

28 Thank you! 28