  In this paper, we present a novel approach to efficiently generate
collision-free optimal trajectories for multiple non-holonomic mobile robots in
obstacle-rich environments. Our approach first employs a graph-based
multi-agent path planner to find an initial discrete solution, and then refines
this solution into smooth trajectories using nonlinear optimization. We divide
the robot team into small groups and propose a prioritized trajectory
optimization method to improve the scalability of the algorithm. Infeasible
sub-problems may arise in some scenarios because of the decoupled optimization
framework. To handle this problem, a novel grouping and priority assignment
strategy is developed to increase the probability of finding feasible
trajectories. Compared to the coupled trajectory optimization, the proposed
approach reduces the computation time considerably with a small impact on the
optimality of the plans. Simulations and hardware experiments verified the
effectiveness and superiority of the proposed approach.
