16-311-Q I NTRODUCTION TO R OBOTICS F ALL ’17 L ECTURE 22: M AP B UILDING ( UNKNOWN # OF LANDMARKS ) EKF SLAM I NSTRUCTOR : G IANNI A. D I C ARO
PROBLEM: WHAT IF THE NUMBER OF LANDMARKS IS UKNOWN? Usually, both the locations and the number M of the landmarks existing in the environment are not known a priori The state vector ξ k must be incrementally expanded Where(?) the M(?) landmarks? by two new coordinate components [ 𝝁 ix 𝝁 iy ] each time a new landmark 𝝁 i is observed State vector ξ of the EKF: (Coordinates of the M landmarks) Formally, the expansion of the state vector ξ k amounts to the following process dynamics (before process dynamics was stationary) , implemented through the use of the auxiliary (non linear!) function q() : ξ k 2 3 2 3 ξ k " # ξ k λ i 6 7 6 7 ξ ∗ k +1 = q ( ξ k , z k +1 ; x k , y k , θ k ) = = x k + ρ k cos( θ k + β k ) 5 = kx 6 7 6 7 g ( x k , y k , θ k , z k +1 ) 4 4 5 λ i y k + ρ k sin( θ k + β k ) ky The order of the landmarks in the state If ξ k is 2 n × 1 , n < M ⇒ ξ ∗ k +1 is 2( n + 1) × 1. vector depends on the order of observation 2
THE STATE EXPANSION FUNCTION ξ k 2 3 2 3 ξ k " # ξ k λ i 6 7 6 7 ξ ∗ k +1 = q ( ξ k , z k +1 ; x k , y k , θ k ) = = x k + ρ k cos( θ k + β k ) 5 = kx 6 7 6 7 g ( x k , y k , θ k , z k +1 ) 4 4 5 λ i y k + ρ k sin( θ k + β k ) ky • The vector function g (x, y, θ , z ), which is the inverse of the observation function l (), gives the measured coordinates (in the world frame) of the observed landmark. • Inputs: known vehicle pose [ ︎ x y θ ] T , sensor observation z = [ ︎ ρ β ] T at time step k . • g() gets the local robot measure and outputs the world coordinates of the observed landmark : x + ρ cos( θ + β ) g ( x, y, θ , z ) = y + ρ sin( θ + β ) 3
EXPANDING BOTH STATE AND COVARIANCE MATRIX • After the new landmark i has been observed, at step k + 1 , the expanded state vector: λ i ⇤ T = ⇤ T ⇥ ⇥ λ t λ t λ s λ s λ j λ j λ i λ i ξ ∗ ξ k k +1 ← x y x y x y x y . . . . . . • Without losing generality, let’s assume that the landmarks are ordered incrementally in the state vector → landmark i is the i -th discovered landmark: λ i ⇤ T = ⇤ T ⇥ ⇥ λ 1 λ 1 λ 2 λ 2 λ 3 λ 3 λ i λ i ξ ∗ ξ k k +1 ← x y x y x y x y . . . . . . • The new expanded covariance matrix , of dimension 2 i × 2 i : σ λ 1 σ λ 1 σ λ 1 σ λ 1 σ λ 1 σ λ 1 x λ 1 x λ 1 x λ 2 x λ 2 x λ i x λ i . . . x y x y x y σ λ 1 σ λ 1 σ λ 1 σ λ 1 σ λ 1 σ λ 1 y λ 1 y λ 1 y λ 2 y λ 2 y λ i y λ i . . . x y x y x y Σ λ 1 λ 1 Σ λ 1 λ 2 Σ λ 1 λ i . . . σ λ 2 σ λ 2 σ λ 2 σ λ 2 σ λ 2 σ λ 2 x λ 1 x λ 1 x λ 2 x λ 2 x λ i x λ i . . . x y x y x y Σ λ 2 λ 1 Σ λ 2 λ 2 Σ λ 2 λ i . . . P ∗ σ λ 2 σ λ 2 σ λ 2 σ λ 2 σ λ 2 σ λ 2 k +1 | k = y λ 1 y λ 1 y λ 2 y λ 2 y λ i y λ i = . . . x y x y x y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Σ λ i λ 1 Σ λ i λ 2 Σ λ i λ i . . . σ λ i σ λ i σ λ i σ λ i σ λ i σ λ i x λ 1 x λ 1 x λ 2 x λ 2 x λ i x λ i . . . x y x y x y σ λ i σ λ i σ λ i σ λ i σ λ i σ λ i y λ 1 y λ 1 y λ 2 y λ 2 y λ i y λ i 4 . . . x y x y x y
T H E E X PA N D E D C O VA R I A N C E M AT R I X 5
P R E D I C T I O N U P D AT E F O L L O W I N G A S TAT E E X PA N S I O N • The process dynamics defined through q () is not linear → the Jacobian of q () needs to be computed to define the EKF process updating equations based on the expanded state and covariance 6
J A C O B I A N M AT R I X i T h Q k ( λ k , z k +1 , w ρ k , w β r q λ 1 r q λ 1 r q λ n r q λ n r g kx r g ky k ; x k , y k , θ k ) = . . . kx ky kx ky 7
J A C O B I A N S F O R L I N E A R I Z AT I O N O F q k ( ) Said n = ( i − 1) , the number of landmarks before the current new observation, the dimension of the covariance matrix before the expansion is 2 n × 2 n , while the dimension of the Jacobian Q is (2 n + 2) × (2 n + 3 + 2) Q k ξ is evaluated at the current observation z k +1 , therefore is also referred to as Q z k +1 The Jacobian Q k ξ of q k () for the linearization of the state dynamics in the case of state expansion can be also expressed in a more compact form: " # 0 I 2 n × 2 n 0 0 2 n × 2 Q k ξ = G ξ G z ˆ ξ k | k , z k +1 ,x k ,y k , θ k , w =0 " # " # 0 . . . 0 cos( θ k + β k ) − ρ k sin( θ k + β k ) G ξ = ∂ g k G z = ∂ g k = = ∂ ξ k ∂ z k 0 . . . 0 sin( θ k + β k ) ρ k cos( θ k + β k ) The Jacobian Q k ξ allows to linearly transform (expand) the covariance matrix P k when adding a new landmark to the state vector: 2 3 0 0 0 2 n × 2 P k | k " # 0 0 0 2 n × 2 P k | k Q T P ∗ k | k +1 = Q k ξ k ξ = 4 5 0 0 0 2 × 2 n W k 0 2 × 2 n G z W k G T 0 0 z which results in an expanded 2( n + 1) × 2( n + 1) matrix 8
E K F I N C L U D I N G S TAT E E X PA N S I O N The Jacobians L k ⇠ and L k w of ` k () , computed before, accounts for the linearization of the observation model when an already discovered landmark is observed ( x k and y k are parameters): λ i λ i 2 ky − y k 3 kx − x k 0 0 . . . . . . 0 0 " # 1 0 r i r i 6 7 k k L k ⇠ = L k w = 6 7 λ i λ i ky − y k 6 7 kx − x k 0 1 4 5 0 0 . . . − . . . 0 0 ( r i k ) 2 ( r i k ) 2 ˆ 0 ⇠ k +1 | k , 0 0 The complete EKF equations: At every time step k + 1 a landmark i is observed When a new landmark i is observed: ⇠ k +1 = ˆ ˆ ⇠ k +1 | k + G k +1 ( z k +1 − ` k (ˆ � k +1 | k , 0 0 0; x k , y k , θ k )) ˆ k +1 | k = q k (ˆ ⇠ ∗ � k | k , z k +1 , x k , y k , θ k ) P k +1 = P k +1 | k − G k +1 L k ⇠ P k +1 | k 2 3 0 P k | k 0 0 G k +1 = P k +1 | k L k ⇠ T S − 1 P ∗ k +1 | k = 4 5 k +1 G z W k G T 0 0 0 S k +1 = L k ⇠ P k +1 | k L k ⇠ T + L k w W k +1 L k w T z 9
S O LV I N G L O C A L I Z AT I O N A N D M A P P I N G T O G E T H E R SOLVING LANDMARK MAPPING AND ROBOT LOCALIZATION TOGETHER The previous derivation was based on the fact that robot’s localization was perfect Since this is not (usually) the case, let’s solve the joint problem: Simultaneous Localization and Mapping (SLAM) The state vector needs to include also robot’s coordinates, since also robot’s pose needs to be estimated ⇤ T x, y, θ , λ 1 x λ 1 y λ 2 x λ 2 ⇥ y . . . λ M λ M ξ = x y ξ has now dimension (2 M + 3) × 1 , and P is a (2 M + 3) × (2 M + 3) matrix 10
T H E S L A M C H A L L E N G E I N A N U T S H E L L THE PROBLEM IN A NUTSHELL Looking for absolute robot pose Looking for absolute landmark positions But only relative measurements of landmarks are available 11
W H Y S L A M I S A H A R D P R O B L E M • Robot path and the landmark map are both unknown • Errors in map and pose estimates are correlated • Mapping between observations and landmarks is unknown • Data Association: Selecting wrong data association can be disastrous (divergence) 12
S TAT E V E C T O R A N D C O VA R I A N C E M AT R I X The state vector and the covariance matrix: the state vector includes both the robot’s generalized coordinates and landmarks’ coordinates; said n = ( i − 1) the number of discovered landmarks at step k : i T ⇤ T = h x k y k θ k λ 1 k λ 2 ⇥ k . . . λ n ξ R k ξ λ n ξ k = ⇥ ⇤ k k and the resulting covariance matrix (omitting the index k ) is: 2 3 σ xx σ xy σ x θ σ x λ 1 σ x λ 1 . . . σ x λ n σ x λ n x y x y 6 7 σ yx σ yy σ y θ σ y λ 1 σ y λ 1 . . . σ y λ n σ y λ n 6 7 x y x y 6 7 6 7 σ θ x σ θ y σ θθ σ θλ 1 σ θλ 1 . . . σ θλ n σ θλ n 6 7 x y x y 6 7 6 7 " # σ λ 1 x x σ λ 1 x y σ λ 1 x θ σ λ 1 x σ λ 1 y . . . σ λ 1 x σ λ 1 Σ RR Σ R λ 6 7 x λ 1 x λ 1 x λ n x λ n P (3+2 n ) × (3+2 n ) = = y 6 7 6 7 Σ T σ λ 1 y x σ λ 1 y y σ λ 1 y θ σ λ 1 x σ λ 1 y . . . σ λ 1 x σ λ 1 R λ Σ λλ 6 y λ 1 y λ 1 7 y λ n y λ n y 6 7 6 7 . . . . . . . . . . . . . . . . . . . . . . . . 6 7 6 7 6 7 σ λ n x x σ λ n x y σ λ n x θ σ λ n x σ λ n y . . . σ λ n x σ λ n x λ n x λ n x λ n x λ n 6 7 y 4 5 σ λ n y x σ λ n y y σ λ n y θ σ λ n x σ λ n y . . . σ λ n x σ λ n y λ n y λ n y λ n y λ n y 13
T H E S L A M M O V I E Robot’s pose prediction using the odometry model Observe the environment while moving: 1 Get a landmark observation 2 Perform a measurement prediction using the map (being) created Compare predicted and observed measure to perform data association (is a new landmark?) and compute innovation Update predictions for robot pose and landmark’s position 14
Recommend
More recommend
