Distributed Robust Localization from Arbitrary Initial Poses via EM and Model Selection

Transcription

1 1 Distributed Robust Localization from Arbitrary Initial Poses via EM and Model Selection Vadim Indelman Collaborators: Erik Nelson, Jing Dong, Nathan Michael and Frank Dellaert IACAS, February 15

2 Collaborative Localization and Mapping Important in a variety of scenarios Exploration in unknown/uncertain, dangerous environments Search and rescue Surveillance, tracking Cooperative inference requires Sharing relevant information (observations, marginals over variables of interest) Correct interpretation (data association) Robustness to outliers

3 3 Motivating Scenario Robots are initially unaware of each others location How to establish collaboration and perform multi-robot localization? Unknown multi-robot data association Unknown initial relative poses between robots * Slide adapted from Nelson14iser

4 4 Related Work Known data association and common reference frames Full SLAM [Howard et al. 6], [Andersson et al. 8] Pose SLAM (direct, indirect) [Roumeliotis et al. ], [Kim et al. 1], [Indelman et al. 1] Anderson et al. 8 Kim et al. 1

5 5 Related Work Known data association and common reference frames Full SLAM [Howard et al. 6], [Andersson et al. 8] Pose SLAM (direct, indirect) [Roumeliotis et al. ], [Kim et al. 1], [Indelman et al. 1] Unknown multi-robot data association and common reference frame Full SLAM [Montijano et al. 11], [Cunningham et al. 1] Montijano et al. 11

6 6 Related Work Known data association and common reference frames Full SLAM [Howard et al. 6], [Andersson et al. 8] Pose SLAM (direct, indirect) [Roumeliotis et al. ], [Kim et al. 1], [Indelman et al. 1] Unknown multi-robot data association and common reference frame Full SLAM [Montijano et al. 11], [Cunningham et al. 1] Robust graph optimization (single robot case loop closures) [Sunderhauf and Protzel 1, 13], [Latif et al. 1], [Lee et al. 13] Lee et al. 13 Latif et al. 1 Sunderhauf and Protzel 1, 13

7 7 This Work Multi-robot framework with Unknown multi-robot data association Unknown initial relative poses between robots Pose SLAM approach How to establish multi-robot data association when robots start operating from unknown locations? Outline: Batch, centralized framework Incremental, distributed framework

8 8 Multi-Robot Correspondences If no common reference frame is available, what information to share? Robots share informative observations (e.g. laser scans) Saliency is calculated based on [Nieto et al., Robot. Auton. Syst., 7]

9 9 Multi-Robot Correspondences If no common reference frame is available, what information to share? Robots share informative observations (e.g. laser scans) Calculate candidate multi-robot relative pose constraints Collect into set F Includes (many) outliers Indoor navigation Arbitrary common reference frame Ground truth 4 4 Robot 1 Robot Robot

10 1 Probabilistic Formulation 15 Notations: F : Multi-robot correspondences set J : Latent variables to indicate inliers/outliers Joint pdf over robot trajectories and multi-robot data association: p (X, J Z) / Y r p (X r Z r ) Y (r 1,r,k,l)F p j r 1,r k,l p u r 1,r k,l x r 1 k,xr l,j r 1,r k,l Only local measurements Data association Multi-robot measurement likelihood, given data association Each multi-robot correspondence

11 11 Measurement likelihood with p u r 1,r k,l x r 1 k,xr 1 l /exp err u r 1,r k,l,x r 1 k,xr l 1 5 err u r 1,r k,l,x r 1.= 5 u r 1,r k,l h (x r 1 k,xr measured 15 l ) 1 predicted 15. = x r 1 k,xr l k T r 1 r x r l Unknown!!

12 1 Y [m] Measurement likelihood with p u r 1,r k,l x r 1 k,xr 1 l /exp err u r 1,r k,l,x r 1 k,xr l 1 5 err u r 1,r k,l,x r 1.= 5 u r 1,r k,l h (x r 1 k,xr measured Unknown!! Error distribution for all correspondences: Arbitrary T r 1 Must first infer a common reference frame T r 1! uncertainty Inliers Outliers Y [m] l ) 1 predicted 15. = x r 1 k,xr l k T r 1 r x r l r Correct T r r 1 uncertainty r Inliers Outliers X [m] X [m]

