ROBOTICS

01PEEQW

Basilio Bona

DAUIN – Politecnico di Torino

Probabilistic Fundamentals in Robotics

Robot Perception

Probabilistic models of mobile robots

� Robot motion

� Kinematics

� Velocity motion model

� Odometry motion model

� Robot perception

� Maps

� Beam model of laser range finders

� Correlation-based measurement models

� Feature-based measurement models

Basilio Bona 3

Maps

� A map of the environment is a list of objects in the environment,

with their properties attached (location, signatures, etc.)

Basilio Bona 4

� Map types:

a) Location-based maps

b) Feature-based maps

Location-based maps

Basilio Bona 5

Example of an occupancy grid map

Basilio Bona 6

Example of a DEM

Basilio Bona 7

X,Y,Z0,0,0.04696118459110,1,0.06361161917450,2,0.06466675549750,3,0.05544840171930,4,0.04897660389540,5,0.05860523879530,6,0.07303504645820,7,0.059456679970,8,0.04684785380960,9,0.05084584653380,10,0.06007701903580,11,0.05820681527260,12,0.06427805870770,13,0.07666000723840,14,0.0766818821430,15,0.06445764005180,16,0.0439602993429

Sensors

� We consider only range sensors, and in particular laser range

sensors, that provides a measurement under the name of laser

range scan

Basilio Bona 8

Range values(scalars)

Sensors

Basilio Bona 9

Multidimensional values

(vectors)

Omnidirectional camera

Sensor models – beam based models

Basilio Bona 10

� Individual measurements are independent from each other, given the robot position and the map

Errors

� Measurement errors (noise) can be caused by …

� a known obstacle

� cross-talk between beams

� 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

Basilio Bona 11

Errors

Basilio Bona 12

1. Beams reflected by obstacles

2. Beams reflected by persons/caused by crosstalk

3. Random measurements

4. Maximum range measurements

Noise models – 1

Basilio Bona 13

Measurement noise

zexp zmax0

Unexpected obstacles

zexpzmax0

Noise models – 2

Basilio Bona 14

Random measurement Max range

zexp zmax0 zexp zmax0

Resulting noise mixture density

Basilio Bona 15

How can we determine the model parameters?

zexp zmax0

Raw Sensor Data

Basilio Bona 16

Measured distances for expected distance of 300 cm

Sonar Laser

Approximation

Basilio Bona 17

� Maximize log likelihood of the data

� Search space of n-1 parameters– Hill climbing– Gradient descent– Genetic algorithms– …

� Deterministically compute the n-thparameter to satisfy normalization constraint

Approximation Results

Basilio Bona 18

Sonar

Laser

300cm 400cm

Example

Basilio Bona 19

zt p(zt|xt,m)

Laser scan of a part of the map

The likelihood evaluated for all positions xt

and projected into the map (shown in gray). The darker a position, the larger p(zt|xt,m)

Discrete model of proximity sensors

� Instead of densities, consider discrete steps along the sensor beam

� Consider dependencies between different cases

Basilio Bona 20

Laser sensor Sonar sensor

Approximation Results – 1

Basilio Bona 21

Laser

Sonar

Approximation Results – 2

Basilio Bona 22

"sonar-0"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.050.1

0.150.2

0.25

"sonar-1"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.050.1

0.150.2

0.250.3

"sonar-2"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.050.1

0.150.2

0.250.3

"sonar-3"

0 10 20 30 40 50 60 70 010

2030

4050

6070

00.050.1

0.150.2

0.25

different angles

Beam-based model summary

� Assumes independence between beams

� Models physical causes for measurements

� Mixture of densities for these causes

� Assumes independence between causes

� 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

Basilio Bona 23

Scan-based models

� Beam-based model is …

� not smooth for small obstacles and at edges

� not very efficient

� Instead of following along the beam, just check the end point

� Likelihood fields

Basilio Bona 24

Likelihood field for range finders

� Likelihood fields represent the likelihood of an obstacle detection as

a function of the global x-y coordinates

Basilio Bona 25

P(z|x,m)

Map m Likelihood field

high likelihood

low likelihood

Scan-based models

� 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

Basilio Bona 26

Scan matching

� Extract likelihood field from scan and use it to match different scan

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

scan

