Efficient Mixed-Integer Programming for Longitudinal and Lateral Motion Planning of Autonomous Vehicles

The application of continuous optimization to motion planning of autonomous vehicles has enjoyed increasing popularity in recent years. In order to maintain low computation times, it is advantageous to have a convex formulation, in general requiring the planning problem to be separated into a longitudinal and lateral component. However, this decoupling of the motion often results in infeasible trajectories in situations in which both components need to be heavily linked, e.g., when planning swerving maneuvers to avoid a collision with obstacles. In this work, we propose an approach which extends the convex optimization problem of the longitudinal component to incorporate changing constraints, allowing us to guarantee feasibility of the resulting combined trajectory. Furthermore, we provide additional safety guarantees for the planned motion by integrating formal safety distances assuming infinite precision arithmetic. Our approach is demonstrated using simulated lane change maneuvers.