Transcript ppt format

Introduction to ROBOTICS
A Taste of Localization Problem
Course Summary
Dr. John (Jizhong) Xiao
Department of Electrical Engineering
City College of New York
[email protected]
City College of New York
1
Topics
• Brief Review (Robot Mapping)
• A Taste of Localization Problem
• Course Summary
City College of New York
2
Mapping/Localization
• Answering robotics’ big questions
– How to get a map of an environment with
imperfect sensors (Mapping)
– How a robot can tell where it is on a map
(localization)
• It is an on-going research
• It is the most difficult task for robot
– Even human will get lost in a building!
City College of New York
3
Review: Use Sonar to Create Map
What should we conclude if this sonar reads 10 feet?
there isn’t
something here
10 feet
there is
something
somewhere
around here
Local Map
unoccupied
no information
occupied
City College of New York
4
What is it a map of?
Several answers to this question have been tried:
pre ‘83
It’s a map of occupied cells.
oxy
cell (x,y) is
occupied
oxy
cell (x,y) is
unoccupied
What information should this map contain,
given that it is created with sonar ?
Each cell is either occupied or unoccupied -- this
was the approach taken by the Stanford Cart.
City College of New York
5
What is it a map of ?
Several answers to this question have been tried:
cell (x,y) is
occupied
pre ‘83
It’s a map of occupied cells.
‘83 - ‘88
It’s a map of probabilities: p( o | S1..i )
oxy
p( o | S1..i )
It’s a map of odds.
oxy
cell (x,y) is
unoccupied
The certainty that a cell is occupied,
given the sensor readings S1, S2, …,
Si
The certainty that a cell is unoccupied,
given the sensor readings S1, S2, …, Si
The odds of an event are expressed relative to the
complement of that event.
probabilities
evidence = log2(odds)
The odds that a cell is occupied, given
the sensor readings S1, S2, …, Si
odds( o | S1..i ) =
City College of New York
p( o | S1..i )
p( o | S1..i )
6
Combining Evidence
• The key to making accurate maps is combining lots of data.
So, how do we combine evidence to create a map?
What we want -odds( o | S2  S1)
the new value of a cell in the
map after the sonar reading S2
What we know -the old value of a cell in the
map (before sonar reading S2)
odds( o | S1)
p( Si | o ) & p( Si | o )
the probabilities that a certain obstacle
causes the sonar reading Si
City College of New York
7
Combining Evidence
• The key to making accurate maps is combining lots of data.
p( o | S2  S1 )
def’n of odds
odds( o | S2  S1) =
p( o | S2  S1 )
.
=
p( S2  S1 | o ) p(o)
p( S2  S1 | o ) p(o)
.
=
p( S2 | o ) p( S1 | o ) p(o)
p( S2 | o ) p( S1 | o ) p(o)
conditional
independence
of S1 and S2
=
p( S2 | o ) p( o | S1 )
p( S2 | o ) p( o | S1 )
Bayes’ rule (+)
.
precomputed values
the sensor model
Bayes’ rule (+)
previous odds
Update step = multiplying the previous odds by a precomputed weight.
City College of New York
8
Mapping Using Evidence Grids
Evidence Grids...
represent space as a collection of cells, each with
the odds (or probability) that it contains an obstacle
evidence = log2(odds)
Lab environment
likely obstacle
not sure
likely free space
lighter areas: lower evidence of obstacles being present
darker areas: higher evidence of obstacles being present
City College of New York
9
Mobot System Overview
high-level
Motion Planning: Given a known world and a
cooperative mechanism, how do I
get there from here ?
Localization: Given sensors and a map, where am I ?
Vision: If my sensors are eyes, what do I do?
Mapping: Given sensors, how do I create a useful map?
Abstraction level
Bug Algorithms: Given an unknowable world but a
known goal and local sensing, how
can I get there from here?
Kinematics: if I move this motor somehow, what
happens in other coordinate systems ?
Control (PID): what voltage should I set over time ?
low-level
Motor Modeling: what voltage should I set now ?
City College of New York
10
Content
• Brief Review (Robot Mapping)
• A Taste of Localization Problem
• Course Summary
City College of New York
11
What’s the problem?
• WHERE AM I?
• But what does this mean, really?
• Reference frame is important
– Local/Relative: Where am I vs. where I was?
– Global/Absolute: Where am I relative to the
world frame?
• Location can be specified in two ways
– Geometric: Distances and angles
– Topological: Connections among landmarks
City College of New York
12
Localization: Absolute
– Proximity-To-Reference
• Landmarks/Beacons
– Angle-To-Reference
• Visual: manual triangulation from physical
points
– Distance-From-Reference
• Time of Flight
– RF: GPS
– Acoustic:
• Signal Fading
– EM: Bird/3Space Tracker
– RF:
– Acoustic:
City College of New York
13
Triangulation
Land
Landmarks
Works great -as long as there
are reference points!
Lines of Sight
Sea
City College of New York
Unique Target
14
Compass Triangulation
cutting-edge 12th century technology
Land
Landmarks
Lines of Sight
North
Sea
City College of New York
Unique Target
15
Localization: Relative
• If you know your speed and direction, you can
calculate where you are relative to where you were
(integrate).
• Speed and direction might, themselves, be absolute
(compass, speedometer), or integrated (gyroscope,
Accelerometer)
• Relative measurements are usually more accurate in
the short term -- but suffer from accumulated error in
the long term
• Most robotics work seems to focus on this.
City College of New York
16
Localization Methods
• Markov Localization:
– Represent the robot’s belief by a probability
distribution over possible positions and uses Bayes’
rule and convolution to update the belief whenever
the robot senses or moves
• Monte-Carlo methods
• Kalman Filtering
• SLAM (simultaneous localization and mapping)
• ….
City College of New York
17
Environment Representation
• Environment Representation
– Continuos Metric
 x,y,q
