Quick Multi-Robot Motion Planning by Combining Sampling and Search

IJCAI-23
https://kei18.github.io/sssp/

Transcript

1. Quick Multi-Robot Motion Planning
   by Combining Sampling and Search
   Keisuke Okumura1,2 & Xavier Defago3
   Macao, 19th – 25th Aug. 2023
   IJCAI-23
   https://kei18.github.io/sssp
   1National Institute of Advanced Industrial Science and Technology (AIST)
   2University of Cambridge
   3Tokyo Institute of Technology
   

   View Slide

2. /35
   2
   MASON TRINCA/THE WASHINGTON POST/GETTY IMAGE
   Kim Kyung Hoon / reuters
   AP Photo/Ross D. Franklin
   

   View Slide

3. /35
   3
   everything happens in real-time;
   neither pre-computed trajectories nor environment representation
   

   View Slide

4. /35
   4
   MRMP: Multi-Robot Motion Planning
   Each robot operates in its own configuration space
   Each robot has initial and goal configurations
   solution: collision-free trajectories
   *not addressed in this study
   in continuous spaces
   allowing heterogeneity
   allowing kinematics/dynamics* constraints
   extremely challenging!
   

   View Slide

5. /35
   5
   connect
   free space
   true
   false
   steer collide
   true
   false
   sample distance
   0.18
   Blackbox Helper Functions
   solve MRMP efficiently only with the five functions
   

   View Slide

6. /35
   6
   Naïve Strategy for MRMP
   two-phase planning
   graph representation of workspace;
   typically done by
   sampling-based motion planning (SBMP)
   compute collision-free paths
   e.g., [Svestka+ 98; Solovery+ IJRR-16; Cohen+ SoCS-19; Solis+ RA-L-21]
   Construct agent-wise roadmaps by some means
   1.
   Solve multi-agent pathfinding (MAPF) on these roadmaps
   2.
   

   View Slide

7. /35
   7
   Pitfall of Two-Phase Planning
   roadmaps truly used in planning process?
   two-phase planning
   Construct agent-wise roadmaps by some means
   Solve multi-agent pathfinding (MAPF) on these roadmaps
   1.
   2.
   decoupled
   

   View Slide

8. /35
   Construct agent-wise roadmaps by some means
   Solve multi-agent pathfinding (MAPF) on these roadmaps
   1.
   2.
   8
   Advanced Strategy
   develop agent-wise roadmaps
   according to MAPF search progress
   coupled
   this study
   

   View Slide

9. /35
   9
   quick MRMP
   multi-robot motion planning
   PRM [Kavraki+ 96]
   EST [Hsu+ ICRA-97]
   RRT [LaValle 98]
   RRT-Connect [Kuffner+ ICRA-00]
   PRM*/RRT* [Karaman+ IJRR-11]
   SPARS [Dobson+ IJRR-14]
   FMT* [Jason+ IJRR-15]
   +
   SBMP:
   sampling-based motion planning
   A*+OD [Standley AAAI-10]
   CBS [Sharon+ AIJ-15]
   M* [Wagner+ AIJ-15]
   PBS [Ma+ AAAI-19]
   BCP [Lam+ COR-22]
   LNS [Li+ IJCAI-21; AAAI-22]
   LaCAM* [Okumura AAAI-23; IJCAI-23]
   +
   MAPF:
   multi-agent pathfinding
   search
   roadmap
   seamless
   integration
   Concept
   

   View Slide

10. /35
    10
    PRM [Kavraki+ 96]
    EST [Hsu+ ICRA-97]
    RRT [LaValle 98]
    RRT-Connect [Kuffner+ ICRA-00]
    PRM*/RRT* [Karaman+ IJRR-11]
    SPARS [Dobson+ IJRR-14]
    FMT* [Jason+ IJRR-15]
    +
    SBMP:
    sampling-based motion planning
    A*+OD [Standley AAAI-10]
    CBS [Sharon+ AIJ-15]
    M* [Wagner+ AIJ-15]
    PBS [Ma+ AAAI-19]
    BCP [Lam+ COR-22]
    LNS [Li+ IJCAI-21; AAAI-22]
    LaCAM* [Okumura AAAI-23; IJCAI-23]
    +
    MAPF:
    multi-agent pathfinding
    Contribution
    seamless
    integration
    search
    roadmap
    example algorithm:
    SSSP
    simultaneous
    sampling-and-search
    planning
    

    View Slide

11. /35
    11
    How SSSP works
    goal
    start
    alternate between vertex and search-node expansions
    exhaustive search while constructing roadmaps by random walks
    [MAPF; Standley AAAI-10] [SBMP; Hsu+ ICRA-97]
    

    View Slide

