Automated Driving: Safe Motion Planning Using Positively Invariant Sets

This paper develops a method for safe lane changes. We leverage feedback control and constraint-admissible positively invariant sets to guarantee collision-free closed-loop trajectory tracking. Starting from an initial state of the vehicle and obstacles in the region of interest, our method steers the vehicle to the desired lane while satisfying constraints associated with the future motion of the obstacles with respect to the vehicle. We connect the initial state with the desired lane using equilibrium points and associated positively invariant sets of the vehicle dynamics, where the positively invariant sets are used to guarantee safe transitions between the equilibrium points. An autonomous highway-driving example with a receding-horizon implementation shows that our method is capable of generating safe dynamically feasible trajectories in real-time while accounting for obstacles in the environment and modeling errors.