4-1 probabilistic robotics: motion and sensing slide credits: wolfram burgard, dieter fox, cyrill...

88
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 Teichman Stanford Artificial Intelligence Lab

Upload: jasmin-black

Post on 17-Dec-2015

229 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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 Teichman

Stanford Artificial Intelligence Lab

Page 2: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-2

Bayes Filters

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

Page 3: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-3

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

ActionSensing

Bayes Filters

Page 4: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-4

Probabilistic Motion Models

Page 5: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-5

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

Action: p(x’ | u, x)

Bayes Filters

Page 6: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-6

Robot Motion

Robot motion is inherently uncertain.

How can we model this uncertainty?

Page 7: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 8: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-8

Locomotion of Wheeled Robots

Locomotion (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

Page 9: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-9

Instantaneous Center of Curvature

ICC

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

Page 10: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-10

Differential Drive

R

ICCw

(x,y)

y

l/2

qx

vl

vr

l

vv

vv

vvlR

vlR

vlR

lr

lr

rl

l

r

)(

)(

2

)2/(

)2/(

]cos,sin[ICC RyRx

Page 11: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-11

Differential Drive: Forward Kinematics

ICC

R

P(t)

P(t+dt)

t

y

x

tt

tt

y

x

y

x

y

x

ICC

ICC

ICC

ICC

100

0)cos()sin(

0)sin()cos(

'

'

'

')'()(

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

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

0

0

0

t

t

t

dttt

dtttvty

dtttvtx

Page 12: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-12

Differential Drive: Forward Kinematics

ICC

R

P(t)

P(t+dt)

t

y

x

tt

tt

y

x

y

x

y

x

ICC

ICC

ICC

ICC

100

0)cos()sin(

0)sin()cos(

'

'

'

')]'()'([1

)(

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

1)(

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

1)(

0

0

0

t

lr

t

lr

t

lr

dttvtvl

t

dtttvtvty

dtttvtvtx

Page 13: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-13

Mecanum Wheels

4

4

4

4

3210

3210

3210

3210

/)vvvv(v

/)vvvv(v

/)vvvv(v

/)vvvv(v

error

x

y

Page 14: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-14

Example

Page 15: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-15

XR4000 Drive

q

y

x

vi(t)

wi(t)

')'()(

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

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

0

0

0

t

t

t

dttt

dtttvty

dtttvtx

ICC

Page 16: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-16

XR4000

[courtesy by Oliver Brock & Oussama Khatib]

Page 17: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-17

Tracked Vehicle: Urban Robot

Page 18: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-18

Tracked Vehicle: OmniTread

[courtesy by Johann Borenstein]

Page 19: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-19

Odometry

Page 20: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-20

Reasons for Motion Errors

bump

ideal case different wheeldiameters

carpetand many more …

Page 21: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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,).

Page 22: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 23: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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/

Page 24: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 25: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Odometry Model

22 )'()'( yyxxtrans

)','(atan21 xxyyrot

12 ' rotrot

Robot moves from to .

Odometry information .

,, yx ',',' yx

transrotrotu ,, 21

trans1rot

2rot

,, yx

',',' yx

Page 26: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-26

The atan2 Function

Extends the inverse tangent and correctly copes with the signs of x and y.

Page 27: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Noise Model for Odometry

• The measured motion is given by the true motion corrupted with noise.

||||11 211

ˆtransrotrotrot

||||22 221

ˆtransrotrotrot

|||| 2143

ˆrotrottranstranstrans

Page 28: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Typical Distributions for Probabilistic Motion Models

2

2

22

1

22

1)(

x

ex

2

2

2

6

||6

6|x|if0)(2

xx

Normal distribution Triangular distribution

Page 29: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

Page 30: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-30

Calculating the Posterior Given x, x’, and u

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

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

1 xxyyrot

12ˆ'ˆrotrot

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

)ˆ|ˆ|,ˆ(prob trans22rot12rot2rot3 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’)

Page 31: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-31

Examples

Page 32: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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’

Page 33: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Sample-based Density Representation

Page 34: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-34

Sample-based Density Representation

Page 35: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

Page 36: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-36

Normally Distributed Samples

106 samples

Page 37: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-37

For Triangular Distribution

103 samples 104 samples

106 samples105 samples

Page 38: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

2.

3.

4.

5.

6.

7.

8. Return

)||sample(ˆ21111 transrotrotrot

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

)||sample(ˆ22122 transrotrotrot

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

21ˆˆ' rotrot

',',' yx

,,,,, 21 yxxu transrotrot

sample_normal_distribution

Page 39: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Sampling from Our Motion Model

Start

Page 40: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Examples

Page 41: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

Map-Consistent Motion Model

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

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

Page 42: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 43: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-43

Probabilistic Sensor Models

Page 44: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-44

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

Sensing: p(z | x)

Bayes Filters

Page 45: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

Page 46: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-46

Ultrasound Sensors

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

Polaroyd 6500

Page 47: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-47

Time of Flight sensors

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

and reception of the echo.

2/tvd

emitter

object

Page 48: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-48

Properties of Ultrasounds

• Signal profile [Polaroid]

Page 49: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-49

Sources of Error

• Opening angle• Crosstalk• Specular reflection

Page 50: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-50

Typical Ultrasound Scan

Page 51: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-51

Laser Range Scanner

Page 52: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-52

Properties

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

detection

Page 53: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-53

Robots Equipped with Laser Scanners

Herbert:Zora: Groundhog:

Page 54: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-54

Typical Scans

Page 55: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 56: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

),|(),|(

Page 57: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-57

Beam-based Sensor Model

K

kk mxzPmxzP

1

),|(),|(

Page 58: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

Page 59: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 60: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-60

Beam-based Proximity Model

Measurement noise

zexp zmax0

b

zz

hit eb

mxzP

2exp )(

2

1

2

1),|(

otherwise

zzmxzP

z

0

e),|( exp

unexp

Unexpected obstacles

zexp zmax0

Page 61: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-61

Beam-based Proximity Model

Random measurement Max range

max

1),|(

zmxzPrand

smallzmxzP

1),|(max

zexp zmax0zexp zmax

0

Page 62: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-62

Resulting Mixture Density

),|(

),|(

),|(

),|(

),|(

rand

max

unexp

hit

rand

max

unexp

hit

mxzP

mxzP

mxzP

mxzP

mxzP

T

How can we determine the model parameters?

Page 63: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-63

Raw Sensor DataMeasured distances for expected distance of 300 cm.

Sonar Laser

Page 64: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

Page 65: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-65

Approximation Results

Sonar

Laser

300cm 400cm

Page 66: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-66

Example

z P(z|x,m)

Page 67: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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

Page 68: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-68

Approximation Results

Laser

Sonar

Page 69: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-69

"sonar-0"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.05

0.10.15

0.20.25

Influence of Angle to Obstacle

Page 70: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-70

"sonar-1"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.05

0.10.15

0.20.25

0.3

Influence of Angle to Obstacle

Page 71: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-71

"sonar-2"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.05

0.10.15

0.20.25

0.3

Influence of Angle to Obstacle

Page 72: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-72

"sonar-3"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.05

0.10.15

0.20.25

Influence of Angle to Obstacle

Page 73: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 74: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 75: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 76: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-76

Example

P(z|x,m)

Map m

Likelihood field

Page 77: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-77

San Jose Tech Museum

Occupancy grid map Likelihood field

Page 78: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-78

Scan Matching

• Extract likelihood field from scan and use it to match different scan.

Page 79: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-79

Scan Matching

• Extract likelihood field from first scan and use it to match second scan.

~0.01 sec

Page 80: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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?

Page 81: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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.

Page 82: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-82

Distance and Bearing

Page 83: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-83

Probabilistic Model

1. Algorithm landmark_detection_model(z,x,m):

2.

3.

4.

5. Return

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

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

,,,,, yxxdiz

))(,)(atan2(ˆ ximyima xy

),|(uniformfpdetdet mxzPzpz

Page 84: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-84

Distributions

Page 85: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-85

Distances OnlyNo Uncertainty

P1 P2

d1 d2

x

X’

a

)(

2/)(22

1

22

21

2

xdy

addax

P1=(0,0)

P2=(a,0)

Page 86: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-86

P1

P2

D1

z1

z2a

P3

D2

bz3

D3

Bearings OnlyNo Uncertainty

P1

P2

D1

z1

z2

a

a

cos2 2122

21

21 zzzzD

)cos(2

)cos(2

)cos(2

2123

21

23

2123

22

22

2122

21

21

zzzzD

zzzzD

zzzzDLaw of cosine

Page 87: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

4-87

Bearings Only With Uncertainty

P1

P2

P3

P1

P2

Most approaches attempt to find estimation mean.

Page 88: 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian

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!