16-311-Q I NTRODUCTION TO R OBOTICS F ALL ’17 L ECTURE 23: SLAM P ROPERTIES D ATA A SSOCIATION P ROBLEM I NSTRUCTOR : G IANNI A. D I C ARO
E K F E Q U AT I O N S F O R S L A M EKF FOR SIMULTANEOUS LOCALIZATION AND MAPPING The SLAM EKF equations: When a new landmark i is observed: At every time step k : ˆ k +1 | k = q k (ˆ ξ ∗ ξ k | k , z k +1 ) ξ k +1 | k = f k (ˆ ˆ ξ k | k , 0 0 0; ∆ S k , ∆ θ k ) " # 0 0 0 P k | k P k +1 | k = F k ξ P k F k ξ T + F k ν V k F k ν T Q k ξ T k +1 | k = Q k ξ P ∗ 0 0 0 W k At every time step k + 1 a landmark i is observed ξ k +1 = ˆ ˆ ξ k +1 | k + G k +1 ( z k +1 − o k (ˆ 0 ξ k +1 | k , 0 0)) P k +1 = P k +1 | k − G k +1 O k ξ P k +1 | k G k +1 = P k +1 | k O k ξ T S − 1 k +1 S k +1 = O k ξ P k +1 | k O k ξ T + O k w W k +1 O k w T The Kalman gain matrix G multiplies innovation from the landmark observation, a 2-vector, so as to update every element of the state vector : the pose of the vehicle and the position of every map feature. 2
S L A M C O VA R I A N C E M AT R I X SLAM PERFORMANCE: COVARIANCE MATRIX " # Σ RR Σ R λ Σ T R λ Σ λλ 3
I N T H E L I M I T, A L L E S T I M AT E S A R E C O R R E L AT E D IN THE LIMIT, ALL ESTIMATES BECOME FULLY CORRELATED From the moment of initialization, the location of a landmark is a function of the robot location, such that errors in the robot location will also appear as errors in feature location Every observation of a feature a ff ects the estimate of every other feature in the map. It’s as they are all tied up together with elastic bands - pulling at one will pull at the others in turn. 4
C O R R E L AT I O N H E L P S T O R E D U C E U N C E RTA I N T Y • Landmark position is either known (from a given map) or has been previously estimated (from the map being built during SLAM) • In (d) the robot senses the first landmark again, whose position was known with relatively little uncertainty For reducing pose’s uncertainty, and fully exploit the correlation in the covariance matrix, it is necessary to see the same landmark multiple times 5
L O O P C L O S U R E LOOP CLOSURE Recognizing an already mapped area → (All) uncertainties collapse Landmark uncertainties shrinks di ff erently according to their initial value and to the distance from the loop closure point Data association is potentially highly ambiguous Environment symmetries make the problem even more di ffi cult Wrong loop closure is catastrophic and leads to divergence 6
A N O T H E R S L A M M O V I E , M O R E R E A L I S T I C ANOTHER SLAM SIMULATION MOVIE, MORE REALISTIC 7
E V O L U T I O N O F T H E C O R R E L AT I O N S T R U C T U R E THE EVOLUTION OF THE CORRELATION STRUCTURE Known number of landmarks, normalized covariance Checkerboard structure (dark and white alternating with regularity): the x coordinates are correlated among themselves, and similarly for the y , but not cross-correlation between the two spatial dimensions 8
U N C E RTA I N T Y D E C R E A S E F O R T H E L A N D M A R K S When a new landmark is initialized, its uncertainty is maximal The determinant of any landmark sub-matrix of the map covariance matrix decreases monotonically → Landmark uncertainties never increase (the landmarks do not move and their prediction step do not increase their covariance) 9 Robot position uncertainty can grow over time if no landmarks are observed
B O U N D S O N T H E U N C E RTA I N T I E S BOUNDS ON THE UNCERTAINTIES The landmark position uncertainties (in the global reference frame) never drop below the initial uncertainty of the robot. In the limit, the covariance associated with any single landmark location estimate is determined only by the initial covariance in the robot location estimate. However, the relative uncertainties can asymptotically go to zero 10
C O M P U TAT I O N A L I S S U E S Convergence is guaranteed when everything is linear Can diverge if nonlinearities are large . . . Cost per step: quadratic in the number n of landmarks: O ( n 2 ) Total cost to build a map with n landmarks: O ( n 3 ) Memory: O ( n 2 ) Problem: becomes computationally intractable for large maps! Approaches exist that make the cost of EKF-SLAM one order less 11
SLAM IN ROS (GMAPPING PACKAGE) Hokuyo URG laser scanner + ROS gmapping package 12
THE VICTORIA PARK DATASET/STUDY The data gathering procedure using a laser on a car The “reference” paper / dataset for SLAM Full paper + videos: http://www-personal.acfr.usyd.edu.au/nebot/victoria_park.htm 13
THE “GOOD” TREES TO BE USED FOR LANDMARKS The complexity of the environment Distinguish between people and trees It’s not easy to single out the trees, distinguishing them from other environment features, passing by people, and cars … “Complex” trees, not truly cylindrical Loop closure is problematic here … 14
L A N D M A R K S ( R E A L A N D W R O N G O N E S ) THE LANDMARKS (REAL AND WRONG ONES) 15
T I M E E V O L U T I O N A N D F I N A L O U P U T THE TIME EVOLUTION AND THE FINAL OUTPUT Animation for the final trajectory and navigation map: http://www-personal.acfr.usyd.edu.au/nebot/publications/slam/video/slam1.gif Evolution of the error on the state: http://www-personal.acfr.usyd.edu.au/nebot/publications/slam/video/dev4.gif 16
T H E P R O B L E M O F C L A S S I F Y I N G L A N D M A R K S THE PROBLEM OF CLASSIFYING LANDMARKS 17
D ATA A S S O C I AT I O N So far, it was assumed that landmark correspondences can be determined with certainty: the identity of the landmark was returned by the sensor Data association problem: Identity has to be determined based on the raw sensor data, or from the extracted features • Which landmark in the map, am I observing? • Is the landmark a new one or an existing one? • The extracted features, to what map element do they correspond? 18
D ATA A S S O C I AT I O N A good choice of the landmarks/features is essential to minimize errors: • Should be easily re-observable • Should be distinguishable from each other • Should be plentiful in the environment • Should be stationary Wrong data association can be catastrophic! DA it's a very general classification problem , with many di ff erent solution approaches like: Nearest Neighbors, Joint Compatibility Branch and Bound (JCBB), InterpretationTree, Joint Probabilistic Data Association filter (JPDA), … Maximum likelihood 19
M L D ATA A S S O C I AT I O N F O R E K F W I T H U N K N O W N L A N D M A R K C O R R E S P O N D E N C E S Scenario : The number of landmarks is known , such that the data association algorithm has to perform only the right association between the observed data and one of the n landmarks in the map, based on their estimated positions λ i State equation for robot motion: 2 3 k ) cos( θ k + ∆ θ k ( ∆ S k + ν s + ν θ 2 3 x k k ) 2 6 7 k ) sin( θ k + ∆ θ k 6 7 y k ( ∆ S k + ν s + ν θ k ) 6 7 6 7 2 6 7 6 7 6 7 ∆ θ k + ν θ θ k 6 7 6 7 6 7 k 6 7 At each step k : ⇠ k +1 = + = f k ( ⇠ k , ⌫ k ; ∆ S k , ∆ θ k ) 6 7 6 7 � 1 6 0 7 0 0 6 7 6 7 k 6 7 6 7 6 7 6 7 6 7 · · · · · · 6 7 6 7 4 5 4 5 0 � n 0 0 k Observation equation for landmarks / features: the identity i is not returned q 2 3 kx − x k ) 2 + ( λ i w ρ ( λ i " # " # ky − y k ) 2 ρ i k k +1 5 + z k +1 = = = o k ( ξ k , w k ) 4 ⇣ ⌘ w β β i ( λ i ky − y k ) / ( λ i arctan kx − x k ) − θ k k +1 k 20
S E N S O R F R A M E , R O B O T F R A M E , M A P F R A M E z k = [ 𝝇 𝜸 ] T is the measure returned from the sensor → Measured position of landmark in the sensor frame {S} 𝝁 i = [ x i y i ] T is the estimated position of landmark i (based on observations) in map M → Landmark position in the map frame {M} • We have (and will) assumed that the sensor frame {S} and the r obot frame (base_link) {R} coincides • Robot pose is in the map frame • The observation equation o k () transforms landmark map coordinates into the sensor frame in order to compare them (+ account for noise) 21
M L C R I T E R I O N F O R L A N D M A R K A S S O C I AT I O N Innovation i ff observed landmark is λ j • Each feature of the observation vector provides a contribution • Which one is the best landmark? ML: The one the best explains the observation • → The one with the minimal distance from the observation, accounting for the uncertainties 22
M A H A L A N O B I S D I S TA N C E Distance between measured and predicted observation is a Gaussian random variable, being the di ff erence between two Gaussian random variables ✏ j z j ˆ k +1 = z k +1 − ˆ k +1 The distance between two Gaussian rv is well defined by the Mahalanobis distance 23
M A H A L A N O B I S D I S TA N C E 24
M A H A L A N O B I S D I S TA N C E 25
M A H A L A N O B I S D I S TA N C E 26
M L C R I T E R I O N F O R L A N D M A R K S E L E C T I O N 27
VA L I D AT I O N G AT E S 28
W H AT VA L U E F O R 𝝳 ? 29
A M O R E D I F F I C U LT P R O B L E M : N E W O R E X I S T I N G ? 30
Recommend
More recommend
