Time-Optimal Trajectory Generation for Dynamic Vehicles: A Bilevel Optimization Approach

Abstract

This paper presents a general framework to find time-optimal trajectories for dynamic vehicles like drones and autonomous cars. Hindered by its nonlinear objective and complex constraints, this problem is hard even for state-of the-art nonlinear programming (NLP) solvers. The proposed framework addresses the problem by bilevel optimization. Specifically, the original problem is divided into an inner layer, which computes a time-optimal velocity profile along a fixed geometric path, and an outer layer, which refines the geometric path by a Quasi-Newton method. The inner optimization is convex and efficiently solved by interior-point methods. A novel variable reordering method is introduced to accelerate the optimization of the velocity profile. The gradients of the outer layer can be derived from the Lagrange multipliers using sensitivity analysis of parametric optimization problems. The method is guaranteed to return a feasible solution at any time, and numerical experiments on a ground vehicle with friction circle dynamics model show that the proposed method performs more robustly than general NLP solvers.