© sebastian thrun, cmu, 20001 cs226 statistical techniques in robotics monte carlo localization...
Post on 20-Dec-2015
217 views
TRANSCRIPT
© sebastian thrun, CMU, 2000 1
CS226 Statistical Techniques In RoboticsMonte Carlo Localization
Sebastian Thrun (Instructor) and Josh Bao (TA)
http://robots.stanford.edu/cs226Office: Gates 154, Office hours: Monday 1:30-3pm
© sebastian thrun, CMU, 2000 2
Bayes Filters
),,|(),,,|( 010010 muzxpmuzxzp ttttttt Bayes
[Kalman 60, Rabiner 85]
x = statet = timem = mapz = measurementu = control
Markov),,|(),|( 010 muzxpmxzp ttttt
101010101 ),,|(),,,|(),|( tttttttttt dxmuzxpmuzxxpmxzp
Markov
1101011 ),,|(),|(),|( ttttttttt dxmuzxpuxxpmxzp
),,|( 00 muzxp ttt
© sebastian thrun, CMU, 2000 3
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 4
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 5
xt-1
ut
p(xt|xt-1,ut)
Probabilistic Localization
110101100 ),,|(),|(),|(),,|( tttttttttttt dxmuzxpuxxpmxzpmuzxp
map mlaser data p(z|x,m)
© sebastian thrun, CMU, 2000 6
What is the Right Representation?
Multi-hypothesis
[Weckesser et al. 98], [Jensfelt et al. 99]
Particles
[Kanazawa et al 95] [de Freitas 98][Isard/Blake 98] [Doucet 98]
Kalman filter
[Schiele et al. 94], [Weiß et al. 94], [Borenstein 96], [Gutmann et al. 96, 98], [Arras 98]
[Nourbakhsh et al. 95], [Simmons et al. 95], [Kaelbling et al. 96], [Burgard et al. 96], [Konolige et al. 99]
Histograms(metric, topological)
© sebastian thrun, CMU, 2000 7
Particle Filters
© sebastian thrun, CMU, 2000 8
Particle Filter
1101110 ),|(),|(),|(),|( tttttttttt dxmdxpuxxpmxzpmdxp
Represent p(xt| d0..t ,m) by set of weighted particles {x(i)t,w(i)
t}
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:
ondistributi proposal
ondistributitarget )( itw
),|( )( mxzp itt
© sebastian thrun, CMU, 2000 9
Monte Carlo Localization (MCL)
11...01...011...0...0 ),|(),|()|(),|( tttttttttttt dxuzxpxuxpxzpuzxp
© sebastian thrun, CMU, 2000 10
Monte Carlo Localization (MCL)
Take i-th sample
“Guess” next pose
Calculate Importance Weights
Resample
),|(~ ][1
][ ittt
it xuxpx
1][1 t
it Xx
)|( ][][ itt
it xzpw
][][ )( itt
it wSxp
© sebastian thrun, CMU, 2000 11
Monte Carlo Localization
© sebastian thrun, CMU, 2000 12
Sample Approximations
© sebastian thrun, CMU, 2000 13
Monte Carlo Localization, cont’d
© sebastian thrun, CMU, 2000 14
Performance Comparison
Monte Carlo localizationMarkov localization (grids)
© sebastian thrun, CMU, 2000 15
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 16
Localization in Cluttered Environments
© sebastian thrun, CMU, 2000 17
Kidnapped Robot Problem
© sebastian thrun, CMU, 2000 18
Probabilistic Kinematics
map m
© sebastian thrun, CMU, 2000 19
Pitfall: The World is not Markov!
99.0),|(),|()short is (?
1...1...1
tttt
zz
tt dxzuxpdzmxzpzpt
© sebastian thrun, CMU, 2000 20
Error as Function of Sensor Noise
sensor noise level (in %)
erro
r (in
cm
)
1,000 samples
© sebastian thrun, CMU, 2000 21
dual
mixed
MCL
Error as Function of Sensor Noise
sensor noise level (in %)
erro
r (in
cm
)
© sebastian thrun, CMU, 2000 22
Avoiding Collisions with Invisible Hazards
Raw sensors
ttttamzt dxzuxpIazpt
),|()( 1..1..1),raytrace(
99.0)(sup* azpa ta
Virtual sensors added