A Unified Framework Integrating Trajectory Planning and Motion Optimization Based on Spatio-Temporal Safety Corridor for Multiple AGVs

Abstract

Planning safe and smooth trajectories for multiple autonomous ground vehicles (MAGVs) in a complex dynamic unstructured environment is a fundamental and challenging task. In this paper, a novel unified framework integrating trajectory planning and motion optimization (TPMO) is proposed based on spatio-temporal safety corridor (STSC), which guarantees collision avoidance and trajectory smoothness. The proposed TPMO framework consists of two parts. The first part is to establish the STSC for each AGV based on the mixed integer quadratic programming (MIQP) algorithm. The proposed STSC method ensures collision avoidance in the environment of static and dynamic obstacles, and provides a longitudinal and lateral coupled trajectory (LLCT) for trajectory planning. The second part is to design a motion optimization methodology, which considers the constraints of AGV geometry as well as longitudinal and lateral coupled motion characteristics. Moreover, our formulation provides a theoretical guarantee that the entire trajectory is optimal under collision avoidance. Finally, the proposed TPMO framework is applied to solve the optimal cooperative trajectory and motion planning problem of MAGVs in a near-natural simulation and real vehicle environments, validating the proposed framework's effectiveness and practicality.