Building Augmented Elevation Maps with a Tarantula Robot Rescue Robotics Camp - Rome 2006

1. Building Augmented Elevation Maps with a Tarantula RobotRescue Robotics Camp - Rome 2006 • Motivation • PART I: Rescue Robots Freiburg • Please have a look to the other talks of mine under: http://www.informatik.uni-freiburg.de/~kleiner/talks.html • PART II: Details on the practical • Module Structure • IPC • Elevation maps

2. Details on the practicalIntroduction • The goal of this year's tutorial is to enable the Tarantula platform for building elevation maps from rough terrain, as typically found within disaster areas. Rescue Robotics and the RoboCup Rescue Challenge

3. Details on the practicalProvided Hardware & Software • All experiments will be carried out on a Tarantula robot „Lurker“, equipped with: • Inertial Measurement Unit (IMU) • 2 Laser Range Finders (LRFs), one pointing parallel to the ground, and one with a freely adjustable pitch angle (e.g. pointing downwards or upwards). • The Robot software is organized by modules (executables) communicating via Inter Process Communication (Simmons and James, 2001) • Based on this communication framework, we provide the following binary modules: • SCAN: a module for sending 2D and 3D scans from both sensors • INERTIA: a module sending the three Euler angles yaw, roll and pitch from the IMU • JOYSTICK: a module sending velocity commands from a joystick • WEBCAM: a module sending data from the connected cameras • CONTROLLER: a module for controlling the robot, e.g. to set the translational and rotational velocity • ROBOGUI: a module for teleoperating the robot • LOC: a localization module that sends the robot's global pose (x, y, ), estimated from LRF readings and Odometry • RECORDER: a module for recording log files • READER: a module for playing log files Rescue Robotics and the RoboCup Rescue Challenge

4. System overviewInterProcess Communication (IPC) Behaviors ( A* Planning & Climbing behaviors) Victim detection Controller Commands Actions Sensors Targets Commands Detected! Exploration ( Frontier Cell-based) Incidence Commander Other Robots SLAM RoboGUI Map Sensors HUMAN Rescue Robotics and the RoboCup Rescue Challenge

5. Details on the practicalExisting Module Structure LOC SCAN INERTIA JOYSTICK ROBOGUI CONTROLLER WEBCAM RECORDER / READER ROBOT Rescue Robotics and the RoboCup Rescue Challenge

6. Details on the practicalNew modules (this year’s assignment) • ELEVATION MAPPER: A module for estimating height values of an elevation map by a Kalman Filter-based integration of data from the modules INERTIA, SCAN and LOC • HEIGHT ESTIMATOR: A module for estimating the robot's current height by a Kalman Filter-based integration of data from INERTA and LOC • ELEVATION VIEWER: A module for displaying the elevation map computed by the ELEVATION MAPPER and the robot's pose calculated by the modules HEIGHT ESTIMATOR and LOC Rescue Robotics and the RoboCup Rescue Challenge

7. Details on the practicalNew modules structure (this year’s assignment) ELEVATION VIEWER ELEVATION MAPPER HEIGHT ESTIMATOR LOC SCAN INERTIA Rescue Robotics and the RoboCup Rescue Challenge

8. Details on the practicalIPC Communication • Inter Process Communication (IPC): • Developed by Reid Simmons at the Carnegie Mellon University (Simmons and James, 2001) • Platform-independent package for distributed network based message passing • Facilities for both publish/subscribe and client/server type communications • It can efficiently pass complicated data structures between different machines • IPC can run in either centralized-routed mode or direct point-to-point mode • With centralized routing, message traffic can be logged • Tools available for visualizing and analyzing the message traffic Rescue Robotics and the RoboCup Rescue Challenge

9. Details on the practicalIPC Communication • Communication between two modules within three steps: • Define your data structure, e.g.: #define RESCUE_TILTED_RANGES_NAME "rescue_tilted_ranges"#define RESCUE_TILTED_RANGES_FMT "{int, int, <double:2>,<double:2>," ROBOT_BASE_FMT "}" typedef struct { int id; // ID of this scan int nranges; // Number of ranges double *ranges; // Range double *angles; // Angle (in deg) robot_base_message robot; // Def. see above} rescue_tilted_ranges_message; • Publish data on the sender side: ComPublishToRobot(RESCUE_TILTED_RANGES_NAME, &msg); • Receive data on the receiver side with a message handler: void HeightMapper::update(const rescue_tilted_ranges_message & msg){ ...}DEFINE_CLASS_HANDLER(msgHandlerTiltedRanges, HeightMapper, rescue_tilted_ranges_message); ComSubscribeToRobot(RESCUE_TILTED_RANGES_NAME, HeightMapper::msgHandlerTiltedRanges, this); Rescue Robotics and the RoboCup Rescue Challenge

10. Details on the practicalModule interface • Base message, defining the sender ID and timestamp #define ROBOT_BASE_NAME "robot_base"#define ROBOT_BASE_FMT "{int,{long,long}}"typedef struct { int id; timeval ts;} robot_base_message; • Global pose calculated by the LOC module #define RESCUE_KALMAN_POSE_NAME "rescue_kalman_pose"#define RESCUE_KALMAN_POSE_FMT "{int, double, double, double, double, double, [double:3,3]," ROBOT_BASE_FMT "}"typedef struct{ int type; // see RESCUE_POSITION_TYPE double posX; // in mm double posZ; // in mm double posTh; // in DEG double transVel; // in mm/s double rotVel; // in DEG/sec double sigma[3][3];robot_base_message robot;}rescue_kalman_pose_message; • … Rescue Robotics and the RoboCup Rescue Challenge

11. Details on the practicalElevation Map representation m n Rescue Robotics and the RoboCup Rescue Challenge

12. Details on the practicalKalman Filter Update Kalman update of height h and variance σ2 from observation z: Problem with vertical structures: Rescue Robotics and the RoboCup Rescue Challenge

13. Details on the practicalKalman Filter Update Solution: Data fusion depending on Mahalanobis distance: Rescue Robotics and the RoboCup Rescue Challenge

14. Details on the practical Calculation of z,σz2 α dx z` z d Linearization with Taylor expansion: Rescue Robotics and the RoboCup Rescue Challenge

15. Details on the practical Variance update from robot motion Problem: Localization error grows incrementally with length of robot trajectory T=t T=k Solution: Update variance according to traveled distance I(t) t Rescue Robotics and the RoboCup Rescue Challenge

16. Details on the practical Map filtering with a convolution kernel Problem: “Mixed pixels” and “surface holes” Solution: Map filtering with a convolution kernel ¼ ½ ¼ 1 ½ ½ ½ ¼ ¼ Rescue Robotics and the RoboCup Rescue Challenge

17. And what about map augmentation?That’s Raymond’s job! Rescue Robotics and the RoboCup Rescue Challenge