Efficient Trajectory Planning for Multiple Non-holonomic Mobile Robots via Prioritized Trajectory Optimization

TL;DR

This paper introduces a scalable and efficient method for generating collision-free, optimal trajectories for multiple non-holonomic robots in complex environments by combining multi-agent path planning with prioritized trajectory optimization.

Contribution

The paper proposes a novel prioritized trajectory optimization framework that enhances scalability and feasibility in multi-robot trajectory planning, outperforming coupled optimization methods.

Findings

Reduces computation time significantly compared to coupled optimization.

Maintains near-optimal trajectories with small compromise.

Validated through simulations and hardware experiments.

Abstract

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.