...

Robot Localization - LaDiSpe

by user

on
Category: Documents
33

views

Report

Comments

Transcript

Robot Localization - LaDiSpe
ROBOTICS
01PEEQW
Basilio Bona
DAUIN – Politecnico di Torino
Mobile Robots Navigation
Overview, examples and ongoing research
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Localization
Mapping and world representations
Simultaneous Localization and Mapping (SLAM)
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
3
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Robot Localization
Mapping and world representations
Simultaneous Localization and Mapping (SLAM)
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
4
Mobile Robotics
 Mobile robotics is an
emerging field of research
and a booming market:
 Robots are no longer
employed in repetitive
tasks within a predefined
workspace
Robotics
Industrial
Robotics
Mobile
Robotics
 They can move
autonomously in the
environment exploiting
their perceptions and
performing required actions
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
5
Application scenarios
Mobile Robotics
Domestic Robots
Basilio Bona - DAUIN - PoliTo
Personal Robots
ROBOTICS 01PEEQW - 2015/2016
Elderly and health care
6
Application Scenarios
Mobile Robotics
Military Robots
Basilio Bona - DAUIN - PoliTo
Service Robots
ROBOTICS 01PEEQW - 2015/2016
Science-oriented
7
Robot perception and action
Reasoning
Perception
Gyros
Accelero
-meters
Wheel
encoders
Basilio Bona - DAUIN - PoliTo
Radio
devices
Actuation
Locomotion
Laser
range
finder
Manipulation
GPS
Other
actuators
Camera
ROBOTICS 01PEEQW - 2015/2016
8
Robot perception and action
 Perception modelling
Perception
Gyros
Accelerometers
Wheel
encoders
They do not measure
external variables but
quantities related to
robot motion
Radio
devices
GPS
Not available Laser
range
in indoor
finder
scenarios
Camera
These measurements
are often referred to
as odometry or
commands
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
9
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Robot Localization
Mapping and world representations
Simultaneous Localization and Mapping
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
10
Mobile Robot Navigation
 A crucial prerequisite for a mobile robot to perform tasks in the
surrounding environment is the capability to autonomously
navigate
 Navigation requires the following capabilities:
 Localization: the robot has to be
able to determine its pose wrt a
given reference frame
 Mapping: the agent has to build
Target
a consistent and meaningful
point
representation of the environment
Rr
 Path planning: the robot has to
plan the motion strategy to reach
a given target position
R0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
11
Robot Localization
 The localization problem for planar indoor scenarios and
wheeled robots can be stated as follows:
 Determine the pose of the
robot in a given reference
frame using odometry
and sensor measurements
 State vector describing robot pose (planar case): pt = [xt yt θt]
 u1:t motion estimates from odometry
(controls)
 z1:t measurements from onboard
sensors (measurements)
u1 u 2 L
z1 z 2 L
Basilio Bona - DAUIN - PoliTo
ut
zt
ROBOTICS 01PEEQW - 2015/2016
12
Robot Localization
 Why you cannot simply trust odometry for motion estimation?
 Let us consider the transition of the state from one instant of
time to the next one:
Since the odometry is affected by noise,
instead of getting the true robot
displacement, you will get a noisy estimate
pˆ t : estimate of pose at
time t+1
pt: pose
at time t
pt+1: pose
at time
t+1
 Error accumulates over
time leading the estimate
to diverge from the true position
 Try to estimate your position after walking 100 m with you eyes closed
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
13
Robot Localization
 In Probabilistic Robotics the target is not only to estimate a
value of the unknown variable (robot pose), but also to quantify
the uncertainty on such estimate:
 The uncertainty is explicitly modeled by a probability density (belief)
 Example: Position of the robot in 2D environment:
pt : prob(x , y )
Deterministic Approach
Basilio Bona - DAUIN - PoliTo
probability
pt = [x , y ]
ROBOTICS 01PEEQW - 2015/2016
Probabilistic Approach
14
Robot Localization
 EXAMPLE: Localization within a
