robot localizationrobotics.unibg.it/teaching/robotics/pdf/09_robotlocalization.pdf · laser scanner...
TRANSCRIPT
Corso di Robotica
Prof. Davide Brugali
Università degli Studi di Bergamo
Robot Localization
Map-based localization
Robotica - UNIBG - Prof. Brugali 2
Map-based localization
Robotica - UNIBG - Prof. Brugali 3
Absolute Localization
Map-based localization
Robotica - UNIBG - Prof. Brugali 4
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
6 Robotica - UNIBG - Prof. Brugali
Probabilistic, Map-Based Localization
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
7 Robotica - UNIBG - Prof. Brugali
Probabilistic, Map-Based Localization
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
8 Robotica - UNIBG - Prof. Brugali
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)
9 Robotica - UNIBG - Prof. Brugali
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
10 Robotica - UNIBG - Prof. Brugali
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}
11 Robotica - UNIBG - Prof. Brugali
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
12 Robotica - UNIBG - Prof. Brugali
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
13 Robotica - UNIBG - Prof. Brugali
4&5 – Matching and Estimation
14 Robotica - UNIBG - Prof. Brugali
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
15 Robotica - UNIBG - Prof. Brugali
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
16 Robotica - UNIBG - Prof. Brugali
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
17 Robotica - UNIBG - Prof. Brugali
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
18 Robotica - UNIBG - Prof. Brugali
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
20 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
21 Robotica - UNIBG - Prof. Brugali
Localization Base on Artificial Landmarks
Image from Borenstein et al 1996, U. Michigan
22 Robotica - UNIBG - Prof. Brugali
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
23 Robotica - UNIBG - Prof. Brugali
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)
24 Robotica - UNIBG - Prof. Brugali
Solution
25 Robotica - UNIBG - Prof. Brugali
Imprecision
26 Robotica - UNIBG - Prof. Brugali
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)
27 Robotica - UNIBG - Prof. Brugali
Linear Constraints With Unknown Heading
28 Robotica - UNIBG - Prof. Brugali
Laser Retroreflector Triangulation
29 Robotica - UNIBG - Prof. Brugali
Circular constraints
• Dati a priori: L1(x1, y1), L2(x2, y2)
• Misure : r1, r2
• Incognite : V(x, y)
30 Robotica - UNIBG - Prof. Brugali
Imprecision
31 Robotica - UNIBG - Prof. Brugali