This document discusses Hector SLAM, a SLAM system developed by Team Hector for robust mapping in harsh USAR environments. Hector SLAM uses laser scan matching and multi-level grid maps to estimate a robot's 2D pose in real-time. It outputs maps and the robot's trajectory as tf transforms. Tools include a trajectory server, map server and Geotiff node to save maps for rescue competitions. An elevation mapping approach is also presented to generate 2D elevation maps from 3D point clouds.
This document discusses Hector SLAM, a SLAM system developed by Team Hector for robust mapping in harsh USAR environments. Hector SLAM uses laser scan matching and multi-level grid maps to estimate a robot's 2D pose in real-time. It outputs maps and the robot's trajectory as tf transforms. Tools include a trajectory server, map server and Geotiff node to save maps for rescue competitions. An elevation mapping approach is also presented to generate 2D elevation maps from 3D point clouds.
This document discusses Hector SLAM, a SLAM system developed by Team Hector for robust mapping in harsh USAR environments. Hector SLAM uses laser scan matching and multi-level grid maps to estimate a robot's 2D pose in real-time. It outputs maps and the robot's trajectory as tf transforms. Tools include a trajectory server, map server and Geotiff node to save maps for rescue competitions. An elevation mapping approach is also presented to generate 2D elevation maps from 3D point clouds.
This document discusses Hector SLAM, a SLAM system developed by Team Hector for robust mapping in harsh USAR environments. Hector SLAM uses laser scan matching and multi-level grid maps to estimate a robot's 2D pose in real-time. It outputs maps and the robot's trajectory as tf transforms. Tools include a trajectory server, map server and Geotiff node to save maps for rescue competitions. An elevation mapping approach is also presented to generate 2D elevation maps from 3D point clouds.
USAR environments ROS RoboCup Rescue Summer School Graz 2012 Stefan Kohlbrecher (with Johannes Meyer, Karen Petersen, Thorsten Graber)
Outline Introduction Team Hector Requirements Hector Mapping Overview Attitude Estimation 2D SLAM Hector SLAM Tools GeoTiff Trajectory Server Map Server Hector Elevation Mapping Examples Conclusion
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Background Team Hector is part of the RTG1362: Cooperative, Adaptive and Responsive Monitoring in Mixed Mode Environments
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Example Monitoring in Normal Operation
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Example Some Monitoring Elements and Channels Knocked-Out
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Motivation Deployment of Additional Equipment (Robots, Sensors)
Motivates fundamental research
questions being addressed by our RTG
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Team Hector Hector: Heterogeneous Cooperating Team of Robots Established in Fall 2008
Transition from RoboFrame to ROS as the central middleware since late
2010 You can also like/follow us on facebook: http://www.facebook.com/TeamHectorDarmstadt ;)
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Team Hector 1st place in the European Micro Air 3rd place (out of 12 & 16 teams) at the Vehicle Conference (EMAV) in category SICK Companys Robot Day, October 2009 Outdoor Autonomy, Sep. 2009, Delft & 2010, Waldkirch
2nd place (out of 27) Best in Class
Autonomy at RoboCup 2010, Singapore Winner and Best in Class Autonomy at Robocup 2011 GermanOpen 2nd place Best in Class Autonomy at RoboCup 2011, Istanbul Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Requirements SLAM in USAR
environments SLAM Map the environment Localize Robot Realtime capable Harsh Terrain Full 6DOF pose estimation Cannot rely on (wheel/drivetrain) odometry Robustness Primary Mission is search for victims/exploration Mapping/Localization should not interfere Georeferenced Map with robot trajectory and objects of interest Saving GeoTiff maps Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Requirements - Related Work
Gmapping (2D SLAM, Rao-Blackwellized particle filter) Odometry needed (can be simulated through separate scanmatcher) Robot pose and map data jump relative to map frame Does not leverage high scan rates of modern LIDARs 6D SLAM Requires acquisition of 3D Laser Scans of the environment Impairs mobility of platform
Sensitive to correct parameter settings
Visual SLAM Not robust enough High CPU load Promising improvements lately Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
10
Hector Mapping - System Overview
Highlevel Control Perception
Localization and Mapping
Navigation Drivers
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
11
Hector Mapping - System Overview
Drivers
Controller
Task Execution
Mission Modeling
HRI
Elevation Mapping Perception
Path Planning
Localization and Mapping
Navigation
Exploration
Highlevel Control
Costmap
2D Mapping
Object Tracking
Object Detection
Pose Estimation
Motors / Sensors
Laser
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Kinect
Cameras
12
Hector Mapping - System Overview
Highlevel Control
Costmap
Task Execution
Mission Modeling
HRI
Exploration
Drivers
Controller
Elevation Mapping Perception
Path Planning
Localization and Mapping
Navigation
This Talk
2D Mapping
Object Tracking
Object Detection
Pose Estimation
Motors / Sensors
Laser
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Kinect
Cameras
13
Hector Mapping Recent Results
Final Mission RoboCup 2012 4 Victims found (3 autonomous, 1 teleop at the end) 35 QR codes detected and mapped >95% of Arena mapped Not a single broken map during missions at RoboCup 2012
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
14
Hector Mapping Recent Results
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
15
Hector Mapping Recent Results
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
16
Hector Mapping ROS API
Main SLAM node: hector_mapping Main Inputs: Scan data on the /scan topic Transformation data via tf (see next slides) Main Outputs: Map on the /map topic tf map -> odom transform (yielding pose of the robot in the map)
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
17
Hector Mapping Coordinate frames
/odom frame not needed, mainly for compatibility with gmapping/ROS /base_stabilized frame needed for transformation of LIDAR data Height estimation non-trivial See also Setup Tutorial
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
18
Hector Mapping Attitude Estimation
Roll/Pitch Estimation of platform/LIDAR required Use IMU for attitude estimation We provide the hector_imu_attitude_to_tf node Provides base_stabilized -> base_link transform
LIDAR not rigidly mounted to base_stabilized frame should be transparent
provided correct robot/tf setup Best results if LIDAR is actuated and kept approximately level
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
19
Hector Mapping Map Representation
Map is represented by a 2D grid holding the (log odds representation of the) probability Pxy of cell occupancy Access map data on non-integer coordinates using bilinear filtering Only approximative But fast (Trilinear would be other option) Cache recently accessed grid points
x Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
20
Hector Mapping One Iteration
Receive scan from LIDAR Transform scan endpoints into /base_stabilized frame Throw out endpoints outside cut-offs: laser_z_min_value laser_z_max_value laser_min_dist laser_max_dist
Perform 2D pose estimation (next slides)
Update map if robot is estimated to have travelled more than thresholds indicated by map_update_distance_thresh map_update_angle_thresh
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
21
Hector SLAM 2D Pose Estimation (1)
Set pose estimate, either: Pose from preceding iteration Pose using a tf start estimate Iterate: 1. Project endpoints onto map based on current pose estimate 2. Estimate map occupancy probability gradients at scan endpoints 3. Perform Gauss-Newton iteration to refine pose estimate
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Reference: S. Kohlbrecher and J. Meyer and O. von Stryk and U. Klingauf : A Flexible and Scalable SLAM System with Full 3D Motion Estimation. IEEE International Symposium on Safety, Security and Rescue Robotics 2011
22
Hector SLAM 2D Pose Estimation (2)
Gradient-based Optimization can get stuck in local minima Solution: Use multi-level map representation Every level updated separately at map update step using scan data No costly downsampling operations between maps anywhere
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
23
Hector SLAM Tools Trajectory Server
hector_trajectory_server Logs trajectory data based on tf Makes Data available as nav_msgs::path via Regularly published topic Service Currently used for Visualization of (travelled) robot path GeoTiff node Can be used log and visualize arbitrary tf based trajectories
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
24
Hector SLAM Tools Geotiff Node
hector_geotiff Provides RC Rescue League compliant GeoTiff maps Trigger for saving the map Regular Intervals On Request (topic)
Runs completely onboard
Uses ROS services to retrieve Map Travelled path Objects of interest can be added using pluginlib based plugins
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
25
Hector SLAM Tools Map Server
map_server Retrieves map data via topic Makes services for map queries available Used on our USAR robot for doing raycasting to project obstacles onto nearest walls Already using 3D query so switching to other server type (for example OctoMap based) is straightforward. # Returns the distance to the next obstacle from the origin of frame point.header.frame_id # in the direction of the point # # All units are meters. geometry_msgs/PointStamped point --float32 distance
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
27
Hector Elevation Mapping
hector_elevation_mapping Motivation: Elevation map is mandatory for ground robots For detection of
stairs ramps step fields
Proposed Map Representation:
Two-dimensional array (x,y) Height value (h) Variance (2) Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
3D Point Clouds
2D Elevation Map
28
Hector Elevation Mapping
Kalman Filter based Approach: h(t )
1 2 2
h ( t
1 )
z ( t ) h ( t 1) m(t ) 2 2 z (t ) h (t 1)
h2(t )
z2(t ) h2(t 1) 2 z (t ) h2(t 1)
More precisely: z (t )
h(t ) h(t 1)
1 2 m2 h(t 1) h2(t 1) m(t ) 2 m h (t 1)
h2(t )
z2(t )
h2(t 1) 2 2 z ( t ) h (t 1) 2 2 h ( t 1) z (t )
where dm denotes the Mahalanobis distance: dm
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
z (t ) h(t 1)2 h2(t )
if z (t ) h(t 1) dm c if z (t ) h(t 1) dm c else Reference: A. Kleiner and C. Dornhege: Real-time Localization and Elevation Mapping within Urban Search and Rescue Scenarios. Journal of Field Robotics, 2007.
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
30
Examples - Handheld Mapping System
Integration of our SLAM system in a small hand-held device Intel Atom processor Same hardware as on our quadrotor UAV Optional connections to GPS receiver, Magnetometer, Barometer for airborne application
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
31
Examples - Handheld Mapping System
RoboCup 2011 Handheld Mapping System dataset Small Box with Hokuyo UTM-30LX LIDAR Low Cost (<100$) IMU Atom Z530 1.6 GHz board Dataset available at our GoogleCode repository Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
32
Examples - Handheld Mapping System
Used to collect data about Wireless Sensor Network (WSN) Five minute walk gave 12787 localized WSN signal strength samples Much less cumbersome than manual annotation using a floor plan
Reference: P. M. Scholl and S. Kohlbrecher and V. Sachidananda and K. van Laerhoven : Fast Indoor Radio-Map Building for RSSIbased Localization Systems. International Conference on Networked Sensing Systems 2012
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
33
Examples- Unmanned Surface Vehicle
SLAM System mounted on USV (Unmanned Surface Vehicle) Self-Contained, no interconnection with USV (apart from power supply)
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
34
Conclusion Hector SLAM: Robust SLAM in simulated USAR environments Accurate enough to not need explicit loop closure in many real world environments Lets you focus on your other research topics (path planning, high level planning etc.) Relies on high update rate laser scanners, does not need odometry People at Google Munich looking into combining Hector Mapping and Octomap for real 3D support (Repo here)
Team Hector Darmstadt | ROS Workshop Graz| 13/09/2011
35
Appendix: Localization and Mapping
Inertial Navigation System Estimation of the full 3D state (position, orientation, velocity) of the robot from different sensor sources: Inertial Measurement Unit (IMU) Compass (Magnetic Field) Global Satellite Navigation Altimeter, Range Sensors etc.
Attitude and Heading Reference System
(AHRS)
Xsens MTi
Problems: Absolute position is not very accurate or not available at all Solution suffers from drift Acceleration can lead to significant orientation errors Strapdown inertial navigation algorithm
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
36
Appendix: Localization and Mapping
Combine SLAM and EKF
SLAM System
Our approach: Couple both localization approaches in a loose manner
LIDAR
Scan Matcher
Orientation, Velocity, Turn rate
Position Feedback Inertial Navigation
Map Representation
Map
Soft Real-Time Hard Real-Time
IMU Odometry
Navigation Filter Position, Velocity, Orientation
GPS
Pose
Compass Plant
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Controller 37
Appendix: Localization and Mapping
Navigation Filter Sensor information is fused using an Extended Kalman Filter (EKF) State Vector
Orientation
Velocity Position
Acceleration error
Angular rate error
System Input (= Inertial Measurements)
Angular rates
Team Hector Darmstadt | ROS Workshop Koblenz | 13/09/2011
Accelerations
38
Appendix: Localization and Mapping
Integration Pose Update from SLAM: Pose estimates from EKF and SLAM have unknown correlation! Solution: Covariance Intersection (CI) approach [5]
with Estimated state and covariance (a-priori): Scan Matcher pose and covariance: Observation matrix Tuning parameter
Hector SLAM Tutorial | Stefan Kohlbrecher | ROS Workshop Graz | 22/08/2012
Instant Access to Beginning Robotics with Raspberry Pi and Arduino: Using Python and OpenCV - PDFDrive.com 1st Edition Jeff Cicolani ebook Full Chapters
Instant Access to Beginning Robotics with Raspberry Pi and Arduino: Using Python and OpenCV - PDFDrive.com 1st Edition Jeff Cicolani ebook Full Chapters
