Introduction to Mobile Robotics EKF Localization Wolfram Burgard, - PowerPoint PPT Presentation

Introduction to Mobile Robotics EKF Localization Wolfram Burgard, Cyrill Stachniss, Maren Bennewitz, Kai Arras Slides by Kai Arras and Wolfram Burgard Last update: June 2010

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] • 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)

Landmark-based Localization EKF Localization: Basic Cycle 3

Landmark-based Localization EKF Localization: Basic Cycle 4

Landmark-based Localization EKF Localization: Basic Cycle posterior encoder measurements state innovation from predicted matched landmarks state landmarks in global coordinates predicted measurements in sensor coordinates landmarks raw sensory data 5

Landmark-based Localization State Prediction (Odometry) Control u k : wheel displacements s l , s r Error model : linear growth Nonlinear process model f : 6

Landmark-based Localization State Prediction (Odometry) Control u k : wheel displacements s l , s r Error model : linear growth Nonlinear process model f : 7

Landmark-based Localization Landmark Extraction (Observation) Raw laser Extracted Extracted lines range data lines in model space Hessian line model 8

Landmark-based Localization Measurement Prediction • ...is a coordinate frame transform world-to-sensor • Given the predicted state (robot pose), predicts the location and location model space uncertainty of expected observations in sensor coordinates 9

Landmark-based Localization Data Association (Matching) • Associates predicted measurements with observations model space • Innovation and innovation covariance • Matching on significance level alpha Green: observation Magenta: measurement prediction 10

Landmark-based Localization Update • Kalman gain • State update (robot pose) • State covariance update Red: posterior estimate 11

Landmark-based Localization • EKF Localization with Point Features

1. EKF_localization ( µ t-1 , Σ t-1 , u t , z t , m ): Prediction: 2. Jacobian of g w.r.t location   ∂ x ' ∂ x '   ∂ v t ∂ ω t   ∂ g ( u t , µ t − 1 ) ∂ y ' ∂ y ' Jacobian of g w.r.t control 3.   B t = =   ∂ u t ∂ v t ∂ ω t   ∂ θ ' ∂ θ '   ∂ v t ∂ ω t     2 ( ) α 1 | v t | + α 2 | ω t | 0 Motion noise 4. Q t =     2 ( ) 0 α 3 | v t | + α 4 | ω t |   Predicted mean 5. T + B t Q t B t Predicted covariance T 6. Σ t = G t Σ t − 1 G t

1. EKF_localization ( µ t-1 , Σ t-1 , u t , z t , m ): Correction: 2. Predicted measurement mean Jacobian of h w.r.t location 3.   2 0 R t = σ r 4.   2 0 σ r   T + R t Innovation covariance 5. S t = H t Σ t H t Kalman gain 6. Updated mean 7. 8. Updated covariance

EKF Prediction Step

EKF Observation Prediction Step

EKF Correction Step

Estimation Sequence (1)

Estimation Sequence (2)

Comparison to GroundTruth

EKF Localization Example • [Arras et al. 98]: • Laser range-finder and vision • High precision (<1cm accuracy) Courtesy of K. Arras

EKF Localization Example • Line and point landmarks

EKF Localization Example • Line and point landmarks

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

EKF Localization Example

Global EKF Localization Interpretation tree 26

Global EKF Localization Env. Dynamics 27

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 α 28

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 29

Global EKF Localization Pygmalion α = 0.95 , p = 2 30

Global EKF Localization Pygmalion α = 0.95 , p = 3 31

Global EKF Localization Pygmalion α = 0.95 , p = 4 t exe : 633 ms PowerPC at 300 MHz 32

Global EKF Localization Pygmalion α = 0.95 , p = 5 t exe : 633 ms (PowerPC at 300 MHz) 33

Global EKF Localization At Expo.02 05.07.02, 17.23 h α = 0.999 [Arras et al. 03]

Global EKF Localization At Expo.02 05.07.02, 17.23 h t exe = 105 ms α = 0.999 [Arras et al. 03]

Global EKF Localization At Expo.02 05.07.02, 17.32 h α = 0.999 [Arras et al. 03]

Global EKF Localization At Expo.02 05.07.02, 17.32 h t exe = 446 ms α = 0.999 [Arras et al. 03]

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