SAFESTOP: Disturbance Handling in Prioritized Multi-Robot Trajectory Planning

Prioritized planning allows the generation of coordinated motion plans for multiple robots in a shared workspace by tracking their spatiotemporal progression along pre-planned paths. With the knowledge where and when collisions would occur, collision-free velocity profiles are calculated according to a prioritization scheme. However, in real-world applications, disturbances in the exact execution of the planned motions are likely. This can affect other planned trajectories and leave the system prone to deadlocks. We present a novel methodology, SAFESTOP, for handling interruptions with minimal impact on other planned tasks and show that it can drastically reduce the number of affected robots as well as the overall delay. The key idea is to maintain the paths and vary the motion of the affected robots, making them yield for others. In contrast to other approaches it supports complex vehicle structures and takes kinematic and dynamic constraints into account.