Spatially Formulated Connected Automated Vehicle Trajectory Optimization with Infrastructure Assistance

This paper presents a constrained connected automated vehicles (CAVs) trajectory optimization method on curved roads with infrastructure assistance. Specifically, this paper systematically formulates trajectory optimization problems in a spatial domain and a curvilinear coordinate. As an alternative of temporal domain and Cartesian coordinate formulation, our formulation provides the constrained trajectory optimization flexibility to describe complex road geometries, traffic regulations, and road obstacles, which are usually spatially varying rather than temporal varying, with assistances vehicle to infrastructure (V2I) communication. Based on the formulation, we first conducted a mathematical proof on the controllability of our system, to show that our system can be controlled in the spatial domain and curvilinear coordinate. Further, a multiobjective model predictive control (MPC) approach is designed to optimize the trajectories in a rolling horizon fashion and satisfy the collision avoidances, traffic regulations, and vehicle kinematics constraints simultaneously. To verify the control efficiency of our method, multiscenario numerical simulations are conducted. Suggested by the results, our proposed method can provide smooth vehicular trajectories, avoid road obstacles, and simultaneously follow traffic regulations in different scenarios. Moreover, our method is robust to the spatial change of road geometries and other potential disturbances by the road curvature, work zone, and speed limit change.