Sampling Augmented Bilevel Trajectory Optimization for Constrained Quadrotor Flights

Planning smooth trajectories through a sequence of waypoints under nonconvex constraints is challenging due to the coupling between coefficient optimization and time allocation. Existing gradient-based spline trajectory optimization methods tend to be susceptible to local minima and poor initializations, or restrained by complicated gradient computations. We propose a sampling augmented bilevel optimization (SABO) approach that integrates gradient-based optimization with correlated spatio-temporal sampling for improved robustness and optimality. Through temporal normalization, the closed-form solution of coefficient optimization becomes an explicit function of segment durations, while the Hessian becomes linear in their powers, enabling analytic bilevel gradient computation without using finite differences or linearized constraints. Correlated mutations are subsequently performed around the gradient-induced solution to further explore the constrained spatio-temporal space, with sample projection and covariance matrix adaptation to guide sampling towards low-cost, feasible regions. Simulations show that SABO outperforms existing methods in terms of optimality and robustness. We validate SABO in flight experiments conducted on a quadrotor.