Transcript Review/SLAM
Advanced Mobile Robotics Probabilistic Robotics:
Review/SLAM Dr. Jizhong Xiao Department of Electrical Engineering CUNY City College [email protected]
City College of New York
1
Bayes Filter Revisit
Bel
(
x t
)
P
(
z t
|
x t
)
P
(
x t
|
u t
,
x t
1 )
Bel
(
x t
1 )
dx t
1
• Prediction (Action)
bel
(
x t
)
p
(
x t
|
u t
,
x t
1 )
bel
(
x t
• Correction (Measurement)
1 )
dx t
1
bel
(
x t
)
p
(
z t
|
x t
)
bel
(
x t
)
City College of New York
Probabilistic Robotics Probabilistic Sensor Models Beam-based Model Likelihood Fields Model Feature-based Model
City College of New York
Beam-based Proximity Model
Measurement noise Unexpected obstacles 0 z exp z max 0 z exp z max
P hit
(
z
|
x
,
m
) 1 2
b
Gaussian Distribution
e
1 2 (
z
z
exp ) 2
b P
unexp (
z
|
x
,
m
) 0 e
z
Exponential Distribution
z
z
exp
otherwise
4
City College of New York
Beam-based Proximity Model
Random measurement Max range 0 z exp z max 0 z exp z max
P rand
(
z
|
x
,
m
) 1
z
max Uniform distribution
P
max (
z
|
x
,
m
) 0 1
if z
z
max
otherwise
Point-mass distribution
City College of New York
5
Resulting Mixture Density
P
(
z
|
x
,
m
) hit unexp max rand
T P
hit (
z
|
x
,
m
)
P
unexp
P
max ( (
z z
| |
x x
, ,
m
)
m
)
P
rand (
z
|
x
,
m
) Weighted average, and
hit
un
exp max
rand
1 How can we determine the model parameters?
{
hit
,
un
exp , max ,
rand
, , } System identification method: maximum likelihood estimator (ML estimator) 6
City College of New York
Likelihood Fields Model
• Project the end points of a sensor scan Zt into the global coordinate space of the map
x z t k y z t k
x y
cos sin sin cos
x ksens y ksens
z t k
cos( sin(
ksens
) )
ksens
• 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.
p
(
z t k x t
,
m
)
hit
p hit
max
p
max
rand
p rand
• Again, independence between different components is 7 assumed.
City College of New York
x z t k y z t k
Likelihood Fields Model
x y
cos sin sin cos
x ksens y ksens
z t k
cos( sin(
ksens
ksens
) ) Distance to the nearest obstacles. Max range reading ignored
City College of New York
8
Example
Example environment P(z|x,m) Sensor probability Likelihood field The darker a location, the less likely it is to perceive an obstacle O1 O2 O3
City College of New York
Zmax Oi : Nearest point to obstacles 9
Feature-Based Measurement Model
• • Feature vector is abstracted from the measurement:
f
(
z t
) {
f t
1
f t
2 Sensors that measure
range, bearing
, & a
signature
} {
r s t t
1 1 1 ,
r
s t t
2 2 2 , } e.g., an average color) • • Conditional independence between features
p
(
f
(
z t
)
x t
,
m
)
i p
(
r t i
,
t i
,
s t i m
m
1
m
2
m j
{
m j
,
x
,
m j
,
y
,
x t
,
m
)
s j
}
T
i.e., a location coordinate in global coordinates & a signature • • Robot pose:
x t
{
x y
}
T
Measurement model:
r t i
s t t i i
a
(
m j
,
x
tan 2 (
m j
,
y x
) 2
r
2
Zero-mean Gaussian error variables with standard deviations City College of New York
s j
y
, (
m j
,
y m j
,
x
y x
) ) 2 )
r
r
2 2
s
2 10
Probabilistic Robotics Probabilistic Motion Models
City College of New York
Odometry Model
u
, . 2 ,
trans
trans
(
x
'
x
)
2
(
y
'
y
)
2
rot
1
atan2 (
y
'
y
,
x
'
x
)
rot
2
'
rot
1
x
' ,
y
' , '
x
,
y
,
rot
1
trans
Relative motion information, “ rotation ” “ translation ” “ rotation ”
City College of New York
rot
2
Noise Model for Odometry • The measured motion is given by the true motion corrupted with
independent
noise.
ˆ
rot
1
rot
1 1 |
rot
1 | 2 |
trans
| How to calculate : ˆ
trans
trans
3 |
trans
| 4 |
rot
1
rot
2 |
p
(
x t u t
,
x t
1 ) ˆ
rot
2
rot
2 1 |
rot
2 | 2 |
trans
| 2 (
x
) 1 2 2
e
1 2
x
2 2 2 (
x
) 0 if | x | 6 2 6 2 | 6 2
x
|
City College of New York
Calculating the Posterior
An initial pose X t-1
Given x
t
, x
t-1
, and u
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
A hypothesized final pose X t A pair of poses u obtained from odometry Algorithm
motion_model_odometry (x t , x t-1 , u)
trans
rot
1
rot
2 atan2 ( '
x
' (
y x
) ' 2
y
rot
1 , (
y
'
x
'
x
)
y
) 2
p p p
ˆ ˆ
trans
rot
1 ˆ
rot
1 2 3 2 atan2 ' prob prob prob ( ( ( (
x
' (
x y
rot 1 trans rot 2 ) ˆ ' 2
y
rot
1 ˆ , (
x
'
y
rot 1 , ˆ trans ˆ rot 2 , ' ,
x
) 1
y
1 | ) 3 | 2 ˆ ˆ trans rot 1 ˆ rot 2 | values of interest (x t-1 , x t ) | 4 2 2 (| ˆ trans ) ˆ rot1 ˆ trans ) | odometry values (u) |
u
ˆ rot2
x t
1
x t
|))
x t
prob
( 1 ( (
x x
a
,
b y y
x
)
t
T
)
T
)
T
11.
return
p 1 · p 2 · p 3
Implements an error distribution over a
p
(
x t u t
,
x t
1 ) with zero mean and standard deviation b 14
City College of New York
Application
• Repeated application of the sensor model for short movements.
• Typical banana-shaped distributions obtained for 2d-projection of 3d posterior.
p
(
x t | u, x t-1
) x ’ u x ’ u Posterior distributions of the robot ’ s pose upon executing the motion command illustrated by the solid line. The darker a location, the more likely it is.
City College of New York
Velocity-Based Model
control
u
v
City College of New York
Rotation radius
r
v
16
Equation for the Velocity Model
Instantaneous center of curvature (ICC) at (x c Initial pose
x t
1
x y
T x y
x c y c
r r
sin
cos , y c ) Keeping constant speed, after ∆t time interval, ideal robot will be at
x t
x
y
T x
y
x c
y c
r
sin(
t
)
r
cos(
t
t
)
x
y
r r
sin cos
r
r
sin( cos(
t
t
)
t
)
City College of New York
Corrected, -90 17
Velocity-based Motion Model
x t
x
'
y
' '
T
x
y
x t
v
ˆ
t
ˆ
v
ˆ
t
ˆ
t t
sin cos
v
ˆ
t
ˆ
v
ˆ
t t
ˆ ˆ
t
sin( cos(
t
t
ˆ ˆ
t t
t
t
) )
T
The true motion is described by a translation velocity and a rotational velocity
t
v
ˆ ˆ
t t
t t
u t
(
v t
( 1 3
v t v t t
)
T
2 4
t
t
) 2 2
v
t t
v
ˆ ( 0 ,
M M t t
) ( 1
v t
0 2
t
) 2 Circular motion assumption leads to degeneracy , 2 noise variables v and w ˆ 3D pose Assume robot rotates when arrives at its final pose ˆ 5
t v
6 ( 3
t
ˆ
t v t
0 4
t City College of New York
) 2 18
Velocity-based Motion Model
Motion Model:
v
ˆ
t
ˆ
t
x y
' ' '
v t
t
x y
( 1
v t
2
v
ˆ
t
ˆ
v
ˆ
t
ˆ
t t
t
3
v t
4
t
cos ) 2 2 sin ˆ
t
v
ˆ
t
ˆ
v
ˆ
t
ˆ
t
t t
sin( cos( ˆ
t
ˆ ˆ
t t
t
t
) ) 1 to 4 are robot-specific error parameters determining the velocity control noise ˆ 5
v
6 5 and 6 are robot-specific error parameters determining the standard deviation of the additional rotational noise
City College of New York
19
Probabilistic Motion Model
Center of circle: Move with a fixed velocity during ∆t resulting in a circular trajectory from
x t
1
x y
T
to
x t
x
y
T
with Radius of the circle:
r
* (
x
x
* ) 2 (
y
y
* ) 2 (
x
x
* ) 2 (
y
y
* ) 2 Change of heading direction:
a
tan 2 (
y
y
* ,
x
x
* )
a
tan 2 (
y
y
* ,
x
x
* )
v
ˆ
dist
t
r
*
t
t
ˆ
t City College of New York
ˆ (angle of the final rotation) 20
Posterior Probability for Velocity Model
Center of circle Radius of the circle Change of heading direction Motion error: v err ,w err and ˆ 21
City College of New York
Examples (velocity based)
City College of New York
Map-Consistent Motion Model
p
(
x
|
u
,
x
' )
p
(
x
|
u
,
x
' ,
m
) Obstacle grown by robot radius Map free estimate of motion model
p
(
x
|
u
,
x
' ) Approximation: “ consistency ” of pose in the map
p
(
x
|
m
) “ =0 ” when placed in an occupied cell
p
(
x
|
u
,
x
' ,
m
)
p
(
x
|
m
)
p
(
x
|
u
,
x
' )
City College of New York
Summary
• We discussed motion models for odometry-based and velocity-based systems • We discussed ways to calculate the posterior probability
p(x| x
’
, u)
.
• Typically the calculations are done in fixed time intervals
t
.
• In practice, the parameters of the models have to be learned.
• We also discussed an extended motion model that takes the map into account. 24
City College of New York
Probabilistic Robotics Localization
“ Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.
” [Cox ’ 91]
City College of New York
Localization, Where am I?
?
• • •
Given
– Map of the environment.
– Sequence of measurements/motions.
Wanted
– Estimate of the robot ’ s position.
Problem classes
– Position tracking (initial robot pose is known) – Global localization (initial robot pose is unknown) – Kidnapped robot problem (recovery)
City College of New York
26
Markov Localization
p
(
x
1 : t |
z
1 : t ,
u
1 : t ,
m
)
City College of New York
27
Bayes Filter Revisit
Bel
(
x t
)
P
(
z t
|
x t
)
P
(
x t
|
u t
,
x t
1 )
Bel
(
x t
1 )
dx t
1
• Prediction (Action)
bel
(
x t
)
p
(
x t
|
u t
,
x t
1 )
bel
(
x t
• Correction (Measurement)
1 )
dx t
1
bel
(
x t
)
p
(
z t
|
x t
)
bel
(
x t
)
City College of New York
EKF Linearization
First Order Taylor Expansion
• Prediction:
g
(
u t
,
x t
1 )
g
(
u t
,
x t
1 )
g
(
u t
,
t
1 )
g
(
u t
,
t
1 )
g
(
u t
x t
, 1
t
1 )
G t
(
x t
1 (
x t
1
t
1 )
t
1 )
• Correction:
h h
(
x t
)
h
( )
t
(
x t
t h
(
x t
)
h
(
t
)
H t
(
x t
) (
x t
t
)
t
)
City College of New York
29
EKF Algorithm
1.
Extended_Kalman_filter
(
t-1 ,
S
t-1 , u t , z t
): 2.
3.
4.
5.
6.
7.
8.
9.
Prediction:
t
S
t
g
(
u t
,
t
1 )
G t
S
t
1
G t T
R t
Correction:
K t
t
S
t
S
t t H
t T
(
K t H
(
t z t
S
t
H t h
(
T
t Q t
)) ) 1 (
I
K t H t
) S
t
Return
t ,
S
t H t
h
(
x t
t
)
City College of New York
S
t t
A t
t
1
B t u t A t
S
t
1
A t T
R t K t
t
S
t
S
t
t C t
T
(
K t C
(
t z
S
t t C t
T C t
Q t t
) (
I
K t C t
) S
t
) 1
G t
g
(
u t
x t
, 1
t
1 ) 30
1.
3.
G t
EKF_localization
(
t-1 ,
S
t-1 , u t , z t , m
):
Prediction:
g
(
u t
x t
, 1
t
1 )
x
'
t y
1 , '
x
t t
1 , 1 , '
x x
x
'
t y
1 , '
y
t t
1 , 1 , '
y y
x
'
t y
1 , '
t t
1 , ' 1 , Jacobian of g w.r.t location 5.
6.
V t M t
g
(
u t
,
u t
t
1 ) 1 |
v t
| 2 |
t
0 | 2
x
'
v y t
'
v t
'
v t
3 |
v t
x
'
y
' '
t t
t
| 0 4 |
t
| 2 7.
8.
t
S
t
g
(
u t
,
t
1 )
G t
S
t
1
G t T
V t M V T
Jacobian of g w.r.t control Motion noise covariance Matrix from the control Predicted mean Predicted covariance 31
x t
Velocity-based Motion Model
v
ˆ
t t
v
ˆ
t
v
ˆ
t t
ˆ
t
sin( cos(
t
t
ˆ
t t
t
)
t
)
x
'
y
' '
T
y
x t x
v
ˆ
t t
sin cos
T v
ˆ
t
v
ˆ
t
ˆ
t
u t
(
v t
t
)
T
v t
t
( 1 3
v t v t
2
t
4
t
) 2 2
v t
t
( 0 ,
M t
)
M t
( 1
v t
2
t
) 2 0
City College of New York
( 3
v t
0 4
t
) 2 ˆ
t
32
Velocity-based Motion Model
Motion Model:
x x
'
y t
' '
g
(
x y u t
,
v t
t v
t t x t
1 ) sin cos
N
( 0 ,
R t
v t
t
sin(
v t
t
cos(
t
t
)
t t
t
)
t
)
N
( 0 ,
R t
)
g
(
u t
,
x t
1 )
g
(
u t
,
x t
1 )
g
(
u t
,
t
1 )
g
(
u t
,
t
1 )
g
(
u
x t t
, 1
t
1 )
G t
(
x t
1 (
x t
1
t
1 )
t
1 )
City College of New York
33
Velocity-based Motion Model
G t x
'
y
' '
x
y
v t
t v t
t
sin cos
v t
t
sin(
v t
t
cos(
t
t
t t
t
)
t
)
N
( 0 ,
R t
) (
x t
1 ,
t
1 )
g
(
u t
x t
, 1
t
1 )
x
'
t
y
1 ,
x
'
t t
1 , 1 ,
x x
x
'
t
y
1 ,
y
t t
1 , 1 ,
y y
x
'
t
y
1 ,
t t
1 , 1 ,
x
t
1 ,
x
Derivative of g along x ’ dimension, w.r.t. x at
t
1 Jacobian of g w.r.t location 34
City College of New York
Velocity-based Motion Model
V t x
'
y
' '
x
y
v t
t v t
t
sin cos
g
(
u t
,
u t
t
1 ) sin cos sin( cos(
t
0
t
v t
t
sin(
v t
t
cos(
t
t
t t
t
)
t
) Mapping between the motion noise in control space to the motion noise in state space
N
( 0 ,
R t
)
t
t
t
)
v t y
'
v t
v t
'
t
)
x
'
v t v t
x
y
(sin '
t
' '
t t
(cos Jacobian of g w.r.t control Derivative of g w.r.t. the motion parameters,
u t
t
1 sin(
t
2 cos(
t
2
t
t t
))
t
t
))
v t v t
cos( sin(
t
t
t
t
t
t
)
t
)
t
S
t
G t
S
t
1
G t T
V t M t V t T
35
City College of New York
1.
3.
EKF_localization
(
t-1 ,
S
t-1 , u t , z t , m
):
Correction:
z
ˆ
t
atan
m x
2
m y
t
,
x
t
2 ,
y
,
m x
m y
t
,
x
t
,
y
2
t
, Predicted measurement mean 5.
6.
7.
8.
9.
10.
H t
h
(
t
x t
,
m
)
Q t S t
0
r
2
H t
0 S
t r
2
H t T
Q t
r t
t t
, ,
t x x
r t
t t
, ,
t y y
r t
t t t
, , Jacobian of h w.r.t location Pred. measurement covariance
K t
S
t H t T S t
1
t
S
t
t
I
K t K t
(
z t H t
S
t z
ˆ
t
) Kalman gain Updated mean Updated covariance
City College of New York
36
Feature-Based Measurement Model
•
r t i
s t t i i
Jacobian of
h
w.r.t location (
m j
,
x
a
tan 2 (
m
j
,
y x
) 2
s j
y
, (
m j
,
y m j
,
x
y x
) ) 2 )
r
2 2
s
2
H t z t i h
(
x t
)
h
(
x t h
(
t
,
h
(
t
x t
,
m
) )
j
,
m
)
h
(
x t N
t
( 0 ,
Q t
) (
x t
)
r t
t t
, ,
t x x
r t
t t
, ,
t y y
t
)
r t
t t t
, ,
j
C t i
Is the landmark that corresponds to the measurement of
z t i City College of New York
37
EKF Localization with known correspondences
City College of New York
38
EKF Localization with unknown correspondences Maximum likelihood estimator
City College of New York
39
EKF Prediction Step
Initial estimate is represented by the ellipse centered at
t
1 Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at
t
Small noise in translational & rotation High rotational noise High translational noise High noise in both translation & rotation
City College of New York
40
EKF Measurement Prediction Step
True robot (white circle) & the observation (bold line circle) Innovations (white arrows) : differences between observed & predicted measurements 41
City College of New York
EKF Correction Step
Update mean estimate & reduce position uncertainty ellipses
City College of New York
42
Estimation Sequence (1)
EKF localization with an accurate landmark detection sensor Dashed line: estimated robot trajectory Dashed line: corrected robot trajectory Solid line: true robot motion Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection
City College of New York
43
Estimation Sequence (2)
EKF localization with a less accurate landmark detection sensor Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection 44
City College of New York
Comparison to Ground Truth
City College of New York
45
UKF Localization
?
• • •
Given
– Map of the environment.
– Sequence of measurements/motions.
Wanted
– Estimate of the robot ’ s position.
UKF localization
City College of New York
46
Unscented Transform
0
i
Sigma points Weights 0
w m
n
w c
0
n
( 1 2 ) (
n
) S
i w i m
w i c
2 (
n
1 ) for
i
1 ,..., 2
n
Pass sigma points through nonlinear function
i
g
(
i
) Recover mean and covariance '
i
2
n
0
w m i
i
For n-dimensional Gaussian λ is scaling parameter that determine how far the sigma points are spread from the mean If the distribution is an exact Gaussian, β=2 is the optimal choice.
S '
i
2
n
0
w c i
(
i
)(
i
)
T City College of New York
47
UKF_localization
(
t-1 ,
S
t-1 , u t , z t , m
):
Prediction:
M t
1 |
v t
| 2 |
t
| 2 0 3 |
v t
| 0 4 |
t
| 2 Motion noise
Q t
t a
1 0
r
2
t T
1 0
r
2
T
Measurement noise Augmented state mean S
t
1 0 0 S
t a
1
t t a
1
x
0 0
g
t a
1
u t
M
0
t
0
Q t
t
t a
1
u
,
t x
1 S
t a
1
t a
1 Augmented covariance S
t a
1 Sigma points Prediction of sigma points
t
S
t
i
2
L
0
w i m
i
2
L
0
w c i
i x
,
t
i
,
t x
Predicted mean
x
T
t i
,
t t City College of New York
Predicted covariance 48
UKF_localization
(
t-1 ,
S
t-1 , u t , z t , m
):
Correction:
t
h
t
t z
t x
to generate the measurement sigma points Measurement sigma points
z
ˆ
t
i
2
L
0
w i m
i
,
t S t
i
2
L
0
w c i
i
,
t
z
ˆ
t
i
,
t
z
ˆ
t T
S
t x
,
z
i
2
L
0
w c i
i
,
t x
t
i
,
t
z
ˆ
t T
K t
S
t x
,
z S t
1
t
t
K t
(
z t
z
ˆ
t
)
Predicted measurement mean Pred. measurement covariance Cross-covariance Kalman gain Updated mean S
t
S
t
K t S t K t T
Updated covariance
City College of New York
49
UKF Prediction Step
High rotational noise Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at
t
High translational noise High noise in both translation & rotation
City College of New York
50
UKF Measurement Prediction Step
City College of New York
51
UKF Correction Step
City College of New York
52
Estimation Sequence
EKF UKF Robot path estimated with different techniques, with UKF being slightly closer
City College of New York
53
Prediction Quality
EKF UKF Robot moves on a circle, estimates based on EKF prediction, & UKF prediction
City College of New York
54
Kalman Filter-based System
• [Arras et al. 98]: • Laser range-finder and vision • High precision (<1cm accuracy)
City College of New York
[Courtesy of Kai Arras] 55
Multi hypothesis Tracking
City College of New York
56
Localization With MHT
• • MHT: Multi-Hypothesis Tracking filter Belief is represented by multiple hypotheses Each hypothesis is tracked by a Kalman filter • •
Additional problems
: • Data association : Which observation corresponds to which hypothesis?
• Hypothesis management : When to add / delete hypotheses?
Huge body of literature on target tracking, motion correspondence etc. 57
City College of New York
MHT: Implemented System (1)
• • • Hypotheses are extracted from Laser Range Finder (LRF) scans Each hypothesis has probability of being the correct one:
H i
{ ˆ
x
, S
i
,
P
(
H i
)}
i
Hypothesis probability is computed using Bayes ’ rule
P
(
H i
|
s
)
P
(
s
|
H i
)
P
(
H i
)
P
(
s
) • • Hypotheses with low probability are deleted.
New candidates are extracted from LRF scans.
C j
{
z j
,
R j
} [Jensfelt et al. ’ 00] 58
City College of New York
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
City College of New York
59
MHT: Implemented System (3) Example run
# hypotheses P(H best ) Map and trajectory Courtesy of P. Jensfelt and S. Kristensen #hypotheses vs. time
City College of New York
60
Probabilistic Robotics
SLAM
City College of New York
SLAM Problem : Chicken or Egg
Fundamental problems for localization and mapping The task of SLAM is to build a map while estimating the pose of the robot relative to this map.
Without a map, robot cannot localize itself Without knowing its location, robot cannot build a map Which needed to be done first? Localization or mapping?
City College of New York
62
The SLAM Problem
A robot is exploring an unknown, static environment.
Given:
– The robot ’ s controls (U 1:t ) – Observations of nearby features (Z 1:t )
Estimate:
– Map of features (m) – Pose / Path of the robot (x t )
City College of New York
63
Why is SLAM a hard problem?
Uncertanties • Error in pose • Error in observation • Error in mapping • Error accumulated
City College of New York
64
Why is SLAM a hard problem?
SLAM
: robot path and map are both
unknown
Robot path error correlates errors in the map
City College of New York
65
Why is SLAM a hard problem?
Robot pose uncertainty
• In the real world, the mapping between observations and landmarks is unknown • Picking wrong data associations can have catastrophic consequences
City College of New York
66
Data Association Problem
• A data association is an assignment of observations to landmarks • In general there are more than (n observations, m landmarks) possible associations • Also called “ assignment problem ”
City College of New York
67
Nature of the SLAM Problem
Continuous component Location of objects in the map Robot ’ s own pose Discrete component Correspondence reasoning Object is the same or not
City College of New York
68
SLAM:
Simultaneous Localization and Mapping
• Full SLAM:
Estimates entire path and map!
p
(
x
1 : t ,
m
|
z
1 : t ,
u
1 : t ) Estimates Given Entire pose (x 1:t ) and map (m) Previous knowledge (Z 1:t-1 , U 1:t-1 ) Current measurement (Z t , U t ) 69
City College of New York
Graphical Model of Full SLAM:
p
(
x
1 :
t
,
m
|
z
1 :
t
,
u
1 :
t
) Compute a joint posterior over the whole path of robot and the map
City College of New York
70
SLAM:
Simultaneous Localization and Mapping
• Online SLAM:
Estimates
p
(
x
t ,
m
|
z
1 : t ,
u
1 : t ) Most recent pose (x t ) and map (m) Given Previous knowledge (Z 1:t-1 , U 1:t-1 ) Current measurement (Z t , U t ) Estimates most recent pose and map!
City College of New York
71
Graphical Model of Online SLAM:
Estimate a posterior over the current robot pose, and the map
p
(
x t
,
m
|
z
1 :
t
,
u
1 :
t
)
p
(
x
1 :
t
,
m
|
z
1 :
t
,
u
1 :
t
)
dx
1
dx
2 ...
dx t
1 Integrations typically done one at a time 72
City College of New York
SLAM with Extended Kalman Filter • Pre-requisites
– Maps are feature-based (landmarks) small number (< 1000) – Assumption - Gaussian Noise – Process only positive sightings No landmark = negative Landmark = positive
City College of New York
73
EKF-SLAM with known correspondences Correspondence Data association problem Landmarks can ’ t be uniquely identified Correspondence variable (C i t ) between feature (f i t ) and real landmark True identity of observed feature
f t i
r t i
t i
Make correspondence variables explicit
S t i
T p
(
x
t ,
m
,
c t p
(
x
1 : t
City College of New York
,
m
,
c
1 :
t
|
z
1 : t ,
u
1 : t ) |
z
1 : t ,
u
1 : t ) 74
EKF-SLAM with known correspondences Signature Numerical value (average color) Characterize type of landmark (integer) Multidimensional vector (height and color)
City College of New York
75
EKF-SLAM with known correspondences Similar development to EKF localization Diff robot pose + coordinates of all landmarks Combined state vector (3N + 3)
y t
x m t
x y
m
1 ,
x m
1 ,
y S
1 ...
m N
,
x m N
,
y S N T
p
(
y t
|
z
1 :
t
,
u
1 :
t
) Online posterior
City College of New York
76
Motion update Iteration through measurements Mean Covariance Test for new landmarks Initialization of elements Expected measurement
City College of New York
Filter is updated 77
EKF-SLAM with known correspondences Observing a landmark improves robot pose estimate eliminates some uncertainty of other landmarks Improves position estimates of the landmark + other landmarks We don ’ t need to model past poses explicitly
City College of New York
78
Example
City College of New York
79
EKF-SLAM with known correspondences Example: • Uncertainty of landmarks are mainly due to robot ’ s pose uncertainty (persist over time) Estimated location of landmarks are correlated
City College of New York
80
EKF-SLAM with unknown correspondences • No correspondences for landmarks • Uses an incremental maximum likelihood (ML) estimator Determines most likely value of the correspondence variable Takes this value for granted later on 81
City College of New York
Motion update Hypotheses of new landmark EKF-SLAM with unknown correspondences Mean Covariance
City College of New York
82
City College of New York
83
General Problem Gaussian noise assumption Unrealistic Spurious measurements Fake landmarks Outliers Affect robot ’ s localization
City College of New York
84
Solutions to General Problem Provisional landmark list New landmarks do not augment the map Not considered to adjust robot ’ s pose Consistent observation regular map
City College of New York
85
Solutions to General Problem Landmark Existence Probability Landmark is observed Observable variable (
o
) increased by fixed value Landmark is NOT observed when it should Observable variable decreased Removed from map when (
o
) drops below threshold
City College of New York
86
Problem with Maximum Likelihood (ML) Once ML estimator determines likelihood of correspondence, it takes value for granted always correct Makes EKF susceptible to landmark confusion Wrong results
City College of New York
87
Solutions to ML Problem Spatial arrangement Greater distance between landmarks Less likely confusion will exist Trade off: few landmarks harder to localize Little is known about optimal density of landmarks Signatures Give landmarks a very perceptual distinctiveness (e,g, color, shape, …) 88
City College of New York
EKF-SLAM Limitations • Selection of appropriate landmarks • Reduces sensor reading utilization to presence or absence of those landmarks Lots of sensor data is discarded • Quadratic update time Limits algorithm to scarce maps (< 1000 features) • Low dimensionality of maps problem harder data association
City College of New York
89
EKF-SLAM Limitations • Fundamental Dilemma of EKF-SLAM It might work well with dense maps (millions of features) It is brittle with scarce maps BUT It needs scarce maps because of complexity of the algorithm (update process)
City College of New York
90
SLAM Techniques • EKF SLAM
(chapter 10)
• Graph-SLAM
(chapter 11)
• SEIF (sparse extended information filter)
(chapter 12)
• Fast-SLAM
(chapter 13)
City College of New York
91
Graph-SLAM
• Solves full SLAM problem • Represents info as a graph of soft constraints • Accumulates information into its graph without resolving it (lazy SLAM) • Computationally cheap • At the other end of EKF-SLAM Process information right away (proactive SLAM) Computationally expensive
City College of New York
92
Graph-SLAM
• Calculates posteriors over robot path (not incremental) • Has access to the full data • Uses inference to create map using stored data Offline algorithm
City College of New York
93
Sparse Extended Information Filter (SEIF) • Implements a solution to online SLAM problem • Calculates current pose and map (as EKF) • Stores information representation of all knowledge (as Graph SLAM) Runs Online and is computationally efficient • Applicable to multi-robot SLAM problem
City College of New York
94
FastSLAM Algorithm
• Particle filter approach to the SLAM problem • Maintain a set of particles • Particles contain a sampled robot path and a map • The features of the map are represented by own local Gaussian • Map is created as a set of separate Gaussians Map features are conditionally independent given the path Factoring out the path (1 per particle) Map feature become independent Eliminates the need to maintain correlation among them
City College of New York
95
FastSLAM Algorithm
• Updating in FastSLAM Sample new pose update the observed features • Update can be performed online • Solves both online and offline SLAM problem • Instances Feature-based maps Grid-based algorithm
City College of New York
96
Approximations for SLAM Problem
• Local submaps [Leonard et al.99, Bosse et al. 02, Newman et al. 03] • Sparse links (correlations) [Lu & Milios 97, Guivant & Nebot 01] • Sparse extended information filters [Frese et al. 01, Thrun et al. 02] • Thin junction tree filters [Paskin 03] • Rao-Blackwellisation (FastSLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]
City College of New York
97
Thank You
98
City College of New York