ΕΚΠ 413 / ΕΚΠ 606 Αυτόνοµοι (Ροµ οτικοί) Πράκτορες

Transcript

1 ΕΚΠ 413 / ΕΚΠ 606 Αυτόνοµοι (Ροµ οτικοί) Πράκτορες Ροµ οτική Χαρτογράφηη Robotic Mapping Τµήµα Ηλεκτρονικών Μηχανικών και Μηχανικών Υ ολογιτών Πολυτεχνείο Κρήτης

2 Ε ανάληψη Στρατηγικές MaxiMin παιχνίδια µηδενικού αθροίµατος αλγόριθµος µαθηµατικού προγραµµατιµού Παιχνίδια µη µηδενικού αθροίµατος επαναλαµβανόµενα Μαρκωβιανά αιχνίδια µοντελοποίηη χεδιαµός µάθηη Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 2

3 Robot Navigation Problems What should I remember? Mapping Where am I? Localization Where should I go? Path Planning How can I go? Motion Control Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 3

4 Σήµερα Mapping problem definition considerations Occupancy Grid Mapping known robot poses occupancy grid update SLAM unknown robot poses online SLAM full SLAM Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 4

5 Mapping What should I remember?

6 Mapping and Localization Localization estimate the robot s pose at each time step easy, when a map of the surrounding environment is known Mapping construct a map of the robot s surrounding environment easy, when the robot s pose is known at each time step Simultaneous Localization and Mapping (SLAM) robot s pose and map are both unknown simultaneously, estimate robot s pose and construct a map Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 6

7 Mapping Factors Size size of the environment relative to robot s perceptual range number of distinct landmarks in the environment Noise noise in perception (reflections, variable illumination) noise in actuation (variable friction, inaccurate odometry) Ambiguity perceptual aliasing (different places appear to be the same) Structure straight corridors are easy, cycles or loops can be really hard correspondence of landmarks Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 7

8 Desired Mapping Result Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 8

9 Mapping with Odometry Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 9

10 Types of Mapping Problems Grid-based maps [Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01; ] Landmark-based maps [Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002; ] Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 10

11 Occupancy Grid Mapping Mapping with Known Poses

12 Occupancy Grid Mapping Assumptions the true robot pose is known perception consists of range measurements Occupancy Grid introduced by Moravec and Elfes in 1985 representation as a field of random variables each variable corresponds to a unique small area (cell) variables arranged in an evenly spaced grid binary variables (free space or obstacle) Algorithm approximate posterior estimation for all random variables probability that each cell is occupied or not Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 12

13 Problem Definition d = { x, z, x, z,, x, z } n n m * = arg max P( m d ) m Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 13

14 Considerations Complexity 100 x 100 grid cells possible maps intractable to calculate the posterior for each map Assumptions map is static m = m t for all t each cell in independent from all others 100 x 100 grid binary estimation problems ignores the dependencies between neighboring cells [ xy ] ( ) (,,, ) ( ) 1 1 Bel m P m x z x z Bel m = = t t x, y Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 14

15 Occupancy Grid Mapping Algorithm Bayesian Filter Bel( m ) = η p( z m ) Bel( m [ xy] [ xy] [ xy] t t t t 1 ) Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 15

16 Log-Odds Representation ( ) B m xy xy : = log odds( m ) [ ] [ ] t odds x P ( x ) ( x ) ( ) : = 1 P t ( ) = log (, ) [ xy ] [ xy ] B m odds m z x t t t t ( ) [ ] 1 xy +B mt log odds m t ( ) xy [ ] Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 16

17 Simple Inverse Sensor Models Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 17

18 Inverse Sensor Model Algorithm Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 18

19 Modern Sensor Models Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 19

20 Occupancy Grid Example Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 20

21 Potential Problems Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 21

22 Example: Tech Museum, San Jose CAD map occupancy grid map Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 22

23 Simultaneous Localization and Mapping Mapping with Unknown Poses

24 The SLAM Problem Given robot s controls robot s observations Estimate map of the environment robot s pose at each time step Assumptions static environment, noisy controls and observations Representation landmark-based grid-based Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 24

25 SLAM Problems Full SLAM Estimates entire path and map! p( x, m z, u : t 1: t 1: 1 t ) Online SLAM p ( x, m z, u ) = p( x, m z, u ) dx dx... dx t 1: t 1: t 1: t 1: t 1: t 1 2 t 1 Integrations typically done one at a time Estimates most recent pose and map! Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 25

26 Online SLAM p ( x, m z, u ) = p( x, m z, u ) dx dx... dx t 1: t 1: t 1: t 1: t 1: t 1 2 t 1 Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 26

27 Full SLAM p( x, m z, u : t 1: t 1: 1 t ) Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 27

28 Nature of SLAM Continuous estimation robot s pose location of objects points in the continuous space Discrete estimation landmarks or object patches object correspondence relationship to previously recognized objects either the same, or different Problems high dimensionality + large number of correspondences Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 28

29 Mapping as Localization Idea mapping is like localization, but in a much larger space find the most possible (x t,m) instead of just (x t ) dimensionality: dimensions of robot pose + dimensions of map use any localization algorithm: Markov, Grid, Gaussian, Monte-Carlo Localization Bel x ) = P( x u, z, u, z ) ( t t 1 1 t t Bel ( x ) Pη ( Px zu 1, xz ) 1 P, u( x, z ) u, x ) t t t t t t t t t Mapping Bel ( x, m) = P( x, m u, z, u, z ) = Bel x dxt = ( ( ) 1 t t t t t Bel ( x, m) P( z x, m) P( x, m u, x, m) Bel ( x, m) dx dm = η t t t t t t t t Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 29

30 Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 30 map with n landmarks:(3+2n)-dimensional Gaussian tracking with an extended Kalman filter can handle a few hundreds of dimensions = , ), ( N N N N N N N N N N N l l l l l l yl xl l l l l l l yl xl l l l l l l yl xl l l l y x yl yl yl y y xy xl xl xl x xy x N t t l l l y x m Belx θ θ θ θ θ θ θ θ θ θ θ θ (E)KF-SLAM

31 Example: EKF-SLAM Map Correlation matrix Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 31

32 Example: EKF-SLAM Map Correlation matrix Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 32

33 Example: EKF-SLAM Map Correlation matrix Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 33

34 Example: KF-SLAM Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 34

35 Μελέτη Σύγγραµµα Ενότητα 25.3 Βιβλία S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, MIT Press 2005, Ch. 9, 10. Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 35