Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

1. Mapping and Exploration for Search and Rescue with Humans and Mobile Robots Dr. rer. nat. Alexander Kleiner Foundations of Artificial Intelligence Institut für Informatik University of Freiburg kleiner@informatik.uni-freiburg.de www.informatik.uni-freiburg.d e/~kleiner Dagstuhl Seminar: Algorithmic Methods for Distributed Cooperative Systems

2. MotivationThe “golden” 72 hours Courtesy S. Tadokoro Courtesy R. Murphy Tom Haus (firemen at 9/11): “We need a tracking system that tells us where we are, where we have been, and where we have to go to” Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

3. Outline • Introduction • RFID SLAM • Centralized • Decentralized (DRFID SLAM) • Multi-robot exploration • Local exploration • Global exploration • SLAM with active RFID • Conclusions Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

4. IntroductionMapping and Exploration within US&R environments • Mapping: Computing globally consistent maps from pose tracking and data association by one or multiple agents • Exploration: Efficient coverage of an unknown environment by one or multiple agents • Requirements within harsh real-world domains (e.g. US&R): • Real-time computation • Decentralized with limited radio communication • Mixed-initiative teams: Integration of robotic solutions into human organizations (e.g. first responders) • Limitations of existing solutions: • Data association problem: • Dynamic illumination conditions • Unstructured environment in 3D • “Loop closure” requirement • Cannot be guaranteed during time critical missions (e.g. victim search) Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

5. IntroductionSolutions presented in this talk • Decentralized team coordination and Simultaneous Localization And Mapping (SLAM) via local information nodes, e.g. RFIDs • World-wide unique labeling of places • No data association problem • Exchange of map pieces between teams of robots and humans • “Loop-closure” on joint maps • Information sharing via local node memories • Facilitates decentralized mapping & exploration with indirect communication • Topological node graph structure • Efficient multi-robot task assignment and path planning Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

6. Human pose tracking“Odometry” from accelerometers and IMU • Human foot steps generate vertical accelerations • We use a method from Ladetto et al. that extracts acceleration maxima for counting foot steps • Individual step length is automatically calibrated from GPS readings (if available), yielding distance estimates from steps • Orientation changesare measured by gyroscope and magnetometer • Kalman filter-based pose tracking from increments yields estimate d = (x,y,θ) with 3x3 covariance matrix Σ Acceleration patterns during walking Tracked pose (green) Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

7. Robot pose trackingSlippage-sensitive odometry Conventional odometry: covariance bound does not hold Odometry distance estimate with 3σ bound compared to ground truth computed from laser-based scan matching Slippage-sensitive odometry: Reduced distance error within valid bounds Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

8. 3 1 1 2 RFID-SLAM (Centralized)Building globally consistent maps • Estimation of inter-node displacements by pose tracking methods • Construction of joint graph G from all observations , consisting of measured distances and 3x3 covariance matrix • Loops are detected if nodes have been observed twice • Modeled by an observation edge with , where Δθ denotes the angle difference, and covariance reflecting max. detection range of the antenna • From G a globally consistent map is calculated by minimization of the Mahalanobis distance (Lu & Milios 97) : • Analogy to spring-mass: Find low energy arrangement of springs (estimates ) and connected masses (nodes): Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

9. Decentralized RFID-SLAM (DRFID-SLAM)Indirect communication via node memories • The basic idea: • To utilize the memory of nodes for learning the topology of the graph • Mobile agents are accumulating graphs Aj  Gfrom observations on their path • Nodes are learning local graphs Ri  G representing the topology of their vicinity • Agents propagate information through the network, i.e. synchronize nodes if they are in range • Information update: • When traveling from node i to node k: • Add new estimate to Rk • Update Aj from Rk by graph merging, and vice versa • Double edges: • Locally fused by adjacent nodes • ID value for each fusion for preventing doubly counting Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

10. 2 1 RFID node 2 Graph A1 1 3 Graph A2 2 1 5 3 Graph A3 3 Third agent trajectory 5 4 First agent trajectory 6 Second agent trajectory 4 DRFID-SLAM cont.Example Decentralized optimization of A3 Decentralized optimization of A2 Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

11. Results RFID-SLAMHardware Setting Robot & Human Glove for sensing RFIDs (TZI Bremen) 13.56 MHz RFIDs IMU Zerg robot RFID antenna and deploy device Test person • Robot: • 4WD (four shaft encoders) • Inertial Measurement Unit (IMU) • RFID antenna for detecting tags lying beneath the robot • RFID deploy device • RFID: • 13.56 MHz (short range below a meter) • 2048 Bit RAM, programmable by the user • Human: • IMU for step counting and angle estimation • RFID glove Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

12. RFID-SLAMAutonomous deployment of RFIDs

13. RFID-SLAM jointly by humans and robots Corrected map Robot (orange) and pedestrian (red) odometry Corrected track (green) compared to GPS ground truth (blue) • Robot driving at 1.58 m/s for 2.5 km • 10 RFIDs • Optimization time below a second Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

14. RFID-SLAM jointly by humans and robotsCovariance bounds 3σ bounds from slippage-sensitive odometry 3σ boundsafter the optimization

15. RFID-SLAM by a team of humansCentralized graph optimization in a large-scale environment Pedestrian tracks recorded in the City of Freiburg Corrected RFID graph Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

16. DRFID-SLAM (from data) Decentralized graph optimization during the outdoor exp. • Simulation of all 720 possible sequences of 6 agents exploring the environment • The more agents visited the area, the better the individual map improvements Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

