Model Predictive Trajectory Optimization and Tracking in Highly Constrained Environments

This paper presents a model predictive trajectory optimization and tracking framework to avoid collisions for autonomous vehicles in highly constrained environments. Firstly, a vehicle model is established in road coordinate system to describe the relationship between the vehicle and the reference road. Secondly, a numerical optimization method is applied to smoothen the reference path generated by waypoints. Then, a multilayer searched method is used to establish a safe driving corridor in highly constrained environments. In addition, an optimal path optimization and tracking framework based on model predictive control is formulated to improve the driving safety and comfort. The proposed framework considers the constraints of path boundaries and vehicle dynamics to provide the optimal control command. Furthermore, the speed profile is optimized based on the longitudinal motion model in space domain to ensure the constraints of speed limits and vehicle acceleration. Finally, the proposed algorithms are evaluated through experiments in various scenarios to demonstrate the effectiveness.