1 / 40

Reliable Range based Localization and SLAM

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

garnet
Download Presentation

Reliable Range based Localization and SLAM

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth

  2. Motivation

  3. Motivation

  4. 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

  5. Outline • Introduction • Range-Only Sensor • Localization • SLAM • Future Work

  6. Landmark Robot 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.

  7. Landmark Robot Range-Only Sensors • We are provided with an annulus instead of an ellipse. • Extending classical approaches to localization requires additional considerations.

  8. r1 r2 ? Range-Only Sensors

  9. Outline • Introduction • Range-Only Sensor • Localization • Kalman Filter • Particle Filter • Results • SLAM • Future Work

  10. Predictor  Corrector • Iterative Process • Predict the new state (and its uncertainty) based on current state and process model • Correct state estimate with new measurement

  11. Outline • Introduction • Range-Only Sensor • Localization • Kalman Filter • Particle Filter • Results • SLAM • Future Work

  12. 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 ^– ^+ + – ^+ ^– ^– – – + –

  13. Last Relative Measurement. (∆d, ∆θ) 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 ^– ^+ + – ^+ ^– ^– – – + –

  14. Measurement range to beacon Measurement variance 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 ^– ^+ + – ^+ ^– ^– – – Estimated range to beacon + –

  15. 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

  16. Outline • Introduction • Range-Only Sensor • Localization • Kalman Filter • Particle Filter • Results • SLAM • Future Work

  17. 1 . σ√2π ^ -(r – rm)2 2σ 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 ^

  18. 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

  19. Outline • Introduction • Range-Only Sensor • Localization • Kalman Filter • Particle Filter • Results • SLAM • Future Work

  20. The Experiments

  21. Dead Reckoning – Results

  22. Kalman Filter – Results

  23. Particle Filter – Results

  24. Outline • Introduction • Range-Only Sensor • Localization • SLAM • Batch Slam • Kalman Filter • Results • Future Work

  25. 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

  26. Outline • Introduction • Range-Only Sensor • Localization • SLAM • Batch Slam • Kalman Filter • Results • Future Work

  27. 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

  28. 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:

  29. 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

  30. Outline • Introduction • Range-Only Sensor • Localization • SLAM • Batch Slam • Kalman Filter • Results • Future Work

  31. 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

  32. Beacon Initialization

  33. 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

  34. Kalman Filter SLAM • Advantages: • Similar to Kalman Filter Localization • Settles to locally accurate solution • Disadvantages: • Wrong Beacon Initialization could lead to diverged solution

  35. Outline • Introduction • Range-Only Sensor • Localization • SLAM • Batch Slam • Kalman Filter • Results • Future Work

  36. Kalman Filter SLAM – Results

  37. Kalman Filter & Batch Slam – Results(Another Example) Batch Slam using only 5% of data set

  38. Outline • Introduction • Range-Only Sensor • Localization • SLAM • Future Work

  39. 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

  40. Questions…

More Related