Post on 23-Dec-2015
Probabilistic Robotics
Key idea: Explicit representation of uncertainty
(using the calculus of probability theory)
•Perception = state estimation
•Control = utility optimization
Bayes Filters: Framework
• Given:• Stream of observations z and action data u:
• Sensor model P(z|x).• Action model P(x|u,x’).• Prior probability of the system state P(x).
• Wanted: • Estimate of the state X of a dynamical system.• The posterior of the state is also called Belief:
),,,|()( 121 tttt zuzuxPxBel
},,,{ 121 ttt zuzud
Markov Assumption
Underlying Assumptions• Static world• Independent noise• Perfect model, no approximation errors
),|(),,|( 1:11:11:1 ttttttt uxxpuzxxp )|(),,|( :11:1:0 tttttt xzpuzxzp
Bayes Filters
),,,|(),,,,|( 121121 ttttt uzuxPuzuxzP Bayes
z = observationu = actionx = state
),,,|()( 121 tttt zuzuxPxBel
Markov ),,,|()|( 121 tttt uzuxPxzP
1111 )(),|()|( ttttttt dxxBelxuxPxzP
Markov1121111 ),,,|(),|()|( tttttttt dxuzuxPxuxPxzP
11211
1121
),,,|(
),,,,|()|(
ttt
ttttt
dxuzuxP
xuzuxPxzP
Total prob.
Bayes Filters are Familiar!
• Kalman filters
• Particle filters
• Hidden Markov models
• Dynamic Bayesian networks
• Partially Observable Markov Decision Processes (POMDPs)
111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel
Localization
• Given • Map of the environment.• Sequence of sensor measurements.
• Wanted• Estimate of the robot’s position.
• Problem classes• Position tracking• Global localization• Kidnapped robot problem (recovery)
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
Proximity Measurement
• Measurement can be caused by …• a known obstacle.• cross-talk.• an unexpected obstacle (people, furniture, …).• missing all obstacles (total reflection, glass, …).
• Noise is due to uncertainty …• in measuring distance to known obstacle.• in position of known obstacles.• in position of additional obstacles.• whether obstacle is missed.
Mixture Density
),|(
),|(
),|(
),|(
),|(
rand
max
unexp
hit
rand
max
unexp
hit
mxzP
mxzP
mxzP
mxzP
mxzP
T
How can we determine the model parameters?
Representations for Bayesian Robot Localization
Discrete approaches (’95)• Topological representation (’95)
• uncertainty handling (POMDPs)• occas. global localization, recovery
• Grid-based, metric representation (’96)• global localization, recovery
Multi-hypothesis (’00)• multiple Kalman filters• global localization, recovery
Particle filters (’99)• sample-based representation• global localization, recovery
Kalman filters (late-80s?)• Gaussians• approximately linear models• position tracking
AI
Robotics
Represent belief by random samples
Estimation of non-Gaussian, nonlinear processes
Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter
Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al., 95]d
Particle Filters
draw xit1 from Bel(xt1)
draw xit from p(xt | xi
t1,ut1)
Importance factor for xit:
)|()(),|(
)(),|()|(ondistributi proposal
ondistributitarget
111
111
tt
tttt
tttttt
it
xzpxBeluxxp
xBeluxxpxzp
w
1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel
Particle Filter Algorithm
• Idea: • Assume we know the true belief.
• Represent this belief as a multinomial distribution.
• Determine number of samples such that we can guarantee that, with probability (1- ), the KL-distance between the true posterior and the sample-based approximation is less than .
• Observation: • For fixed and , number of samples only depends on
number k of bins with support:
KLD-sampling
3
12
)1(9
2
)1(9
21
2
1)1,1(
2
1
zkk
kkn
•Prediction
•Correction
Bayes Filter Reminder
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
Gaussians
2
2)(
2
1
2
2
1)(
:),(~)(
x
exp
Nxp
-
Univariate
)()(2
1
2/12/
1
)2(
1)(
:)(~)(
μxΣμx
Σx
Σμx
t
ep
,Νp
d
Multivariate
Kalman Filter Updates in 1D
1)(with )(
)()(
tTtttttt
tttt
ttttttt QCCCK
CKI
CzKxbel
2,
2
2
22 with )1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
ttttt uBA 1
tTtttt RAA 1
1)( tTtttttt QCCCK
)( tttttt CzK
tttt CKI )(
Nonlinear Dynamic Systems
•Most realistic robotic problems involve nonlinear functions
),( 1 ttt xugx
)( tt xhz
EKF Algorithm
1. Extended_Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
),( 1 ttt ug
tTtttt RGG 1
1)( tTtttttt QHHHK
))(( ttttt hzK
tttt HKI )(
1
1),(
t
ttt x
ugG
t
tt x
hH
)(
ttttt uBA 1
tTtttt RAA 1
1)( tTtttttt QCCCK
)( tttttt CzK
tttt CKI )(
EKF Summary
•Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
•Not optimal!•Can diverge if nonlinearities are large!•Works surprisingly well even when all
assumptions are violated!
Unscented Transform
nin
wwn
nw
nw
ic
imi
i
cm
2,...,1for )(2
1 )(
)1( 2000
Sigma points Weights
)( ii g
n
i
Tiiic
n
i
iim
w
w
2
0
2
0
))(('
'
Pass sigma points through nonlinear function
Recover mean and covariance
UKF Summary
•Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications
•Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term)
•Derivative-free: No Jacobians needed
•Still not optimal!
SLAM: Simultaneous Localization and Mapping
•Full SLAM:
•Online SLAM:
Integrations typically done one at a time
121:1:1:1:1:1 ,...,,),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
),|,( :1:1:1 ttt uzmxp
• Map with N landmarks:(2N+3)-dimensional Gaussian
• Can handle hundreds of dimensions
2
2
2
2
2
2
2
1
21
21
21
21
2222221
1111211
,),(
yxlll
yyxyylylyl
xxyxxlxlxl
lylxllllll
lylxllllll
lylxllllll
Ntt
N
N
N
NNNNNN
N
N
y
x
l
l
l
mxBel
SLAM: Mapping with Kalman Filters
Graph-SLAM
•Full SLAM technique
•Generates probabilistic links
•Computes map only occasionally
•Based on Information Filter form
Robot Poses and Scans [Lu and Milios 1997]
• Successive robot poses connected by odometry
• Sensor readings yield constraints between poses
• Constraints represented by Gaussians
• Globally optimal estimate
ijijij QDD
ijijij QDD
Loop Closure
Before loop closure After loop closure
• Use scan patches to detect loop closure
• Add new position constraints
• Deform the network based on covariances of matches
Efficient Map Recovery
•Minimize constraint function JGraphSLAM using standard optimization techniques (gradient descent, Levenberg Marquardt, conjugate gradient)
Rao-Blackwellized Mapping
),|,( 1:0:1:1 ttt uzmxp
),|(),,|( 1:0:1:11:0:1:1 tttttt uzxpuzxmp
Compute a posterior over the map and possible trajectories of the robot :
robot motionmap trajectory
map and trajectory
measurements
FastSLAM
Robot Pose 2 x 2 Kalman Filters
Landmark 1 Landmark 2 Landmark N…x, y,
Landmark 1 Landmark 2 Landmark N…x, y, Particle#1
Landmark 1 Landmark 2 Landmark N…x, y, Particle#2
Landmark 1 Landmark 2 Landmark N…x, y, Particle#3
ParticleM
…
[Begin courtesy of Mike Montemerlo]
FastSLAM – Simulation
• Up to 100,000 landmarks
• 100 particles
• 103 times fewer parameters than EKF SLAM
Blue line = true robot pathRed line = estimated robot pathBlack dashed line = odometry
Victoria Park Results
• 4 km traverse
• 100 particles
• Uses negative evidence to remove spurious landmarks
Blue path = odometryRed path = estimated path
[End courtesy of Mike Montemerlo]
Motion Model for Scan Matching
'
'
d'
final pose
d
measured pose
initial pose
path
Raw OdometryScan Matching
Example (Intel Lab)
15 particles
four times faster than real-timeP4, 2.8GHz
5cm resolution during scan matching
1cm resolution in final map
joint work with Giorgio Grisetti
Outdoor Campus Map
30 particles
250x250m2
1.75 km (odometry)
20cm resolution during scan matching
30cm resolution in final map
joint work with Giorgio Grisetti
30 particles
250x250m2
1.088 miles (odometry)
20cm resolution during scan matching
30cm resolution in final map
Fast-SLAM Summary
• Full and online version of SLAM
• Factorizes posterior into robot trajectories (particles) and map (EKFs).
• Landmark locations are independent!
• More efficient proposal distribution through Kalman filter prediction
• Data association per particle
Ball Tracking in RoboCup
Extremely noisy (nonlinear) motion of observer
Inaccurate sensing, limited processing power
Interactions between target and environment
Interactions between robot(s) and targetGoal: Unified framework for modeling
the ball and its interactions.
Tracking Techniques
Kalman Filter Highly efficient, robust (even for nonlinear) Uni-modal, limited handling of nonlinearities
Particle Filter Less efficient, highly robust Multi-modal, nonlinear, non-Gaussian
Rao-Blackwellised Particle Filter, MHT Combines PF with KF Multi-modal, highly efficient
Dynamic Bayes Network for Ball Tracking
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
Robot Location
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
Robot and Ball Location (and velocity)
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
Ball-Environment Interactions
None Grabbed
BouncedKicked
(0.8)
Robot loses grab (0.2)
(1.0)
Ro
bo
t kicks b
all (0.9)
(1.0
)
Within grab range and robot grabs
(prob. from model )
(0.1
)
Kick fails (0.1)
Deflected
(residual prob .) (1
.0)
Col
lisio
n w
ith
obje
cts
on m
ap (1
.0)
Integrating Discrete Ball Motion Mode
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
Grab Example (1)
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
Grab Example (2)
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
Inference: Posterior Estimation
k-2b k-1b kb
r k-2 r k-1 r k
z k-2 z k-1 z k
u k-2 u k-1
z k-1 z kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
),,|,,( 1:1:1:1 klk
bkkkk uzzrmbp
Rao-Blackwellised PF for Inference Represent posterior by random samples
Each sample
contains robot location, ball mode, ball Kalman filter
Generate individual components of a particle stepwise using the factorization
iiiiiii myxbmrs ,,,,,,,
),|(),,|(),,,|(
),|,,(
1:1:1:11:1:1:1:11:1:1:1:1
1:1:1:1:1
kkkkkkkkkkkk
kkkkk
uzrpuzrmpuzrmbp
uzrmbp
Rao-Blackwellised Particle Filter for Inference
k-1b kb
r k-1 r k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Bal
l tra
ckin
gR
obot
loca
liza
tion
)(1
)(1
)(1 ,, i
ki
ki
k bmr
Draw a sample from the previous sample set:
Generate Robot Location
r k-1 r k
u k-1
z k
Map and robot location
Robot control
Landmark detection
Rob
ot lo
cali
zati
on l
k-1b
k-1m
Ball location and velocity
Ball motion mode
Bal
l tra
ckin
g
kb
km
__,,),,,,|(~ )(1
)(1
)(1
)(1
)( ikkk
ik
ik
ikk
ik ruzbmrrpr
Generate Ball Motion Model
k-1b
r k-1 r k
u k-1
z k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l
kb
_,,),,,,|(~ )()(1
)(1
)(1
)()( ik
ikkk
ik
ik
ikk
ik mruzbmrmpm
Update Ball Location and Velocity
k-1b
r k-1 r k
u k-1
z k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l
kb
z k b
)()()()(1
)()()( ,,),,,|(~ ik
ik
ikk
ik
ik
ikk
ik bmrzbmrbpb
Importance Resampling
Weight sample by
if observation is landmark detection and by
if observation is ball detection.
Resample
)|( )()( ik
lk
ik rzpw
kM
ki
ki
ki
kki
ki
kkbk
ik ubmrMpbrMzpw ),,,|(),,|( 1
)(1
)(1
)()(1
)()(
Tracking and Finding the Ball
Cluster ball samples by discretizing pan / tilt angles
Uses negative information
Experiment: Real Robot
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
0 5 10 15 20 25 30 35 40 45 50
Pe
rce
nta
ge
of
ba
ll lo
st
Number of ball samples
With MapWithout Map
Robot kicks ball 100 times, tries to find it afterwards
Finds ball in 1.5 seconds on average
Comparison to KF* (optimized for straight motion)
RBPFKF*
Reference* Observations
RBPFKF*
Reference* Observations
RBPFKF’
Reference* Observations
RBPFKF’
Reference* Observations
Comparison to KF’ (inflated prediction noise)
Orientation Errors
0
20
40
60
80
100
120
140
160
180
2 3 4 5 6 7 8 9 10 11
Orie
nta
tion
Err
or [
deg
rees
]
Time [sec]
RBPFKF*KF'