Formation change for robot groups in occluded environments

We study formation change for robot groups in known environments. We are given a team of robots partitioned into groups, where robots in the same group are interchangeable with each other. A formation specifies the locations occupied by each group. The objective is to find collision-free paths that move all robots from a given start formation to a given goal formation. Our algorithm TAPF* has the following features: (a) it incorporates kinematic constraints of robots in form of velocity limits; (b) it maintains a user-specified safety distance between robots; (c) it attempts to minimize the makespan; and (d) it runs efficiently for hundreds of robots and dozens of groups even in dense 3D environments with narrow corridors and other occlusions. We demonstrate the efficiency and effectiveness of TAPF* in simulation and on robots.