probabilistic robotics: motion and sensing

4-1

Probabilistic Robotics: Motion and Sensing

Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others

Sebastian Thrun & Alex TeichmanStanford Artificial Intelligence Lab

4-2

Bayes Filters

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

4-3

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

ActionSensing

Bayes Filters

4-4

Probabilistic Motion Models

4-5

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

Action: p(x’ | u, x)

Bayes Filters

4-6

Robot MotionRobot motion is inherently uncertain.How can we model this uncertainty?

4-7

Probabilistic Motion Models• To implement the Bayes Filter, we

need the transition model p(x | x’, u).• The term p(x | x’, u) specifies a

posterior probability, that action u carries the robot from x’ to x.

• In this section we will specify, how p(x | x’, u) can be modeled based on the motion equations.

4-8

Locomotion of Wheeled RobotsLocomotion (Oxford Dict.):

Power of motion from place to place

• Differential drive (AmigoBot, Pioneer 2-DX)• Car drive (Ackerman steering)• Synchronous drive (B21)• Mecanum wheels, XR4000

y

roll

z motion

x

y

4-9

Instantaneous Center of Curvature

ICC

For rolling motion to occur, each wheel has to move along its y-axis

4-10

Differential Drive

R

ICCw

(x,y)

y

l/2

qx

vl

vr

lvv

vvvvlR

vlRvlR

lr

lr

rl

l

r

w

