Transcript Document 7687376

Reliable Range based
Localization and SLAM
Joseph Djugash
Masters Student
Presenting work done by:
Sanjiv Singh, George Kantor, Peter Corke and
Derek Kurth
Motivation
Motivation
Introduction
 Much research has been done to
perform localization under
normal/ideal conditions
 Classical sensors fail to provide
reliable results under non-ideal
scenarios
 Alternative methods such as rangeonly sensors have not received
enough attention in the research field
Outline





Introduction
Range-Only Sensor
Localization
SLAM
Future Work
Range & Bearing Sensors
Landmark
Robot
 Errors in estimation of
robot location and
landmark locations are
represented as
ellipses.
 Each landmark ellipse
contains the error of
both the robot’s
current error and the
error within the
sensors.
Range-Only Sensors
Landmark
Robot
 We are provided
with an annulus
instead of an
ellipse.
 Extending classical
approaches to
localization
requires additional
considerations.
Range-Only Sensors
r1
r2
Outline
 Introduction
 Range-Only Sensor
 Localization
 Kalman Filter
 Particle Filter
 Results
 SLAM
 Future Work
Predictor  Corrector
 Iterative Process
 Predict the new
state (and its
uncertainty) based
on current state
and process model
 Correct state
estimate with new
measurement
Predictor
Corrector
Outline
 Introduction
 Range-Only Sensor
 Localization
 Kalman Filter
 Particle Filter
 Results
 SLAM
 Future Work
Kalman Filter
 Belief Representation
 Error Function – Gaussian
 Mean and Covariance
 Process Model (State qk = [xr, yr, θr]T)
–
^
q
^ + + B·u
 k = A·q
k-1
k-1 + wk-1
–
+
 Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1
 Measurement Model



–
^
q+= ^
q
^
k
k-1 + Kk·(zk – H·qk-1)
–
–
T
Kk = Pk·H ·(H·Pk·HT + Rk)-1
–
+
Pk = (I – Kk·H)·Pk
–
Kalman Filter
 Belief Representation
 Error Function – Gaussian
 Mean and Covariance
 Process Model (State qk = [xr, yr, θr]T)
–
^
q
^ + + B·u
 k = A·q
k-1
k-1 + wk-1
–
+
 Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1
 Measurement Model



–
^
q+= ^
q
^
k
k-1 + Kk·(zk – H·qk-1)
–
–
T
Kk = Pk·H ·(H·Pk·HT + Rk)-1
–
+
Pk = (I – Kk·H)·Pk
–
Last Relative
Measurement.
(∆d, ∆θ)
Kalman Filter
 Belief Representation
 Error Function – Gaussian
 Mean and Covariance
 Process Model (State qk = [xr, yr, θr]T)
–
^
q
^ + + B·u
 k = A·q
k-1
k-1 + wk-1
–
+
 Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1
 Measurement Model



–
^
q+= ^
q
^
k
k-1 + Kk·(zk – H·qk-1)
–
–
T
Kk = Pk·H ·(H·Pk·HT + Rk)-1
–
+
Pk = (I – Kk·H)·Pk
Measurement
range to beacon
–
Measurement
variance
Estimated range
to beacon
Kalman Filter
 Advantages:
 Computationally Efficient
 Able to handle high dimensionality with
limited or no extra computational cost
 Handles short periods of sensor silence
 Disadvantages:
 Able to represent only Gaussian
distributions
 Assumptions are too restrictive
Outline
 Introduction
 Range-Only Sensor
 Localization
 Kalman Filter
 Particle Filter
 Results
 SLAM
 Future Work
Particle Filter
 Representing belief by sets of samples or
particles
 Each particle is represented as (xp, yp),
(orientation is not maintained)
 Updating procedure is a sequential
importance sampling approach with resampling
 Sampling – Standard Gaussian Formula:
 P(x|rm) =
1
-(r^ – rm)2
(
)
e
2σ
σ√2π
.
 Where rm is the measured range and ^r is the
range estimate from the particle to beacon
Particle Filter
 Advantages:
 Able to represent arbitrary density
 Converging to true posterior even for
non-Gaussian and nonlinear system
 Efficient in the sense that particles tend
to focus on regions with high probability
 Disadvantages:
 Worst-case complexity grows
