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

Slide 1

Slide 1 text

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.

Slide 2

Slide 2 text

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

Slide 3

Slide 3 text

Slide 4

Slide 4 text

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

Slide 5

Slide 5 text

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

Slide 6

Slide 6 text

Slide 7

Slide 7 text

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.

Slide 8

Slide 8 text

Slide 9

Slide 9 text

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.

Slide 10

Slide 10 text

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

Slide 11

Slide 11 text

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

Slide 12

Slide 12 text

• 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].

Slide 13

Slide 13 text

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]

Slide 14

Slide 14 text

Slide 15

Slide 15 text

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

Slide 16

Slide 16 text

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

Slide 17

Slide 17 text

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.

Slide 18

Slide 18 text

Slide 19

Slide 19 text

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.

Slide 20

Slide 20 text

Slide 21

Slide 21 text

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

Slide 22

Slide 22 text

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

Slide 23

Slide 23 text

Slide 24

Slide 24 text

Slide 25

Slide 25 text

Slide 26

Slide 26 text

Slide 27

Slide 27 text

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.

Slide 28

Slide 28 text

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.

Slide 29

Slide 29 text

Slide 30

Slide 30 text

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

Slide 31

Slide 31 text

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.

Slide 32

Slide 32 text

Slide 33

Slide 33 text

Slide 34

Slide 34 text

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.