Optimization-Based Trajectory Planning for Multiple Robots in Unstructured Environments

This work focuses on a trajectory planning problem for multiple ground mobile robots in unstructured environments. Considering the problem, we propose a safety-enhancing multi-robot trajectory planning method. Our proposed method uses a two-stage strategy, path planning and trajectory optimization, to solve the multi-robot trajectory planning problem. To re-duce computational burden of the trajectory optimization, we construct safe corridor for each robot. In the safe corridor construction, we introduce an extended distance to solve its invalid construction problem. In a
$90\mathrm{m}\times 90\mathrm{m}$
experiment sce-nario, our method saves almost 91 % safe corridor construction time. Moreover, to enhance safety of the trajectory that the proposed method generates, a distance penalty term is added to the objective function of trajectory optimization problems. Compared to a state-of-the-art method, the proposed method outperforms it. Simulations and real-world experiments validate the feasibility of the proposed method.