Introduction to Mobile Robotics EKF...
Transcript of Introduction to Mobile Robotics EKF...
![Page 1: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/1.jpg)
Wolfram Burgard, Cyrill Stachniss,
Maren Bennewitz, Kai Arras
EKF Localization
Introduction to Mobile Robotics
Slides by Kai Arras and Wolfram Burgard Last update: June 2010
![Page 2: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/2.jpg)
Localization
• Given • Map of the environment. • Sequence of sensor measurements.
• Wanted • Estimate of the robot’s position.
• Problem classes • Position tracking • Global localization • Kidnapped robot problem (recovery)
“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]
![Page 3: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/3.jpg)
Landmark-based Localization
EKF Localization: Basic Cycle
3
![Page 4: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/4.jpg)
Landmark-based Localization
EKF Localization: Basic Cycle
4
![Page 5: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/5.jpg)
Landmark-based Localization
EKF Localization: Basic Cycle
5
raw sensory data
landmarks
innovation from matched landmarks
predicted measurements in
sensor coordinates
landmarks in global coordinates
encoder measurements
predicted state
posterior state
![Page 6: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/6.jpg)
State Prediction (Odometry)
Landmark-based Localization
6
Control uk: wheel displacements sl , sr
Error model: linear growth
Nonlinear process model f :
![Page 7: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/7.jpg)
State Prediction (Odometry)
Landmark-based Localization
7
Control uk: wheel displacements sl , sr
Error model: linear growth
Nonlinear process model f :
![Page 8: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/8.jpg)
Landmark-based Localization
Landmark Extraction (Observation)
8
Extracted lines
Hessian line model
Extracted lines in model space
Raw laser range data
![Page 9: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/9.jpg)
Landmark-based Localization
Measurement Prediction
• ...is a coordinate frame transform world-to-sensor
• Given the predicted state (robot pose), predicts the location and location uncertainty of expected observations in sensor coordinates
9
model space
![Page 10: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/10.jpg)
Data Association (Matching)
• Associates predicted measurements with observations
• Innovation and innovation covariance
• Matching on significance level alpha
Landmark-based Localization
10
model space
Green: observation Magenta: measurement prediction
![Page 11: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/11.jpg)
Landmark-based Localization
Update
• Kalman gain
• State update (robot pose)
• State covariance update
11
Red: posterior estimate
![Page 12: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/12.jpg)
• EKF Localization with Point Features
Landmark-based Localization
![Page 13: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/13.jpg)
1. EKF_localization ( µt-1, Σt-1, ut, zt, m):
Prediction:
2.
3.
4.
5.
6.
€
Σt =GtΣt−1GtT + BtQtBt
T
€
Bt =∂g(ut ,µt−1)
∂ut=
∂x'∂vt
∂x'∂ω t
∂y'∂vt
∂y'∂ω t
∂θ'∂vt
∂θ '∂ω t
€
Qt =α1 | vt |+α2 |ω t |( )2 0
0 α3 | vt |+α4 |ω t |( )2
Motion noise
Jacobian of g w.r.t location
Predicted mean
Predicted covariance
Jacobian of g w.r.t control
![Page 14: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/14.jpg)
1. EKF_localization ( µt-1, Σt-1, ut, zt, m):
Correction:
2.
3.
4.
5.
6.
7.
8. €
St = HtΣ tHtT + Rt
€
Rt =σ r2 00 σ r
2
Predicted measurement mean
Innovation covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
![Page 15: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/15.jpg)
EKF Prediction Step
![Page 16: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/16.jpg)
EKF Observation Prediction Step
![Page 17: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/17.jpg)
EKF Correction Step
![Page 18: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/18.jpg)
Estimation Sequence (1)
![Page 19: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/19.jpg)
Estimation Sequence (2)
![Page 20: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/20.jpg)
Comparison to GroundTruth
![Page 21: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/21.jpg)
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Courtesy of K. Arras
EKF Localization Example
![Page 22: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/22.jpg)
EKF Localization Example
• Line and point landmarks
![Page 23: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/23.jpg)
EKF Localization Example
• Line and point landmarks
![Page 24: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/24.jpg)
EKF Localization Example • Expo.02: Swiss National Exhibition 2002 • Pavilion "Robotics" • 11 fully autonomous robots • tour guides, entertainer, photographer • 12 hours per day • 7 days per week • 5 months
• 3,316 km travel distance • almost 700,000 visitors • 400 visitors per hour
• Localization method: Line-Based EKF
![Page 25: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/25.jpg)
EKF Localization Example
![Page 26: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/26.jpg)
26
Global EKF Localization
Interpretation tree
![Page 27: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/27.jpg)
27
Global EKF Localization
Env. Dynamics
![Page 28: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/28.jpg)
28
Global EKF Localization Geometric constraints we can exploit
Location independent constraints
Unary constraint: intrinsic property of feature e.g. type, color, size
Binary constraint: relative measure between features e.g. relative position, angle
All decisions on a significance level α
![Page 29: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/29.jpg)
29
Global EKF Localization
Interpretation Tree [Grimson 1987], [Drumheller 1987], [Castellanos 1996], [Lim 2000]
Algorithm
• backtracking • depth-first • recursive • uses geometric constraints • worst-case exponential
complexity
![Page 30: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/30.jpg)
30
Global EKF Localization
Pygmalion
α = 0.95 , p = 2
![Page 31: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/31.jpg)
31
Global EKF Localization
α = 0.95 , p = 3
Pygmalion
![Page 32: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/32.jpg)
32
Global EKF Localization
α = 0.95 , p = 4 texe: 633 ms PowerPC at 300
MHz
Pygmalion
![Page 33: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/33.jpg)
33
Global EKF Localization
α = 0.95 , p = 5
texe: 633 ms (PowerPC at 300 MHz)
Pygmalion
![Page 34: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/34.jpg)
05.07.02, 17.23 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
![Page 35: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/35.jpg)
texe = 105 ms
05.07.02, 17.23 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
![Page 36: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/36.jpg)
05.07.02, 17.32 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
![Page 37: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/37.jpg)
05.07.02, 17.32 h
Global EKF Localization
α = 0.999 texe = 446 ms
At Expo.02
[Arras et al. 03]
![Page 38: Introduction to Mobile Robotics EKF Localizationais.informatik.uni-freiburg.de/.../09d-ekf-localization.pdf · 2011-06-28 · Introduction to Mobile Robotics ... • Global localization](https://reader035.fdocuments.us/reader035/viewer/2022081514/5ec879d67dc0d5242f66e648/html5/thumbnails/38.jpg)
EKF Localization Summary
• EKF localization implements pose tracking
• Very efficient and accurate (positioning error down to subcentimeter)
• Filter divergence can cause lost situations from which the EKF cannot recover
• Industrial applications
• Global EKF localization can be achieved using interpretation tree-based data association
• Worst-case complexity is exponential
• Fast in practice for small maps