sensor infrastructure
 Mobile Robot can acquire odometric
measurements and distance information
from sensors in known positions
Planar simulation of mobile
robot scenario
Belief of the robot
bel (pt ) = prob(x , y | u 1:t , z 1:t )
Range Sensors (RFID Tags)
Mobile Robot
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
15
Robot Localization
 Why is the uncertainty important:
 The robot has to be conscious of
the uncertainty otherwise it risks
to act in a misleading optimism
 How to model the problem?
 The evolution of robot pose can be
easily modeled as a dynamic system:
 pt  f  pt 1 , ut , t 

 zt  h  pt , map,t 
PROCESS MODEL: it describes how the pose evolves in
function of the odometric information
MEASUREMENT MODEL: it describes what quantities
(function of robot pose) are measured by the robot
 We need to estimate pt:
 Localization is a state estimation problem
 But the state is a random variable
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
16
Robot Localization
 A general algorithm to perform this task is the Bayes Filter:
 For each time instant compute the current estimate as:
PREDICTION
A PRIORI ESTIMATE
bel( pt ) = ò prob( pt | pt - 1, u t ) ×bel( pt - 1 )dpt - 1 (includes the
commands in the belief)
D
UPDATE
bel( pt ) = h ×prob(z t | pt - 1 ) ×bel( pt )
A POSTERIORI ESTIMATE
(includes the
measurements in the
belief)
 Such approach is based on the Markov assumption, which
postulates that future states are independent on past ones, given
the current state
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
17
Robot Localization
 In general the prediction phase of the Bayes filter cannot be
computed in closed form, because of the integral and this makes
the previous formulation unsuitable for real time computation
(the filter needs be iterated at each time step)
 Effective implementations of Bayes filter requires further
assumptions on the representation of the probability densities
involved in the estimation:
 Kalman filter: the probability densities are supposed
to be multivariate Gaussians
 Particle filters: the probability densities are
approximated with a set of weighted particles
 Several other solutions do exist, although they will not be treated
in this course (histogram filters, graph-based models, etc)
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
18
EKF LOCALIZATION
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
19
Robot Localization – Kalman Filter Localization
 Preliminaries on Gaussian Densities
 The general equation for a multivariate density with
n
support in
is given by the following expression:
f  x 
1
 2 
n/2

1/ 2
Can be parameterized in:
T
 1

exp    x    1  x    
 2



mean value
covariance
With probability 1-  the random variable x is
contained in the ellipsoid described by the following
inequality:
x  
T
x2
n2
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
 1  x     g 1   
x1
20
Robot Localization – Kalman Filter Localization
 It is possible to demonstrate that if the dynamic system
describing robot motion and measurements is linear, i.e.
 pt  At pt 1  Bt ut  t

zt  Ct pt  t

PROCESS MODEL
MEASUREMENT MODEL
the noises t and  t are zero mean Gaussian noise, with covariance
matrices Rt and Qt respectively, the initial state is p0 N  0 , 0 
then the state at time t will be distributed according to pt N  t , t 
for all the future time steps t (Gaussianity is preserved by the linear
transformation with additive Gaussian noise)
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
21
Robot Localization – Kalman Filter Localization
 How we can use the process model for estimation?
pt  At pt 1  Bt ut  t
 If we have an initial estimate pˆ 0 , 0 and we know that the process
noise is t N  0, Qt , for all time
steps we can iteratively
compute the pose estimate:
pˆ t , t
 pˆ t  At pˆ t1  Bt ut
 
T


A

A
 t
t
t 1 t  Qt
pt
If we use only
odometry for localization
the localization error
accumulates over time!
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
22
Robot Localization – Kalman Filter Localization
 pˆ t , t is called a-priori estimate since it only includes the
information about the command ut
 In order to refine our pose estimate we have to take into account
also the measurements acquired from sensors, i.e., zt
 In order to include zt we simply correct the a-priori estimate by
