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.
Introduction
Coordinating a fleet of autonomous mobile robots in crowded environments, such as modern automated warehouses, presents a significant computational challenge. The difficulty arises from two primary factors: the non-holonomic constraints of differential-drive robots, which require complex nonlinear modeling, and the exponential growth in complexity when optimizing paths for many robots simultaneously (coupled optimization). While sequential planning is faster, it often leads to infeasible solutions as earlier robots' paths may trap later ones. This research proposes a "prioritized trajectory optimization" framework that balances computational speed with high success rates and path optimality.
Methodology
The proposed framework utilizes a three-stage pipeline to transform high-level goals into smooth, executable trajectories.
1. Discrete Path Planning
The process begins with Enhanced Conflict-Based Search (ECBS) on a grid. To ensure the initial paths are physically feasible for differential-drive robots, the planner utilizes pre-calculated motion primitives. These primitives allow the discrete planner to account for turning radii and kinematic constraints from the outset, preventing the generation of "jagged" paths that would be difficult to optimize later.
2. Safe Corridor Construction
Once discrete paths are established, the algorithm constructs "safe corridors" around them. These corridors consist of a series of overlapping convex boxes guaranteed to be free of static obstacles, providing a safe search space for the subsequent optimization stage.
3. Prioritized Trajectory Optimization
The core innovation lies in the grouping and prioritization strategy. Rather than optimizing the entire fleet at once, robots are clustered into small groups based on their spatial interactions (identifying "triples" of robots that frequently conflict). Groups are assigned priorities based on conflict frequency; high-priority groups optimize their trajectories first, while lower-priority groups treat the resulting paths as dynamic obstacles. This decoupling significantly reduces the problem's dimensionality.
Experimental Results
The algorithm was evaluated in simulated warehouse environments with up to 32 robots and validated with real-world hardware using Pioneer 3-AT robots.
- Computational Efficiency: In a 32-robot scenario, the proposed method achieved a 7x speedup over standard coupled optimization, reducing computation time from 27.2 seconds to just 3.9 seconds.
- Path Quality: The prioritized approach maintained high optimality, with path costs only 3.7% higher than the theoretically optimal coupled solutions.
- Robustness: The novel grouping strategy maintained a 90% success rate in dense environments, significantly outperforming randomized grouping strategies (60%).
- Scalability: Runtime increases almost linearly with the number of robots, indicating strong potential for large-scale industrial applications.
Conclusion and Future Work
This prioritized optimization approach offers a scalable solution for multi-robot coordination by exploiting the spatial sparsity of robot interactions. While currently centralized, future iterations could explore distributed implementations and the integration of reactive components to handle dynamic obstacles like human workers in real-time. The current strategy effectively mitigates the incompleteness typical of prioritized methods, providing a reliable path for large-scale autonomous fleets.
Summary updated