Robot Localizationrobotics.unibg.it/teaching/robotics/pdf/09_RobotLocalization.pdf · Laser Scanner Extracted Lines Extracted Lines in Model Space r rr j r R j k ... Triangolazione

Corso di Robotica

Prof. Davide Brugali

Università degli Studi di Bergamo

Robot Localization

Map-based localization

Robotica - UNIBG - Prof. Brugali 2



Absolute Localization



Relative Localization

Probabilistic, Map-Based Localization

Consider a mobile robot moving in a known environment.

As it start to move, say from a precisely known location, it might

keep track of its location using odometry.

However, after a certain movement the robot will get very uncertain

about its position.

update using an observation of its environment.

observation lead also to an estimate of the robots position which can

than be fused with the odometric estimation to get the best possible

update of the robots actual position.

5 Robotica - UNIBG - Prof. Brugali

Sensor Aliasing

In robots, non-uniqueness of sensors readings is the norm

Even with multiple sensors, there is a many-to-one mapping from

environmental states to robot’s perceptual inputs

Therefore the amount of information perceived by the sensors is

generally insufficient to identify the robot’s position from a single

reading

Robot’s localization is usually based on a series of readings

Sufficient information is recovered by the robot over time



Action update

action model ACT with ot: Encoder Measurement, st-1: prior belief state

increases uncertainty

Perception update

perception model SEE with it: exteroceptive sensor inputs, s’1: updated belief state

decreases uncertainty



Given

the position estimate

its covariance for time k,

the current control input

the current set of observations and

the map

Compute the

new (posteriori) position estimate and

its covariance

Such a procedure usually involves five steps:

)( kkp

)( kkp

)( ku

)1( kZ

)( kM

)11( kkp

)11( kkp


The Five Steps for Map-Based Localization

Observation on-board sensors

Map data base

Prediction of Measurement and Position (odometry)

P e

r c e

p t i

o n

Matching

Estimation (fusion)

raw sensor data or extracted features

p r e

d i c

t e d

f e

a t u

r e

o b

s e

r v a

t i o

n s

position estimate

matched predictions and observations

YES

Encoder

1. Prediction based on previous estimate and odometry

2. Observation with on-board sensors

3. Measurement prediction based on prediction and map

4. Matching of observation and map

5. Estimation -> position update (posteriori position)


1 - Robot Position Prediction: Odometry

b

ss

b

ssss

b

ssss

k

ky

kx

kukkpkkp

lr

lrlr

lrlr

)2

sin(2

)2

