Positive Invariant Sets for Safe Integrated Vehicle Motion Planning and Control

This paper describes a method for real-time integrated motion planning and control of autonomous vehicles. Our method leverages feedback control, positive invariant sets, and equilibrium trajectories of the closed-loop system to guarantee collision-free closed-loop trajectory tracking. Our method jointly steers the vehicle to a target region and controls the velocity while satisfying constraints associated with the future motion of the obstacles with respect to the vehicle. We develop a receding-horizon implementation and verify the method in a simulated road scenario. The results show that our method generates safe dynamically feasible trajectories while accounting for obstacles in the environment and modeling errors. In addition, the computation times indicate that the method is sufficiently efficient for real-time implementation.