ww

)()(

2

)2/()2/(

]cos,sin[ICC qq RyRx

4-11

Differential Drive: Forward Kinematics

ICC

R

P(t)

P(t+dt)

tyx

tttt

yx

y

x

y

x

wdqwdwdwdwd

qICCICC

ICCICC

1000)cos()sin(0)sin()cos(

'''

')'()(

')]'(sin[)'()(

')]'(cos[)'()(

0

0

0

t

t

t

dttt

dtttvty

dtttvtx

wq

q

q

4-12

Differential Drive: Forward Kinematics

ICC

R

P(t)

P(t+dt)

tyx

tttt

yx

y

x

y

x

wdqwdwdwdwd

qICCICC

ICCICC

1000)cos()sin(0)sin()cos(

'''

')]'()'([1)(

')]'(sin[)]'()'([21)(

')]'(cos[)]'()'([21)(

0

0

0

t

lr

t

lr

t

lr

dttvtvl

t

dtttvtvty

dtttvtvtx

q

q

q

4-13

Mecanum Wheels

444

4

3210

3210

3210

3210

/)vvvv(v/)vvvv(v/)vvvv(v

/)vvvv(v

error

x

y

q

4-14

Example

4-15

XR4000 Drive

q

y

xvi(t)

wi(t)

')'()(

')]'(sin[)'()(

')]'(cos[)'()(

0

0

0

t

t

t

dttt

dtttvty

dtttvtx

wq

q

q

ICC

4-16

XR4000

[courtesy by Oliver Brock & Oussama Khatib]

4-17

Tracked Vehicle: Urban Robot

4-18

Tracked Vehicle: OmniTread

[courtesy by Johann Borenstein]

4-19

Odometry

4-20

Reasons for Motion Errors

bump

ideal case different wheeldiameters

carpetand many more …

4-21

Coordinate Systems• In general the configuration of a robot can be

described by six parameters.• Three-dimensional cartesian coordinates plus

three Euler angles pitch, roll, and tilt.• Throughout this section, we consider robots

operating on a planar surface.The state space of such

systems is three-dimensional (x,y,q).

4-22

Typical Motion Models• In practice, one often finds two types of

motion models:• Odometry-based• Velocity-based (dead 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.

• They calculate the new pose based on the velocities and the time elapsed.

4-23

Example Wheel EncodersThese modules require +5V and GND to power them, and provide a 0 to 5V output. They provide +5V output when they "see" white, and a 0V output when they "see" black.

These disks are manufactured out of high quality laminated color plastic to offer a very crisp black to white transition. This enables a wheel encoder sensor to easily see the transitions.

Source: http://www.active-robots.com/

4-24

Dead Reckoning• Derived from “deduced reckoning.”• Mathematical procedure for

determining the present location of a vehicle.

• Achieved by calculating the current pose of the vehicle based on its velocities and the time elapsed.

Odometry Model

22 )'()'( yyxxtrans d

qd )','(atan21 xxyyrot

12 ' rotrot dqqd

Robot moves from to . Odometry information .

q,, yx ',',' qyx

transrotrotu ddd ,, 21

transd1rotd

2rotd

q,, yx

',',' qyx

4-26

The atan2 FunctionExtends the inverse tangent and correctly

copes with the signs of x and y.

Noise Model for Odometry• The measured motion is given by the

true motion corrupted with noise.

||||11 211

ˆtransrotrotrot dddd

||||22 221

ˆtransrotrotrot dddd

|||| 2143

ˆrotrottranstranstrans ddddd

Typical Distributions for Probabilistic Motion Models

2

2

221

221)(

x

ex

2

2

2

6||6

6|x|if0)(2

xx

Normal distribution Triangular distribution

4-29

Calculating the Probability (zero-centered)• For a normal distribution

• For a triangular distribution

1. Algorithm prob_normal_distribution(a,b):

2. return

1. Algorithm prob_triangular_distribution(a,b):

2. return

4-30

Calculating the Posterior Given x, x’, and u

22 )'()'( yyxxtrans dqd )','(atan21 xxyyrot

12 ' rotrot dqqd 22 )'()'(ˆ yyxxtrans dqd )','(atan2ˆ

1 xxyyrot

12ˆ'ˆrotrot dqqd

)ˆ|ˆ|,ˆ(prob trans21rot11rot1rot1 dddd p|))ˆ||ˆ(|ˆ,ˆ(prob rot2rot14trans3transtrans2 ddddd p

)ˆ|ˆ|,ˆ(prob trans22rot12rot2rot3 dddd p

1. Algorithm motion_model_odometry(x,x’,u)2. 3. 4. 5. 6. 7. 8. 9. 10.

11. return p1 · p2 · p3

odometry values (u)

values of interest (x,x’)

4-31

Examples

Application• Repeated application of the sensor model

for short movements.• Typical banana-shaped distributions

obtained for 2d-projection of 3d posterior.

x’u

p(x|u,x’)

u

x’

Sample-based Density Representation

4-34

Sample-based Density Representation

4-35

How to Sample from Normal or Triangular Distributions?• Sampling from a normal distribution

• Sampling from a triangular distribution

1. Algorithm sample_normal_distribution(b):

2. return

1. Algorithm sample_triangular_distribution(b):

2. return

4-36

Normally Distributed Samples

106 samples

4-37

For Triangular Distribution

103 samples 104 samples

106 samples105 samples

Sample Odometry Motion Model1. Algorithm sample_motion_model(u, x):

2.

3. 4.

5. 6. 7.

8. Return

)||sample(ˆ21111 transrotrotrot dddd

|))||(|sample(ˆ2143 rotrottranstranstrans ddddd

)||sample(ˆ22122 transrotrotrot dddd

)ˆcos(ˆ' 1rottransxx dqd )ˆsin(ˆ' 1rottransyy dqd

21ˆˆ' rotrot ddqq

',',' qyx

qddd ,,,,, 21 yxxu transrotrot

sample_normal_distribution

Sampling from Our Motion Model

Start

Examples

Map-Consistent Motion Model

)',|( xuxp ),',|( mxuxp

)',|()|(),',|( xuxpmxpmxuxp Approximation:

4-42

Summary• We discussed motion models for odometry-based

motion (see book for velocity-based systems)• We discussed ways to calculate the posterior

probability p(x| x’, u).• We also described how to sample from p(x| x’, u).• Typically the calculations are done in fixed time

intervals t.• In practice, the parameters of the models have to

be learned.• We also discussed an extended motion model that

takes the map into account.

4-43

Probabilistic Sensor Models

4-44

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

Sensing: p(z | x)

Bayes Filters

4-45

Sensors for Mobile Robots• Contact sensors: Bumpers, Whiskers• Internal sensors

• Accelerometers (spring-mounted masses)• Gyroscopes (spinning mass, laser light)• Compasses, inclinometers (earth magnetic field, gravity)

• Proximity sensors• Sonar (time of flight)• Radar (phase and frequency)• Laser range-finders (triangulation, tof, phase)• Infrared (intensity)• Stereo

• Visual sensors: Cameras, Stereo• Satellite-based sensors: GPS

4-46

Ultrasound Sensors• Emit an ultrasound signal• Wait until they receive the echo• Time of flight sensor

Polaroyd 6500

4-47

Time of Flight sensors

v: speed of the signalt: time elapsed between broadcast of signal

and reception of the echo.

2/tvd

emitterobject

4-48

Properties of Ultrasounds• Signal profile [Polaroid]

4-49

Sources of Error• Opening angle• Crosstalk• Specular reflection

4-50

Typical Ultrasound Scan

4-51

Laser Range Scanner

4-52

Properties• High precision• Wide field of view • Approved security for collision

detection

4-53

Robots Equipped with Laser Scanners

Herbert:Zora: Groundhog:

4-54

Typical Scans

4-55

Proximity Sensors

• The central task is to determine P(z|x), i.e., the probability of a measurement z given that the robot is at position x.

• Question: Where do the probabilities come from?• Approach: Let’s try to explain a measurement.

4-56

Beam-based Sensor Model• Scan z consists of K measurements.

• Individual measurements are independent given the robot position.

},...,,{ 21 Kzzzz

K

kk mxzPmxzP

1

),|(),|(

4-57

Beam-based Sensor Model

K

kk mxzPmxzP

1

),|(),|(

4-58

Typical Measurement Errors of an Range Measurements

1. Beams reflected by obstacles

2. Beams reflected by persons / caused by crosstalk

3. Random measurements

4. Maximum range measurements

4-59

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.

4-60

Beam-based Proximity ModelMeasurement noise

zexp zmax0

bzz

hit eb

mxzP2

exp )(21

21),|(

otherwisezz

mxzPz

0e

),|( expunexp

Unexpected obstacles

zexp zmax0

4-61

Beam-based Proximity ModelRandom measurement Max range

max

1),|(z

mxzPrand smallz

mxzP 1),|(max

zexp zmax0zexp zmax0

4-62

Resulting Mixture Density

),|(),|(),|(

),|(

),|(

rand

max

unexp

hit

rand

max

unexp

hit

mxzPmxzPmxzP

mxzP

mxzP

T

How can we determine the model parameters?

4-63

Raw Sensor DataMeasured distances for expected distance of 300 cm.

Sonar Laser

4-64

Approximation• Maximize log likelihood of the data

• Search space of n-1 parameters.• Hill climbing• Gradient descent• Genetic algorithms• …

• Deterministically compute the n-th parameter to satisfy normalization constraint.

)|( expzzP

4-65

Approximation Results

Sonar

Laser

300cm 400cm

4-66

Example

z P(z|x,m)

4-67

Discrete Model of Proximity Sensors • Instead of densities, consider discrete steps along the

sensor beam.• Consider dependencies between different cases.

Laser sensor Sonar sensor

4-68

Approximation Results

Laser

Sonar

4-69

"sonar-0"

0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 700

0.050.1

0.150.2

0.25

Influence of Angle to Obstacle

4-70

"sonar-1"

0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 700

0.050.1

0.150.2

0.250.3

Influence of Angle to Obstacle

4-71

"sonar-2"

0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 700

0.050.1

0.150.2

0.250.3

Influence of Angle to Obstacle

4-72

"sonar-3"

0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 700

0.050.1

0.150.2

0.25

Influence of Angle to Obstacle

4-73

Summary Beam-based Model• Assumes independence between beams.

• Justification?• Overconfident!

• Models physical causes for measurements.• Mixture of densities for these causes.• Assumes independence between causes. Problem?

• Implementation• Learn parameters based on real data.• Different models should be learned for different angles at

which the sensor beam hits the obstacle.• Determine expected distances by ray-tracing.• Expected distances can be pre-processed.

4-74

Scan-based Model• Beam-based model is …

• not smooth for small obstacles and at edges.

• not very efficient.

• Idea: Instead of following along the beam, just check the end point.

4-75

Scan-based Model• Probability is a mixture of …

• a Gaussian distribution with mean at distance to closest obstacle,

• a uniform distribution for random measurements, and

• a small uniform distribution for max range measurements.

• Again, independence between different components is assumed.

4-76

Example

P(z|x,m)

Map m

Likelihood field

4-77

San Jose Tech Museum

Occupancy grid map Likelihood field

4-78

Scan Matching• Extract likelihood field from scan and

use it to match different scan.

4-79

Scan Matching• Extract likelihood field from first scan

and use it to match second scan.

~0.01 sec

4-80

Properties of Scan-based Model

• Highly efficient, uses 2D tables only.• Smooth w.r.t. to small changes in robot

position.• Allows gradient descent, scan matching.• Ignores physical properties of beams.• Will it work for ultrasound sensors?

4-81

Landmarks• Active beacons (e.g., radio, GPS)• Passive (e.g., visual, retro-reflective)• Standard approach is triangulation

• Sensor provides• distance, or• bearing, or• distance and bearing.

4-82

Distance and Bearing

4-83

Probabilistic Model1. Algorithm landmark_detection_model(z,x,m):

2.

3.

4.

5. Return

22 ))(())((ˆ yimximd yx

),ˆprob(),ˆprob(det dddp

q ,,,,, yxxdiz

q ))(,)(atan2(ˆ ximyima xy

),|(uniformfpdetdet mxzPzpz

4-84

Distributions

4-85

Distances OnlyNo Uncertainty

P1 P2d1 d2

x

X’

a

)(2/)(

221

22

21

2

xdyaddax

P1=(0,0)P2=(a,0)

4-86

P1

P2

D1

z1

z2

P3

D2bz3

D3

Bearings OnlyNo Uncertainty

P1

P2

D1

z1

z2

cos2 2122

21

21 zzzzD

)cos(2

)cos(2

)cos(2

2123

21

23

2123

22

22

2122

21

21

b

b

zzzzD

zzzzD

zzzzDLaw of cosine

4-87

Bearings Only With Uncertainty

P1

P2

P3

P1

P2

Most approaches attempt to find estimation mean.

4-88

Summary of Sensor Models• Explicitly modeling uncertainty in sensing is key to

robustness.• In many cases, good models can be found by the following

approach:1. Determine parametric model of noise free measurement.2. Analyze sources of noise.3. Add adequate noise to parameters (eventually mix in densities

for noise).4. Learn (and verify) parameters by fitting model to data.5. Likelihood of measurement is given by “probabilistically

comparing” the actual with the expected measurement.• This holds for motion models as well.• It is extremely important to be aware of the underlying

assumptions!