– Discrete Metric
metric grid
– Discrete Topological
topological grid
Real environment
Metric grid
Continuous metric
Discrete Topological
City College of New York
18
Environment Representation
• Continuous Metric, (x,y,q)
• Topological (landmark-based, state space
organized according to the topological
structure of the environment)
• Grid-Based (the world is divided in cells of
fixed size; resolution and precision of state
estimation are fixed beforehand)
• The latter suffers from computational
overhead
City College of New York
19
Probability Review
Discrete Random Variables
• X denotes a random variable.
• X can take on a countable number of values in {x1,
x2, …, xn}.
• P(X=xi), or P(xi), is the probability that the random
variable X takes on value xi.
.
• P( ) is called probability mass function.
• E.g.
P( Room)  0.7,0.2,0.08,0.02
City College of New York
20
Probability Review
Continuous Random Variables
• X takes on values in the continuum.
• p(X=x), or p(x), is a probability density function.
b
Pr( x  (a, b))   p( x)dx
a
p(x)
• E.g.
x
City College of New York
21
Probability Review
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)
City College of New York
22
Law of Total Probability, Marginals
Discrete case
Continuous case
 P( x)  1
 p( x) dx  1
x
P ( x )   P ( x, y )
y
P( x)   P( x | y ) P( y )
y
p ( x)   p( x, y ) dy
p ( x)   p ( x | y ) p ( y ) dy
City College of New York
23
Probability Review
• Law of total probability:
P ( x)   P( x, z )dz
P ( x)   P( x | z ) P ( z )dz
P( x y )   P( x | y, z ) P( z | y ) dz
City College of New York
24
Conditional Independence
P( x, y z )  P( x | z ) P( y | z )
equivalent to
P( x z )  P( x | z , y )
and
P ( y z )  P( y | z , x )
City College of New York
25
Bayes Formula
P ( x, y )  P ( x | y ) P ( y )  P ( y | x ) P ( x )

P( y | x) P( x)
P( x y ) 
P( y )
If y is a new sensor reading
p (x )
 Prior probability distribution
p( x y )

Posterior probability distribution
p ( y x)
p( y)

Generative model, characteristics of the sensor

Does not depend on x
City College of New York
26
Bayes Rule
with Background Knowledge
P ( y | x, z ) P ( x | z )
P( x | y, z ) 
P( y | z )
City College of New York
27
Markov Localization
• What is Markov Localization ?
– Special case of probabilistic state estimation
applied to mobile robot localization
– Initial Hypothesis:
• Static Environment
– Markov assumption
– The robot’s location is the only state in the environment
which systematically affects sensor readings
– Further Hypothesis
• Dynamic Environment
City College of New York
28
Markov Localization
• Applying probability theory to robot localization
• Markov localization uses an
explicit, discrete representation for the probability of
all position in the state space.
• This is usually done by representing the environment by a
grid 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.
City College of New York
29
Markov Localization
– Instead of maintaining a single hypothesis as
to where the robot is, Markov localization
maintains a probability distribution over the
space of all such hypothesis
– Uses a fine-grained and metric discretization
of the state space
City College of New York
30
Example
• Assume the robot position is onedimensional
The robot is placed
somewhere in the
environment but it is
not told its location
The robot queries its
sensors and finds out it
is next to a door
City College of New York
31
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 a
door
City College of New York
32
Basic Notation
Bel(Lt=l ) Is the probability (density) that the robot
assigns to the possibility that its location at time t is l
The belief is updated in response to two different types of events:
• sensor readings,
• odometry data
City College of New York
33
Notation
• Goal:
City College of New York
34
Markov assumption
(or static world assumption)
City College of New York
35
Markov Localization
• Measurement
• Action
City College of New York
36
Update Phase
a
City College of New York
b
c
37
Update Phase
City College of New York
38
Recursive Bayesian Updating
P( zn | x, z1,, zn  1) P( x | z1,, zn  1)
P( x | z1,, zn) 
P( zn | z1,, zn  1)
Markov assumption: zn is independent of z1,...,zn-1 if
we know x.
P ( zn | x) P ( x | z1,  , zn  1)
P ( x | z1,  , zn ) 
P ( zn | z1,  , zn  1)
  P ( zn | x) P ( x | z1,  , zn  1)
 1... n
 P( z | x) P( x)