12. /35
    12
    How SSSP works
    0
    1
    2
    0
    1 2
    3
    initial roadmap construction
    by single-robot SBMP
    

    View Slide

13. /35
    13
    How SSSP works
    0
    1
    2
    0
    1 2
    3
    search tree
    00
    next action
    

    View Slide

14. /35
    14
    How SSSP works
    vertex expansion
    0
    00
    invoked
    

    View Slide

15. /35
    15
    How SSSP works
    vertex expansion
    4 5
    0
    00
    sampling new vertices
    by random walk
    

    View Slide

16. /35
    16
    How SSSP works
    4 5
    0
    00
    1 2
    3
    update roadmap
    vertex expansion
    

    View Slide

17. /35
    17
    How SSSP works
    4 5
    0
    00
    1 2
    3
    search node expansion
    

    View Slide

18. /35
    18
    How SSSP works
    00
    search node expansion
    4 5
    0
    1 2
    

    View Slide

19. /35
    19
    How SSSP works
    search node expansion
    4 5
    0
    1 2
    00 10 20 40 50
    00
    

    View Slide

20. /35
    20
    How SSSP works
    00 10 20 40 50
    00
    closed
    next node
    

    View Slide

21. /35
    21
    How SSSP works
    vertex expansion
    00 10 20 40 50
    00
    0
    

    View Slide

22. /35
    22
    How SSSP works
    vertex expansion
    00 10 20 40 50
    00
    0
    3
    4
    sampling new vertices
    by random walk
    

    View Slide

23. /35
    23
    How SSSP works
    00 10 20 40 50
    00
    0
    3
    4
    update roadmap
    1
    2
    

    View Slide

24. /35
    24
    How SSSP works
    00 10 20 40 50
    00
    0
    3
    4
    1
    2
    search node expansion
    

    View Slide

25. /35
    25
    How SSSP works
    search node expansion
    0
    3
    4
    1
    00 10 20 40 50
    00
    

    View Slide

26. /35
    26
    How SSSP works
    50 51 53 54
    00 10 20 40 50
    00
    search node expansion
    0
    3
    4
    1
    

    View Slide

27. /35
    27
    How SSSP works
    50 51 53 54
    00 10 20 40 50
    00
    next node
    continue until
    all agents reach their goal
    Combined with some techniques, SSSP eventually finds a solution for solvable instances
    c.f., probabilistic completeness in SBMP
    

    View Slide

28. /35
    28
    Demo & Evaluation
    

    View Slide

29. /35
    29
    SSSP is applicable to various problems thanks to the minimum assumptions
    

    View Slide

30. /35
    30
    
    
    
    
    
    
    
    
    runtime (sec)
    solved instances (%)
    Point2d
    DOF: 2N
    - 100 instances with 10 random seeds
    - each instance is randomly generated
    - number of robots: 2–10
    - heterogeneous robots
    Comparison with
    Standard Approaches
    

    View Slide

31. /35
    31
    RRT-Connect [Kuffner+ ICRA-00]
    RRT [LaValle 98]
    PRM [Kavraki+ 96]
    CBS [Sharon+ AIJ-15] on PRMs
    (with adjusted heuristics)
    PP [Silver AIIDE-05] on PRMs
    SSSP
    two-phase planning
    direct application of SBMP
    
    
    
    
    
    
    
    
    runtime (sec)
    solved instances (%)
    Point2d
    DOF: 2N
    - 100 instances with 10 random seeds
    - each instance is randomly generated
    - number of robots: 2–10
    - heterogeneous robots
    Comparison with
    Standard Approaches
    

    View Slide

32. /35
    32
    
    
    
    
    
    
    
    
    runtime (sec)
    solved instances (%)
    Point2d
    DOF: 2N
    Point3d
    DOF: 3N
    Line2d
    DOF: 3N
    Capsule3d
    DOF: 6N
    Arm22
    DOF: 2N
    Arm33
    DOF: 6N
    Dubins2d
    DOF: 3N
    Snake2d
    DOF: 6N
    

    View Slide

33. /35
    33
    
    
    
    
    
    
    
    
    runtime (sec)
    solved instances (%)
    SSSP can quickly solve various MRMP
    

    View Slide

34. /35
    34
    

    View Slide

35. /35
    35
    Concluding Remarks
    https://kei18.github.io/sssp
    SSSP
    example algorithm combining sampling and search for quick multi-robot motion planning
    future directions
    improving solution quality (current: not excellent)
    incoporating dynamics
    further integration of SBMP and MAPF algorithms
    

    View Slide