Trajectory Optimization with Feasibility Guidance for Agile UAV Path Planning Under Geometric Constraints

This paper presents a practical optimization framework for improving trajectory feasibility in constrained nonlinear optimal control problems for agile unmanned aerial vehicles (UAVs). The proposed method addresses trajectory optimization problems with non-convex geometric constraints, where gradient-based solvers often fail to converge to feasible solutions. The framework combines Model Predictive Path Integral (MPPI) control and the Augmented Lagrangian iterative Linear Quadratic Regulator (AL-iLQR). MPPI is employed as a fast sampling-based guidance mechanism to explore feasible regions of the trajectory space, while AL-iLQR is used to efficiently refine locally optimal solutions with high numerical accuracy. By decoupling feasibility exploration from local optimal refinement, the proposed method mitigates the sensitivity of gradient-based trajectory optimization to initialization in highly constrained environments. Numerical simulations involving both simplified two-dimensional dynamics and full quadrotor models demonstrate that the proposed approach significantly improves the probability of converging to feasible and dynamically consistent trajectories compared with AL-iLQR alone. The proposed method does not aim to provide theoretical guarantees of global optimality; instead, it offers a practical and computationally efficient strategy for enhancing feasibility and robustness in real-time UAV trajectory optimization.