An integrated framework for motion planning and trajectory optimization of AGVs using spatio-temporal safety corridors

Abstract

Efficiently generating safe and smooth trajectories for autonomous ground vehicles (AGVs) is a crucial and challenging task, particularly in dynamic environments with moving obstacles. This paper proposes an integrated motion planning and trajectory optimization (MPTO) framework that employs an optimization-based spatio-temporal safety corridors (STSC) to ensure trajectory smoothness and safety from a three-dimensional spatio-temporal perspective. The proposed MPTO framework comprises two layers. In the first layer, a multi-objective quadratic programming (MOQP) method was developed with the objective of rapidly generating smoothly varying STSC. The multi-objective cost function provides a comprehensive evaluation of the corridors in terms of their size, direction, and smoothness. Additionally, a convex polygonal feasible area (CPFA) was proposed to provide a linear obstacle-avoidance constraint for the MOQP. The smooth STSC provides within-corridor constraints for trajectory optimization, thereby ensuring collision avoidance of obstacles and reducing the dependence of trajectory optimization on the reference trajectory. In the second layer, an optimal trajectory generation method using polynomials is proposed to generate smooth and efficient trajectories. With smooth STSC constraints, the trajectory optimization model primarily focuses on smoothness, ensuring that the trajectory remains safe and smooth even with sudden changes in the feasible area. Finally, the proposed MPTO framework is validated through simulations and real vehicle experiments.