Enhancing Bilevel Optimization for UAV Time-Optimal Trajectory using a Duality Gap Approach

Time-optimal trajectories for dynamic robotic vehicles are difficult to compute even for state-of-the-art nonlinear programming (NLP) solvers, due to nonlinearity and bang-bang control structure. This paper presents a bilevel optimization framework that addresses these problems by decomposing the spatial and temporal variables into a hierarchical optimization. Specifically, the original problem is divided into an inner layer, which computes a time-optimal velocity profile along a given 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. The gradients of the outer layer can be analytically obtained using sensitivity analysis of parametric optimization problems. A novel contribution is to introduce a duality gap in the inner optimization rather than solving it to optimality; this lets the optimizer realize warm-starting of the interior-point method, avoids non-smoothness of the outer cost function caused by active inequality constraint switching. Like prior bilevel frameworks, this method is guaranteed to return a feasible solution at any time, but converges faster than gap-free bilevel optimization. Numerical experiments on a drone model with velocity and acceleration limits show that the proposed method performs faster and more robustly than gap-free bilevel optimization and general NLP solvers.