13 13 Key Observation Given robot local trajectories, relative initial pose can be calculated from each candidate multi-robot correspondence Only inliers produce similar transformations Objective: identify cluster Initial relative pose between two robots (planar case: ) [synthetic data] x, y, Y [m] Y [m] Y [m] X [m] X [m] X [m] 1% outliers 4% outliers 85% outliers

14 14 Key Observation Given robot local trajectories, relative initial pose can be calculated from each candidate multi-robot correspondence Only inliers produce similar transformations Objective: identify cluster x, y, Initial relative pose between two robots (planar case: ) [real data]

15 15 Inference Over Common Reference Frame via EM 15 1 ˆX SR 5 MAP estimate of T r 1 r given robot local trajectories ( ): ˆT r 1 r = arg max T r 1 r p T r 1 r ˆX SR,Z 5 1 X = arg max p 15 T r 1 r J T r 1 r, J ˆX SR,Z J : Latent binary variables to indicate inliers/outliers EM formulation ( T =. T r 1) : r Local trajectories ˆX r = arg max p X (Xr Z r ) n r. o R = ˆXr ˆX SR r=1 ˆT (i) = arg max T X p J J ˆT (i 1), ˆX h i SR,Z log p T,J ˆX SR,Z E step M step

16 Inference Over Common Reference Frame via EM (Cont.) 16 Convergence only to local minima Therefore: T r 1 Start process from several initial guesses of r T r 1 Results in several locally-optimal hypotheses (inliers/outliers, estimated ) Which one to choose? (next) r Y [m] 5 Y [m] X [m] X [m]

17 17 Inference Over Robot Trajectories Once a common reference frame is established: Multi-robot localization becomes possible Robot trajectories can be expressed in the same frame Infer robot trajectories via EM: ˆX = arg max X X p J J ˆX,Z log p (X, J Z) Identified common reference frame is used as initial guess within measurement likelihood p u r 1,r k,l x r 1 k,xr 1 l /exp err u r 1,r k,l,x r 1 k,xr l

18 18 Results (Batch, Centralized) 15 Local trajectories; Arbitrary common reference frame 15 Indoor navigation Estimated Ground truth 4 Inliers Outliers 4 4 Robot 1 Robot Robot

19 19 Incremental Framework Challenges Multiple hypotheses How to know when to make a decision? Robot trajectories and observed environments may initially not overlap Perceptual aliasing Candidate correspondences: Arbitrary common reference frame Ground truth trajectories

20 Incremental Framework (Cont.) Choosing an incorrect hypothesis: Y [m] Hypothesis # inliers X [m]

21 1 Incremental Framework (Cont.) Approach Hypothesis model-based selection Chinese restaurant process hypothesis prior

22 Hypothesis Model-Based Selection Calculate probability of each hypothesis h H h = {I,O} p h Z, ˆX SR Inliers Outliers Explicitly: p h Z, ˆX SR 6 = c p 4 h ˆX SR p Z h, ˆX SR Measurement likelihood: Prior 4 Prioritizes hypotheses with many inliers Does not address: Y [m] Measurement likelihood Is sufficient data available to choose a hypothesis? Perceptual aliasing X [m]

23 3 Hypothesis Prior Introduce null-hypothesis corresponds to perceptual aliasing All correspondences are actually outliers Chinese restaurant process, assuming: Robots operate in closed indoor environment Eventually, will observe common places (not necessarily concurrently)

24 4 Hypothesis Prior (Cont.) Chinese restaurant process Probability of observing a new place reduces over time Use to discriminate between different hypotheses As more data comes in hypotheses priors become distinguishable Example Probability.6.4 Probability.6.4 Probability h h1 h Hypotheses h h1 h Hypotheses h h1 h Hypotheses More data

25 5 Results Inliers of chosen hypothesis Hypothesis # inliers time Result As opposed to

26 6 Results - CMU Trial T1 Trial T Trial T3

27 Results - CMU [ICRA 15] 7

28 8 Conclusions Collaborative inference from unknown initial poses and data association Key observation (clusters for inlier correspondences) EM approach to infer common reference frames and data association Once established, joint inference over robot poses Distributed and incremental framework: Challenges: How to know when to make a decision? Perceptual aliasing Model-based selection + hypothesis prior