i
i 1... n
City College of New York
39
Example: Closing the door
City College of New York
40
Example: Second Measurement
• P(z2|open) = 0.5
P(z2|open) = 0.6
• P(open|z1)=2/3
P( z2 | open) P(open | z1 )
P(open | z2 , z1 ) 
P( z2 | open) P(open | z1 )  P( z2 | open) P(open | z1 )
1 2

5
2 3


 0.625
1 2 3 1
8
  
2 3 5 3
• z2 lowers the probability that the door is open.
City College of New York
41
Action: Prediction Phase
• 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?
City College of New York
42
Modeling Actions
• To incorporate the outcome of an action
u into the current “belief”, we use the
conditional pdf
P(x|u,x’)
• This term specifies the pdf that
executing u changes the state from x’
to x.
City College of New York
43
State Transitions
P(x|u,x’) for u = “close door”:
0.9
0.1
open
closed
1
0
If the door is open, the action “close door”
succeeds in 90% of all cases.
City College of New York
44
Integrating the Outcome of Actions
Continuous case:
P( x | u )   P( x | u, x' ) P( x' )dx'
Discrete case:
P( x | u)   P( x | u, x' ) P( x' )
City College of New York
45
Example: The Resulting Belief
P(closed | u )   P(closed | u , x' ) P( x' )
 P(closed | u, open) P(open)
 P(closed | u , closed ) P(closed )
9 5 1 3 15
    
10 8 1 8 16
P(open | u )   P(open | u , x' ) P( x' )
 P(open | u , open) P(open)
 P(open | u, closed ) P(closed )
1 5 0 3 1
    
10 8 1 8 16
 1  P(closed | u )
City College of New York
46
Bayes Filters: Framework
• Given:
– Stream of observations z and action data u:
dt  {u1 , z1 , ut , zt }
– 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:
Bel ( xt )  P( xt | u1 , z1 , ut , zt )
City College of New York
47
Markov Assumption
p( zt | x0:t , z1:t , u1:t )  p( zt | xt )
 Measurement probability
p( xt | x1:t 1 , z1:t , u1:t )  p( xt | xt 1 , ut )  State transition probability
Markov Assumption:
–past and future data are independent if one knows the current state
Underlying Assumptions
• Static world, Independent noise
• Perfect model, no approximation errors
City College of New York
48
Bayes Filters
z = observation
u = action
x = state
Bel ( xt )  P( xt | u1, z1 , ut , zt )
Bayes
  P( zt | xt , u1, z1, , ut ) P( xt | u1, z1, , ut )
Markov
  P( zt | xt ) P( xt | u1, z1, , ut )
Total prob.
  P( zt | xt )  P( xt | u1 , z1 , , ut , xt 1 )
P( xt 1 | u1 , z1 , , ut ) dxt 1
Markov
  P( zt | xt )  P( xt | ut , xt 1 ) P( xt 1 | u1 , z1 , , ut ) dxt 1
Markov
  P( zt | xt )  P( xt | ut , xt 1 ) P( xt 1 | u1 , z1 , , zt 1 ) dxt 1
  P( zt | xt )  P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1
City College of New York
49
Summary
• Measurement
• Action
City College of New York
50
Content
• Brief Review (Robot Mapping)
• A Taste of Localization Problem
• Course Summary
City College of New York
51
Mobile Robot
City College of New York
52
Mobile Robot Locomotion
Locomotion: the process of causing a robot to move
 Differential Drive
 Tricycle
R
 Synchronous Drive
 Ackerman Steering
City College of New York
Swedish Wheel
 Omni-directional
53
Differential Drive
Property: At each time instant, the left and right wheels must
follow a trajectory that moves around the ICC at the same
angular rate , i.e.,
L
L
 ( R  )  VR
2
 ( R  )  VL
2
• Kinematic equation
q
90  q
• Nonholonomic Constraint
 x 
sin q  cos q    x sin q  y cos q  0
 y 