using the acquired measurements:
a-posteriori
estimate
a-priori
estimate
measured
quantity
quantity that we expect to measure
from our a-priori estimate
pˆ t  pˆ t  K t ( zt  Ct pˆ t )
t  ( I  Kt Ct ) t
INNOVATION
 Kt is the Kalman Gain, and weights the innovation in order to
have a minimum covariance a-posteriori estimate
K t  t CtT (Ct t CtT  Qt ) 1
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
23
Robot Localization – Kalman Filter Localization
 The previous equations constitute the fundamental blocks of the
Kalman filter:
 For each time instant compute the current estimate as:
PREDICTION
A PRIORI ESTIMATE
pˆ t  At pˆ t1  Bt ut

t
(includes the
commands in the belief)
  At t 1 A  Qt
T
t
UPDATE
K t  t CtT (Ct t CtT  Qt ) 1
A POSTERIORI ESTIMATE
pˆ t  pˆ t  K t ( zt  Ct pˆ t )
(includes the
measurements in the
belief)
t  ( I  Kt Ct ) t
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
24
Robot Localization – Extended Kalman Filter (EKF)
 The Kalman filter provides the minimum variance solution for
linear Gaussian systems (optimality)
 pt  f  pt 1 , ut , t 
 In most of the real world problems,

 zt  h  pt , map,t 
however, both the process and
measurements models are expressed by non-linear equations
 Kalman filter can still cope with this case: for filter prediction and
update, the models are linearized, and the approach becomes
f
At 
pt
h
Ct 
pt
pt  pˆ t 1
pˆ t  f  pˆ t1 , ut 
t  At t 1 AtT  Qt
Basilio Bona - DAUIN - PoliTo
UPDATE
PREDICTION
(with no guarantee of optimality!!):
pt  pˆ t
K t  t CtT (Ct t CtT  Qt ) 1
pˆ t  pˆ t  K t  zt  h( pˆ t ) 
t  ( I  Kt Ct ) t
ROBOTICS 01PEEQW - 2015/2016
25
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
Fixed Sensors (deployed in the
environment in known positions)
True position of
the mobile robot
 Mobile Robot
can acquire
odometric
measurements
and distance
information
from sensors in
known
positions
t=0
KF estimate
(time zero)
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
26
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
27
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
- Filter Prediction
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
28
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
29
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
30
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
TIME STEP 2:
- Acquire odometry
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
31
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
TIME STEP 2:
- Acquire odometry
- Filter Prediction
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
32
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
TIME STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
33
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
TIME STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
34
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
TIME STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
...
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
35
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
 EXAMPLE: EKF Localization within a sensor infrastructure
TIME STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
TIME STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Filter Update
...
t=0
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
36
Luca Carlone – Politecnico di Torino
Robot Localization – Extended Kalman Filter (EKF)
Trajectory
estimated
using EKF
Localization
Trajectory
estimated using
measurements
only
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
37
Luca Carlone – Politecnico di Torino
Robot Localization – EKF Localization summary
 Extended Kalman Filter is an
effective solution for mobile
robot localization, but a naïve
implementation does not
always suffice to cope
with the problem at hand:
?
zt  h  pt 
 Linearization can cause filter
divergence if the original problem is highly non-linear
 It can be hard to precisely model
the available information according
to the process and measurement models
 Multivariate Gaussian are unimodal
distributions, hence they can be
unsuitable to model a multimodal
probability belief
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
38
Luca Carlone – Politecnico di Torino
Robot Localization – EKF Localization summary
 Several variants do exist that allow to mitigate the issues
mentioned before:




Multi Hypotheses Tracking
Unscented Kalman filter
Particle Filters
…
 In the following we will focus on
the use of Particle filters for the
estimation of mobile robot pose
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
39
Luca Carlone – Politecnico di Torino
MONTE CARLO
LOCALIZATION
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
40
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 Particle-filters based localization (Monte Carlo Localization)
uses a set of weighted random samples for approximating
robot pose belief:
[i ]
ˆ
bel  pt       pt  pt 
n
i 1
•
[i ]
t
 n [i ]



