Upload
stefano-romeu-zeplin
View
222
Download
0
Embed Size (px)
Citation preview
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 1/44
City College of New York 1
Dr. Jizhong Xiao
Department of Electrical Engineering
City College of New York [email protected]
Probabilistic Robotics
Advanced Mobile Robotics
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 2/44
City College of New York 2
Robot Navigation
Fundamental problems to provide a mobile
robot with autonomous capabilities:
•Where am I going
•What’s the best way there?
•Where have I been? how to create an
environmental map with imperfect sensors?
• Where am I? how a robot can tell where
it is on a map?
• What if you’re lost and don’t have a map?
Mapping
Localization
Robot SLAM
Path Planning
Mission Planning
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 3/44
City College of New York 3
Representation of the Environment• Environment Representation
– Continuos Metric x,y,q
– Discrete Metricmetric grid
– Discrete Topological
topological grid
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 4/44
City College of New York 4
Localization, Where am I?
?
• Odometry, Dead Reckoning
• Localization base on external sensors,
beacons or landmarks
• Probabilistic Map Based Localization
Observation
Mapdata base
Prediction of Position(e.g. odometry)
P e r c e p
t i o n
Matching
Position Update(Estimation?)
raw sensor data or extracted features
predicted position
position
matched observations
YES
Encoder
P e r c e p t i o n
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 5/44
City College of New York 5
Localization Methods• Mathematic Background, Bayes Filters• Markov Localization:
– Central idea: represent the robot’s belief by a probability distributionover possible positions, and uses Bayes’ rule and convolution to updatethe belief whenever the robot senses or moves
– Markov Assumption: past and future data are independent if one knows thecurrent state
• Kalman Filtering – Central idea: posing localization problem as a sensor fusion problem
– Assumption: gaussian distribution function
• Particle Filtering – Central idea: Sample-based, nonparametric Filter – Monte-Carlo method
• SLAM (simultaneous localization and mapping)
• Multi-robot localization
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 6/44
City College of New York 6
Markov Localization• Applying probability theory to robot localization
• Markov localization uses anexplicit, discrete representation for the probability ofall position in the state space.
• This is usually done by representing the environment by agrid or a topological graph with a finite number of possible
states (positions).
• During each update, the probability for each state (element) of the entire space is updated.
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 7/44City College of New York 7
Markov Localization Example• Assume the robot position is one-
dimensionalThe robot is placed
somewhere in the
environment but it isnot told its location
The robot queries its
sensors and finds out it
is next to a door
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 8/44City College of New York 8
Markov Localization Example
The robot moves one
meter forward. To
account for inherent
noise in robot motion the
new belief is smoother
The robot queries its
sensors and again it
finds itself next to adoor
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 9/44City College of New York 9
Probabilistic Robotics
• Falls in between model-based and behavior-based techniques – There are models, and sensor measurements, but they are
assumed to be incomplete and insufficient for control
– Statistics provides the mathematical glue to integratemodels and sensor measurements
• Basic Mathematics
– Probabilities – Bayes rule
– Bayes filters
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 10/44City College of New York 10
• The next slides are provided by theauthors of the book "Probabilistic
Robotics“, you can download from thewebsite:
http://www.probabilistic-robotics.org/
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 11/44City College of New York 11
Probabilistic Robotics
Mathematic Background
ProbabilitiesBayes rule
Bayes filters
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 12/44City College of New York 12
Probabilistic Robotics
Key idea:Explicit representation of uncertainty usingthe calculus of probability theory
– Perception = state estimation
– Action = utility optimization
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 13/44City College of New York 13
Pr(A) denotes probability that proposition A is true.
•
•
•
Axioms of Probability Theory
1)Pr(0 A
1)Pr( True
)Pr()Pr()Pr()Pr( B A B A B A
0)Pr( False
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 14/44City College of New York 14
A Closer Look at Axiom 3
B
B A A BTrue
)Pr()Pr()Pr()Pr( B A B A B A
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 15/44City College of New York 15
Using the Axioms
)Pr(1)Pr(
0)Pr()Pr(1
)Pr()Pr()Pr()Pr(
)Pr()Pr()Pr()Pr(
A A
A A
False A ATrue
A A A A A A
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 16/44City College of New York 16
Discrete Random Variables
• X denotes a random variable.
• X can take on a countable number of values in {x1,x2, …, xn}.
• P(X=x i ), or P(x i ), is the probability that the randomvariable X takes on value x i.
• P( ) is called probability mass function.
• E.g. 02.0,08.0,2.0,7.0)( RoomP
.
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 17/44
City College of New York 17
Continuous Random Variables
• X takes on values in the continuum.
• p(X=x), or p(x), is a probability density function.
• E.g.
b
a
dx x pba x )()),(Pr(
x
p(x)
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 18/44
City College of New York 18
Joint and Conditional Probability
• P(X=x and Y=y) = P(x,y)
• If X and Y are independent then
P(x,y) = P(x) P(y)
• P(x | y) is the probability of x given y
P(x | y) = P(x,y) / P(y)
P(x,y) = P(x | y) P(y)• If X and Y are independent then
P(x | y) = P(x)
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 19/44
City College of New York 19
Law of Total Probability, Marginals
y
y xP xP ),()(
y
yP y xP xP )()|()(
x
xP 1)(
Discrete case
1)( dx x p
Continuous case
dy y p y x p x p )()|()(
dy y x p x p ),()(
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 20/44
City College of New York 20
Bayes Formula
)( y x p
evidence
priorlikelihood
)(
)()|()(
yP
xP x yP y xP
)()|()()|(),( xP x yP yP y xP y xP
Posterior probability distribution
)( x y p
)( x p Prior probability distribution
If y is a new sensor reading
Generative model, characteristics of the sensor
)( y p
Does not depend on x
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 21/44
City College of New York 21
Normalization
)()|(
1)(
)()|()(
)()|(
)(
1
xP x yP yP
xP x yP yP
xP x yP
y xP
x
y x
x
y x
y x
y xP x
xP x yP x
|
|
|
aux)|(:
aux
1
)()|(aux:
Algorithm:
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 22/44
City College of New York 22
Bayes Rulewith Background Knowledge
)|(
)|(),|(),|(
z yP
z xP z x yP z y xP
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 23/44
City College of New York 23
Conditioning
• Total probability:
dz zP z y xP y xP
dz zP z xP xP
dz z xP xP
)(),|()(
)()|()(
),()(
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 24/44
City College of New York 24
Conditional Independence
)|()|(),( z yP z xP z y xP
),|()( y z xP z xP
),|()( x z yP z yP
equivalent to
and
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 25/44
City College of New York 25
Simple Example of State Estimation
• Suppose a robot obtains measurement z • What is P(open|z)?
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 26/44
City College of New York 26
Causal vs. Diagnostic Reasoning
• P(open|z) is diagnostic.
• P(z|open) is causal.
• Often causal knowledge is easier toobtain.
• Bayes rule allows us to use causalknowledge:
)(
)()|()|(
zP
openPopen zP zopenP
count frequencies!
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 27/44
City College of New York 27
Example
• P(z|open) = 0.6 P(z|open) = 0.3
• P(open) = P(open) = 0.5
67.03
2
5.03.05.06.0
5.06.0)|(
)()|()()|(
)()|(
)|(
zopenP
open popen zPopen popen zP
openPopen zP
zopenP
• z raises the probability that the door is open.
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 28/44
City College of New York 28
Combining Evidence
• Suppose our robot obtains anotherobservation z2.
• How can we integrate this newinformation?
• More generally, how can we estimate P(x| z1...zn )?
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 29/44
City College of New York 29
Recursive Bayesian Updating
),,|(),,|(),,,|(),,|(
11
11111
nn
nnnn
z z zP z z xP z z x zP z z xP
Markov assumption: zn is independent of z 1 ,...,z n-1 if we know x.
)()|(
),,|()|(
),,|(
),,|()|(),,|(
...1
...1
11
11
111
xP x zP
z z xP x zP
z z zP
z z xP x zP z z xP
ni
in
nn
nn
nnn
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 30/44
City College of New York 30
Example: Second Measurement
• P(z2|open) = 0.5 P(z2|open) = 0.6
• P(open|z1)=2/3
625.08
5
3
1
5
3
3
2
2
13
2
2
1
)|()|()|()|(
)|()|(
),|(1212
12
12
zopenPopen zP zopenPopen zP
zopenPopen zP
z zopenP
• z 2 lowers the probability that the door is open.
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 31/44
City College of New York 31
Actions
• Often the world is dynamic since
– actions carried out by the robot,
– actions carried out by other agents,
– or just the time passing by
change the world.
• How can we incorporate such actions?
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 32/44
City College of New York 32
Typical Actions
• The robot turns its wheels to move
• The robot uses its manipulator to grasp anobject
• Plants grow over time…
• Actions are never carried out with absolute
certainty.• In contrast to measurements, actions generally
increase the uncertainty.
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 33/44
City College of New York 33
Modeling Actions
• To incorporate the outcome of an actionu into the current “belief”, we use theconditional pdf
P(x|u,x’)
• This term specifies the pdf thatexecuting u changes the state from x’ to x .
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 34/44
City College of New York 34
Integrating the Outcome of Actions
')'()',|()|( dx xP xu xPu xP
)'()',|()|( xP xu xPu xP
Continuous case:
Discrete case:
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 35/44
City College of New York 35
Example: Closing the door
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 36/44
City College of New York 36
State Transitions
P(x|u,x’) for u = “close door”:
If the door is open, the action “close door”succeeds in 90% of all cases.
open closed0.1 1
0.9
0
P(open)=5/8 P(closed)=3/8
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 37/44
City College of New York 37
Example: The Resulting Belief
)|(1
16
1
8
3
1
0
8
5
10
1
)(),|(
)(),|(
)'()',|()|(
16
15
8
3
1
1
8
5
10
9
)(),|(
)(),|(
)'()',|()|(
uclosed P
closed Pclosed uopenP
openPopenuopenP
xP xuopenPuopenP
closed Pclosed uclosed P
openPopenuclosed P
xP xuclosed Puclosed P
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 38/44
City College of New York 38
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:
),,,|()( 11 t t t t zu zu xP x Bel
},,,{ 11 t t t zu zud
M k A ti
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 39/44
City College of New York 39
Markov Assumption
Underlying Assumptions
• Static world, Independent noise
• Perfect model, no approximation errors
),|(),,|( 1:1:11:1 t t t t t t t u x x pu z x x p
)|(),,|( :1:1:0 t t t t t t x z pu z x z p
State transition probability
Measurement probability
Markov Assumption:
– past and future data are independent if one knows the current state
z = observation
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 40/44
City College of New York 40111
)(),|()|(
t t t t t t t
dx x Bel xu xP x zP
Bayes Filters
),,,|(),,,,|( 1111 t t t t t u zu xPu zu x zP Bayes
u = action x = state
),,,|()( 11 t t t t zu zu xP x Bel
Markov ),,,|()|( 11 t t t t u zu xP x zP
Markov11111 ),,,|(),|()|(
t t t t t t t t dxu zu xP xu xP x zP
1111
111
),,,|(
),,,,|()|(
t t t
t t t t t
dxu zu xP
xu zu xP x zP
Total prob.
Markov111111 ),,,|(),|()|( t t t t t t t t dx z zu xP xu xP x zP
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 41/44
City College of New York 41
Bayes Filter Algorithm
1. Algorithm Bayes_filter( Bel(x),d ):
2. 0
3. If d is a perceptual data item z then
4. For all x do
5.
6.7. For all x do
8.
9. Else if d is an action data item u then
10. For all x do11.
12. Return Bel’(x)
)()|()(' x Bel x zP x Bel
)(' x Bel
)(')(' 1 x Bel x Bel
')'()',|()(' dx x Bel xu xP x Bel
111 )(),|()|()( t t t t t t t t dx x Bel xu xP x zP x Bel
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 42/44
City College of New York 42
Bayes Filters are Familiar!
• Kalman filters
• Particle filters
• Hidden Markov models
• Dynamic Bayesian networks
• Partially Observable Markov Decision Processes(POMDPs)
111 )(),|()|()( t t t t t t t t dx x Bel xu xP x zP x Bel
8/2/2019 2 Probabilistic Robot
http://slidepdf.com/reader/full/2-probabilistic-robot 43/44
City College of New York 43
Summary
• Bayes rule allows us to computeprobabilities that are hard to assessotherwise.
• Under the Markov assumption, recursiveBayesian updating can be used toefficiently combine evidence.
• Bayes filters are a probabilistic tool forestimating the state of dynamic systems.