Advanced Mobile Robotics

City College of New York

1

Dr. Jizhong XiaoDepartment of Electrical Engineering

CUNY City [email protected]

Advanced Mobile Robotics


Probabilistic Robotics

Probabilistic Sensor Models

Beam-based ModelLikelihood Fields ModelFeature-based Model


• Prediction (Action)

• Correction (Measurement)

Bayes Filter Reminder

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel


4

Sensors for Mobile Robots

• Contact sensors: Bumpers

• 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)

• Visual sensors: Cameras

• Satellite-based sensors: GPS


5

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.


6

Noise IssuesSensors are imperfect!

Specular Reflection


7

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

• Individual measurements are independent given the robot position and the map of environment.

},...,,{ 21 Kzzzz

K

kk mxzPmxzP

1

),|(),|(

A cyclic array of k ultrasound sensors


8

Beam-based Sensor Model

K

kk mxzPmxzP

1

),|(),|(


9

Typical Measurement Errors of an Range Measurements

1. Beams reflected by obstacles

2. Beams reflected by persons / caused by crosstalk

3. Random measurements4. Maximum range

measurements


10

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.


11

Beam-based Proximity ModelMeasurement noise

zexp zmax0

2

2exp )(

21

22

1),|(

zz

hithit emxzP

otherwisezz

mxzPz

0e

),|( expunexp

Unexpected obstacles

zexp zmax0

Gaussian Distribution

Exponential Distribution

Likelihood of sensing unexpected objects decreased with range


12

Beam-based Proximity ModelRandom measurement Max range/Failures

max

1),|(z

mxzPrand

otherwise

zzifmxzP

01

),|( maxmax

zexp zmax0zexp zmax0

Uniform distribution

Point-mass distribution

If fail to detect obstacle, report maximum range


13

Resulting Mixture Density

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

),|(

),|(

rand

max

unexp

hit

rand

max

unexp

hit

mxzPmxzPmxzPmxzP

mxzP

T

How can we determine the model parameters?

Weighted average, and 1maxexp randunhit

System identification method: maximum likelihood estimator (ML estimator)

},,,,,{ maxexp randunhit


14

Raw Sensor DataMeasured distances for expected distance of 300 cm.

Sonar Laser


15

Approximation

• The likelihood of the data Z is given by

• Our goal is to identify intrinsic parameters that maximize this log likelihood– Mathematic derivation can be found pp163~167– Psudo-code can be found in pp160

• An iterative procedure for estimating parameters

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

},,,,,{ maxexp randunhit


16

Approximation Results

Sonar

300cm 400cm

The smaller range, the more accurate the measurement

Laser is more accurate than sonar, narrower Gaussians

Laser


17

Example

z P(z|x,m)

Laser scan, projected into a previously acquired map

The likelihood, evaluated for all positions and projected into the map. The darker a position, the larger

P(z|x,m)


18

Summary Beam-based Model

• Assumes independence between beams.– Justification?– Overconfident! Suboptimal result

• 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.


19

Likelihood Fields Model

• Beam-based model is …– not smooth for small obstacles and at edges

(small changes of a robot’s pose can have a tremendous impact on the correct range of sensor beam, heading direction is particularly affected).

– not very efficient (computationally expensive in ray casting)

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


20

Likelihood Fields Model• Project the end points of a sensor scan Zt into the global

coordinate space of the map

• 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.

randrandhithittkt pppmxzp maxmax),(

)sin()cos(

cossinsincos

ksens

ksenskt

ksens

ksens

z

z zyx

yx

yx

kt

kt


21

Likelihood Fields Model

)sin()cos(

cossinsincos

ksens

ksenskt

ksens

ksens

z

z zyx

yx

yx

kt

kt

Distance to the nearest obstacles. Max range reading ignored


22

Example

P(z|x,m)

Example environment Likelihood fieldThe darker a location, the less likely it is to perceive an obstacleSensor

probability

O1

O2

O3

Oi : Nearest point to obstaclesZmax


23

San Jose Tech Museum

Occupancy grid map Likelihood field


24

Scan Matching

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

Sensor scan, 180 dots

The darker a region, the smaller likelihood for sensing an object there


25

Properties of Likelihood Fields 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?


26

Additional Models of Proximity Sensors

• Map matching (sonar,laser): generate small, local maps from sensor data and match local maps against global model. (e.g., transform scans into occupancy maps)

• Scan matching (laser): map is represented by scan endpoints, match scan into this map. (e.g. ICP algorithm to stitch scans together)

• Features (sonar, laser, vision): Extract features such as doors, hallways from sensor data.


27

Features/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.


28

Distance and Bearing


29

Feature-Based Measurement Model• Feature vector is abstracted from the measurement:

• Sensors that measure range, bearing, & a signature (a numerical value, e.g., an average color)

• Conditional independence between features

• Feature-Based map: withi.e., a location coordinate in global coordinates & a signature • Robot pose:

• Measurement model:

Zero-mean Gaussian error variables with standard deviations

},,{}{)(2

2

2

1

1

1

21

t

t

t

t

t

t

ttt

s

r

s

rffzf

),,,(),)(( mxsrpmxzfp tit

i

it

ittt

},,{ 21 mmm

2

2

2

)),(2tan)()(

,,

2,

2,

s

r

j

xjyj

yjxj

it

it

it

sxmymaymxm

s

r

Tt yxx }{

Tjyjxjj smmm },,{ ,,

2r

r


30

Sensor Model with Known Correspondence• A key problem for range/bearing sensors: data association, (arise

when the landmarks cannot be uniquely identified)• Introduce a correspondence variable , between feature

and landmark . If , then the i-th feature observed at time t corresponds to the j-th landmark in the map

Algorithm for calculating the probability of a landmark measurement: inputs: feature , with know correspondence , the robot pose and the map, Its output is the numerical probability

itf jm

}1,,1{ Nc it Njc it

itf Nc it

),,( mxcfp tit

it


31

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!


32

Thanks

Homework 4, 5 posted

Documents

Advanced Mobile Robotics