Positive Invariant Sets for Safe Integrated Vehicle Motion Planning and Control

This article describes a method for real-time integrated motion planning and control aimed at autonomous vehicles. Our method leverages feedback control, positive invariant sets, and equilibrium trajectories of the closed-loop system to produce and track trajectories that are collision-free with guarantees according to the vehicle model. Our method jointly steers the vehicle to a target region and controls the velocity while satisfying constraints associated with future motion of surrounding obstacles. We develop a receding-horizon implementation of the control policy and verify the method in both a simulated road scenario and an experimental validation using a scaled mobile robot with car-like dynamics using only onboard sensing. The results show that our method generates dynamically feasible and safe (i.e., collision-free) trajectories in real time, and indicate that the proposed planner is robust to sensing and mapping errors.