cos(2

)(ˆ

)(ˆ

)(ˆ

)()(ˆ)1(ˆ

T

uuu

T

ppppf)k(ff)kk(f)kk( 1

ll

rr

u

sk

skk

0

0)(

Odometry


2 - Observation

The second step it to obtain the observation Z(k+1) (measurements)

from the robot’s sensors at the new location at time k+1

The observation usually consists of a set n0 of single observations zj(k+1)

extracted from the different sensors signals. It can represent raw data

scans as well as features like lines or doors

The parameters of the targets are observed in the sensor frame {S}.

Therefore the observations have to be transformed to the world frame {W}

or

the measurement prediction have to be transformed to the sensor frame {S}


2 - Observation: Example

j

r j

l in e j

Raw Date of

Laser Scanner

Extracted Lines Extracted Lines

in Model Space

jrrr

r

jRk

)1(

,

j

j

R

jr

kz )1(

Sensor

(robot)

frame


3 - Measurement Prediction

In the next step we use the predicted robot position

and the map M(k) to generate multiple predicted observations zt.

They have to be transformed into the sensor frame

We can now define the measurement prediction as the set containing all ni predicted

observations

The function hi is mainly the coordinate transformation between the world frame and the

sensor frame

kkp 1

kkp,zhkztii

11

ii

nikzkZ 111


4&5 – Matching and Estimation


Monte Carlo Localization using Particle Filtering

Maintain multiple estimates of robot’s location

Track possible robot positions, given all previous measurements

Key idea: represent the belief that a robot is at a particular location by a set of

“samples”, or “particles”

Represent Bel(l) by set of N weighted, random samples, called particles:

where a sample, si , is of the form: <<x, y, >, p>

<x, y, > represents robot’s position

p represents a weight, where sum of all p’s is 1

{ | 1 .. }i

S s i N


Updating beliefs using Monte Carlo Localization

Robot Motion Model:

When robot moves, MCL generates N new samples that approximate robot’s

position after motion command.

Each sample is generated by randomly drawing from previous sample set, with

likelihood determined by p values.

For sample drawn with position l, new sample l is generated from P(l | l, a)

(From Fox, et al, AAAI-99)

Sampling-based

approximation

of position belief

for non-sensing

robot


Robot Sensing Model:

Re-weight sample set, according to the likelihood that robot’s current sensors match

what would be seen at a given location

Let < l, p> be a sample.

Then, p P(s | l) Here, s is the sensor measurement;

a normalization constant to enforce

the sum of p’s equaling 1

After applying Motion model and Sensing model:

Resample, according to latest weights

Add a few uniformly distributed, random samples

Very helpful in case robot completely loses track of its location

Updating beliefs using Monte Carlo Localization


Adapting the Size of the Sample Set

Number of samples needed to achieve a desired level of accuracy

varies dramatically depending on the situation:

During global localization: robot is ignorant of where it is need lots

of samples

During position tracking: robot’s uncertainty is small

don’t need as many samples

MCL determines sample size “on the fly”

Compare P(l) and P(l | s) (I.e., belief before and after sensing) to

determine sample size

The more divergence, the more samples that are kept


Marker-based Localization

and Navigation

Corso di Robotica

Prof. Davide Brugali

Università degli Studi di Bergamo

Sensor Noise

Sensor noise is mainly influenced by environment

e.g. surface, illumination …

or by the measurement principle itself

e.g. interference between ultrasonic sensors

Sensor noise drastically reduces the useful information of sensor

readings. The solution is:

to take multiple reading into account

employ temporal and/or multi-sensor fusion


Sensor Aliasing

In robots, non-uniqueness of sensors readings is the norm

Even with multiple sensors, there is a many-to-one mapping from

environmental states to robot’s perceptual inputs

Therefore the amount of information perceived by the sensors is

generally insufficient to identify the robot’s position from a single

reading

Robot’s localization is usually based on a series of readings

Sufficient information is recovered by the robot over time


Localization Base on Artificial Landmarks

Image from Borenstein et al 1996, U. Michigan


Triangolazione

All 3 parameter triangles but AAA are solvable, provided the parameters are consistent. (ASS and SSA have existence and uniqueness issues)

There is not always a triangle, but name “triangulation” has endured.

General idea is solution of simultaneous algebraic constraint equations in which navigation variables appear as unknowns

Observables are usually “bearings” or ranges to landmarks


Linear Constraints With Known Heading

Dati a priori: L1(x1, y1) e L2(x2, y2)

Misure : θv, θ1 = θv + b1, θ2 = θv + b2

Incognite : V(x, y)


Solution


Imprecision


Linear Constraints With Unknown Heading

• Dati a priori: L1(x1, y1), L2(x2, y2), L3(x3, y3)

• Misure : θ1 = θv + b1, θ2 = θv + b2 θ3 = θv + b3

• Incognite : V(x, y, θv)


Linear Constraints With Unknown Heading


Laser Retroreflector Triangulation


Circular constraints

• Dati a priori: L1(x1, y1), L2(x2, y2)

• Misure : r1, r2

• Incognite : V(x, y)


Imprecision


Documents

Robot Localizationrobotics.unibg.it/teaching/robotics/pdf/09_RobotLocalization.pdf · Laser Scanner Extracted Lines Extracted Lines in Model Space r rr j r R j k ... Triangolazione