L12. EKF-SLAM: PART II. NA568 Mobile Robotics: Methods & Algorithms

Transcription

1 L12. EKF-SLAM: PART II NA568 Mobile Robotics: Methods & Algorithms

2 Today s Lecture Feature-based EKF-SLAM Review Data Association Configuration Space Incremental ML (i.e., Nearest Neighbor) Joint Compatibility

3 SLAM Using Landmarks 1. Move 2. Sense 3. Associate measurements with known features 4. Update state estimates for robot and previously mapped features 5. Find new features from unassociated measurements 6. Initialize new features 7. Repeat MIT Indoor Track

4 Incremental Map Making O 3 O 1 O 2 observation R 0 motion R 1 Courtesy J. Leonard

5 (Planar) EKF-SLAM Review State Vector (3+2n) column vector Belief

6 EKF-SLAM Process Model Robot Process Model (Stationary) Landmark Process Model Joint Process Model

7 EKF-SLAM Prediction

8 EKF-SLAM Observation Model Observation model for landmark i Sparse Jacobian (i.e., ) z is a k-dim column vector (e.g., k=2 for range and bearing) Hence

9 Stacked Observation Model Stacked observation model for m measurements where stacked Jacobian is given by and

10 EKF-SLAM Stacked (Batch) Update Update K t is a (3+2n) (2k) gain matrix composed of a weighted linear combination of the columns of t associated with R and m i indices Hence, in general, because K t is full, it non-trivially changes all of the elements in t during the covariance update

11 EKF-SLAM Sequential Update for i=1:k Think of this loop as a zero motion prediction + update step endfor Return the updated state

12 Batch vs Sequential Updates When can we do this? When sensor measurements are independent, i.e. Why? Because of conditional independence Are Batch vs. Sequential Updates Equivalent? Yes when observation models are linear (KF) Not quite when observation models are nonlinear (EKF) Why? because our linearization point for each H t i changes as we perform the updates sequentially

13 EKF-SLAM Landmark Initialization z t needs to be bijective in order to be able to perform landmark initialization from a single observation In the case of range/bearing observations, h( ) is bijective Hence, we can instantiate a new landmark into our state vector as follows

14 EKF-SLAM State Augmentation Augmentation model True measurement Actual measurement corrupted by noise where

15 State Augmentation of Belief Before After

16 EKF-SLAM Algorithm Pseudocode while 1 Do Prediction if no z t are available return else Determine correspondence variables c t if any c t are new Do State Augmentation endif Do Update (either batch or sequential) endwhile

17 Data Association 17 We ve assumed known data association Observation i is of landmark j What happens if we don t have data association? What are the consequences if we get data association wrong? Wormholes