City College of New York
54
Differential Drive
• Basic Motion Control
R : Radius of rotation
• Straight motion
R = Infinity
• Rotational motion
R= 0
City College of New York
VR = VL
VR = -VL
55
Tricycle
• Steering and power are provided through the front wheel
• control variables:
– angular velocity of steering wheel ws(t)
– steering direction α(t)
d: distance from the front wheel to the rear axle
City College of New York
56
Tricycle
Kinematics model in the world frame
---Posture kinematics model
City College of New York
57
Synchronous Drive
• All the wheels turn
in unison
– All wheels point in the
same direction and turn
at the same rate
– Two independent
motors, one rolls all
wheels forward, one
rotate them for turning
• Control variables
(independent)
– v(t), ω(t)
City College of New York
58
Ackerman Steering (Car Drive)
• The Ackerman Steering equation:
– :
d
cot q i  cot q o 
l
cos q
cot q 
sin q
cot q i  cot q o
Rd /2 Rd /2


l
l
d

l
R
City College of New York
59
Car-like Robot
Driving type: Rear wheel drive, front wheel steering
q  R  u1
l

q
 u1
tan 
Rear wheel drive car model:
ICC
Y


R
x  u1 cos q
y  u1 sin q
q
l
u1

q  tan 
l
  u2
X
non-holonomic constraint:
x sin q  y cos q  0
x, y
u1 : forward velocity of the rear wheels
u2 : angular velocity of the steering wheels
l : length between the front and rear wheels
City College of New York
60
Robot Sensing
• Collect information about the world
• Sensor - an electrical/mechanical/chemical device
that maps an environmental attribute to a
quantitative measurement
• Each sensor is based on a transduction
principle - conversion of energy from one form to
another
• Extend ranges and modalities of Human Sensing
City College of New York
61
Gas Sensor
Gyro
Accelerometer
Pendulum Resistive
Tilt Sensors
Metal Detector
Piezo Bend Sensor
Gieger-Muller
Radiation Sensor
Pyroelectric Detector
UV Detector
Resistive Bend Sensors
Digital Infrared Ranging
CDS Cell
Resistive Light Sensor
Pressure Switch
Miniature Polaroid Sensor
Limit Switch
Touch Switch
Mechanical Tilt Sensors
IR Pin
Diode
IR Sensor w/lens
Thyristor
Magnetic Sensor
IR Reflection
Sensor
Magnetic Reed Switch
IR Amplifier Sensor
Hall Effect
Magnetic Field
Sensors
Polaroid Sensor Board
IRDA Transceiver
Lite-On IR
Remote Receiver
Radio Shack
Remote Receiver
IR Modulator
Receiver
Solar Cell
Compass
City College of New
York
Compass
62
Piezo Ultrasonic Transducers
Sensors Used in Robot
• Resistive sensors:
– bend sensors, potentiometer, resistive photocells, ...
• Tactile sensors: contact switch, bumpers…
• Infrared sensors
– Reflective, proximity, distance sensors…
• Ultrasonic Distance Sensor
• Motor Encoder
• Inertial Sensors (measure the second derivatives of position)
– Accelerometer, Gyroscopes,
• Orientation Sensors: Compass, Inclinometer
• Laser range sensors
• Vision, GPS, …
City College of New York
63
Motion Planning
Path Planning: Find a path connecting an
initial configuration to goal configuration
without collision with obstacles
– Configuration Space
– Motion Planning Methods
•
•
•
•
Roadmap Approaches
Cell Decomposition
Potential Fields
Bug Algorithms
City College of New York
64
Motion Planning
• Motion Planning Methodololgies
– Roadmap
– Cell Decomposition
– Potential Field
Global methods
Local methods
• Roadmap
– From Cfree a graph is defined (Roadmap)
– Ways to obtain the Roadmap
• Visibility graph
• Voronoi diagram
• Cell Decomposition
– The robot free space (Cfree) is decomposed into simple regions (cells)
– The path in between two poses of a cell can be easily generated
• Potential Field
– The robot is treated as a particle acting under the influence of a potential field U,
where:
• the attraction to the goal is modeled by an additive field
• obstacles are avoided by acting with a repulsive force that yields a negative field
City College of New York
65
Full-knowledge motion planning
Roadmaps
Cell decompositions
visibility graph
exact free space
represented via
convex polygons
voronoi diagram
approximate free
space represented
via a quadtree
City College of New York
66
Potential field Method
• Usually assumes some knowledge at the global level
The goal is known;
the obstacles sensed
Each contributes forces, and the
robot follows the resulting gradient.
City College of New York
67
Thank you!
This Thurday: Final Exam
Time: Dec. 13, 6:30pm-8:30pm, Place: SH-378
Coverage: Mobile Robot
Close-book with 2 pages cheat sheet, but Do Not Cheat
City College of New York
68