Lateral Vehicle Trajectory Optimization Using Constrained Linear Time-Varying MPC

In this paper, a trajectory optimization algorithm is proposed, which formulates the lateral vehicle guidance task along a reference curve as a constrained optimal control problem. The optimization problem is solved by means of a linear time-varying model predictive control scheme that generates trajectories for path following under consideration of various time-varying system constraints in a receding horizon fashion. Formulating the system dynamics linearly in combination with a quadratic cost function has two great advantages. First, the system constraints can be set up not only to achieve collision avoidance with both static and dynamic obstacles, but also aspects of human driving behavior can be considered. Second, the optimization problem can be solved very efficiently, such that the algorithm can be run with little computational effort. In addition, due to an elaborate problem formulation, reference curves with discontinuous, high curvatures will be effortlessly smoothed out by the algorithm. This makes the proposed algorithm applicable to different traffic scenarios, such as parking or highway driving. Experimental results are presented for different real-world scenarios to demonstrate the algorithm's abilities.