17. DRFID-SLAM ExperimentDecentralized graph optimization during the outdoor exp. Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

18. RFID-based explorationHybrid: local exploration and global planning • Local exploration (LE): • Indirect communication • Scales-up with # of robots and environment size • Inefficient exploration due to local minima • Global task assignment and path planning • Based on node graph abstraction of the environment • Monitors LE and computes new agent-node assignment If exploration overlap is high • Requires communication Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

19. Local explorationNavigation and target selection • Navigation based on (limited) robot-centric grid map generated from laser ranges • Exploration targets taken from grid frontier cells [Yamauchi, 1997] • Coordination: • Automatic node deployment w. r. t. a pre-defined density • Discretization of node vicinity into equally sized patches • Node memory for counting visits of each patch [Svennebring and Koenig, 2004]) • Frontier selection by minimizing the following cost function: • lfi : frontier cell location, • LRS: set of nodes within range, • Pr: set of patches around node r, • d(.): the Euclidean distance Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

20. RFID node π Robot trajectories Discretized visited areas counted in memory Local exploration cont.Discretization of the node’s vicinity π Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

21. Global explorationTask assignment and planning • Task assignment: • Sequential robot planning to best targets [Burgard et al., 2005] • Genetic algorithm (GA) for finding optimal planning sequence • Score computed from multi-robot plan cost • Initialized by greedy sequence • Computation of multi-robot plan: • A* time space planning to multiple goals [Bennewitz et al., 2001] • Plan costs: joint plan length + conflict penalties (infinite if deadlock) • Heuristic: based on pre-computed shortest Dijkstra tree ignoring conflicts Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

22. Results Local Team CoordinationVirtual rescue scenarios from NIST (RoboCup’06) Largest explored area (by 8 robots) Each color denotes the path of a single robot Final 1 (indoor, 1276m2) Final 2 (outdoor. 1203m2) Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

23. Results Global Team CoordinationTask assignment and planning on node graph (USARSim outdoor map) Goal nodes Robot start nodes Multi-robot plan Conflicts vs. # of robots: Greedy (red), GA assignment (blue), GA sequence (green) Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

24. Results Local Team CoordinationVirtual rescue scenarios from NIST (RoboCup’06) Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

25. SLAM with active RFID Zigbee Sensor node developed in Freiburg (Dept. of Microsystems Engineering) Sensor nodes placed outdoors for experiments USARSim environment for simulated large scale experiments Robot team with 9 sensor nodes

26. SLAM with active RFIDDistance estimation from RSSI (signal strength) Relation between Transceiver-Receiver (TR) separation d and signal strength P (Seidel & Rapport 1992): Path loss at reference distance d0 Noise with variance σ However, many outliers in the data! We use the RANSAC (Random Sample Consensus) method to identify outliers. Resulting model for the utilized Zigbee modules

27. SLAM with active RFIDBearing estimation by voting grids • Omni-directional antennas provide no bearing information! • Bearing can be determined from intersections of range measurements at different robot locations (by odometry) • Each cell on the grid votes for the RFID location Observations during navigation Step-wise integration yields location estimate

28. Odometry on “Pywood” map Results from experiments on different USARSim maps RSLAM on “Plywood” map Groundtruth of “Plywood” map SLAM with active RFIDResults from USARSim experiments

29. SLAM with active RFIDResults from real world experiments Experiment 1: RSLAM Robot odometry Experiment 2:

30. Conclusions and Future Work • Robust and efficient methods for DSLAM and exploration • Limited radio communication • No requirement for direct loop closure • Local information exchange • Joint human and robot exploration • Coordination scalable in terms of communication and computation • Open Issues: • Range/Energy tradeoff of sensor nodes needs to be improved • Communication between nodes, e.g. building up a local network • Active computation on sensor nodes, e.g. for improving pose estimates • Decision for automatic node deployment Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

31. Many thanks to... • RescueRobots Freiburg team 2005 • Christian Dornhege, Johann Prediger, Rainer Kümmerle, Daniel Meyer-Delius , Michael Schnell, Mathias Luber, Tobias Bräuer • RescueRobots Freiburg team “real robots” 2006 • Christian Dornhege, Rainer Kümmerle, Bastian Steder, Michael Ruhnke • RescueRobots Freiburg Team “virtual robots” 2006 • Vittorio Ziparo • Sick Competition 2007 • Jörg Müller, Christian Dornhege • Supervisor • Prof. Dr. Bernhard Nebel • Institutions • DFG, MFG, and University of Freiburg (financial Support) Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

32. Thanks for your attention, any questions? Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

33. Robot pose trackingSlip detection from redundant wheel odometry • Wheels on the same side turn at different speeds under slip!  Measuring with 4 shaft encoders • Decision tree model for slip detection • with the wheel-velocity differences Δvleft, Δvright, Δvrear, Δvfront, as classifier input • Determination of error estimate σslip by computing the RMS error with scan matching GT Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

34. ResultsSlippage-sensitive odometry • Turning, accelerating, and straight ahead driving with and without slippage • Correct situation detection for most of the cases: Algorithmic Methods for Distributed Cooperative Systems: Mapping and Exploration for Search and Rescue with Humans and Mobile Robots

35. Rescue robots All-terrain navigating (TechX challenge) Foundations of AI @ Freiburg UniversityAutonomous Systems Table soccer Soccer robots Multi Agent Rescue Fast robots