1 / 52

Am I Lost? Or Has the World Changed? Persistent Autonomous Navigation in Changing Environments

Am I Lost? Or Has the World Changed? Persistent Autonomous Navigation in Changing Environments. Aisha Walcott PhD Candidate, EECS, Robotics Advisor: Professor John Leonard [draft of algorithm and framework]. SLAM in Changing Environments.

acton-barlow
Download Presentation

Am I Lost? Or Has the World Changed? Persistent Autonomous Navigation in Changing Environments

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Am I Lost? Or Has the World Changed?Persistent Autonomous Navigation in Changing Environments Aisha Walcott PhD Candidate, EECS, Robotics Advisor: Professor John Leonard [draft of algorithm and framework]

  2. SLAM in Changing Environments • Goal is to maintain an up-to-date map in a changing environment Change Indicator Map is function of time Subset of trajectory Related Work [Montessano et al, 2005, Wang et. Al, 2007, Wolf, Sukhatme, 2005]

  3. Talk Outline • Introduction to SLAM • Traditional SLAM vs. Life-long SLAM • Mobile Robot Model • Probabilistic Problem Formulation • Pose Graph Approach • Dynamic Pose Graph SLAM (DPG-SLAM) • Simulated Data Analysis • Conclusion

  4. Pose Graph SLAM

  5. Pose Graph SLAM Scan Matching Odometry Scan Matching and Loop Constraints 50m

  6. Pose Graph Formulation • Network of spatial relations • Estimate relative geometric constraints between two poses • Translation in the x and y, and Rotation relative to pose • Odometry and scan matching (improved estimate) Constraints T2,3 x1 T1,2 T3,4 x7

  7. Pose Graph Formulation • Network of spatial relations • Estimate relative geometric constraints between two poses • Translation in the x and y, and Rotation relative to pose • Odometry and scan matching (improved estimate) Constraints T2,3 x1 T1,2 T3,4 x7 Ground Truth

  8. Pose Graph Formulation • Network of spatial relations • Estimate relative geometric constraints between two poses • Translation in the x and y, and rotation ω relative to pose • Odometry and scan matching (improved estimate) Zero-mean Gaussian Noise Non-linear Motion Model Constraints T2,3 x1 T1,2

  9. Pose Graph Formulation • “Closing the loop” • X1 and x7 are the same location • Similar to minimizing energy in spring network Constraint Optimization x1 x1 T7,1 T7,1 x7 x7 Loop Constraint Ground Truth Ground Truth

  10. Pose Graph SLAM • Scan matching (ICP) [Lu and Milios, 1997] • Detect loops • Pose graph optimization- iSAM “Incremental Smoothing and Mapping” [Kaess et al., 2008] • Returns estimate of robot trajectory • Poses and covariances are given in global frame • Computes best configuration of poses according to a cost function

  11. Talk Outline • Introduction to SLAM • Traditional SLAM vs. Life-long SLAM • Mobile Robot Model • Probabilistic Problem Formulation • Pose Graph Approach • Dynamic Pose Graph SLAM (DPG-SLAM) • Experiments and Results • Conclusion

  12. Dynamic Pose Graph SLAM (DPG SLAM) • Objective • Environment Model • Assumptions • Challenges • DPG-SLAM Algorithm • Analysis

  13. Objective • If the world changes and … • …Robot is certain about its position then not likely to be lost • or • …Robot is not certain about its position then likely to become lost • Continuous map update and change detection to keep map current

  14. Environment Model • Static entities S = {s1…sj} • Semi-static entities E= {e1…ek} • Dynamic entities D= {d1…dm} • Env = S  E  D Trashcan Walls Dynamic Robot Human Door Semi-Static Sofa Static

  15. DPG-SLAM Challenges D A E A A A B B C C C C time • Tractability • Computationally feasible for constraint optimization • Pose graph grows with time (storing matrices, matrix operations) • Maintain up-to-date map • Incorrect map ok for pair-wise scan matching • Loop constraints, insert bad constraint or never insert constraints (reducing to scan matching ) Incorrect Map Correct Map D A D E E A A B C C

  16. Assumptions • Laser range scanners (accurate) • Bounded 2D indoor environment • Dynamic • Robot makes N passes through the environment • Change occurs after each pass • Pose graph representation • Known start (home) location • Focus on semi-static • Pair-wise (ICP) scan matching

  17. DPG-SLAM Framework • Build a pose graph, run scan matcher on sequential nodes to obtain relative constraints • Attempt to insert loop closing constraints using a nearby poses heuristic • Detect change on current pose and corresponding nodes from different passes, compute Ci • Repair the pose graph, compute Xc • Choose next robot pose Traditional Pose Graph SLAM

  18. DPG-SLAM Example x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 Nodes Poses, xi Scans, zi Edges Spatial Constraints, Tij

  19. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T1,6 T7,2 Generate new waypoint Drive to waypoint Take laser range scan Add constraint x6 x7 x8 T6,7 T7,8 time

  20. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T1,6 T7,2 Generate new waypoint Drive to waypoint Take laser range scan Add constraint Get corresponding node Compare scans (Detect Change) x6 x7 x8 T6,7 T7,8 time

  21. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T1,6 T7,2 T8,3 Generate new waypoint Drive to waypoint Take laser range scan Add constraint Get corresponding node Compare scans (Detect Change) Add constraint x6 x7 x8 T6,7 T7,8 time

  22. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T1,6 T7,2 T8,3 x6 x7 x8 x9 T6,7 T7,8 T8,9 . . . Compare scans (detect change) time

  23. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T1,6 T7,2 T8,3 Change Detected x6 x7 x8 x9 T6,7 T7,8 T8,9 . . . Compare scans by scan matching Repair pose graph Update map time

  24. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T1,6 T7,2 T8,3 x6 x7 x8 x9 T6,7 T7,8 T8,9 . . . Compare scans Repair pose graph Update map *Invalidate node and arcs time *Check for map overlap, remove additional nodes

  25. DPG-SLAM Example (cont.) x1 x2 x3 x4 x5 T1,2 T2,3 T4,5 T3,4 T16, T7,2 T83 x6 x7 x8 x9 T6,7 T7,8 T8,9 . . . Compare scans Repair pose graph Update map *Invalidate node and arcs Add constraints time *Check for map overlap, remove additional nodes

  26. DPG-SLAM Example (cont.) x1 x2 x3 x5 T1,2 T2,3 T3,9 T1,6 T7,2 T8,3 T9,5 x6 x7 x8 x9 T6,7 T7,8 T8,9 . . . Compare scans Repair pose graph Update map *Invalidate node and arcs Add constraints (repair pose graph by sewing in constraints) . . . Compare scans Repair pose graph Update map *Invalidate node and arcs time *Check for map overlap, remove additional nodes

  27. DPG-SLAM Example (cont.) x1 x2 x3 x5 T1,2 T2,3 T3,9 T1,6 T7,2 T8,3 T9,5 x6 x7 x8 x9 T6,7 T7,8 T8,9 . . . Compare scans Repair pose graph Update map Invalidate node and arcs Add constraints (repair pose graph by sewing in constraints) time

  28. DPG-SLAM Example (cont.) Corresponding node: paired with a later node x2 x9 Change node: change detected at node x12 Normal node: neither change nor corresponding Node Types: Normal, Change, Corresponding Edge Types: Loop, Change, Sequential, Corresponding time

  29. DPG-SLAM Example (cont.) Corresponding edge: added between corresponding node pairs T8,3 T9,5 Change edge: added during repair T10,3 T11,12 Sequential edge: added between consecutive node Loop edge: added during loop detection Node Types: Normal, Change, Corresponding Edge Types: Loop, Change, Sequential, Corresponding time

  30. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 T8,3 T9,5 x6 x7 x8 x9 T7,8 T8,9 T10,3 x10 x11 x12 T11,12 Change at x12 time

  31. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 T8,3 x6 x7 x8 x9 T7,8 T8,9 T10,3 x10 x11 x12 T11,12 Robot takes measurement at x12 time

  32. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 T8,3 x6 x7 x8 x9 T7,8 T8,9 T10,3 x10 x11 x12 T11.12 Get corresponding node, x8 time

  33. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 T8,3 x6 x7 x9 x8 T7,8 T8,9 Change Detected T10,3 x10 x11 x12 C11,12 Get corresponding node, x8 Compare measurements time

  34. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 T8,3 x6 x7 x9 x8 T7,8 T8,9 T10,3 x10 x11 x12 T11,12 Update map and repair pose graph Remove x8 time

  35. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 T8,3 x6 x7 x9 x8 T7,8 T8,9 T12,9 T10,3 T7,12 x10 x11 x12 T11,12 Update map and repair pose graph Remove x8 Add new edges time

  36. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 x6 x7 x9 T12,9 T10,3 T7,12 x10 x11 x12 T11,12 Update map and repair pose graph Remove x8 Add new edges Remove x3 corresponding node of x8 time

  37. DPG-SLAM Example (cont.) x1 x2 x3 x5 T2,3 T3,9 T7,2 x6 x7 T2,12 x9 T12,9 T10,3 T7,12 x10 x11 x12 T11,12 Update map and repair pose graph Remove x8 Add new edges Remove x3 corresponding node of x8 T10,12 time

  38. DPG-SLAM Example (cont.) x1 x2 x5 T7,2 x6 x7 T2,12 x9 T12,9 T7,12 x10 x11 x12 T11,12 Update map and repair pose graph Remove x8 Add new edges Remove x3 corresponding node of x8 T10,12 time

  39. Talk Outline • Introduction to SLAM • Traditional SLAM vs. Life-long SLAM • Mobile Robot Model • Probabilistic Problem Formulation • Pose Graph Approach • Dynamic Pose Graph SLAM (DPG-SLAM) • Simulated Data Analysis • Conclusion

  40. DPG-SLAM on Simulated Data Set D A E A A A B B C C C C time time

  41. DPG-SLAM on Simulated Data Set

  42. DPG-SLAM on Simulated Data Set time

  43. DPG-SLAM on Simulated Data Set

  44. DPG-SLAM on Simulated Data Set

  45. DPG-SLAM on Simulated Data Set

  46. DPG-SLAM Issues (cont.) • Navigation strategy • Once achieve change threshold can recursively go back and fix previous pairs that overlap • This has problem based on different vantage points from different maps and different navigation strategy

  47. DPG-SLAM Issues • If there are loop closing constraints you might remove a good one and then the estimate become worse • Blocking points that are static

  48. DPG-SLAM Issues (cont.) • Number of edges will not change when there is a DPG repair • Number of nodes can stay constant (a lot of change detected) or increase • Repeated little change that turns huge • Do we need to add all the corresponding edges? (mutual information)

  49. Talk Outline • Introduction to SLAM • Traditional SLAM vs. Life-long SLAM • Mobile Robot Model • Probabilistic Problem Formulation • Pose Graph Approach • Dynamic Pose Graph SLAM (DPG-SLAM) • Simulated Data Analysis • Conclusion

More Related