Transcript Probabilistic Robotics

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 | z0t , u0t , m)
Bayes
 p( zt | xt , z0t 1, u0t , m) p( xt | z0t 1, u0t , m)
Markov
 p( zt | xt , m) p( xt | z0t 1, u0t , m)
 p( zt | xt , m)  p( xt | xt 1 , z0t 1 , u0t , m) p( xt 1 | z0t 1 , u0t , m) dxt 1
 p( zt | xt , m)  p( xt | xt 1 , ut ) p( xt 1 | z0t 1 , u0t 1 , m) dxt 1
Markov
[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 aren’t random)
© sebastian thrun, CMU, 2000
4
Probabilistic Localization
p(xt|xt-1,ut)
xt-1
ut
laser data
map m
p(z|x,m)
p( xt | z0t , u0t , m)  p( zt | xt , m)  p( xt | xt 1 , ut ) p( xt 1 | z0t 1 , u0t 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 0t , m)   p( zt | xt , m)  p( xt | xt 1 , ut 1 ) p( xt 1 | d 0t 1 , m) dxt 1
draw x(i)t1 from p(xt-1|d0..t1,m)
draw x(i)t from p(xt | x(i)t1,ut1,m)
Importance factor for x(i)t:
wt(i ) 
targetdistribution
proposaldistribution
 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 ] )
 Resample
p( xt[i ] )  St  wt
[i ]
[i ]
© sebastian thrun, CMU, 2000
[i ]
10
Monte Carlo Localization
© sebastian thrun, CMU, 2000
11
Sample Approximations
© sebastian thrun, CMU, 2000
12
Monte Carlo Localization, cont’d
© 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 | xt , m) dz 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