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

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

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

/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!

/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

/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.

/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

/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

/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

/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

/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]

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

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

/35 14 How SSSP works vertex expansion 0 00 invoked

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

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

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

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

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

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

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

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

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

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

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

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

/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

/35 28 Demo & Evaluation

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

/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

/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

/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

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

/35 34

/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