1
 t

 i 1

where
• n is the particle set size
• ^
p[i]t is the pose of the i-th particle at time t
• ω[i]t is the importance weight of the sample i
• δ( · ) is the Dirac delta function
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
41
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 Monte Carlo Localization:
Particle-based Representation of position belief
Particle-based
approximation
Gaussian approximation
(ellipse: 95% acceptance region)
ROBOTICS 01PEEQW - 2015/2016
Basilio Bona - DAUIN - PoliTo
42
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 Using particle filters approximation, the Bayes Filter can be
reformulated as follows (starting from the robot initial belief at
time zero):

1. PREDICTION: Generate a new particle set given motion
model and controls applied
2. UPDATE: Assign to each particle an importance weight
according to sensor measurements
3. RESAMPLING: Randomly resample particles in function of their
importance weight
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
43
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
1. PREDICTION: Generate a new particle set given motion model
and controls applied
 For each particle:

Given the particle pose at time step t-1 and from the commands ,
you predict particle pose at time t, using the motion model:
pˆ t[ i ]  f  pˆ [t i]1 , ut , t 


The variable 
t
is randomly
extracted
according to the
noise distribution
We obtain a set of predicted particles
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
 t
44
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
2. UPDATE: Assign to each particle an importance weight
according to sensor measurements
 For each particle:
 Compare particle’s prediction of measurements
with actual measurements
zt VS h( pˆ t[ i ] )
 Particles whose predictions match the measurements are given a
high weight
RED: particles
with high weights
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
45
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
3. RESAMPLING: Resample particles based on weight
 For n times draw a particle from  t (with replacement) with
probability given by the importance weights and put it in the set  t
RED: particles
with high weights
 The new set
provides the particle-based approximation of the
t
robot pose at time t
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
46
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
Fixed Sensors (deployed in the
environment in known positions)
True position of
the mobile robot
 Mobile Robot
can acquire
odometric
measurements
and distance
information
from sensors in
known
positions
Initial
particle set
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
47
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
48
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
- Filter Prediction
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
49
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
50
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
51
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
Filter Prediction
Acquire meas.
Weights Update
Resampling
52
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
53
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
54
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
55
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
56
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
57
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
STEP 3:
- Acquire odometry
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
58
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
STEP 3:
- Acquire odometry
- Filter Prediction
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
59
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
STEP 3:
- Acquire odometry
- Filter Prediction
- Acquire meas.
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
60
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
STEP 3:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
61
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
STEP 1:
- Acquire odometry
-
Filter Prediction
Acquire meas.
Weights Update
Resampling
STEP 2:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
STEP 3:
- Acquire odometry
- Filter Prediction
- Acquire meas.
- Weights Update
- Resampling
62
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
Luca Carlone – Politecnico di Torino
Robot Localization – Particle Filters (MCL)
 EXAMPLE: Monte Carlo Localization within a sensor infrastructure
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
63
Luca Carlone – Politecnico di Torino
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Robot Localization
Mapping and world representations
Simultaneous Localization and Mapping (SLAM)
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
64
Luca Carlone – Politecnico di Torino
Mapping and world representations
 Mapping is the task of building a consistent representation of the
prob(m | x1:t , z1:t )
environment assuming that we know robot poses
 Depending on the scenario it is convenient to use different world
representations (to mention just a few):
Digital elevation map
landmarks
Point-cloud
representation
Landmark-based representation
(stochastic map)
Basilio Bona - DAUIN - PoliTo
Occupancy
grid-map
ROBOTICS 01PEEQW - 2015/2016
65
Luca Carlone – Politecnico di Torino
Mapping and world representations
 We will briefly describe two particular representations which are
