reliable range based localization and slam joseph djugash masters student presenting work done by:...
Post on 19-Dec-2015
216 views
TRANSCRIPT
![Page 1: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/1.jpg)
Reliable Range based Localization and SLAM
Joseph DjugashMasters Student
Presenting work done by:Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth
![Page 2: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/2.jpg)
Motivation
![Page 3: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/3.jpg)
Motivation
![Page 4: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/4.jpg)
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 range-only sensors have not received enough attention in the research field
![Page 5: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/5.jpg)
Outline
Introduction Range-Only Sensor Localization SLAM Future Work
![Page 6: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/6.jpg)
Range & Bearing Sensors 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.
Robot
Landmark
![Page 7: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/7.jpg)
Range-Only Sensors We are provided
with an annulus instead of an ellipse.
Extending classical approaches to localization requires additional considerations.
Robot
Landmark
![Page 8: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/8.jpg)
Range-Only Sensors
r1 r2
![Page 9: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/9.jpg)
Outline
Introduction Range-Only Sensor Localization
Kalman Filter Particle Filter Results
SLAM Future Work
![Page 10: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/10.jpg)
Predictor Corrector Iterative Process
Predict the new state (and its uncertainty) based on current state and process model
Correct state estimate with new measurement
CorrectorPredictor
![Page 11: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/11.jpg)
Outline
Introduction Range-Only Sensor Localization
Kalman Filter Particle Filter Results
SLAM Future Work
![Page 12: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/12.jpg)
Kalman Filter Belief Representation
Error Function – Gaussian Mean and Covariance
Process Model (State qk = [xr, yr, θr]T) qk = A·qk-1 + B·uk-1 + wk-1
Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1
Measurement Model qk = qk-1 + Kk·(zk – H·qk-1) Kk = Pk·HT·(H·Pk·HT + Rk)-1
Pk = (I – Kk·H)·Pk
^ –
– +
^ +
^ + ^ – ^ –
– –
+ –
![Page 13: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/13.jpg)
Kalman Filter Belief Representation
Error Function – Gaussian Mean and Covariance
Process Model (State qk = [xr, yr, θr]T) qk = A·qk-1 + B·uk-1 + wk-1
Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1
Measurement Model qk = qk-1 + Kk·(zk – H·qk-1) Kk = Pk·HT·(H·Pk·HT + Rk)-1
Pk = (I – Kk·H)·Pk
^ –
– +
^ +
^ + ^ – ^ –
– –
+ –
Last Relative Measurement. (∆d, ∆θ)
![Page 14: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/14.jpg)
Kalman Filter Belief Representation
Error Function – Gaussian Mean and Covariance
Process Model (State qk = [xr, yr, θr]T) qk = A·qk-1 + B·uk-1 + wk-1
Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1
Measurement Model qk = qk-1 + Kk·(zk – H·qk-1) Kk = Pk·HT·(H·Pk·HT + Rk)-1
Pk = (I – Kk·H)·Pk
^ –
– +
^ +
^ + ^ – ^ –
– –
+ –
Measurement range to beacon
Estimated range to beaconMeasurement
variance
![Page 15: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/15.jpg)
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
![Page 16: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/16.jpg)
Outline
Introduction Range-Only Sensor Localization
Kalman Filter Particle Filter Results
SLAM Future Work
![Page 17: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/17.jpg)
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 re-sampling
Sampling – Standard Gaussian Formula: P(x|rm) = e( )
Where rm is the measured range and r is the range estimate from the particle to beacon
1 .
σ√2π-(r – rm)2
2σ
^
^
![Page 18: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/18.jpg)
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
![Page 19: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/19.jpg)
Outline
Introduction Range-Only Sensor Localization
Kalman Filter Particle Filter Results
SLAM Future Work
![Page 20: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/20.jpg)
The Experiments
![Page 21: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/21.jpg)
Dead Reckoning – Results
![Page 22: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/22.jpg)
Kalman Filter – Results
XTE ATE
Mean Abs. Error
0.5539 m0.3976 m
Max. Error 1.9033 m2.0447 m
Std (σ) 0.4173 m0.3558 m
![Page 23: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/23.jpg)
Particle Filter – Results
XTE ATE
Mean Abs. Error
2.8200 m 2.0898 m
Max. Error 8.6526 m 7.7012 m
Std (σ) 1.0345 m 1.1943 m
![Page 24: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/24.jpg)
Outline
Introduction Range-Only Sensor Localization SLAM
Batch Slam Kalman Filter Results
Future Work
![Page 25: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/25.jpg)
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
![Page 26: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/26.jpg)
Outline
Introduction Range-Only Sensor Localization SLAM
Batch Slam Kalman Filter Results
Future Work
![Page 27: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/27.jpg)
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
![Page 28: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/28.jpg)
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:
![Page 29: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/29.jpg)
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
![Page 30: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/30.jpg)
Outline
Introduction Range-Only Sensor Localization SLAM
Batch Slam Kalman Filter Results
Future Work
![Page 31: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/31.jpg)
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
![Page 32: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/32.jpg)
Beacon Initialization
![Page 33: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/33.jpg)
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
![Page 34: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/34.jpg)
Kalman Filter SLAM
Advantages: Similar to Kalman Filter Localization Settles to locally accurate solution
Disadvantages: Wrong Beacon Initialization could lead to
diverged solution
![Page 35: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/35.jpg)
Outline
Introduction Range-Only Sensor Localization SLAM
Batch Slam Kalman Filter Results
Future Work
![Page 36: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/36.jpg)
Kalman Filter SLAM – Results
Raw XTE ATE
Mean Abs. Error
8.5544 m 5.1776 m
Max. Error 18.0817 m 19.2575 m
Std (σ) 4.8216 m 4.5486 m
Aff. Trans. XTE ATE
Mean Abs. Error
0.7297 m 0.6872 m
Max. Error 2.6787 m 2.7621 m
Std (σ) 0.6004 m 0.5745 m
![Page 37: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/37.jpg)
Kalman Filter & Batch Slam – Results(Another Example)
Batch Slam XTE ATE
Mean Abs. Error
1.5038 m 2.0871 m
Max. Error 4.9149 m 5.8212 m
Std (σ) 1.0527 m 1.4968 m
Batch Slam using only 5% of data set
![Page 38: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/38.jpg)
Outline
Introduction Range-Only Sensor Localization SLAM Future Work
![Page 39: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/39.jpg)
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
![Page 40: Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth](https://reader035.vdocuments.us/reader035/viewer/2022062516/56649d385503460f94a11627/html5/thumbnails/40.jpg)
Questions…