Where’s the Robot? Ross Mead April 3 rd, 2008. Where’s the Robot? Given an initial estimate P(0)...
-
Upload
kathleen-harrison -
Category
Documents
-
view
213 -
download
0
Transcript of Where’s the Robot? Ross Mead April 3 rd, 2008. Where’s the Robot? Given an initial estimate P(0)...
Where’s the Robot?
Ross MeadApril 3rd, 2008
Where’s the Robot?
• Given an initial estimate P(0) of the robot’s location in the configuration space, maintain an ongoing estimate of the robot pose P(t) at time t with respect to the map.
)(
)(
)(
)(
t
ty
tx
tP
Configuration Space (C-space)
• A set of “reachable” areas constructed from knowledge of both the robot and the world.
• How to create it…– abstract the robot as a point object– enlarge the obstacles to account for the robot’s
footprint and degrees-of-freedom
Configuration Space (C-space)• Footprint
– the amount of space a robot occupies
• Degrees-of-Freedom (DoF)– number of variables
necessary to fully describe a robot’s “pose” in space
– How many DoF does the Create have?
)(
)(
)(
)(
t
ty
tx
tP
Configuration Space (C-space)
Obstacles
Free Space
Robot(treat as point object)(x, y, )
“We don’t need no stinkin’ sensors!”
• Send a movement command to the robot.
• Assume command was successful…– set pose to expected pose following the command
• But… robot movement is not perfect…– imperfect hardware (yes… blame hardware… )– wheel slippage– discrepancies in wheel circumferences– skid steering
Reasons for Motion Errors
bump
ideal casedifferent wheeldiameters
carpet
and many more …
What Do We Know About the World?
Proprioception• sensing things about one's
own internal status• Common proprioceptive
sensors are:– thermal– hall effect– optical– contact
Exteroception• sensing things about the
environment• Common exteroceptive
sensors are:– electromagnetic spectrum– sound– touch– smell/odor– temperature– range– attitude (Inclination)
Overview of Location
Locomotion• Power of motion from place to place.
• Differential drive (Pioneer 2-DX, iRobot Create)• Car drive (Ackerman steering)• Synchronous drive (B21)• Mecanum wheels, XR4000
y
roll
z motion
x
y
ICC
For rolling motion to occur, each wheel has to move along its y-axis.
Instantaneous Center of Curvature (ICC)
Differential Drive
• Differences in the velocities of wheels determines the turning angle of the robot.
• Forward Kinematics– Given the wheels’ velocities (or positions), what is
the robot’s velocity/position ?
Motion Model• Kinematics
– The effect of a robot’s geometry on its motion.– If the motors move this much, where will the robot be?
• Two types of common motion models:– Odometry-based– Velocity-based (“ded reckoning”)
• Odometry-based models are used when systems are equipped with wheel encoders.
• Velocity-based models have to be applied when no wheel encoders are given…– calculate new pose based on velocities and time elapsed– in the case of the Creates, we focus on this model
Ded Reckoning“Do you mean, ‘dead reckoning’”?
Ded Reckoning
• That’s not a typo…– “ded reckoning” = deduced reckoning
• reckon to determine by reference to a fixed basis
• Keep track of the current position by noting how far the robot has traveled on a specific heading…
– used for maritime navigation– uses proprioceptive sensors
• in the case of the Creates, we utilize velocity control
Ded Reckoning1. Specify system measurements…
• consider possible coordinate systems
2. Determine the point (radius) about which the robot is turning…• to minimize wheel slippage, this point (the ICC) must lie at the
intersection of the wheels’ axles
3. Determine the angular velocity ω at which the robot is turning to obtain the robot velocity v…
1. each wheel must be traveling at the same ω about the ICC
4. Integrate to find position P(t)…• the ICC changes over time t
R
ICC
(x, y)
y
x
vl
vr
d
vv
vv
vvdR
lr
lr
lr
)(
)(
2
]cos,sin[ RyRxICC
Ded Reckoning
2
d
l
r
vd
R
vd
R
)2
(
)2
(
Of these five, what’s known and what’s not?
Thus,
(vr and vl in mm/sec)
ICC
R
P(t)
P(t+t)
t
ICC
ICC
ICCy
ICCx
tt
tt
y
x
y
x
y
x
100
0)cos()sin(
0)sin()cos(
'
'
'
t
t
t
dttt
dtttvty
dtttvtx
0
0
0
')'()(
')]'(sin[)'()(
')]'(cos[)'()(
Ded Reckoning
or
ICC
R
P(t)
P(t+t)
t
ICC
ICC
ICCy
ICCx
tt
tt
y
x
y
x
y
x
100
0)cos()sin(
0)sin()cos(
'
'
'
Ded Reckoning
2
)()()()()(
tvtvttRtv lr
')]'()'([1
)(
')]'(sin[)]'()'([2
1)(
')]'(cos[)]'()'([2
1)(
0
0
0
t
lr
t
lr
t
lr
dttvtvd
t
dtttvtvty
dtttvtvtx
or
')'()(
')]'(sin[)'()(
')]'(cos[)'()(
0
0
0
t
t
t
dttt
dtttvty
dtttvtx
This is kinematics…
Sucks,… don’t it… ?
(Adding It All Up)
• Update the wheel velocities and, thus, robot velocity information at each sensor update…
• How large can/should a single segment be?
)()1(
)]1(sin[)()1(
)]1(cos[)()1(
)(
)(
)(
)(
tt
ttvty
ttvtx
t
ty
tx
tP
Example Move Function// move x centimeters (x > 0), // vel in mm/sec (-500 to 500)void move(float x, int vel) // move is approximate{ int dist = (int)(x * 10.0); // change cm to mm create_distance(); // update Create internal distance gc_distance = 0; // and initialize IC’s distance global msleep(50L); // pause before next signal to Create if (dist != 0) { create_drive_straight(vel); if (vel > 0) while (gc_distance < dist) create_distance(); else while (gc_distance > -dist) create_distance(); msleep(50L); // pause between distance checks } create_stop(); // stop} // move(float, int)
Example Turn Function// deg > 0 turn left (CCW),// deg < 0 turn right (CW),// vel in mm/sec (0 to 500)void turn(int deg, int vel) // turn is approximate{ create_angle(); msleep(50L); // initialize angle gc_total_angle = 0; // and update IC’s angle global if (deg > 0) { create_spin_CCW(vel); while (gc_total_angle < deg) { create_angle(); msleep(50L); } } else { create_spin_CW(vel); while (gc_total_angle > deg) { create_angle(); msleep(50L); } } create_stop(); // stop } // turn(int, int)
Putting It All Together
• How can we modify move(..) and turn(..) to implement ded reckoning to maintain the robot’s pose at all times?– I leave this to you as an exercise…
Types of Mobile Robot Bases
Holonomic
• A robot is holonomic if it can move to change its pose instantaneously in all available directions.
Non-holonomic
• A robot is non-holonomic if it can not move to change its pose instantaneously in all available directions.
Types of Mobile Robot Bases
• Ackerman Drive– typical car steering– non-holonomic
Types of Mobile Robot Bases
• Omni Drive– wheel capable of rolling in any direction– robot can change direction without rotating base
• Synchro Drive
Types of Mobile Robot Bases
Dead Reckoning
• Ded reckoning makes hefty assumptions…– perfect traction with ground (no slippage)– identical wheel circumferences– ignores surface area of wheels (no skid steering)– sensor error and uncertainty…
Dead Reckoning
What’s the Problem?
• Sensors are the fundamental input for the process of perception…– therefore, the degree to which sensors can
discriminate the world state is critical
• Sensor Aliasing– many-to-one mapping between environmental states
to the robot’s perceptual inputs– amount of information is generally insufficient to
identify the robot’s position from a single reading
What’s the Problem?
• Sensor Noise– adds a limitation on the consistency of sensor readings– often the source of noise is that some environmental
features are not captured by the robot’s representation
• Dynamic Environments
• Unanticipated Events
• Obstacle Avoidance
robot tracking robot kidnapping
global problemlocal problem
Where am I?
?
Localization
robot tracking robot kidnapping
global problemlocal problem
Only local data! (even perfect data)
Localization
robot tracking robot kidnapping
global problemlocal problem
Direct map-matching can be overwhelming
Localization
Initial (uniform) distribution
Key idea: keep track of a probability distribution for where the robot might be in the known map
black - blue - red - cyan
Where’s this?
Monte Carlo Localization (MCL)
Key idea: keep track of a probability distribution for where the robot might be in the known map
Initial (uniform) distribution Intermediate stage 1blue
black - blue - red - cyan
Monte Carlo Localization (MCL)
Key idea: keep track of a probability distribution for where the robot might be in the known map
Initial (uniform) distribution Intermediate stage 2black - blue - red - cyan
redblue
Monte Carlo Localization (MCL)
Key idea: keep track of a probability distribution for where the robot might be in the known map
Initial (uniform) distribution Intermediate stages Final distribution
But how?
cyan
black - blue - red - cyan
Monte Carlo Localization (MCL)
Bag o’ tricks
• Bayes’ rule p( A | B ) = p( B | A ) • p( A )
p( B )
• Definition of marginal probability
p( A ) = p( A | B ) • p(B) all B
p( A ) = p( A B )all B
• Definition of conditional probability
p( A B ) = p( A | B ) • p(B)
What are these saying?
Deriving MCL
The robot does (or can be modeled to) alternate between
We do know m -- the map of the environment
p( o | r, m )
p( rnew | rold, a, m )
-- the sensor model
= the accuracy of performing action a -- the motion model
(or will know)
We want to know P(t) -- the pose of the robot at time t• but we’ll settle for p(P(t)) -- a probability distribution for P(t)
! What kind of thing is p(P(t)) ?
• sensing -- getting range observations o1, o2, o3, …, ot-1, ot
• acting -- driving around (or ferrying?) a1, a2, a3, …, at-1
“local maps”
whence?
Setting Up the Problem
p( o | r, m ) sensor modelmap m and location r
p( | r, m ) = .95
p( | r, m ) = .05
potential observations o
“probabilistic kinematics” -- encoder uncertainty
• red lines indicate commanded action• the cloud indicates the likelihood of various final states
p( rnew | rold, a, m ) action model
Sensor Model
Key question: We may know where our robot is supposed to be, but in reality it might be somewhere else…
VR(t)
VL (t)
starting position
supposed final pose
x
y
lots of possibilities for the actual final pose
What should we do?
Probabilistic Kinematics
p( o | r, m ) sensor model p( rnew | rold, a, m ) action model
(0) Model the physics of the sensor/actuators(with error estimates)
theoretical modeling
(1) Measure lots of sensing/action resultsand create a model from them
empirical modeling
• take N measurements, find mean (m) and st. dev. (), and then use a Gaussian model
• or, some other easily-manipulated (probability?) model...
p( x ) = p( x ) = 0 if |x-m| > s
1 otherwise 1- |x-m|/s otherwise
0 if |x-m| > s
Robot Models: How-To
• Create a program that will run your robot in a square (~2m to a side), pausing after each side before turning and proceeding.
1
2
3
4
• For 10 runs, collect both the odometric estimates of where the robot thinks it is and where the robot actually is after each side.
• You should end up with two sets of 30 angle measurements and 40 length measurements: one set from odometry and one from “ground-truth.”
• Find the mean and the standard deviation of the differences between odometry and ground truth for the angles and for the lengths – this is the robot’s motion uncertainty model.start and
“end”
This provides a probabilistic kinematic model.
MODEL the error in order to reason about it!
Running around in squares
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with a “probability” of 1/K
dimensionality?!“Particle Filter” representation of a probability distribution
Monte Carlo Localization (MCL)
Start by assuming p( r0 ) is the uniform distribution.
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
take K samples of r0 and weight each with a “probability” of 1/K
“probability”
Monte Carlo Localization (MCL)
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m) and the distribution is no longer uniform
How did this change?
Start by assuming p( r0 ) is the uniform distribution.
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
take K samples of r0 and weight each with a “probability” of 1/K
Monte Carlo Localization (MCL)
Create new samples by dividing up large clumpseach point spawns new ones in proportion to its importance factor
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m) and the distribution is no longer uniform
How did this change?
Start by assuming p( r0 ) is the uniform distribution.
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
take K samples of r0 and weight each with a “probability” of 1/K
Monte Carlo Localization (MCL)
The robot moves, a1
For each sample r1, move it according to the model p(r2 | a1, r1, m)
Where do the purple ones go?
Create new samples by dividing up large clumpseach point spawns new ones in proportion to its importance factor
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m) and the distribution is no longer uniform
How did this change?
Start by assuming p( r0 ) is the uniform distribution.
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
take K samples of r0 and weight each with a “probability” of 1/K
Monte Carlo Localization (MCL)
The robot moves, a1
For each sample r1, move it according to the model p(r2 | a1, r1, m)
Where do the purple ones go?
Create new samples by dividing up large clumpseach point spawns new ones in proportion to its importance factor
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m) and the distribution is no longer uniform
How did this change?
Start by assuming p( r0 ) is the uniform distribution.
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
take K samples of r0 and weight each with a “probability” of 1/K
Increase all the indices by 1 and keep going!
Monte Carlo Localization (MCL)
“Monte Carlo” Localization -- refers to the resampling of the distribution each time a new observation is integrated
Rhino
Minerva
Monte Carlo Localization (MCL)
“Monte Carlo” Localization -- refers to the resampling of the distribution each time a new observation is integrated
Monte Carlo Localization (MCL)
Plusses
Drawbacks
Simple algorithm
Well-motivated via probabilistic reasoning
It has worked well in practice!
Taking a step back…
Plusses
Naturally fuses data from very disparate sensors!
Drawbacks
Simple algorithm
Well-motivated via probabilistic reasoning
Doesn’t require control of the robot: passive localization
It’s an any-time algorithm
It has worked well in practice!
Taking a step back…
Plusses
Naturally fuses data from very disparate sensors!
Drawbacks
Simple algorithm
Well-motivated via probabilistic reasoning
Any-time may not be enough !
Empty distributions
Doesn’t require control of the robot: passive localization
Doesn’t use the robot control available: active localization
It’s an any-time algorithm
It has worked well in practice!
Taking a step back…
Questions?
Thanks!