widespread in the applications of mobile robotics in indoor
scenarios
Landmark-based maps: stochastic maps
that contain a probabilistic description
(usually mean+covariance) of the position
of salient features of the scenario
Occupancy grid maps: high- resolution
models of the environment. It is a grid
in which each cell contains the probability
of the cell being occupied
Landmarks (doors,
corners, objects..)
Free cell
Unknow cell
Occupied cell
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
66
Luca Carlone – Politecnico di Torino
Mapping and world representations
Landmark-based map:
+ compact world representations
(low memory occupation)
+ efficient management
+ the problem can be solved in analogy
with the approaches presented for
robot localization
- do not directly provide information for
navigation (i.e. obstacle avoidance)
- How to distinguish features?
(data association)
Basilio Bona - DAUIN - PoliTo
Occupancy grid map:
+ fast probabilistic update rules
+ suitable for navigation: widespread path
planning algorithms (A*, D*, etc.) can
be adapted to deal with grid maps
+ are intuitive models of the environment
- remarkable memory occupation
ROBOTICS 01PEEQW - 2015/2016
67
Luca Carlone – Politecnico di Torino
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Robot Localization
Mapping and world representations
Simultaneous Localization and Mapping (SLAM)
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
68
Luca Carlone – Politecnico di Torino
Simultaneous Localization and Mapping (SLAM)
 Localization is based on the knowledge of a map of the
environment, while mapping presupposes the knowledge
of robot pose
 However in real indoor scenarios:
 It is rare to have a map of the environment
 No absolute position information is available
 The robot has to acquire the map
of the environment while localizing
itself wrt to this map
 What happens if we simply perform
localization using the tools learned so
far and we do mapping with the estimated pose?
 Mapping and localization are not independent, hence a joint
solution is needed: Simultaneous Localization and Mapping (SLAM)
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
69
Luca Carlone – Politecnico di Torino
Simultaneous Localization and Mapping (SLAM)
 We need to estimate the joint distribution of both robot pose
and map representation of the environment:
belt  p ( x1:t , m | z1:t , u1,t )
 The algorithms employed depend mainly on which type
of map needs to be estimated:
 Landmark-based maps




EKF-SLAM
Sparse Extended Information Filters
GraphSLAM
Rao-Blackwellized Particle Filters (FastSLAM)
 Occupancy grid maps
 Rao-Blackwellized Particle Filters
 GraphSLAM
 …
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
70
Luca Carlone – Politecnico di Torino
EKF-SLAM
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
71
Luca Carlone – Politecnico di Torino
EKF-SLAM
 Intuition: include the position of the landmarks in the state
vector and perform estimation over an augmented state:
xt  [ xr ,t , yr ,t , r ,t , l1x , l1 y ,..., lNx , lNy ]T
localization
l1
mapping
 According to EKF framework the SLAM
belief will be described by a multivariate
Gaussian in 3 2 N
   xr 
  xr xr



   yr 
  xr y r
  

  r 
 xr r
  l 
 x l
xt ~ N   1 x  ,  r 1 x
t=0
   l1 y 
  xr l1 y
  
 



   l Nx 
  xr l Nx
  
 x l
 r Ny
  l Ny 
Basilio Bona - DAUIN - PoliTo
x y
y y
y
yl
yl
x
y
 
 l
 l


yl
yl
 l
 l
r r
r r
r r
r 1x
r 1y
r Nx
r Ny
…
r r
r r
r r
r 1x
r 1y
r Nx
r Ny
x l
yl
 l
l l
l l
xl
yl
 l
l l
l l


r 1x
r 1x
r 1x
1x 1x
1x 1 y
l
l
1 x l Nx
1 x l Ny
r 1y
r 1y
r 1y
1x 1 y
1y 1y
l
l
1 y l Nx
1 y l Ny
  xr l Nx
  yr l Nx
   r l Nx
  l1 xl Nx
  l1 y l Nx


  l Nxl Nx
  l Nxl Ny
ROBOTICS 01PEEQW - 2015/2016
x l
yl
 l
l l
l l



r Ny

r Ny  

1 x Ny

1 y Ny  
 

 l Nxl Ny  
 l Nyl Ny  

