Prioritized Safe Interval Path Planning for Multi-Agent Pathfinding with Continuous Time on 2D Roadmaps

Kazumi Kasaura*, Mai Nishimura*, Ryo Yonetani*
*OMRON SINIC X Corporation
Presented at International Conference on Intelligent Robots and Systems (IROS) 2022

Transcript

1. Prioritized Safe Interval Path Planning for Multi-Agent Pathfinding with Continuous Time on 2D Roadmaps Kazumi Kasaura, Mai Nishimura, and Ryo Yonetani OMRON SINIC X (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
2. Multi Agent Pathfinding (MAPF) Multi-Agent Pathfinding is a classical problem with many applications, including warehouse management and autonomous car coordination. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Source: Warehouse Robotics - Bleum Robotics Source: The Scary Efficiency of Autonomous Intersections - IEEE Spectrum
3. Goal: To Solve MAPF Problems with… (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. • 2D Roadmaps • Continuous Time • A Large Number of Agents
4. Goal: To Solve MAPF Problems with… • 2D Roadmaps • Not necessary grid maps • Continuous Time • A Large Number of Agents (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Example: Probabilistic Roadmap
5. Goal: To Solve MAPF Problems with… (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. • 2D Roadmaps • Continuous Time • Agents not necessary start or stop synchronously. • A Large Number of Agents
6. Goal: To Solve MAPF Problems with… (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. • 2D Roadmaps • Continuous Time • A Large Number of Agents • Hundreds or thousands
7. Overview Roadmap Generation Conflict Annotation Prioritized Safe Interval Path Planning MAPF with continuous time Local Planning Etc… Finding Fixed Radius Near Neighbor Simplex Range Searching Finding Segments Intersections Enumeration Calculation of time condition for collision (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
8. Overview Roadmap Generation Conflict Annotation Prioritized Safe Interval Path Planning MAPF with continuous time Local Planning Etc… Finding Fixed Radius Near Neighbor Simplex Range Searching Finding Segments Intersections Enumeration Calculation of time condition for collision (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
9. Approach: Prioritized Safe Interval Path Planning Our planning algorithm is built on top of the prioritized planning [1]. [1] M. Erdmann and T. Lozano-Perez, “On multiple moving objects,” Algorithmica, vol. 2, no. 1, pp. 477–521, 1987. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
10. Approach: Prioritized Safe Interval Path Planning (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. [1] M. Erdmann and T. Lozano-Perez, “On multiple moving objects,” Algorithmica, vol. 2, no. 1, pp. 477–521, 1987. • First, planning for the red agent Our planning algorithm is built on top of the prioritized planning [1].
11. Approach: Prioritized Safe Interval Path Planning (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. [1] M. Erdmann and T. Lozano-Perez, “On multiple moving objects,” Algorithmica, vol. 2, no. 1, pp. 477–521, 1987. • First, planning for the red agent • Second, planning for the green agent avoiding collision with the red agent Our planning algorithm is built on top of the prioritized planning [1].
12. • First, planning for the red agent • Second, planning for the green agent avoiding collision with the red agent • Third, planning for the blue agent avoiding collision with the red and green agents. Approach: Prioritized Safe Interval Path Planning (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. [1] M. Erdmann and T. Lozano-Perez, “On multiple moving objects,” Algorithmica, vol. 2, no. 1, pp. 477–521, 1987. Our planning algorithm is built on top of the prioritized planning [1].
13. Approach: Prioritized Safe Interval Path Planning For each agent, we must plan a path avoiding collision with already planned agents. We use safe-interval path planning [2] (SIPP) to determine each agent path. In SIPP, timeline of each vertex or edge is divided to “collision intervals” and “safe intervals”. Each safe intervals of vertices are considered as a node while path searching. [2] M. Phillips and M. Likhachev, “Sipp: Safe interval path planning for dynamic environments,” in IEEE International Conference on Robotics and Automation, 2011, pp. 5628–5635. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Figure in [2]
14. Approach: Prioritized Safe Interval Path Planning First, planning for the red agent (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. timelines v e v
15. Approach: Prioritized Safe Interval Path Planning (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. timelines v e v v e Second, planning for the green agent avoiding collision with the red agent
16. Approach: Prioritized Safe Interval Path Planning (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Third, planning for the blue agent avoiding collision with the red and green agents. timelines v e v
17. Challenges: Collision Check on 2D roadmaps in continuous time To use SIPP, we need to detect all possible collisions between agents. This is however time consuming. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
18. Overview Roadmap Generation Conflict Annotation Prioritized Safe Interval Path Planning MAPF with continuous time Local Planning Etc… Finding Fixed Radius Near Neighbor Simplex Range Searching Finding Segments Intersections Enumeration Calculation of time condition for collision (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
19. Key Idea: Continuous-Time Conflict Annotation (CTC) We propose to pre-calculate conflicting vertices and edges that can cause collisions. Our approach can do so efficiently before planning using computational geometry algorithms. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
20. Continuous-Time Conflict Annotation (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Vertex-Edge Conflict Edge-Edge Conflict We must consider two types of conflicts.
21. Continuous-Time Conflict Annotation (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Whether agents collide or not depends on their moving time. We must annotate the condition of time for collision. The method to calculate the condition is known[3]. The remaining task is to enumerate conflicts. They collide if they start at the same time They collide if the red agent start before the blue agent [3]: T. T. Walker, N. R. Sturtevant, Collision Detection for Agents in Multi-Agent Pathfinding. arxiv: 1908.09707v3
22. Annotation Algorithm (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. We assume that all agents have circular shapes, and all edges are segments. Then the annotation problem is reduced to three classical geometric enumeration problems by case analysis. These problems can be solved by known algorithms. For example, enumerating segment intersections can be done by Bentley-Ottmann Algorithm. Fixed Radius Near Neighbors Simplex Range Searching Segment intersections
23. Overview Roadmap Generation Conflict Annotation Prioritized Safe Interval Path Planning MAPF with continuous time Local Planning Etc… Finding Fixed Radius Near Neighbor Simplex Range Searching Finding Segments Intersections Enumeration Calculation of time condition for collision (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
24. Results (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
25. Results: Various Environments Agents: 340, Map Size: 128 x 128 (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Our method works with roadmaps in various environments.
26. Results: Various Environments Agents: 671, Map Size: 164 x 320 (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Our method works with roadmaps in various environments.
27. Results: Various Environments Agents: 365, Map Size: 256 x 256 Agents: 693, Map Size: 256 x 256 (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. Our method works with roadmaps in various environments.
28. Results: Various Environments Agents: 137, Map Size: 64 x 64 (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. It seems that Prioritized Planning tends to produce inefficient plan when the environments has many narrow parts. Our method works with roadmaps in various environments.
29. Results: A Large Number of Agents Agents: 1935 Map Size: 256 x 256 Our method can find paths for almost 2000 agents in 30 seconds. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
30. Evaluation of Results (c) 2022 OMRON SINIC X Corporation. All Rights Reserved. For the warehouse env. For the empty env. and dense roadmap For various environments and roadmaps, our method reduces runtime for planning significantly, comparing with Prioritized Safe Interval Path Planning without Continuous-Time Conflict Annotation and baseline method (Continuous Conflict-Based Search).
31. Results: Simulation Agents: 15 We simulated the plan produced by our method with differential wheeled robots. To produce plans executable by wheeled robots, we consider times for rotation, acceleration and deceleration. (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
32. Results: Simulation Agents: 70 (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
33. Agents: 300 Results: Simulation (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.
34. Conclusion Problem Multi Agent Pathfinding with 1. 2D roadmap 2. Continuous time 3. A large number of agents Approach Prioritized Safe Interval Path Planning Challenge Collision Check Key Idea Continuous-Time Conflict Annotation before Planning Result Planning for thousands of agents (c) 2022 OMRON SINIC X Corporation. All Rights Reserved.