Where’s the Robot? Ross Mead April 3 rd, 2008. Where’s the Robot? Given an initial estimate P(0) of the robot’s location in the configuration space, maintain

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

http://sln.fi.edu/pieces/cych/apollo%2010/students/activities/offline/roll.html

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’”?

http://www.straightdope.com/mailbag/mdeadreckoning.html

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.


• Ackerman Drive– typical car steering– non-holonomic


• Omni Drive– wheel capable of rolling in any direction– robot can change direction without rotating base

• Synchro Drive


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



Only local data! (even perfect data)

Localization



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)


Initial (uniform) distribution Intermediate stage 1blue




Initial (uniform) distribution Intermediate stage 2black - blue - red - cyan

redblue



Initial (uniform) distribution Intermediate stages Final distribution

But how?

cyan



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



Get the current sensor observation, o1

For each sample point r0 multiply the importance factor by p(o1 | r0, m)


“probability”


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?






Create new samples by dividing up large clumpseach point spawns new ones in proportion to its importance factor









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?










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?









Increase all the indices by 1 and keep going!


“Monte Carlo” Localization -- refers to the resampling of the distribution each time a new observation is integrated

Rhino

Minerva


“Monte Carlo” Localization -- refers to the resampling of the distribution each time a new observation is integrated


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


Doesn’t require control of the robot: passive localization

It’s an any-time algorithm



C:\ZachOld\Zach\Robotics\Lec17\smithsonian_loc_anim_big.gif

Plusses

Naturally fuses data from very disparate sensors!

Drawbacks

Simple algorithm


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



C:\ZachOld\Zach\Robotics\Lec17\smithsonian_loc_anim_big.gif

Questions?

Thanks!

Documents

Where’s the Robot? Ross Mead April 3 rd, 2008. Where’s the Robot? Given an initial estimate P(0) of the robot’s location in the configuration space, maintain