Dynamic Collision and Deadlock Avoidance for Multiple Robotic Manipulators

A flexible operation of multiple robotic manipulators operating in a dynamic environment requires online trajectory planning to ensure collision-free trajectories. In this work, we propose a real-time capable motion control algorithm, based on nonlinear model predictive control, which accounts for static and dynamic obstacles. The proposed algorithm is realized in a distributed scheme, where each robot optimizes its own trajectory with respect to the related objective and constraints. We propose a novel approach for collision avoidance between multiple robotic manipulators, where each robot accounts for the predicted movement of the neighboring robots. Additionally, we propose a method to reliably detect and resolve deadlocks occurring in a setup of multiple robotic manipulators. We validate our approach on pick and place scenarios involving multiple robotic manipulators operating in a common workspace in a realistic simulation environment set up in Gazebo. The robots are controlled using the Robot Operating System. Our approach scales up to 4 manipulators and computes a path for each robot in a simultaneous pick and place operation in 94% of all investigated cases without deadlock detection and 100 % of cases with the proposed deadlock resolution algorithm. In contrast, the investigated conventional path planners, such as PRM, PRM*, CHOMP and RRT-Connect, successfully plan a trajectory in at most 54% of all investigated cases for a simultaneous operation of 4 robotic manipulators hindering their application in setups of multiple manipulators.