18 Importance of Data Association Measurement z is used to improve estimate of x: K t T T = ΣtH ( Ht ΣtHt + Qt ) t µ = µ + K z h( µ )) t t t ( t t Σ = ( I K H ) Σ t t t t 1 If the association of measurement E i with feature F j is.. error: covariance: correct: spurious: Consistency Divergence! Courtesy Jose Neira

19 Data Association Given an environment map And a set of sensor observations Associate observations with map elements Vision Laser Courtesy Jose Neira

20 Difficulties: clutter Influence of the type, density, precision and robustness of features considered: Laser scanner: Small amount of features (n) Small amount of measurements (m) Low spuriousness Low clutter Courtesy Jose Neira

21 Difficulties: clutter Vertical Edge Monocular vision: Many features (n large) Many measurements (m large) no depth information higher spuriousness High clutter Courtesy Jose Neira

22 Difficulties: imprecision Both the sensor and the vehicle introduce imprecision Vertical Edge Trinocular vision: variable depth precision good angular precision Robot imprecision: introduces CORRELATED error Courtesy Jose Neira

23 Approaches to Data Association Search in configuration space: find robot location with maximal data to map overlapping Can be done with raw data Or with features» Speed of convergence? Search in correspondence space: find a consistent correspondence hypothesis and compute robot location 1. Extract features from data 2. Feature-based map (points, lines, trees, ) 3. Search for measurement to map feature correspondences» Exponential number of solutions? Courtesy Jose Neira

24 Vehicle with SICK laser Example Detect trees: Victoria Park, Sydney Courtesy Jose Neira

25 Search in configuration space Where is the vehicle? Courtesy Jose Neira

26 Search in configuration space Evaluate all vehicle locations looking for tree matchings ; (ui, vi): observation i relative to robot ; (xj, yj): absolute location of map feature j ; mv = 0; for x = xmin to xmax step xstep, for y = ymin to ymax step ystep, for t = tmin to tmax step tstep, v = 0; for i = 1 to m, (xi, yi) = compose(x,y,t,ui,vi); for j = 1 to n, d = dist (xi, yi, xj, vj); if d < dmax v = v + 1; fi rof rof if v > mv mv = v; best = (x,y,t); fi rof rof rof Courtesy Jose Neira

27 Data-driven search in configuration space (let the trees vote) Each pairing constrains the possible location of the vehicle (to a circle in this case). Stochastic map with 145 features Courtesy Jose Neira

28 Data-driven search in configuration space (let the trees vote) ; (ui, vi): observation i relative to robot ; (xj, yj): absolute location of map feature j ; for t = tmin to tmax step tstep, R = rotation(t); for i = 1 to m, (xi, yi) = R * (ui, vi); for j = 1 to n, (x, y) = (xj, yj) (xi, vi); addvote(x, y, t); rof rof rof Courtesy Jose Neira

29 Results The most voted solution(s) stands out Courtesy Jose Neira

30 n map features: Data Association (in correspondence space) m sensor measurements: Interpretation tree (Grimson et al. 87):... Goal: obtain a hypothesis that associates each observation E i with a feature F j i Non matched observations: possible hypotheses Courtesy Jose Neira

31 Map: Use constraints to prune the tree F 1 F 6 F 5 F 4 F 3 F 2 X Observations:... E 1 E 2 E 3 E 4 E5 X X Constraints: Feature location (needs an estimation of robot location) Geometric relations: angles, distances, (location independent) Courtesy Jose Neira

32 Individual Compatibility Measurement equation for observation E i and feature F j E i and F j are compatible if: d = length(z i ) Nearest Neighbour (NN) rule: associate E i with the feature F j having smallest Mahalanobis distance D ij Courtesy Jose Neira

33 Data Association: NN 33 Simplest data association scheme: Nearest neighbor. We have probabilistic estimates of where our landmarks are When we get an observation, which landmark does it match best? Mahalanobis distance Observation Mahl = 4.2 Landmark 2 Landmark 1

34 Nearest Neighbor 34 Mahalanobis distance What if Mahalanobis distance is really big? Minimum Mahalanobis distance > 3 Landmark 2 New Landmark 3 Landmark 1

35 Nearest Neighbor NN: for i = 1 to m -- measurement E i D2min = Mahalanobis2(E i, F 1 ) nearest = 1 for j = 2 to n -- feature F j Dij2 = Mahalanobis2(E i, F j ) if Dij2 < D2min then nearest = j D2min = Dij2 fi rof if D2min <= Chi2(d i,alpha) then H(i) = nearest else H(i) = 0 fi rof Greedy algorithm: O(mn) Courtesy Jose Neira

36 Nearest Neighbor 36 Computational Cost? Greedy algorithm: O(mn) Shortcomings? How do we make NN robust? Reject matches if two close landmarks. Avoid making decisions if unsure. Associate Landmark Ambiguous Create new landmark Mahalanobis distance

37 Why NN is brittle What we want to do is choose the joint correspondences that maximizes the likelihood of the observations given the data, i.e. Note this has exponentially many assignments Incremental ML, assumes conditional independence to simplify the problem so that the joint maximization becomes the maximization over individual assignments

38 Example: MonoRob SPURIOUS Nearest Neighbor: True Association: Courtesy Jose Neira

39 The fallacy of the Nearest Neighbour Parings may be individually compatible, but not jointly compatible: Feature CORRECT (y 1,x 1 ) is IC compatible (y 2, x 2 ) is IC compatible (y 3, x 2 ) is IC compatible {(y 1,x 1 ), (y 2, x 2 ) } Feature 1 Courtesy Jose Neira

40 Joint Compatitibily Given a hypothesis Joint measurement equation The joint hypothesis is compatible if: d = length(z) Courtesy Jose Neira

41 Individual.vs. Joint Compatibility Joint Compatibility assures consistency: Feature CORRECT (y 1,x 1 ) and (y 3, x 2 ) are jointly compatible (y 1,x 1 ) and (y 2, x 2 ) are NOT jointly compatible Feature 1 J. Neira, J.D. Tardós. Data Association in Stochastic Mapping using the Joint Compatibility Test, IEEE Trans. Robotics and Automation, Vol. 17, No. 6, Dec 2001, pp Courtesy Jose Neira

42 Joint Data Association 42 Data association on individual objects is highly error prone Wait until we have more data Process several observations jointly Pick best joint assignment Enforce mutual exclusion Each observation can match to only one landmark Each landmark can match to only one observation

43 Joint Assignment 43 In EKF, landmark positions tend to be correlated They have trajectory error in common i.e., landmark estimates move together. Which of these assignments is more jointly compatible? A B

44 Example of how robot pose introduces correlation in predicted observations Suppose we have two measurements, i.e., m=2 Our observation Jacobian looks like: The observation covariance is then:

45 Computing Joint Compatibility 45 Hypothetical data association observed three landmarks: (Nice linear observation model this time) What is the Mahalanobis distance of the observation with respect to the noisy observation of the RHS? What is the mean and covariance of the RHS?

46 Joint Compatibility: Problems 46 We need to search over possible assignments. Still an exponential number! For each, we have to compute Mahalanobis distance Involves inverting a matrix of size 2N 2N (where N = # of jointly estimated landmarks) Joint Compatibility Branch & Bound is (partial) solution. Reduce search space via heuristics Use incremental matrix inversion trick

47 47 Joint Compatibility Branch and Bound (JCBB) Purpose: Find the largest hypothesis with jointly consistent pairings JCBB(H, i): -- find pairings for feature E i, i <= m if i > m -- leaf node? if pairings(h) > pairings(best) -- did better? Best = H else for j = 1 to n if individual_compatibility(e i, F j ) and then joint_compatibility(h, E i, F j ) JCBB([H j], i + 1) - pairing (E i, F j ) accepted if pairings(h) + m i >= pairings(best) - can do better? JCBB([H 0], i + 1) -- star node: E i not paired

48 JCBB Discussion 48 Upsides: Nice probabilistic foundation Quite effective in practice Downsides: Still exponential time in the worst case Like all joint assignment schemes, adds latency to data association.

49 Next Lecture Sparse Extended Information Filters Read ProbRob Chap 3.5, 12