Autonomous Intelligent Robotics Instructor: Shiqi Zhang - PowerPoint PPT Presentation

Spring 2017 CIS 493, EEC 492, EEC 592: Autonomous Intelligent Robotics Instructor: Shiqi Zhang http://eecs.csuohio.edu/~szhang/teaching/17spring/

● About Assignment 2 ● About Proposal Draft (Due Feb 20, 5 PM ) ● Teaming: it’s your responsibility to convince people that you can contribute to the project ● Grading will be based on your proposal draft, proposal and final report ● Grading will be SUBJECTIVE ● Proposal presentation on March 9 ● Peer review

The SLAM Problem  SLAM stands for simultaneous localization and mapping  The task of building a map while estimating the pose of the robot relative to this map  Why is SLAM hard? Chicken and egg problem: a map is needed to localize the robot and a pose estimate is needed to build a map Slides adapted from Probabilistic Robotics book 3

The SLAM Problem A robot moving though an unknown, static environment Given: – The robot’s controls – Observations of nearby features Estimate: – Map of features – Path of the robot 4

Why is SLAM a hard problem? SLAM : robot path and map are both unknown! Robot path error correlates errors in the map 5

Why is SLAM a hard problem? Robot pose uncertainty ● In the real world, the mapping between observations and landmarks is unknown ● Picking wrong data associations can have catastrophic consequences ● Pose error correlates data associations 6

Data Association Problem ● A data association is an assignment of observations to landmarks ● In general there are more than (n observations, m landmarks) possible associations ● Also called “assignment problem” 7

Particle Filters  Represent belief by random samples  Estimation of non-Gaussian, nonlinear processes  Sampling Importance Resampling (SIR) principle  Draw the new generation of particles  Assign an importance weight to each particle  Resampling  T ypical application scenarios are tracking, localization, … 8

Localization vs. SLAM  A particle fjlter can be used to solve both problems  Localization: state space  x, y,   SLAM: state space  x, y,  , map   for landmark maps =  l 1 , l 2 , …, l m   for grid maps =  c 11 , c 12 , …, c 1n , c 21 , …, c nm   Problem: The number of particles needed to represent a posterior grows exponentially with the dimension of the state space! 9

Dependencies  Is there a dependency between the dimensions of the state space?  If so, can we use the dependency to solve the problem more effjciently? 10

Dependencies  Is there a dependency between the dimensions of the state space?  If so, can we use the dependency to solve the problem more effjciently?  In the SLAM context  The map depends on the poses of the robot.  We know how to build a map given the position of the sensor is known. 11

Factored Posterior (Landmarks) poses map observations & movements SLAM posterior Robot path posterior landmark positions Does this help to solve the problem? 12 Factorization first introduced by Murphy in 1999

Factored Posterior (Landmarks) poses map observations & movements 13 Factorization first introduced by Murphy in 1999

Mapping using Landmarks Landmark 1 l 1 z 1 z 3 observations . . . Robot poses x 0 x 1 x 2 x 3 x t controls u 1 u 1 u t-1 u 0 z 2 z t Landmark 2 l 2 Knowledge of the robot’s true path renders landmark positions conditionally independent 14

Factored Posterior Robot path posterior Conditionally (localization problem) independent landmark positions 15

Rao-Blackwellization  This factorization is also called Rao-Blackwellization  Given that the second term can be computed effjciently, particle fjltering becomes possible! 16

FastSLAM  Rao-Blackwellized particle fjltering based on landmarks [Montemerlo et al., 2002]  Each landmark is represented by a 2x2 Extended Kalman Filter (EKF)  Each particle therefore has to maintain M EKFs Particle … x, y,  Landmark 1 Landmark 2 Landmark M #1 Particle x, y,  Landmark 1 Landmark 2 … Landmark M #2 … Particle x, y,  Landmark 1 Landmark 2 Landmark M … 17 N

FastSLAM – Action Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3 18

FastSLAM – Sensor Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3 19

FastSLAM – Sensor Update Particle #1 Weight = 0.8 Particle #2 Weight = 0.4 Particle #3 Weight = 0.1 20

FastSLAM - Video 21 https://youtu.be/KqGXoaLGm08

Data Association Problem  Which observation belongs to which landmark? ● A robust SLAM must consider possible data associations ● Potential data associations depend also on the pose of the robot 22

Multi-Hypothesis Data Association ● Data association is done on a per-particle basis ● Robot pose error is factored out of data association decisions 23

Per-Particle Data Association Was the observation generated by the red or the blue landmark? P(observation|red) = 0.3 P(observation|blue) = 0.7  T wo options for per-particle data association  Pick the most probable match  Pick an random association weighted by the observation likelihoods  If the probability is too low, generate a new landmark 24

Results – Victoria Park ● 4 km traverse ● < 5 m RMS position error ● 100 particles Blue = GPS Yellow = FastSLAM 25 Dataset courtesy of University of Sydney

Results – Victoria Park 26 Dataset courtesy of University of Sydney

Grid-based SLAM  Can we solve the SLAM problem if no pre-defjned landmarks are available?  Can we use the ideas of FastSLAM to build grid maps?  As with landmarks, the map depends on the poses of the robot during data acquisition  If the poses are known, grid-based mapping is easy (“mapping with known poses”) 27

Mapping using Raw Odometry https://youtu.be/tilcwBVO4MY 28

Rao-Blackwellized Mapping  Each particle represents a possible trajectory of the robot  Each particle  maintains its own map and  updates it upon “mapping with known poses”  Each particle survives with a probability proportional to the likelihood of the observations relative to its own map 29

Particle Filter Example 3 particles map of particle 3 map of particle 1 30 map of particle 2

Problem  Each map is quite big in case of grid maps  Since each particle maintains its own map  Therefore, one needs to keep the number of particles small  Solution : Compute better proposal distributions!  Idea : Improve the pose estimate before applying the particle fjlter 31

Pose Correction Using Scan Matching Maximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map   ˆ ˆ ˆ   x arg max p ( z | x , m ) p ( x | u , x )    t t t t 1 t t 1 t 1 x t current measurement robot motion map constructed so far 32

Motion Model for Scan Matching Raw Odometry Scan Matching 33 https://youtu.be/sIMM73Was74

Conclusion ● The ideas of FastSLAM can also be applied in the context of grid maps ● Utilizing accurate sensor observation leads to good proposals and highly efficient filters ● It is similar to scan-matching on a per-particle base ● The number of necessary particles and re-sampling steps can seriously be reduced ● Improved versions of grid-based FastSLAM can handle larger environments than naïve implementations in “real time” since they need one order of magnitude fewer samples 34

Demo on a real robot 35