Multi-Robot Pose Graph Localization and Data Association from Unknown Initial Relative Poses

Transcription

1 1 Muti-Robot Pose Graph Locaization and Data Association from Unknown Initia Reative Poses Vadim Indeman, Erik Neson, Nathan Michae and Frank Deaert Institute of Robotics and Inteigent Machines (IRIM) Georgia Institute of Technoogy The Robotics Institute Carnegie Meon University Internationa Conference on Robotics and Automation (ICRA), June 214

2 2 Coaborative Locaization and Mapping Important in a variety of scenarios Exporation in unknown/uncertain, dangerous environments Search and rescue Surveiance, tracking Cooperative inference requires Sharing reevant information (observations, marginas over variabes of interest) Correct interpretation (data association) Robustness to outiers Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

3 3 Motivating Scenario Robots/sensors are depoyed in an environment (e.g. buiding) Initiay unaware of each others ocation How to estabish coaboration and perform muti-robot ocaization? Unknown muti-robot data association Unknown initia reative poses between robots

4 4 Reated Work Known data association and common reference frames Fu SLAM [Howard et a. 26], [Andersson et a. 28] Pose SLAM (direct, indirect) [Roumeiotis et a. 22], [Kim et a. 21], [Indeman et a. 212] Anderson et a. 28 Kim et a. 21 Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

5 5 Reated Work Known data association and common reference frames Fu SLAM [Howard et a. 26], [Andersson et a. 28] Pose SLAM (direct, indirect) [Roumeiotis et a. 22], [Kim et a. 21], [Indeman et a. 212] Unknown muti-robot data association and common reference frame Fu SLAM [Montijano et a. 211], [Cunningham et a. 212] Montijano et a. 211 Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

6 6 Reated Work Known data association and common reference frames Fu SLAM [Howard et a. 26], [Andersson et a. 28] Pose SLAM (direct, indirect) [Roumeiotis et a. 22], [Kim et a. 21], [Indeman et a. 212] Unknown muti-robot data association and common reference frame Fu SLAM [Montijano et a. 211], [Cunningham et a. 212] Robust graph optimization (singe robot case oop cosures) [Sunderhauf and Protze 212, 213], [Latif et a. 212], [Lee et a. 213] Lee et a. 213 Latif et a. 212 Sunderhauf and Protze 212, 213 Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

7 7 This Work Muti-robot framework with Unknown muti-robot data association Unknown initia reative poses between robots Pose SLAM approach How to estabish muti-robot data association when robots start operating from unknown ocations? Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

8 8 Muti-Robot Correspondences If no common reference frame is avaiabe, what information to share? Robots share informative observations (e.g. aser scans) Cacuate candidate muti-robot reative pose constraints Coect into set F Incudes (many) outiers Arbitrary common reference frame Ground truth Robot 1 Robot 2 Robot Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

9 9 Probabiistic Formuation 15 Notations: F : Muti-robot correspondences set J : Latent variabes to indicate iniers/outiers Joint pdf over robot trajectories and muti-robot 2 data association: p (X, J Z) / Y r p (X r Z r ) Y (r 1,,)2F p j r 1, p u r 1, x r 1 k,x,j r 1, Ony oca measurements Data association Muti-robot measurement ikeihood, given data association Each muti-robot correspondence Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

10 1 Measurement ikeihood with p u r 1, x r 1 k,x 1 /exp err u r 1,,x r 1 k,x err u r 1,,x r 1.= 5 u r 1, h (x r 1 k,x measured 15 ) 1 predicted 15. = x r 1 2 k,x 2 k T r 1 x Unknown!! Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

11 11 Y [m] Measurement ikeihood with p u r 1, x r 1 k,x 1 /exp err u r 1,,x r 1 k,x err u r 1,,x r 1.= 5 u r 1, h (x r 1 k,x measured Unknown!! Error distribution for a correspondences: Arbitrary T r 1 Must first infer a common reference frame T r 1! uncertainty Iniers Outiers Y [m] ) 1 predicted 15. = x r 1 2 k,x k T r 1 x Correct T r uncertainty Iniers Outiers X [m] X [m]

12 12 Key Observation Given robot oca trajectories, reative initia pose can be cacuated from each candidate muti-robot correspondence Ony iniers produce simiar transformations Objective: identify custer Initia reative pose between two robots (panar case: ) [synthetic data] x, y, Y [m] Y [m] Y [m] X [m] X [m] X [m] 1% outiers 4% outiers 85% outiers Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

13 13 Inference Over Common Reference Frame via EM MAP estimate of ˆT r 1 T r 1 = arg max T r 1 given robot oca trajectories (using ony oca data): p T r 1 ˆX SR,Z = arg max T r 1 X p J T r 1, J ˆX SR,Z J : Latent binary variabes to indicate iniers/outiers EM formuation: Loca trajectories ˆX r = arg max p X (Xr Z r ) n r. o R = ˆXr ˆX SR r=1 ˆT r 1 = arg max T r 1 p J ˆT r 1, ˆX SR,Z og p T r 1, J ˆX SR,Z E step M step Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

14 Inference Over Common Reference Frame via EM (Cont.) 14 Convergence ony to oca minima Therefore: T r 1 Start process from severa initia guesses of T r 1 Resuts in severa ocay-optima soutions (iniers/outiers, estimated ) Choose most ikey soution (best support) Ongoing research: mode seection, sensitivity to perceptua aiasing Y [m] 5 Y [m] X [m] X [m] Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

15 15 Inference Over Robot Trajectories Once a common reference frame is estabished: Muti-robot ocaization becomes possibe Robot trajectories can be expressed in the same frame Infer robot trajectories via EM (see paper for fu derivation): ˆX = arg max X p J ˆX,Z og p (X, J Z) Identified common reference frame is used as initia guess within measurement ikeihood p u r 1, x r 1 k,x 1 2 /exp 2 err u r 1,,x r 1 k,x Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

16 16 Resuts Indoor navigation Shared saient aser scans Ground truth x Robot 1 Robot 2 Robot

17 17 Resuts (Cont.) Loca trajectories of 3 robots; Arbitrary common reference frame Estimated Ground truth 4 Iniers Outiers Robot 1 Robot 2 Robot Indeman et a., Muti-Robot Data Association with Unknown Initia Reative Poses

18 18 Concusions and Future Work Coaborative inference from unknown initia poses and data association Key observation (custers for inier correspondences) EM approach to infer common reference frames and data association Once estabished, EM approach for inference over robot poses Extensive experimenta study to appear in ISER 214 Future Work Distributed and incrementa framework Perceptua aiasing How to know when to make a decision? Vision sensors