exponentially in the dimensions
Outline
 Introduction
 Range-Only Sensor
 Localization
 Kalman Filter
 Particle Filter
 Results
 SLAM
 Future Work
The Experiments
Dead Reckoning – Results
Kalman Filter – Results
XTE
ATE
Mean Abs.
0.5539 m0.3976 m
Error
Max. Error 1.9033 m2.0447 m
Std (σ)
0.4173 m0.3558 m
Particle Filter – Results
XTE
ATE
Mean Abs.
2.8200 m 2.0898 m
Error
Max. Error 8.6526 m 7.7012 m
Std (σ)
1.0345 m 1.1943 m
Outline




Introduction
Range-Only Sensor
Localization
SLAM
 Batch Slam
 Kalman Filter
 Results
 Future Work
SLAM
 Beacon Locations are
unknown
 Measurements are
used to predict beacon
locations
 Due to errors in
measurements, not all
measurements can be
weighed equally
 Consistency between
inliers help provide a
reliable estimates
Outline




Introduction
Range-Only Sensor
Localization
SLAM
 Batch Slam
 Kalman Filter
 Results
 Future Work
Batch Slam
 Approaches the SLAM problem by
solving two non-linear optimization
problems:
 One to generate the initial estimate of
the beacon locations
 One to simultaneously refine the vehicle
and beacon estimates
 Estimated Beacon locations are feed
to the Kalman filter localization
algorithm
Batch Slam
 Initializing the Beacons
 Assumes robot’s odometry is perfect
 Using the range measurements predicts the most
likely beacon estimates
 Estimates are acquired by minimizing the cost
function:
and,
 Refining estimates
 Assumes error distributions of each measurement is
independent
 Most likely beacon positions and vehicle relative
motion can be found by minimizing the cost function:
Batch Slam
 Advantages:
 Produces accurate estimates of beacon
locations
 Requires very little data to acquire good
results
 Disadvantages:
 Computationally Expensive
 Requires fairly accurate dead reckoning
data to acquire its initial beacon estimate
Outline




Introduction
Range-Only Sensor
Localization
SLAM
 Batch Slam
 Kalman Filter
 Results
 Future Work
Beacon Initialization
 Find all pair wise intersections of a
set of range measurements
 Create a histogram grid with the
circle intersections
 Find the first two peaks on the grid
 When the ratio between the peaks
reaches a threshold (set to ‘2’),
declare the higher of the peaks as the
beacon location
Beacon Initialization
Kalman Filter SLAM
 Kalman filter localization algorithm can be
easily extended for SLAM
 The state vector becomes:
 qk = [xr, yr, θk, xb1, yb1, … , xbn, ybn]T
 As new beacons are initialized, expand the
state vector and covariance matrix to the
correct dimension
 q ~ 2n+3
 P ~ 2n+3 square
 where n is the number of initialized beacons
Kalman Filter SLAM
 Advantages:
 Similar to Kalman Filter Localization
 Settles to locally accurate solution
 Disadvantages:
 Wrong Beacon Initialization could lead to
diverged solution
Outline




Introduction
Range-Only Sensor
Localization
SLAM
 Batch Slam
 Kalman Filter
 Results
 Future Work
Kalman Filter SLAM – Results
Raw
XTE
ATE
Mean Abs.
8.5544 m 5.1776 m
Error
Max. Error 18.0817 m 19.2575 m
Std (σ)
Aff. Trans.
4.8216 m 4.5486 m
XTE
ATE
Mean Abs.
0.7297 m 0.6872 m
Error
Max. Error 2.6787 m 2.7621 m
Std (σ)
0.6004 m 0.5745 m
Kalman Filter & Batch Slam – Results
(Another Example)
Batch Slam using only
5% of data set
Batch Slam
XTE
ATE
Mean Abs.
1.5038 m 2.0871 m
Error
Max. Error 4.9149 m 5.8212 m
Std (σ)
1.0527 m 1.4968 m
Outline





Introduction
Range-Only Sensor
Localization
SLAM
Future Work
Future Work
 Develop robust algorithms that
produce reliable results with poor
sensor data
 Develop an approach that relies on
multiple algorithms at various points
during the data set to produce better
results
Questions…