r Ny
l2
r
 xr ,t 
y 
 r ,t 
 r ,t   ptr 

  1
l
 1x    
 l1 y   ... 

  N
 ...   
l 
 Nx 
 l Ny 
lN
72
Luca Carlone – Politecnico di Torino
EKF-SLAM
1
 We can simply extend the expression
of the dynamic system used for
EKF Localization
 [l1x , l1 y ]
N
 [lNx , lNy ]
 pt  f  pt 1 , ut , t 

 zt  h  pt , map,t 
PROCESS MODEL
 ptr   f  ptr.1 , ut , t  

 1 
1

 

 ...  
...

 N 
N

  
MEASUREMENT MODEL
zt  h  ptr ,
1
t
,...,
N
t
,t 
 So the estimation procedure can proceed according to EKF,
iterating prediction and update phases
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
73
Luca Carlone – Politecnico di Torino
EKF-SLAM
 EKF-SLAM: a typical situation
(1) Robot starts; first
measurement of feature A
(2) Robot drives forwards
(uncertainty grows)
(3) Robot makes first
measurements of B and C
(4) Robot
drivesvector
back towards
(6)discovered.
Robot re-observes B; also
State
xt canthe
be grown
new landmarks
(5) Robotasre-observes
A (loop are
start (uncertainty grows more)
the uncertainty of C shrinks
closure); uncertainty shrinks
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
74
Luca Carlone – Politecnico di Torino
EKF-SLAM summary
 Effective and easy to implement solution to landmark-based SLAM
 Have been applied successfully in large-scale environments
ISSUES:
 Complexity is quadratic in the
number of landmarks: O(n2)
G
N
LI
SCA
 Can diverge if non linearities are large!
 Data association: how do we decide if
we are observing an already seen landmark?
 Dynamic environments: EKF-SLAM requires
the environment to be static!
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
75
75
Luca Carlone – Politecnico di Torino
RBPF-SLAM
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
76
Luca Carlone – Politecnico di Torino
Rao-Blackwellized Particle Filters (RBPF-SLAM)
 Rao-Blackwellized Particle Filters (RBPF) are a sample-based
techniques for solving SLAM
 They are particularly suitable for estimating occupancy gridmaps of an indoor environment
 Rao-Blackwellized Particle Filters are based on the following
factorization of the SLAM belief:
Robot
trajectory
Map of the
environment
Measurements
Commands
belt  prob( x1:t , m | z1:t , u1,t ) 
prob(m | x1:t , z1:t )  prob( x1:t | z1:t , u1,t )
SLAM
belief
Basilio Bona - DAUIN - PoliTo
mapping
localization
77
ROBOTICS 01PEEQW - 2015/2016
77
Luca Carlone – Politecnico di Torino
Rao-Blackwellized Particle Filters (RBPF-SLAM)
 The previous formula is known as Rao-Blackwell factorization, which
gives the name to the corresponding solution to SLAM
 The underlying math traduces in the following simple concept:
 The particle filter estimate potential trajectories of the robot and for
each hypothesis the SLAM reduces to mapping with known poses
 Contrarily to localization, each particle is not a pose hypothesis but a
trajectory hypothesis!
The filter estimates potential
trajectories and a map is associated
to each trajectory hypothesis
78
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
78
Luca Carlone – Politecnico di Torino
RBPF-SLAM summary
 Rao-Blackwellized Particle Filters (RBPF) have been
demonstrated to be an effective solution for the estimation of
occupancy grid maps
 No data association (grid maps does not distinguish elements in
the environment)
ISSUES:
 Each particle carries on a complete map hypothesis: a common
computer is not able to manage more than few hundreds of particles
but: to cope with large uncertainty you need a large number of particles
 RBPF-SLAM requires the environment to be static!
79
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
79
Luca Carlone – Politecnico di Torino
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Robot Localization
Mapping and world representations
Simultaneous Localization and Mapping (SLAM)
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
80
Luca Carlone – Politecnico di Torino
Path planning
 After the estimation of a map (occupancy grid-map for instance)
