probabilistic robotics: review/slam
Post on 21-Mar-2016
91 Views
Preview:
DESCRIPTION
TRANSCRIPT
City College of New York
1
Dr. Jizhong XiaoDepartment of Electrical Engineering
CUNY City Collegejxiao@ccny.cuny.edu
Probabilistic Robotics:
Review/SLAM
Advanced Mobile Robotics
City College of New York
• Prediction (Action)
• Correction (Measurement)
Bayes Filter Revisit
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel
City College of New York
Probabilistic Robotics
Probabilistic Sensor Models
Beam-based ModelLikelihood Fields ModelFeature-based Model
City College of New York
4
Beam-based Proximity ModelMeasurement noise
zexp zmax0
bzz
hit eb
mxzP2
exp )(21
21),|(
otherwisezz
mxzPz
0e
),|( expunexp
Unexpected obstacles
zexp zmax0
Gaussian Distribution
Exponential Distribution
City College of New York
5
Beam-based Proximity ModelRandom measurement Max range
max
1),|(z
mxzPrand
otherwisezzif
mxzP01
),|( maxmax
zexp zmax0zexp zmax0
Uniform distribution
Point-mass distribution
City College of New York
6
Resulting Mixture Density
),|(),|(),|(
),|(
),|(
rand
max
unexp
hit
rand
max
unexp
hit
mxzPmxzPmxzP
mxzP
mxzP
T
How can we determine the model parameters?
Weighted average, and 1maxexp randunhit
System identification method: maximum likelihood estimator (ML estimator)
},,,,,{ maxexp randunhit
City College of New York
7
Likelihood Fields Model• Project the end points of a sensor scan Zt into the global
coordinate space of the map
• 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.
• Again, independence between different components is assumed.
randrandhithittkt pppmxzp maxmax),(
)sin()cos(
cossinsincos
ksens
ksenskt
ksens
ksens
z
z zyx
yx
yx
kt
kt
City College of New York
8
Likelihood Fields Model
)sin()cos(
cossinsincos
ksens
ksenskt
ksens
ksens
z
z zyx
yx
yx
kt
kt
Distance to the nearest obstacles. Max range reading ignored
City College of New York
9
Example
P(z|x,m)
Example environment Likelihood fieldThe darker a location, the less likely it is to perceive an obstacleSensor
probability
O1
O2
O3
Oi : Nearest point to obstaclesZmax
City College of New York
10
Feature-Based Measurement Model• Feature vector is abstracted from the measurement:
• Sensors that measure range, bearing, & a signature (a numerical value, e.g., an average color)
• Conditional independence between features
• Feature-Based map: withi.e., a location coordinate in global coordinates & a signature • Robot pose:
• Measurement model:
Zero-mean Gaussian error variables with standard deviations
},,{}{)(2
2
2
1
1
1
21
t
t
t
t
t
t
ttt
s
r
s
rffzf
),,,(),)(( mxsrpmxzfp tit
i
it
ittt
},,{ 21 mmm
2
2
2
)),(2tan)()(
,,
2,
2,
s
r
j
xjyj
yjxj
it
it
it
sxmymaymxm
s
r
Tt yxx }{
Tjyjxjj smmm },,{ ,,
2r
r
City College of New York
Probabilistic Robotics
Probabilistic Motion Models
City College of New York
Odometry Model
22 )'()'( yyxxtrans
)','(atan21 xxyyrot
12 ' rotrot
• Robot moves from to . • Odometry information .
,, yx ',',' yx
transrotrotu ,, 21
trans1rot
2rot
,, yx
',',' yx
Relative motion information, “rotation” “translation” “rotation”
City College of New York
Noise Model for Odometry• The measured motion is given by the true
motion corrupted with independent noise.
||||11 211
ˆtransrotrotrot
||||22 221
ˆtransrotrotrot
|||| 2143
ˆrotrottranstranstrans
2
2
221
221)(
x
ex
2
2
2
6||6
6|x|if0)(2
xx
),( 1ttt xuxp
How to calculate :
City College of New York
14
Calculating the Posterior Given xt, xt-1, and u
22 )'()'( yyxxtrans )','(atan21 xxyyrot
12 ' rotrot 22 )'()'(ˆ yyxxtrans )','(atan2ˆ
1 xxyyrot
12ˆ'ˆrotrot
)ˆ|ˆ|,ˆ(prob trans21rot11rot1rot1 p|))ˆ||ˆ(|ˆ,ˆ(prob rot2rot14trans3transtrans2 p
)ˆ|ˆ|,ˆ(prob trans22rot12rot2rot3 p
1. Algorithm motion_model_odometry (xt, xt-1, u)
2. 3. 4. 5. 6. 7. 8. 9. 10. 11. return p1 · p2 · p3
odometry values (u)
values of interest (xt-1, xt)
Ttt xxu 1
Tt yxx )(1
Tt yxx )(
An initial pose Xt-1
A hypothesized final pose Xt
A pair of poses u obtained from odometry
),( baprobImplements an error distribution over a with zero mean and standard deviation b),( 1ttt xuxp
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.
x’ u
p(xt| u, xt-1)
u
x’
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
16
Velocity-Based Model
v
ucontrol vr Rotation
radius
City College of New York
17
Equation for the Velocity ModelInstantaneous center of curvature (ICC) at (xc , yc)
sinrxx c cosryy c
Initial pose Tt yxx 1
Keeping constant speed, after ∆t time interval, ideal robot will be at Tt yxx
ttrytrx
yx
c
c
)cos()sin(
ttrrtrr
yx
)cos(cos)sin(sin Correcte
d, -90
City College of New York
18
Velocity-based Motion Model
With and are the state vectors at time t-1 and t respectively
t
tvv
tvv
yx
yx
t
tt
t
t
t
tt
t
t
t
ˆ
)ˆcos(ˆˆ
cosˆˆ
)ˆsin(ˆˆ
sinˆˆ
'
'
'
Tt yxx 1 Tt yxx '''
The true motion is described by a translation velocity and a rotational velocity
tv tMotion Control with additive Gaussian noise
),0(
ˆˆ
243
221 )(
tt
t
v
v
t
t
t
t Mvvv
tt
tt
Tttt vu )(
243
221
)(00)(
tt
ttt v
vM
Circular motion assumption leads to degeneracy ,2 noise variables v and w 3D poseAssume robot rotates when arrives at its final pose
tt ˆ
65
ˆ v
City College of New York
19
Velocity-based Motion ModelMotion Model:
tt
tvv
tvv
yx
yx
t
tt
t
t
t
tt
t
t
t
ˆˆ
)ˆcos(ˆˆ
cosˆˆ
)ˆsin(ˆˆ
sinˆˆ
'
'
'
243
221 )(
ˆˆ
tt
tt
v
v
t
t
t
t vv
65
ˆ v
1 to 4 are robot-specific error parameters determining the velocity control noise
5 and 6 are robot-specific error parameters determining the standard deviation of the additional rotational noise
City College of New York
20
Probabilistic Motion Model
Center of circle:
with
How to compute ?),( 1ttt xuxp Move with a fixed velocity during ∆t resulting in a circular trajectory from to
Tt yxx 1 Tt yxx
Radius of the circle:
2*2*2*2** )()()()( yyxxyyxxr
Change of heading direction: ),(2tan),(2tan **** xxyyaxxyya
tr
tdistv
*
ˆt ˆˆ
t
(angle of the final rotation)
City College of New York
21
Posterior Probability for Velocity Model
Motion error: verr ,werr and
Center of circleRadius of the circle
Change of heading direction
City College of New York
Examples (velocity based)
City College of New York
Map-Consistent Motion Model
)',|( xuxp
)',|()|(),',|( xuxpmxpmxuxp Approximation:
),',|( mxuxp)',|( xuxp
Map free estimate of motion model
)|( mxp“consistency” of pose in the map
“=0” when placed in an occupied cell
Obstacle grown by robot radius
City College of New York
24
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.
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
26
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
27
Markov Localization
),,|( t:1t:1t:1 muzxp
City College of New York
• Prediction (Action)
• Correction (Measurement)
Bayes Filter Revisit
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel
City College of New York
29
• Prediction:
• Correction:
EKF Linearization
)(),(),(
)(),(),(),(
1111
111
111
ttttttt
ttt
tttttt
xGugxug
xx
ugugxug
)()()(
)()()()(
ttttt
ttt
ttt
xHhxh
xx
hhxh
First Order Taylor Expansion
City College of New York
30
EKF Algorithm 1. Extended_Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
),( 1 ttt ug
tTtttt RGG 1
1)( tTttt
Tttt QHHHK
))(( ttttt hzK
tttt HKI )(
1
1),(
t
ttt x
ugG
t
tt x
hH
)(
ttttt uBA 1
tTtttt RAA 1
1)( tTttt
Tttt QCCCK
)( tttttt CzK tttt CKI )(
City College of New York
31
1. EKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2.
3.
4.
5.
6.
),( 1 ttt ug T
tttTtttt VMVGG 1
,1,1,1
,1,1,1
,1,1,1
1
1
'''
'''
'''
),(
tytxt
tytxt
tytxt
t
ttt
yyy
xxx
xugG
tt
tt
tt
t
ttt
v
yvy
xvx
uugV
''
''
''
),( 1
243
221
||||00||||
tt
ttt v
vM
Motion noise covariance
Matrix from the control
Jacobian of g w.r.t location
Predicted meanPredicted covariance
Jacobian of g w.r.t control
City College of New York
32
Velocity-based Motion Model
With and are the state vectors at time t-1 and t respectively
t
tvv
tvv
yx
yx
t
tt
t
t
t
tt
t
t
t
ˆ
)ˆcos(ˆˆ
cosˆˆ
)ˆsin(ˆˆ
sinˆˆ
'
'
'
Tt yxx 1 Tt yxx '''
The true motion is described by a translation velocity and a rotational velocity
tv t
Motion Control with additive Gaussian noise
),0(
ˆˆ
243
221 )(
tt
t
v
v
t
t
t
t Mvvv
tt
tt
Tttt vu )(
243
221
)(00)(
tt
ttt v
vM
City College of New York
33
Velocity-based Motion Model
),0()cos(cos
)sin(sin
'
'
'
t
t
tt
t
t
t
tt
t
t
t
RN
t
tvv
tvv
yx
yx
),0(),( 1 tttt RNxugx
)(),(),(
)(),(
),(),(
1111
111
111
ttttttt
ttt
tttttt
xGugxug
xx
ugugxug
Motion Model:
City College of New York
34
Velocity-based Motion Model
),0()cos(cos
)sin(sin
'
'
'
t
t
tt
t
t
t
tt
t
t
t
RN
t
tvv
tvv
yx
yx
,1,1,1
,1,1,1
',1
'
,1
'
,1
'
1
111
),(),(
tytxt
tytxt
tytxt
t
ttttt
yyy
xxx
xug
xG
Derivative of g along x’ dimension, w.r.t. x at
1t
xt
x
,1
Jacobian of g w.r.t location
City College of New York
35
Velocity-based Motion Model
),0()cos(cos
)sin(sin
'
'
'
t
t
tt
t
t
t
tt
t
t
t
RN
t
tvv
tvv
yx
yx
Derivative of g w.r.t. the motion parameters, evaluated at and
1t
tt
tt
tt
t
ttt
v
yvy
xvx
uugV
''
''
''
),( 1
t
ttvtvt
ttvtvt
t
tt
t
tt
t
t
t
tt
t
tt
t
t
0
)sin())cos((cos)cos(cos
)cos())sin((sin)sin(sin
2
2
Tttt
Ttttt VMVGG 1
Mapping between the motion noise in control space to the motion noise in state space
Jacobian of g w.r.t control
tu
City College of New York
36
1. EKF_localization ( t-1, t-1, ut, zt, m):
Correction:
2.
3.
4.
5.
6.
7.
8.
)ˆ( ttttt zzK
tttt HKI
,
,
,
,
,
,),(
t
t
t
t
yt
t
yt
t
xt
t
xt
t
t
tt
rrr
xmhH
,,,
2,
2,
,2atanˆ
txtxyty
ytyxtxt
mmmmz
tTtttt QHHS
1 tTttt SHK
2
2
00
r
rtQ
Predicted measurement mean
Pred. measurement covarianceKalman gain
Updated meanUpdated covariance
Jacobian of h w.r.t location
City College of New York
37
Feature-Based Measurement Model
2
2
2
)),(2tan)()(
,,
2,
2,
s
r
j
xjyj
yjxj
it
it
it
sxmymaymxm
s
r
)()(
)()( ttt
ttt x
xh
hxh
,
,
,
,
,
,),(
t
t
t
t
yt
t
yt
t
xt
t
xt
t
t
tt
rrr
xmhH
),0(),,( ttit QNmjxhz
• Jacobian of h w.r.t location
Is the landmark that corresponds to the measurement of
itzi
tCj
City College of New York
38
EKF Localizationwith known
correspondences
City College of New York
39
EKF Localizationwith unknown
correspondences
Maximum likelihood estimator
City College of New York
40
EKF Prediction StepInitial estimate is represented by the ellipse centered at 1t
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
High rotational noiseSmall noise in translational & rotation
City College of New York
41
EKF Measurement Prediction Step
Innovations (white arrows) : differences between observed & predicted measurements
True robot (white circle) & the observation (bold line circle)
Measurem
ent Prediction
City College of New York
42
EKF Correction Step
Measurem
ent Prediction
Resulting correction
Update mean estimate & reduce position uncertainty ellipses
City College of New York
43
Estimation Sequence (1)EKF localization with an accurate landmark detection sensor
Dashed line: estimated robot trajectory
Solid line: true robot motion
Dashed line: corrected robot trajectory Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection
City College of New York
44
Estimation Sequence (2)EKF localization with a less accurate landmark detection sensor
Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection
City College of New York
45
Comparison to Ground Truth
City College of New York
46
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
47
Unscented Transform
nin
wwn
nw
nw
ic
imi
i
cm
2,...,1for )(2
1 )(
)1( 2000
Sigma points Weights
)( ii g
n
i
Tiiic
n
i
iim
w
w
2
0
2
0
))(('
'
Pass sigma points through nonlinear function
Recover mean and covariance For n-dimensional Gaussianλ is scaling parameter that determine how far the sigma points are spread from the meanIf the distribution is an exact Gaussian, β=2 is the optimal choice.
City College of New York
48
UKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
243
221
||||00||||
tt
ttt v
vM
2
2
00
r
rtQ
TTTt
at 000011
t
t
tat
QM00
00001
1
at
at
at
at
at
at 111111
xt
utt
xt ug 1,
L
i
Tt
xtit
xti
ict w
2
0,,
L
i
xti
imt w
2
0,
Motion noise
Measurement noise
Augmented state mean
Augmented covariance
Sigma points
Prediction of sigma points
Predicted mean
Predicted covariance
City College of New York
49
UKF_localization ( t-1, t-1, ut, zt, m):
Correction:
zt
xtt h
L
iti
imt wz
2
0,ˆ
Measurement sigma points
Predicted measurement mean
Pred. measurement covariance
Cross-covariance
Kalman gain
Updated mean
Updated covariance
Ttti
L
itti
ict zzwS ˆˆ ,
2
0,
Ttti
L
it
xti
ic
zxt zw ˆ,
2
0,
,
1, tzx
tt SK
)ˆ( ttttt zzK
Tttttt KSK
The predicted robot locations are used to generate the measurement sigma points
xt
City College of New York
50
UKF Prediction Step
Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at
t
High translational noise
High rotational noise
High noise in both translation & rotation
City College of New York
51
UKF Measurement Prediction Step
Measurem
ent Prediction
Predicted Sigma points
City College of New York
52
UKF Correction Step
Measurem
ent Prediction
Resulting correction
City College of New York
53
Estimation Sequence
EKF UKF
Robot path estimated with different techniques, with UKF being slightly closer
City College of New York
54
Prediction Quality
EKF UKF Robot moves on a circle, estimates based on EKF prediction, & UKF prediction
City College of New York
55
• [Arras et al. 98]: • Laser range-finder and vision• High precision (<1cm accuracy)
Kalman Filter-based System
[Courtesy of Kai Arras]
City College of New York
56
Multi-hypothesisTracking
City College of New York
57
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.
Localization With MHT
City College of New York
58
• Hypotheses are extracted from Laser Range Finder (LRF) scans• Each hypothesis has probability of being the correct one:
• Hypothesis probability is computed using Bayes’ rule
• Hypotheses with low probability are deleted.• New candidates are extracted from LRF scans.
MHT: Implemented System (1)
)}(,,ˆ{ iiii HPxH
},{ jjj RzC
)()()|()|(
sPHPHsPsHP ii
i
[Jensfelt et al. ’00]
City College of New York
59
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
City College of New York
60
MHT: Implemented System (3)Example run
Map and trajectory
# hypotheses
#hypotheses vs. time
P(Hbest)
Courtesy of P. Jensfelt and S. Kristensen
City College of New York
Probabilistic Robotics
SLAM
City College of New York
62
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
63
Given:– The robot’s controls (U1:t)
– Observations of nearby features (Z1:t)
Estimate:– Map of features (m)
– Pose / Path of the robot (xt)
The SLAM ProblemA robot is exploring an unknown, static environment.
City College of New York
64
Why is SLAM a hard problem?
Uncertanties
• Error in pose
• Error in observation
• Error in mapping• Error accumulated
City College of New York
65
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
66
Why is SLAM a hard problem?
• In the real world, the mapping between observations and landmarks is unknown
• Picking wrong data associations can have catastrophic consequences
Robot poseuncertainty
City College of New York
67
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
Nature of the SLAM Problem
68
Continuous Location of objects in component the map
Robot’s own pose
Discrete Correspondence component
Object is the same or notreasoning
City College of New York
69
SLAM: Simultaneous Localization and Mapping
• Full SLAM:
Estimates Entire pose (x1:t) and map (m)
Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)
),|,( t:1t:1t:1 uzmxp
Estimates entire path and map!
City College of New York
70
Graphical Model of Full SLAM:
),|,( :1:1:1 ttt uzmxp Compute a joint posterior over the whole path of robot and the map
City College of New York
71
SLAM: Simultaneous Localization and Mapping
• Online SLAM:
Estimates Most recent pose (xt) and map (m)
Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)
),|,( t:1t:1t uzmxp
Estimates most recent pose and map!
City College of New York
72
Graphical Model of Online SLAM:
121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
Integrations typically done one at a time
Estimate a posterior over the current robot pose, and the map
City College of New York
73
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
74
EKF-SLAM with known correspondences
Correspondence Data association problem
Landmarks can’t be uniquely identified
Correspondence variable (Cit)
between feature (fit) and real landmark
Tit
it
it
it Srf
True identity of observed feature
),|,,( t:1t:1t uzcmxp t
),|,,( t:1t:1:1t:1 uzcmxp t
Make correspondence variables explicit
City College of New York
75
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
76
EKF-SLAM with known correspondences
Similar development to EKF localization
Diff robot pose + coordinates of all landmarks
Combined state vector
TNyNxNyxt
t SmmSmmyxmx
y ,,1,1,1 ...
(3N + 3)
),|( :1:1 ttt uzyp Online posterior
City College of New York
77
Motion update
Mean
Covariance
Iteration through measurements
Test for new landmarks
Initialization of elements
Expected measurement
Filter is updated
City College of New York
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
78
City College of New York
Example
79
City College of New York
EKF-SLAM with known correspondences
80
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
EKF-SLAM with unknown correspondences
• No correspondences for landmarks
• Uses an incremental maximum likelihood (ML) estimator
81
Determines most likely value of the correspondence variable
Takes this value for granted later on
City College of New York
EKF-SLAM with unknown correspondences
82
Motion update
Hypotheses of new landmark
Mean
Covariance
City College of New York
83
City College of New York
General Problem
Gaussian noise assumption Unrealistic
Spurious measurements Fake landmarks
Outliers
Affect robot’s localization
84
City College of New York
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
85
City College of New York
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
86
City College of New York
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
87
City College of New York
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
88
SignaturesGive landmarks a very perceptual
distinctiveness(e,g, color, shape, …)
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 harder data association problem
89
City College of New York
EKF-SLAM Limitations• Fundamental Dilemma of EKF-SLAM
It might work well with dense maps (millions of features)
It is brittle with scarce maps
90
BUT
It needs scarce maps because of complexity of the algorithm (update process)
City College of New York
91
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
92
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
93
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
94
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
95
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
96
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
97
• 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]
Approximations for SLAM Problem
City College of New York
98
Thank You
top related