CS226 Statistical Techniques In Robotics Monte Carlo Localization Sebastian Thrun (Instructor) and Josh Bao (TA) http://robots.stanford.edu/cs226 Office: Gates 154, Office hours: Monday 1:30-3pm sebastian thrun, CMU, 2000 1 x = state t = time m = map
z = measurement u = control Bayes Filters p( xt | z0t , u0t , m) Bayes p( zt | xt , z0t 1 , u0t , m) p( xt | z0t 1 , u0t , m) Markov p( zt | xt , m) p( xt | z0t 1 , u0t , m) p( zt | xt , m) p( xt | xt 1 , z0t 1 , u0t , m) p( xt 1 | z0t 1 , u0t , m) dxt 1
Markov p( zt | xt , m) p( xt | xt 1 , ut ) p( xt 1 | z0t 1 , u0t 1 , m) dxt 1 [Kalman 60, Rabiner 85] sebastian thrun, CMU, 2000 2 Bayes filters Linear Gaussian: Kalman filters (KFs, EKFs) Discrete: Hidden Markov Models (HMMs) With controls: Partially Observable Markov Decision Processes (POMDPs)
Fully observable with controls: Markov Decision Processes (MDPs) With graph-structured model: Dynamic Bayes networks (DBNs) sebastian thrun, CMU, 2000 3 Markov Assumption Past independent of future given current state Violated:
Unmodeled world state Inaccurate models p(x|x,u), p(z|x) Approximation errors (e.g., grid, particles) Software variables (controls arent random) sebastian thrun, CMU, 2000 4 Probabilistic Localization p(xt|xt-1,ut)
xt-1 laser data ut map m p(z|x,m) p( xt | z0t , u0t , m) p( zt | xt , m) p( xt | xt 1 , ut ) p( xt 1 | z0t 1 , u0t 1 , m) dxt 1 sebastian thrun, CMU, 2000
5 What is the Right Representation? Multi-hypothesis Kalman filter [Schiele et al. 94], [Wei et al. 94], [Borenstein 96], [Gutmann et al. 96, 98], [Arras 98] [Weckesser et al. 98], [Jensfelt et al. 99] Histograms (metric, topological)
Particles [Nourbakhsh et al. 95], [Simmons et al. 95], [Kaelbling et al. 96], [Burgard et al. 96], [Konolige et al. 99] sebastian thrun, CMU, 2000 [Kanazawa et al 95] [de Freitas 98] [Isard/Blake 98] [Doucet 98] 6 Particle Filters
sebastian thrun, CMU, 2000 7 Particle Filter Represent p(xt| d0..t ,m) by set of weighted particles {x(i)t,w(i)t} p( xt | d 0t , m) p( zt | xt , m) p( xt | xt 1 , ut 1 ) p( xt 1 | d 0t 1 , m) dxt 1 draw x(i)t1 from p(xt-1|d0..t1,m) draw x(i)t from p(xt | x(i)t1,ut1,m) Importance factor for x(i)t: wt( i )
target distributi on proposal distributi on p ( zt | xt( i ) , m) sebastian thrun, CMU, 2000 8 Monte Carlo Localization (MCL) p( xt | z0...t , u0...t ) p( zt | xt ) p( xt | ut , xt 1 ) p( xt 1 | z0...t 1 , u0...t 1 ) dxt 1 sebastian thrun, CMU, 2000
9 Monte Carlo Localization (MCL) Take i-th sample xt[i ]1 X t 1 Guess next pose xt ~ p( xt | ut , xt[i ]1 ) Calculate Importance Weights
wt p ( zt | xt[i ] ) [i ] Resample [i ] p ( xt[i ] ) St wt sebastian thrun, CMU, 2000 [i ]
10 Monte Carlo Localization sebastian thrun, CMU, 2000 11 Sample Approximations sebastian thrun, CMU, 2000 12
Monte Carlo Localization, contd sebastian thrun, CMU, 2000 13 Performance Comparison Markov localization (grids) Monte Carlo localization sebastian thrun, CMU, 2000 14
What Can Go Wrong? Model limitations/false assumptions Map false, robot outside map Independence assumption in sensor measurement noise Robot goes through wall Presence of people Kidnapped robot problem Approximation (Samples)
Small number of samples (eg, n=1) ignores measurements Perfect sensors Resampling without robot motion Room full of chairs (discontinuities) sebastian thrun, CMU, 2000 15 Localization in Cluttered Environments
sebastian thrun, CMU, 2000 16 Kidnapped Robot Problem sebastian thrun, CMU, 2000 17 Probabilistic Kinematics map m
sebastian thrun, CMU, 2000 18 Pitfall: The World is not Markov! p( zt is short ) p( z | x , m) dz t ? p( xt | u1...t , z1...t 1 ) dxt 0.99
zt z sebastian thrun, CMU, 2000 19 error (in cm) Error as Function of Sensor Noise 1,000 samples sensor noise level (in %)
sebastian thrun, CMU, 2000 20 Error as Function of Sensor Noise error (in cm) dual MCL mixed
sensor noise level (in %) sebastian thrun, CMU, 2000 21 Avoiding Collisions with Invisible Hazards Raw sensors Virtual sensors added p ( zt a ) I
raytrace(zt , m ) a p( xt | u1..t , z1..t 1 )dxt a * sup p( zt a ) 0.99 a sebastian thrun, CMU, 2000 22