it is straightforward to apply common path planning algorithms
 Without entering into details we just mention few well-known
approaches to path planning, common in robotics (and not
only..):








Dijkstra algorithm, A*, D*
Value iteration
Vector Field Histogram
Wavefronts
Markov Decision Processes
Potential fields
Sampling-based schemes
…
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
81
Luca Carlone – Politecnico di Torino
Contents
 Introduction
 Mobile Robotics
 Application scenarios
 Robot perception and action
 Mobile Robots Navigation




Robot Localization
Mapping and world representations
Simultaneous Localization and Mapping (SLAM)
Path Planning
 Examples and current research effort
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
82
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Rao-Blackwellized Particle Filters SLAM
 A robot moving
in an unknown
environment
has to
perform
Simultaneous
Localization
and Mapping
using wheel
odometry and
a laser scanner
RBPF-SLAM
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
Odometry based mapping
83
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Rao-Blackwellized Particle Filters SLAM in multi robot systems
Map of Robot 1
Map of Robot 2
Final Map
Filter estimation is based on virtual
data exchanged among robots when
they meet each other
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
84
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Simultaneous Localization and Mapping using
omnidirectional camera
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
85
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Simultaneous Localization and Mapping using
omnidirectional camera
Visual Odometry
(filter prediction)
Basilio Bona - DAUIN - PoliTo
Image
segmentation
for
estimating
distance from
obstacles
(weights
update)
ROBOTICS 01PEEQW - 2015/2016
86
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Simultaneous Localization and Mapping using
omnidirectional camera
 Simulations
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
87
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Simultaneous Localization and Mapping
using omnidirectional camera
 Real tests
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
88
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Active SLAM and exploration
 A robot, deployed in an unknown environment, is required to
actively control its movements in order to:
Maximize the explored areas
(minimize time required for
exploration)
Minimize the uncertainty in
Simultaneous Localization
and Mapping
In order to properly plan its policy, the robot needs
reliable metrics for evaluating the expected gain of a
motion policy (information gain-based exploration).
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
89
Luca Carlone – Politecnico di Torino
Examples and current research effort
 Active SLAM and exploration
 Expected Information from a policy
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
90
Luca Carlone – Politecnico di Torino
Stereo Vision
OBJECTIVE
 Extract 3D information from stereo
devices to support fully autonomous
robot navigation
 Preliminary test with a cheap
stereo webcam for computing the
depth map of the environment
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
91
Luca Carlone – Politecnico di Torino
Stereo Vision
 System has been tested on an I-Robot Create Platform
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
92
Luca Carlone – Politecnico di Torino
LOCALIZATION
AND SERVICE
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
93
Federico Tibaldi – Politecnico di Torino
Outline
 Multirobot localization
 Intrusion detection and tracking
 Task allocation
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
94
Federico Tibaldi – Politecnico di Torino
Multi Robot Localization
 Estimation of the pose for a team of robots in an highly
symmetrical environment ( e.g. logistic area )
 the robots are able to spread the knowledge about small
asymmetries


localization speed-up
Robustness
 Information exchange is also used to:
 Monitoring the team localization conditions

“When can I start to execute my tasks?”
 Finite state machine:

Switch state according to received position estimates
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
95
Federico Tibaldi – Politecnico di Torino
Multi Robot Localization
 Robots can:
 Detect and recognize another robot (camera + markers)
 Estimate its position ( camera + laser scan )
 Send the estimate ( wi-fi network )
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
96
Federico Tibaldi – Politecnico di Torino
Intrusion detection and tracking
 Detection of unexpected agents
 Laser range reading shape
 Omnidirectional camera
 Tracking
 KF for each intruder
 “intuders” can be followed by the robot
 a video stream can be acquired for situation
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
97
Federico Tibaldi – Politecnico di Torino
Target tracking
OBJECTIVE
 Application of the Kalman Filter for the estimation of the position
