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