Basilio Bona 27

Properties of scan-based model

� Highly efficient, uses 2D tables only

� Smooth with respect to small changes in robot position

� Allows gradient descent, scan matching

� Ignores physical properties of beams

� Will it work for ultrasound sensors

Basilio Bona 28

Feature-based maps

� Features are distinctive elements or geometric primitives of the

environment

� In feature-based maps the element mk contains the property (or

properties) of the feature and its location on the map

� Feature-based maps only specify the shape of the environment at

specific locations. They are popular in robotics, where particular

features can be measured at no cost with vision sensors (e.g., color

of an object)

� Features can be extracted from measurements and mathematically

described as …

� low-level features (geometric primitives) like lines, circles

� high-level features like walls, edges, corners, doors, trees, tables or

trash cans

Basilio Bona 29

Example of feature-based maps

Basilio Bona 30

Features extraction

Basilio Bona 31

�Line extraction using statistical methods: weighted least-squares

r

α

�Different line tracts are extracted and clustered

Landmarks on maps

� Instead of raw data from the environment ... zt

� ... we extract a reduced set of features, i.e., f(zt)

� Reduction of computational complexity: high-dimensional

measurement space ⇒ low-dimensional feature space

� It is common to define places as features, such as hallways,

corridors, intersections, etc.

� Features often corresponds to distinct objects or places in the

environment

� We call these objects or places landmarks

Basilio Bona 32

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

Basilio Bona 33

Landmarks

Basilio Bona 34

Landmarks

Basilio Bona 35

Landmark measurements

� We assume that sensors can measure the range and bearing of the

landmark with respect to the local robot coordinate frame

� In addition the feature extractor f(zt) can provide one or more

signatures (height, color, binary code, etc.)

Basilio Bona 36

Landmark measurement model

� Landmark measurement models are defined only for feature-based

maps

� The maps consist of a list of features

Basilio Bona 37

� Usually we have location properties mk = (mk,x mk,y)

� Given the i-th feature at time t, that corresponds to the j-th landmark on the map, we can formulated the following model

Sensor model with known correspondence

� Key problem: data association. Who is what?

� To solve this problem many algorithms exist

� We assume that a correspondence variable exists, able to uniquely

identifying the landmark

Basilio Bona 38

1

i

t

i

t

c j N i j

c N i

= ≤

= +

m

if then the -th feature corresponds to the -th landmark

if then the -th feature does not corresponds to any

feature in the map

Probabilistic model

Basilio Bona 39

Landmark detection model

Basilio Bona 40

2 2

, ,

, ,

( , , , )

1 :

ˆ2 : ( ) ( )

ˆ3 : ( , )

ˆˆ4 : ( , ) ( , ) ( , )

5 :

i i

t t t t

i

t

j x j y

j y j x

i i i

t r t t j s

f c

j c

r m x m y

m y m x

q N r r N N s s

q

φ

φ

σ φ φ σ σ

=

= − + −

= − −

= − ⋅ − ⋅ −

x mLandmark_known_correspondence_model

return

atan2

Distributions

Basilio Bona 41

Distances only – no uncertainty

Basilio Bona 42

2 2 2

1 2

2 2

1

( ) / 2

( )

x a d d a

y d x

= + −

= ± −x ′

x

2P

1P a

1d 2

d1

2

(0,0)

( ,0)

P

P a

=

=

Bearings only – no uncertainty

Basilio Bona 43

2 2 2

1 1 2 1 22 cosD z z z z α= + −

Law of cosine2 2 2

1 1 2 1 2

2 2 2

2 2 3 1 2

2 2 2

3 1 3 1 2

2 cos( )

2 cos( )

2 cos( )

D z z z z

D z z z z

D z z z z

α

β

α β

= + −

= + −

= + − +

2P

1P

1D

α

α

2z

1z

2P

1P

1D

3z

1z

2z α

3D

2D

β

3P

Bearings only with uncertainty

Basilio Bona 44

Most approaches attempt to find estimation mean

2P

1P

2P

1P

3P

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:

� Determine parametric model of noise free measurement

� Analyze sources of noise

� Add adequate noise to parameters (eventually mix in densities for

noise)

� Learn (and verify) parameters by fitting model to data

� 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

Basilio Bona 45