of a target of interest, using OpenCV primitives
•
Monocular
approach
Basilio Bona - DAUIN - PoliTo
•
ST SmartCam
(RVS)
ROBOTICS 01PEEQW - 2015/2016
•
Tracking from
stereo vision
98
Federico Tibaldi – Politecnico di Torino
Target tracking
 Master students are currently
working on the integration of
a pan-tilt stereo camera on a
mobile platform for target
tracking and following
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
99
Federico Tibaldi – Politecnico di Torino
Task allocation
 Tasks are assigned to the team of robots:
 Allocation algorithm should be distributed


NO single point of failure
System is completely autonomous
 Market based algorithm
 An auction for each task

One auctioneer


Different for each task
Many bidders
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
100
Federico Tibaldi – Politecnico di Torino
THANK YOU
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
101
Robot Localization
 A general algorithm to perform this task is the Bayes Filter:
 For each time instant compute the current estimate :
PREDICTION
bel( pt ) = ò prob( pt | pt - 1, u t ) ×bel( pt - 1 )dpt - 1
A PRIORI ESTIMATE
(includes the
commands in the belief)
D
UPDATE
bel( pt ) = h ×prob(z t | pt - 1 ) ×bel( pt )
A POSTERIORI ESTIMATE
(includes the
measurements in the belief)
 The expressions for the belief cannot be computed in closed form,
because of the integral
 Effective implementations of Bayes filter require further assumptions:
 Kalman filter: the probability densities are supposed to be
multivariate Gaussians
 Particle filters: the probability densities are approximated with
a set of weighted particles
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
102
Robot Localization
 A general algorithm to perform this task is the Bayes Filter:
 For each time instant compute the current estimate as:
PREDICTION
bel( pt ) = ò prob( pt | pt - 1, u t ) ×bel( pt - 1 )dpt - 1
A PRIORI ESTIMATE
(includes the
commands in the belief)
D
UPDATE
bel( pt ) = h ×prob(z t | pt ) ×bel( pt )
A POSTERIORI ESTIMATE
(includes the
measurements in the belief)
 In general the previous expressions cannot be computed in closed form,
because of the integral
 Effective implementations of Bayes filter require further assumptions:
 Kalman filter: the probability densities are supposed to be
multivariate Gaussians
 Particle filters: the probability densities are approximated with
a set of weighted particles
103
Basilio Bona - DAUIN - PoliTo
ROBOTICS 01PEEQW - 2015/2016
Luca Carlone – Politecnico di Torino
•
•
•
An hyperbolic catadioptric camera is
a central catadioptric camera iff it is
set in single effective viewpoint
condition (SVP)
SVP occurs if the camera sensor and
the focus of the hyperbola are at a
distance equal to twice the
hyperbola focal length f along the
hyperboloid central axis [Baker and
Nayar 98]
A calibrated central catadioptric
camera could be treated in a similar
way to a common perspective
camera
7/5/2010
Basilio Bona - DAUIN - PoliTo
NOT SVP
Other mirror shapes that allow
SVP condition are ellipsoidal
mirror or parabolic mirror (+
ortographic lens). See [Baker and
Nayar 98]
Large field of view dioptric camera
(i.e. using fisheye lens) could be
calibrated using the same model
104
ROBOTICS 01PEEQW - 2015/2016
104
• The central catadioptric camera
model that we adopt is the one
described in [Scaramuzza et al.
2006] and related Matlab®
toolbox
• Camera calibration consists in a
polynomial estimation of the
forward and inverse projection
function
• Calibration is performed once,
then the system intrinsic
extrinsic parameters are stored
on a file. It takes into account
errors induced by non perfect
SVP condition and hardware
quality.
f(u,v) is the
forward
projection
function
• Once calibration is performed, a
relation between a pixel and a 3D
point can be established up to a
scale factor.
7/5/2010
Basilio Bona - DAUIN - PoliTo
105
ROBOTICS 01PEEQW - 2015/2016
105
Fly UP