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

Abstract

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.

1. Introduction

Trajectory planning for agile unmanned aerial vehicles (UAVs) operating in cluttered environments is commonly formulated as a constrained optimal control problem. Such missions require the simultaneous satisfaction of nonlinear system dynamics, actuator limitations, and collision avoidance constraints imposed by environmental obstacles. These requirements often lead to highly non-convex feasible regions, making the reliable computation of safe and feasible trajectories a challenging task, particularly in aggressive flight scenarios. Accordingly, a wide range of optimal control-based approaches have been investigated to address UAV trajectory planning under geometric and dynamical constraints [1,2,3,4,5,6,7,8].

Existing UAV trajectory planning methods can be broadly categorized into three approaches: convex relaxation-based optimization, learning-based planning, and nonlinear trajectory optimization methods. Convex relaxation techniques, such as half-space approximations, transform non-convex obstacle avoidance constraints into convex constraints, enabling efficient optimization. However, these methods often introduce conservatism, particularly in cluttered environments where narrow feasible corridors exist between obstacles. In parallel, recent studies have explored data-driven approaches using deep Reinforcement Learning (RL) and large Artificial Intelligence (AI) models for UAV path planning. These learning-based methods can learn navigation policies in complex environments through large-scale training data. Nevertheless, their performance often depends on the availability of representative training environments and may require additional mechanisms to ensure strict constraint satisfaction. In contrast, nonlinear trajectory optimization methods directly solve the optimal control problem while considering system dynamics and constraints. These methods can generate high-precision trajectories but may suffer from infeasible local minima in highly constrained environments.

In practical implementations, to enable real-time computation, UAV dynamics is often simplified and modeled as a point mass [9,10,11]. Subsequently, for path tracking, nonlinear control methods such as Incremental Nonlinear Dynamic Inversion (INDI) are commonly employed [12,13]; however, these methods do not guarantee optimality of minimum-time or minimum-energy planning. Consequently, a path planning approach that accounts for system dynamics more accurately is desirable. Although popular path planning methods exist, including Rapidly exploring Random Trees (RRT) [14,15] and Probabilistic Roadmaps (PRM) [16], explicitly incorporating system dynamics is essential for achieving agile control in complex missions. In addition, large AI models and deep RL have recently gained popularity for aerial navigation [17,18]. AI and RL are, however, not considered in our work because our study focuses on improving the feasibility of local solutions in optimization solvers and is motivated by the fact that these learning-based methods often struggle to generalize scenarios outside their training data and to bridge the sim-to-real gap [19,20,21]. In contrast, model-based approaches [9,22] can robustly compute feasible solutions in unknown environments by explicitly leveraging the system model and current-time state information.

A notable study addressing coupled translational and rotational motion is reported in [23]. This study introduced a unified system model that incorporates both translational dynamics and rotational kinematics, enabling simultaneous feedback control of position and attitude. By applying a state-dependent linear quadratic regulator to track target states, the approach in [23] achieves highly agile control performance. In our previous work [24], the model based on [23] was extended and adapted for trajectory optimization rather than feedback control. Specifically, simple rotational dynamics and thrust-related states were explicitly incorporated into the optimization model, allowing the generation of dynamically feasible and collision-free trajectories under geometric constraints. To solve the resulting constrained nonlinear optimal control problem, the Augmented Lagrangian iterative Linear Quadratic Regulator (AL-iLQR), implemented in ALTRO [25], was employed as the numerical solver. This choice was motivated by the computational efficiency of iLQR for nonlinear systems and the ability of the Augmented Lagrangian Method (ALM) [26] to handle geometric constraints with high accuracy. Since the introduction of iLQR [27], the effectiveness of iLQR-based methods for real-time applications has been demonstrated in numerous studies [28,29]. In particular, ALM plays a critical role in collision avoidance problems, where optimal solutions often lie on or near constraint boundaries.

Despite the success of optimization solvers based on Sequential Quadratic Programming (SQP) [30] and interior point methods (IPM), inequality constraints associated with collision avoidance are known to be computationally challenging [31,32,33]. Similar difficulties arise in practical optimal control solvers such as ALTRO [25], Crocoddyl [34], FATROP [35] and Acados [36], which are specifically designed for trajectory optimization problems, including UAV trajectory planning.

Numerous studies have investigated methods for handling both convex and non-convex constraints [37,38]. While projection methods (PMs) are well suited to convex geometric constraints, addressing complex non-convex constraints—such as those arising from the presence of numerous obstacles in UAV missions—remains challenging. In addition, the Alternating Direction Method of Multipliers (ADMM), which can be viewed as an extension of the Augmented Lagrangian Method (ALM), has been applied to constrained trajectory optimization in several studies [39,40]. However, although ADMM is well suited for distributed control problems, it often exhibits slow convergence due to problem splitting, particularly in environments containing a large number of uncontrollable obstacles.

Some works reformulate the problem into a half-space convex relaxation [41,42,43], thereby utilizing fast convex optimization solvers. While half-space convex relaxation is a powerful method because of benefit from general fast convex solvers, it relies on the separating hyperplane theorem, which tends to generate overly conservative trajectories, particularly in densely cluttered environments.

In complex constrained trajectory optimization problems, these solvers frequently fail to find feasible solutions due to conflicting interactions among constraint gradients. Such failures are particularly critical in UAV trajectory optimization, where safety and reliability are paramount. Moreover, when these solvers fail to identify feasible solutions, a guidance mechanism is required; however, identifying a partially feasible region that contains the global optimum is itself challenging in complex scenarios. To address these problems, several hybrid approaches that combine sampling-based methods with existing local solvers have been proposed [44,45,46,47]. However, the learning costs associated with complex problems remain high in [44,45,46]. Specifically, Ref. [45] identifies feasible solutions using Bayesian optimization (BO) during the Interior Point OPTimizers (IPOPT) [48] iterations but does not prioritize real-time capability. In Ref. [46], BO is employed to learn complex constraints, after which optimal solutions are identified using iLQR within the resulting feasible regions. In Ref. [47], circular safety zones are generated around obstacles, enabling trajectory optimization within these predefined safe regions. While Model Predictive Path Integral (MPPI) control [49] demonstrates excellent real-time performance, its integration with IPDDP [50] may suffer from degraded numerical accuracy near constraint boundaries due to the inherent characteristics of interior point methods.

From the above discussion, despite the growing demand for agile control based on advanced dynamic models such as [23] and for robust optimization of UAV trajectories, path planning—particularly trajectory optimization—often fails to identify feasible solutions under non-convex constraints. Furthermore, existing approaches that explicitly ensure feasibility are generally unsuitable for real-time UAV trajectory optimization in environments containing obstacles. Therefore, real-time UAV trajectory optimization requires a modified optimization framework that effectively leverages advanced UAV dynamics, the characteristics of collision avoidance constraints, and the computational efficiency of established high-accuracy solvers. This observation motivates the development of the approach proposed in this paper.

This study proposes a plugin-style method that is integrated into the AL-iLQR iteration process. The proposed approach explores feasible regions by employing Model Predictive Path Integral (MPPI) control as a fast, supplementary guidance mechanism, without requiring fundamental modifications to the underlying solver algorithm. Namely, the proposed approach first estimates a coarse, low-precision optimal solution and then performs high-precision local refinement around the solution. This allows our method to effectively exploit narrow passages between obstacles rather than being restricted to conservative regions. Notably, the method exploits failure behaviors in optimization that are specific to geometric constraints and effectively guides infeasible trajectories toward feasible regions. The main contributions of this study are summarized as follows:

• Integration of active guidance as an additional constraint within the iteration process of an established optimal control solver;
• Trajectory optimization that leverages advanced UAV dynamics in the presence of static and dynamic obstacles;
• Improved robustness and solution quality through an MPPI-based guidance mechanism.

2. System Model

In this work, the system dynamics describes a quadrotor model that accounts for first-order rotor dynamics and thrust limitations. The state value is defined as $\mathbf{x}={\left[\begin{array}{ccccc}{\mathbf{p}}^{\mathrm{T}}& {\dot{\mathbf{p}}}^{\mathrm{T}}& {\mathbf{q}}^{\mathrm{T}}& {\mathsf{\omega }}^{\mathrm{T}}& {\mathbf{F}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^{17}$, where $\mathbf{p}\in {\mathbb{R}}^3$ denotes the position, $\dot{\mathbf{p}}\in {\mathbb{R}}^3$ the velocity, $\mathbf{q}={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^4$ the orientation with a quaternion, $\mathsf{\omega }={\left[\begin{array}{ccc}{\omega }_1& {\omega }_2& {\omega }_3\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^3$ the angular velocity, and $\mathbf{F}={\left[\begin{array}{cccc}{f}_1& f_2& f_3& f_4\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^4$ the thrust generated by each rotor along the body-frame $z$-axis. The control input is defined as the commanded rotor speeds, $\mathbf{u}={\left[\begin{array}{cccc}{\Omega }_{\mathrm{c}1}& {\Omega }_{\mathrm{c}2}& {\Omega }_{\mathrm{c}3}& {\Omega }_{\mathrm{c}4}\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^4$. The rotor dynamics is modeled as a first-order lag system with time constant $T_r$. The rotational speed ${\mathsf{\Omega }}_i$ of the $i$-th rotor evolves according to

$${\displaystyle \frac{d{\Omega }_i}{dt}}={\displaystyle \frac{1}{T_r}}\left({\Omega }_{\mathrm{c}i}-{\Omega }_i\right), i\in \left\{1,\dots ,4\right\}.$$

(1)

Using the relationship between thrust and rotor speed, $f_i=b{\Omega }_i^2$, with coefficient $b$, the system dynamics can be written as

$${\displaystyle \frac{d\mathbf{x}}{dt}}={\displaystyle \frac{d}{dt}}\left[\begin{array}{c}\mathbf{p}\\ \dot{\mathbf{p}}\\ \mathbf{q}\\ \mathsf{\omega }\\ \mathbf{F}\end{array}\right]=\left[\begin{array}{c}\dot{\mathbf{p}}\\ \mathbf{R}\left(\mathbf{q}\right)\left[\begin{array}{c}0\\ 0\\ a\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]\\ {\displaystyle \frac{1}{2}}{\mathbf{Q}}_{\times }\left(\mathsf{\omega }\right)\mathbf{q}\\ {\mathbf{J}}^{-1}\left(\mathbf{T}\mathbf{F}-\mathsf{\omega }\times \mathbf{J}\mathsf{\omega }-\mathbf{D}\mathsf{\omega }\right)\\ {\displaystyle \frac{2}{T_r}}{\mathbf{F}}^{{\displaystyle \frac{1}{2}}}\circ \left(b^{{\displaystyle \frac{1}{2}}}\mathbf{u}-{\mathbf{F}}^{{\displaystyle \frac{1}{2}}}\right)\end{array}\right],$$

(2)

$$\mathbf{R}\left(\mathbf{q}\right)=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_0{q}_2+q_1{q}_3\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_0{q}_1+q_2{q}_3\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right],$$

(3)

$${\mathbf{Q}}_{\times }\left(\mathsf{\omega }\right)=\left[\begin{array}{cccc}0& -{\omega }_1& -{\omega }_2& -{\omega }_3\\ {\omega }_1& 0& {\omega }_3& -{\omega }_2\\ {\omega }_2& -{\omega }_3& 0& {\omega }_1\\ {\omega }_3& {\omega }_2& -{\omega }_1& 0\end{array}\right],$$

(4)

$$\mathbf{T}=\left[\begin{array}{cccc}-{\displaystyle \frac{l}{\sqrt{2}}}& -{\displaystyle \frac{l}{\sqrt{2}}}& {\displaystyle \frac{l}{\sqrt{2}}}& {\displaystyle \frac{l}{\sqrt{2}}}\\ -{\displaystyle \frac{l}{\sqrt{2}}}& {\displaystyle \frac{l}{\sqrt{2}}}& {\displaystyle \frac{l}{\sqrt{2}}}& -{\displaystyle \frac{l}{\sqrt{2}}}\\ k_r& -k_r& k_r& -k_r\end{array}\right],$$

(5)

$$\mathbf{D}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& k_a\end{array}\right].$$

(6)

The direction cosine matrix $\mathbf{R}\left(\mathbf{q}\right)$ and the quaternion multiplication matrix ${\mathbf{Q}}_{\times }\left(\mathsf{\omega }\right)$ are defined in Equations (3) and (4), respectively. The thrust allocation matrix $\mathbf{T}$ and the aerodynamic damping matrix $\mathbf{D}$ are given in Equations (5) and (6). The quaternion is constrained to satisfy

$$q_0^2+q_1^2+q_2^2+q_3^2=1.$$

(7)

Here, the thrust satisfies $0<f_{\mathrm{m}\mathrm{i}\mathrm{n}}\le f_i\le f_{\mathrm{m}\mathrm{a}\mathrm{x}}$, $a=\sum (f_i/m)$, where $m$ is the vehicle mass, $g$ is the gravitational acceleration, $\mathbf{J}$ denotes the inertia matrix, $l$ the arm length, $k_r$ the rotor torque coefficient, and $k_a$ the aerodynamic drag coefficient. ${\mathbf{F}}^{{\displaystyle \frac{1}{2}}}={\left[\begin{array}{cccc}{f}_1^{{\displaystyle \frac{1}{2}}}& f_2^{{\displaystyle \frac{1}{2}}}& f_3^{{\displaystyle \frac{1}{2}}}& f_4^{{\displaystyle \frac{1}{2}}}\end{array}\right]}^T$, and $\circ$ denotes the Hadamard product.

Quadrotor control systems are commonly designed using separate subsystems for position and attitude control to avoid singularities and simplify controller design. However, for trajectory optimization problems such as minimum-time or minimum-energy planning, it is necessary to employ a unified control formulation that simultaneously considers translational and rotational dynamics. In Ref. [23], a subsystem combining translational dynamics and attitude kinematics was proposed, while neglecting rotational dynamics. In this study, that subsystem is modified and extended for trajectory optimization. The subsystem used for path planning is defined as

$${\displaystyle \frac{d}{dt}}\left[\begin{array}{c}\mathbf{p}\\ \dot{\mathbf{p}}\\ \mathbf{q}\\ \mathsf{\omega }\\ a\end{array}\right]=\left[\begin{array}{c}\dot{\mathbf{p}}\\ \mathbf{R}\left(\mathbf{q}\right)\left[\begin{array}{c}0\\ 0\\ a\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]\\ {\displaystyle \frac{1}{2}}{\mathbf{Q}}_{\times }\left(\mathsf{\omega }\right)\mathbf{q}\\ \left({\mathsf{\omega }}_{\mathrm{c}}-\mathsf{\omega }\right)/T_{\omega }\\ \left(a_{\mathrm{c}}-a\right)/T_a\end{array}\right],$$

(8)

$$\mathrm{subject} \mathrm{to} {\mathsf{\omega }}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le {\mathsf{\omega }}_{\mathrm{c}}\le {\mathsf{\omega }}_{\mathrm{m}\mathrm{a}\mathrm{x}},$$

(9)

$$a_{\mathrm{m}\mathrm{i}\mathrm{n}}\le a_{\mathrm{c}}\le a_{\mathrm{m}\mathrm{a}\mathrm{x}}.$$

(10)

For practical implementation, both the rotational dynamics and thrust acceleration are modeled as first-order lag systems. The optimal state trajectories ${\mathsf{\omega }}_{\mathrm{o}\mathrm{p}\mathrm{t}}$ and $a_{\mathrm{o}\mathrm{p}\mathrm{t}}$ obtained from path planning are treated as reference trajectories and constraints in the path tracking stage. The path tracking model is defined as

$${\displaystyle \frac{d}{dt}}\left[\begin{array}{c}\mathsf{\omega }\\ \mathbf{F}\end{array}\right]=\left[\begin{array}{c}{\mathbf{J}}^{-1}\left(\mathbf{T}\mathbf{F}-\mathsf{\omega }\times \mathbf{J}\mathsf{\omega }-\mathbf{D}\mathsf{\omega }\right)\\ {\displaystyle \frac{2}{T_r}}{\mathbf{F}}^{{\displaystyle \frac{1}{2}}}\circ \left(b^{{\displaystyle \frac{1}{2}}}\mathbf{u}-{\mathbf{F}}^{{\displaystyle \frac{1}{2}}}\right)\end{array}\right],$$

(11)

$$\mathrm{subject} \mathrm{to} \sum _i\left\{f_i\left(t\right)\right\}-m{a}_{\mathrm{r}\mathrm{e}\mathrm{f}}\left(t\right)=0,$$

(12)

$$0<f_{\mathrm{m}\mathrm{i}\mathrm{n}}\le f_i\left(t\right)\le f_{\mathrm{m}\mathrm{a}\mathrm{x}}.$$

(13)

Figure 1 illustrates the overall framework of the path planning and path tracking system. Here, ${\mathbf{p}}_G$, ${\dot{\mathbf{p}}}_G$, ${\mathbf{q}}_G$, ${\mathsf{\omega }}_G$, and $a_G$ denote the target states. In the path tracking stage, appropriate weighting of ${\mathit{\omega }}_{\mathrm{r}\mathrm{e}\mathrm{f}}$ and ${\mathit{F}}_{\mathrm{r}\mathrm{e}\mathrm{f}}$ is required when applying AL-iLQR.

3. Problem Formulation and Solution

This section formulates the constrained trajectory optimization problem and describes the corresponding solution algorithm. The proposed method is categorized as a modified Augmented Lagrangian iterative Linear Quadratic Regulator (AL-iLQR) framework for trajectory optimization under geometric constraints. The constrained optimization problem is handled using the Augmented Lagrangian Method (ALM), while the resulting relaxed subproblems are efficiently solved using iLQR.

3.1. Constrained Optimization Problem for Model Predictive Control

In optimal control theory, a discrete-time nonlinear optimal control problem with constraints is formulated as follows:

$$J_{\mathrm{v}\mathrm{a}\mathrm{r}}\left(\mathbf{X},\mathbf{U}\right)=L_N\left({\mathbf{x}}_N\right)+\sum _{k=0}^{N-1}{L}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right),$$

(14)

$$\mathrm{subject} \mathrm{to} {\mathbf{x}}_{k+1}={\mathbf{f}}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k,k\right),$$

(15)

$$c_{i,k}\left({\mathbf{x}}_k,{\mathbf{u}}_k,k\right)=0, i\in \mathcal{G},$$

(16)

$$c_{i,k}\left({\mathbf{x}}_k,{\mathbf{u}}_k,k\right)\le 0, i\in \mathcal{H},$$

(17)

where $k$ is the index of the discrete time, $L_N$ is the terminal cost, and $L_k$ is the stage cost. The state and control input are denoted by ${\mathbf{x}}_k\in {\mathbb{R}}^n$ and ${\mathbf{u}}_k\in {\mathbb{R}}^m$, respectively. The stacked state and control sequences are defined as, $\mathbf{X}=\left[{\mathbf{x}}_0,\dots ,{\mathbf{x}}_N\right]\in {\mathbb{R}}^{n\times \left(N+1\right)}$, and $\mathbf{U}=\left[{\mathbf{u}}_0,\dots ,{\mathbf{u}}_{N-1}\right]\in {\mathbb{R}}^{m\times N}$. Here, $J_{\mathrm{v}\mathrm{a}\mathrm{r}}:{\mathbb{R}}^{n\times \left(N+1\right)}\times {\mathbb{R}}^{m\times N}\to \mathbb{R}$, ${\mathbf{f}}_k$, $\mathcal{G}$, and $\mathcal{H}$ denote the sets of equality and inequality constraints, respectively. The objective is to determine a control sequence $\mathbf{U}$ that minimizes $J_{\mathrm{v}\mathrm{a}\mathrm{r}}$.

3.2. Constrained Optimization Algorithm

The constrained optimization problem is transformed into an unconstrained one by constructing the augmented Lagrangian:

$${\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},\mathbf{L}\right)=J_{\mathrm{v}\mathrm{a}\mathrm{r}}+J_{\mathrm{c}\mathrm{o}\mathrm{n}},$$

(18)

$$J_{\mathrm{c}\mathrm{o}\mathrm{n}}=\sum _{k=0}^N\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,k}{c}_{i,k}+{\displaystyle \frac{{\rho }_{i,k}}{2}}{c}_{i,k}^2\right),$$

(19)

$${\rho }_{i,k}=\left\{\begin{array}{cc}0& \mathrm{i}\mathrm{f} i\in \mathcal{H} \mathrm{a}\mathrm{n}\mathrm{d} c_{i,k}<0\wedge {\lambda }_{i,k}=0\hfill \\ {\mu }_{i,k}& \mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}.\hfill \end{array}\right.,$$

(20)

where $\mathbf{L}=\left\{{\lambda }_{i,k}\right\}$ denotes the set of Lagrange multipliers and ${\mu }_{i,k}\in \mathcal{M}$ is the penalty parameter. ALM, summarized in Algorithm 1, seeks a solution that minimizes ${\mathcal{L}}_{\rho }\left(\mathbf{X},\mathbf{U},\mathbf{L}\right)$. The multiplier and penalty parameters are updated according to

$${\lambda }_{i,k}=\left\{\begin{array}{cc}{\lambda }_{i,k}+{\mu }_{i,k}{c}_{i,k}& ,i\in \mathcal{G}\\ \max \left(0,{\lambda }_{i,k}+{\mu }_{i,k}{c}_{i,k}\right)& ,i\in \mathcal{H}\end{array}\right.,$$

(21)

$${\mu }_{i,k}=\eta {\mu }_{i,k},$$

(22)

where $\eta >1$ is a constant scaling factor. As ${\mu }_{i,k}\to \mathrm{\infty }$, the solution asymptotically satisfies the constraints strictly.

Algorithm 1: Augmented Lagrangian Method
	Inputs:
	$\mathrm{Initial} \mathrm{guess} \mathbf{U}$
	$\mathrm{Boundary} \mathrm{conditions} {\mathbf{x}}_0$
	Outputs:
	$\mathrm{Optimal} \mathrm{state} \mathbf{X}$$, \mathrm{input} \mathbf{U}$  $\mathrm{and} \mathrm{multiplier} \mathbf{L}$
	Procedure:
1:	$\mathrm{Predict} \mathbf{X}$  $\mathrm{using} \mathbf{U}$  $\mathrm{and} {\mathbf{x}}_0$
2:	$\mathrm{Initialize} \mathbf{L}$$,\mathbf{M}$
3:	while
4:	$\mathbf{X},\mathbf{U}\leftarrow \underset{\mathbf{X},\mathbf{U}}{\mathrm{argmin}} {\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},\mathbf{L}\right)$
5:	if the convergence conditions are satisfied
6:	Break
7:	end if
8:	$\mathrm{Update} \mathbf{L},\mathbf{M}$
9:	end while

3.3. Minimization of the Augmented Lagrangian ${\mathcal{L}}_{\mathit{M}}$

To minimize ${\mathcal{L}}_{\mathbf{M}}$, Differential Dynamic Programming (DDP) [51] and iLQR are employed. In the ALM framework, ${\mathcal{L}}_{\mathbf{M}}$ is minimized while keeping $\mathbf{L}$ and $\mathbf{M}$ fixed:

$$\mathbf{X},\mathbf{U}\leftarrow \underset{\mathbf{X},\mathbf{U}}{\mathrm{argmin}} {\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},\mathbf{L}\right).$$

(23)

In the trajectory optimization, the value function is defined as

$$V_k\left({\mathbf{x}}_k\right)\equiv \underset{\left[{\mathbf{u}}_k,\dots ,{\mathbf{u}}_{N-1}\right]}{\min }\left\{L_N\left({\mathbf{x}}_N\right)+\sum _{j=k}^{N-1}{L}_j\left({\mathbf{x}}_j,{\mathbf{u}}_j\right)+\sum _{j=k}^N\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,j}{c}_{i,j}+{\displaystyle \frac{{\rho }_{i,j}}{2}}{c}_{i,j}^2\right)\right\},$$

(24)

$$V_N\left({\mathbf{x}}_k\right)\equiv L_N\left({\mathbf{x}}_N\right)+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,N}{c}_{i,N}+{\displaystyle \frac{{\rho }_{i,N}}{2}}{c}_{i,N}^2\right).$$

(25)

Applying Bellman’s principle of optimality, the following recursion is obtained.

$$V_k\left({\mathbf{x}}_k\right)=\underset{\left[{\mathbf{u}}_k,\dots ,{\mathbf{u}}_{N-1}\right]}{\min }\left\{L_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,k}{c}_{i,k}+{\displaystyle \frac{{\rho }_{i,j}}{2}}{c}_{i,k}^2\right)+L_N\left({\mathbf{x}}_N\right)+\sum _{j=k+1}^{N-1}{L}_j\left({\mathbf{x}}_j,{\mathbf{u}}_j\right)+\sum _{j=k+1}^N\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,j}{c}_{i,j}+{\displaystyle \frac{{\rho }_{i,j}}{2}}{c}_{i,j}^2\right)\right\}$$

$$=\underset{{\mathbf{u}}_k}{\min }\left\{L_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,k}{c}_{i,k}+{\displaystyle \frac{{\rho }_{i,k}}{2}}{c}_{i,k}^2\right)+V_{k+1}\left({\mathbf{x}}_{k+1}\right)\right\}.$$

(26)

The corresponding active-value function is defined as

$$Q_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\equiv L_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\lambda }_{i,k}{c}_{i,k}+{\displaystyle \frac{{\rho }_{i,k}}{2}}{c}_{i,k}^2\right)+V_{k+1}\left({\mathbf{x}}_{k+1}\right).$$

(27)

Since Equation (28) is nonlinear, a second-order Taylor expansion is employed, as shown in Equation (29).

$$V_k\left({\mathbf{x}}_k\right)=\underset{{\mathbf{u}}_k}{\min }{Q}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right).$$

(28)

Equation (28) is a nonlinear problem and must be approximated as follows:

$${\delta Q}_k={\left[\begin{array}{c}{Q}_{\mathbf{x},k}\\ Q_{\mathbf{u},k}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{\delta \mathbf{x}}_k\\ {\delta \mathbf{u}}_k\end{array}\right]+{\displaystyle \frac{1}{2}}{\left[\begin{array}{c}{\delta \mathbf{x}}_k\\ {\delta \mathbf{u}}_k\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{Q}_{\mathbf{x}\mathbf{x},k}& Q_{\mathbf{x}\mathbf{u},k}\\ Q_{\mathbf{u}\mathbf{x},k}& Q_{\mathbf{u}\mathbf{u},k}\end{array}\right]\left[\begin{array}{c}{\delta \mathbf{x}}_k\\ {\delta \mathbf{u}}_k\end{array}\right].$$

(29)

According to iLQR, we assume that the value function can be expressed in quadratic form.

$${\delta V}_k=V_{\mathbf{x},k}^{\mathrm{T}}{\delta \mathbf{x}}_k+{\displaystyle \frac{1}{2}}{\delta \mathbf{x}}_k^{\mathrm{T}}{V}_{\mathbf{x}\mathbf{x},k}{\delta \mathbf{x}}_k.$$

(30)

By using Equations (27) and (29),

$$Q_{\mathbf{x},k}=L_{\mathbf{x},k}+{\mathbf{f}}_{\mathbf{x}}^{\mathrm{T}}{V}_{\mathbf{x},k+1}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}{c}_{\mathbf{x},i,k}^{\mathrm{T}}\left({\lambda }_{i,k}+{\rho }_{i,k}{c}_{i,k}\right),$$

(31)

$$Q_{\mathbf{u},k}=L_{\mathbf{u},k}+{\mathbf{f}}_{\mathbf{u}}^{\mathrm{T}}{V}_{\mathbf{x},k+1}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}{c}_{\mathbf{u},i,k}^{\mathrm{T}}\left({\lambda }_{i,k}+{\rho }_{i,k}{c}_{i,k}\right),$$

(32)

$$Q_{\mathbf{x}\mathbf{x},k}=L_{\mathbf{x}\mathbf{x},k}+{\mathbf{f}}_{\mathbf{x}}^{\mathrm{T}}{V}_{\mathbf{x}\mathbf{x},k+1}{\mathbf{f}}_{\mathbf{x}}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\rho }_{i,k}{c}_{\mathbf{x},i,k}^{\mathrm{T}}{c}_{\mathbf{x},i,k}\right),$$

(33)

$$Q_{\mathbf{u}\mathbf{x},k}=L_{\mathbf{u}\mathbf{x},k}+{\mathbf{f}}_{\mathbf{u}}^{\mathrm{T}}{V}_{\mathbf{x}\mathbf{x},k+1}{\mathbf{f}}_{\mathbf{x}}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\rho }_{i,k}{c}_{\mathbf{u},i,k}^{\mathrm{T}}{c}_{\mathbf{x},i,k}\right),$$

(34)

$$Q_{\mathbf{u}\mathbf{u},k}=L_{\mathbf{u}\mathbf{u},k}+{\mathbf{f}}_{\mathbf{u}}^{\mathrm{T}}{V}_{\mathbf{x}\mathbf{x},k+1}{\mathbf{f}}_{\mathbf{u}}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\rho }_{i,k}{c}_{\mathbf{u},i,k}^{\mathrm{T}}{c}_{\mathbf{u},i,k}\right).$$

(35)

Note,

$$V_{\mathbf{x},N}=L_{\mathbf{x},N}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}{c}_{\mathbf{x},i,N}^{\mathrm{T}}\left({\lambda }_{i,N}+{\rho }_{i,N}{c}_{i,N}\right),$$

(36)

$$V_{\mathbf{x}\mathbf{x},N}=L_{\mathbf{x}\mathbf{x},N}+\sum _{i\in \mathcal{G}\cup \mathcal{H}}\left({\rho }_{i,N}{c}_{\mathbf{x},i,N}^{\mathrm{T}}{c}_{\mathbf{x},i,N}\right).$$

(37)

When the optimal solution ${\delta \mathbf{u}}_k^{\ast }$ is found, the following equation is satisfied by using Equation (29):

$$\mathbf{0}=Q_{\mathbf{u},k}+Q_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k+Q_{\mathbf{u}\mathbf{u},k}{\delta \mathbf{u}}_k^{\ast },$$

(38)

$$\therefore {\delta \mathbf{u}}_k^{\ast }=-Q_{\mathbf{u}\mathbf{u},k}^{-1}{Q}_{\mathbf{u},k}-Q_{\mathbf{u}\mathbf{u},k}^{-1}{Q}_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k.$$

(39)

Since $Q_{\mathbf{u}\mathbf{u},k}$ may not be a regular matrix, the parameter ${\mu }_r$ for regularization is often used,

$${\delta \mathbf{u}}_k^{\ast }=-{\left(Q_{\mathbf{u}\mathbf{u},k}+{\mu }_r\mathbf{I}\right)}^{-1}{Q}_{\mathbf{u},k}-{\left(Q_{\mathbf{u}\mathbf{u},k}+{\mu }_r\mathbf{I}\right)}^{-1}{Q}_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k\equiv {\mathbf{d}}_k+{\mathbf{K}}_k{\delta \mathbf{x}}_k.$$

(40)

By using Equations (29), (30) and (40) and ${\delta V}_k\left({\delta \mathbf{x}}_k\right)={\delta Q}_k\left({\delta \mathbf{x}}_k,{\delta \mathbf{u}}_k^{\ast }\right)$,

$$\begin{array}{ll}{\delta Q}_k=Q_{\mathbf{x},k}^{\mathrm{T}}{\delta \mathbf{x}}_k+& Q_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{d}}_k+Q_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{K}}_k{\delta \mathbf{x}}_k\\ & +{\displaystyle \frac{1}{2}}[{\delta \mathbf{x}}_k^{\mathrm{T}}{Q}_{\mathbf{x}\mathbf{x},k}{\delta \mathbf{x}}_k+{\delta \mathbf{x}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k+{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k\\ & +{\delta \mathbf{x}}_k^{\mathrm{T}}{Q}_{\mathbf{x}\mathbf{u},k}{\mathbf{d}}_k+{\delta \mathbf{x}}_k^{\mathrm{T}}{Q}_{\mathbf{x}\mathbf{u},k}{\mathbf{K}}_k{\delta \mathbf{x}}_k+{\delta \mathbf{x}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{d}}_k\\ & +{\delta \mathbf{x}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{K}}_k{\delta \mathbf{x}}_k+{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{d}}_k+{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{K}}_k{\delta \mathbf{x}}_k],\end{array}$$

(41)

$$\begin{array}{ll}{\delta Q}_k=Q_{\mathbf{x},k}^{\mathrm{T}}{\delta \mathbf{x}}_k+& Q_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{d}}_k+Q_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{K}}_k{\delta \mathbf{x}}_k\\ & +{\displaystyle \frac{1}{2}}[{\delta \mathbf{x}}_k^{\mathrm{T}}{Q}_{\mathbf{x}\mathbf{x},k}{\delta \mathbf{x}}_k+{\delta \mathbf{x}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k+2{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{x},k}{\delta \mathbf{x}}_k\\ & +{\delta \mathbf{x}}_k^{\mathrm{T}}{Q}_{\mathbf{x}\mathbf{u},k}{\mathbf{K}}_k{\delta \mathbf{x}}_k+{\delta \mathbf{x}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{K}}_k{\delta \mathbf{x}}_k+{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{d}}_k\\ & +2{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{K}}_k{\delta \mathbf{x}}_k],\end{array}$$

(42)

$$V_{\mathbf{x},k}=Q_{\mathbf{x},k}+{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u},k}+Q_{\mathbf{u}\mathbf{x},k}^{\mathrm{T}}{\mathbf{d}}_k+{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{d}}_k,$$

(43)

$$V_{\mathbf{x}\mathbf{x},k}=Q_{\mathbf{x}\mathbf{x},k}+{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{x},k}+Q_{\mathbf{x}\mathbf{u},k}{\mathbf{K}}_k+{\mathbf{K}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{K}}_k.$$

(44)

The expected decrease in the value function is

$${\Delta V}_k=Q_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{d}}_k+{\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u},k}{\mathbf{d}}_k.$$

(45)

Based on the above, iLQR calculates the feedback gains ${\mathbf{d}}_k, {\mathbf{K}}_k$ in backward pass, then use those gains to find the input variables that minimize the cost function by using a linear search in forward pass.

$${\alpha }^{\ast }=\underset{\alpha }{\mathrm{argmin}} {\mathcal{L}}_{\mathbf{M}}\left(\overline{\mathbf{X}},\overline{\mathbf{U}},\mathbf{L}\right),$$

(46)

where

$$\overline{\mathbf{U}}\ni {\overline{\mathbf{u}}}_k={\mathbf{u}}_k+{\mathbf{K}}_k\left({\overline{\mathbf{x}}}_k-{\mathbf{x}}_k\right)+\alpha {\mathbf{d}}_k, \left(0<\alpha <1\right).$$

(47)

Algorithm 2 shows the iLQR methods. We define the optimization problem in iLQR in detail.

$$L_N\left({\mathbf{x}}_N\right)={\mathbf{q}}_N^{\mathrm{T}}{\mathbf{e}}_{\mathbf{x},N}+{\displaystyle \frac{1}{2}}{\mathbf{e}}_{\mathbf{x},N}^{\mathrm{T}}{\mathbf{Q}}_N{\mathbf{e}}_{\mathbf{x},N},$$

(48)

$$L_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)={\mathbf{q}}_k^{\mathrm{T}}{\mathbf{e}}_{\mathbf{x},k}+{\displaystyle \frac{1}{2}}{\mathbf{e}}_{\mathbf{x},k}^{\mathrm{T}}{\mathbf{Q}}_k{\mathbf{e}}_{\mathbf{x},k}+{\mathbf{r}}_k^{\mathrm{T}}{\mathbf{e}}_{\mathbf{u},k}+{\displaystyle \frac{1}{2}}{\mathbf{e}}_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{R}}_k{\mathbf{e}}_{\mathbf{u},k}+{\mathbf{e}}_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{e}}_{\mathbf{x},k},$$

(49)

where ${\mathbf{q}}_k,{\mathbf{Q}}_k,{\mathbf{r}}_k,{\mathbf{R}}_k,{\mathbf{H}}_k$ are weight matrices, ${\mathbf{e}}_{\mathbf{x},k}={\mathbf{x}}_k-{\mathbf{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},k}$, and the ref means the design values.

Algorithm 2: iLQR for finding $\min {\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},\mathbf{L}\right)$
	Inputs:
	$\mathrm{State} \mathbf{X}$$, \mathrm{input} \mathbf{U}$$, \mathrm{multiplier} \mathbf{L}$$, \mathrm{the} \mathrm{design} \mathrm{values} {\mathbf{X}}_{\mathrm{r}\mathrm{e}\mathrm{f}}$$, {\mathbf{U}}_{\mathrm{r}\mathrm{e}\mathrm{f}}$
	Outputs:
	$\mathrm{Optimal} \mathrm{state} \mathbf{X}$$\mathrm{and} \mathrm{input} \mathbf{U}$
	Procedure:
1:	$J_{\mathrm{p}\mathrm{r}\mathrm{e}}\leftarrow {\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},\mathbf{L}\right)$
2:	While
3:	${\mu }_r\leftarrow 0$ in Equation (40)
4:	$V_{\mathbf{x},N},V_{\mathbf{x}\mathbf{x},N}\leftarrow$ Equations (36), (37), (48) and (49)
5:	$\mathbf{for} k=N-1,\dots , 0$
6:	Calculations from Equations (31)–(35)
7:	$\mathbf{if} \mathrm{Cholesky} \mathrm{decomposition} (Q_{\mathbf{u}\mathbf{u}}$) failed
8:	$k\leftarrow N-1$
9:	${\mu }_r\leftarrow$  $\mathrm{Increasing} {\mu }_r$
10:	Go to line 5
11:	end if
12:	${\mathbf{K}}_k,{\mathbf{d}}_k\leftarrow$ Equation (40)
13:	end for
14:	$\alpha \leftarrow 1$$, {\overline{\mathbf{x}}}_0\leftarrow {\mathbf{x}}_0$
15:	While
16:	$\mathrm{Prediction} \overline{\mathbf{X}}$ using Equations (15) and (47)
17:	$J_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}\leftarrow {\mathcal{L}}_{\mathbf{M}}\left(\overline{\mathbf{X}},\overline{\mathbf{U}},\mathbf{L}\right)$
18:	$\epsilon \leftarrow \left(J_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}-J_{\mathrm{p}\mathrm{r}\mathrm{e}}\right)/\left\{\alpha \Sigma \left(Q_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{d}}_k\right)+{\alpha }^2\Sigma \left({\mathbf{d}}_k^{\mathrm{T}}{Q}_{\mathbf{u}\mathbf{u}}{\mathbf{d}}_k\right)\right\}$
19:	$\mathbf{if} \mathrm{the} \mathrm{terminal} \mathrm{conditions} \mathrm{are} \mathrm{satisfied} \left({10}^{-4}\le \epsilon \le 10 \mathrm{or} \alpha <{10}^{-4}\right)$
20:	$\mathbf{X},\mathbf{U}\leftarrow \overline{\mathbf{X}},\overline{\mathbf{U}}$
21:	Break
22:	end if
23:	$\alpha \leftarrow \varphi \alpha$ $(\varphi =0.5$)
24:	end while
25:	$\mathbf{if} \left|J_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}-J_{\mathrm{p}\mathrm{r}\mathrm{e}}\right|<\mathrm{T}\mathrm{o}\mathrm{l}\mathrm{e}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}$
26:	Break
27:	end if
28:	$J_{\mathrm{p}\mathrm{r}\mathrm{e}}\leftarrow J_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}$
29:	end while
30:	$\mathbf{return} \mathbf{X}$$,\mathbf{U}$

4. Our Proposed Method for Feasibility

This section presents the proposed guidance mechanism designed to steer the AL-iLQR algorithm away from infeasible local minima and to improve convergence toward feasible solutions.

Figure 2 illustrates the overall quadrotor system architecture, encompassing the complete pipeline from trajectory planning to trajectory tracking while incorporating the associated constraints. The constraints are visually categorized in the diagram: the left block represents general system constraints; the central block corresponds to position constraints relevant to the path planning stage; and the right block describes constraints imposed during path tracking. When trajectory optimization fails to identify a feasible solution, the proposed method dynamically modifies the geometric constraints. This modification acts as a corrective mechanism by transmitting a guidance signal from the path planning module to the constraint formulation. Note that the left block also includes constraints related to system dynamics.

4.1. What Is Feasibility for Collision Avoidance?

General-purpose optimization solvers, such as SQP and IPOPT, often struggle to robustly converge to feasible regions under complex constraints. This difficulty also arises in AL-iLQR, which is employed in this study. The problem is particularly pronounced in collision avoidance problems, which impose the following position constraint.

$$‖{\mathbf{p}}_{\mathrm{o}\mathrm{b}\mathrm{s},i}\left(k\right)-\mathbf{p}\left(k\right)‖\ge r_{\mathrm{o}\mathrm{b}\mathrm{s},i}+r_{\mathrm{v}\mathrm{e}\mathrm{h}},$$

(50)

where ${\mathbf{p}}_{\mathrm{o}\mathrm{b}\mathrm{s},i}$ denotes the position of the $i$-th obstacle, $k$ is the discrete time index, and the collision radii define spherical approximations of the obstacle and the vehicle geometry. While such algorithms can reliably find feasible trajectories in simple scenarios (e.g., a single obstacle), they often converge to infeasible local minima in complex environments with multiple obstacles. This behavior is a direct consequence of their gradient-based nature.

Figure 3a illustrates this failure mode using a two-dimensional example. The color-graded curve extending toward the target represents the predictive trajectory generated by AL-iLQR. The red crosses (×) denote infeasible states that violate the position constraints. This result indicates convergence to an infeasible local minimum.

In AL-iLQR, constraint multipliers are initialized with small values and gradually increased. Consequently, the algorithm initially solves a relaxed problem in which constraint violations have only a weak influence on the cost function. As a result, even when starting from a feasible initial guess, early iterations tend to prioritize minimizing the primary cost over satisfying constraints. Once the trajectory converges to an infeasible region, such as that shown in Figure 3a, subsequent increases in the multipliers are often insufficient to escape the infeasible solution.

Therefore, the solution we propose is to actively guide the predictive trajectory into the feasible region during the iterative optimization process. Our guidance mechanism is represented by the region $D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$ in Figure 3b. $\mathcal{J}$ constitutes the set of discrete time indices of constraint violations that are highly unlikely to be resolved by further AL-iLQR iterations.

To address this problem, we propose actively guiding the predictive trajectory into a feasible region during the iterative optimization process. This guidance mechanism is illustrated by the region $D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$ in Figure 3b. The set $\mathcal{J}$ consists of discrete time indices at which constraint violations are unlikely to be resolved by further AL-iLQR iterations alone.

To identify such indices, we first define the set of active constraints at each time step.

$$\mathcal{F}\left(k\right)=\left\{i|c_{i,k}\left({\mathbf{x}}_k\right)>0, \mathrm{where} i\in \left\{1,\dots ,N_{\mathrm{o}\mathrm{b}\mathrm{s}}\right\}\right\},$$

(51)

where

$$c_{i,k}\left({\mathbf{x}}_k\right)={\left(r_{\mathrm{o}\mathrm{b}\mathrm{s},i}+r_{\mathrm{v}\mathrm{e}\mathrm{h}}\right)}^2-{‖{\mathbf{p}}_{\mathrm{o}\mathrm{b}\mathrm{s},i}\left(k\right)-\mathbf{p}\left(k\right)‖}^2.$$

(52)

Here, $N_{\mathrm{o}\mathrm{b}\mathrm{s}}$ denotes the number of obstacles. If $\left|\mathcal{F}\left(k\right)\right|=0$, the trajectory is feasible at time $k$; if $\left|\mathcal{F}\left(k\right)\right|=1$, feasibility can often be recovered through iterative optimization. On the other hand, when only one geometric constraint is active, AL-iLQR can successfully find a solution by continuously deforming the trajectory using gradient information, assuming spherical obstacles. In contrast, when two or more constraints are active, the spatial arrangement of the obstacles can cause the gradient vectors to cancel each other out in the augmented Lagrangian method. Consequently, there is no guarantee that the gradient direction calculated by AL-iLQR will coincide with the correct direction toward the feasible region. Accordingly, the index set $\mathcal{J}$ is defined as

$$\mathcal{J}=\left\{k|2\le \left|\mathcal{F}\left(k\right)\right|\right\}.$$

(53)

4.2. How to Find $D_{Guide}\left(\mathcal{J}\right)$

The challenge of determining $D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$ extends beyond identifying any feasible solution; it lies in simultaneously satisfying constraints while optimizing the mission cost. The objective is not to compute a single optimal trajectory, but rather to estimate a region in which optimal solutions are likely to be distributed. To this end, we employ a sampling-based solver, Model Predictive Path Integral control (MPPI), which is well suited for rapidly approximating the distribution of optimal solutions.

The MPPI cost function is defined as

$$J_{\mathrm{m}\mathrm{p}\mathrm{p}\mathrm{i}}=J_{\mathrm{v}\mathrm{a}\mathrm{r}}+J_{\mathrm{c}\mathrm{o}\mathrm{n}},$$

(54)

$$J_{\mathrm{c}\mathrm{o}\mathrm{n}}=\sum _{k=0}^N\sum _{i=1}^{N_{\mathrm{o}\mathrm{b}\mathrm{s}}}{c}_{i,k},$$

(55)

$$c_{i,k}=\left\{\begin{array}{l}{\rho }_{\mathrm{m}\mathrm{p},k}\in {\mathbb{R}}_{+} \left(r_{\mathrm{o}\mathrm{b}\mathrm{s},i}+r_{\mathrm{v}\mathrm{e}\mathrm{h}}\ge ‖{\mathbf{p}}_{\mathrm{o}\mathrm{b}\mathrm{s},i}\left(k\right)-\mathbf{p}\left(k\right)‖\right)\\ 0 \mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e},\end{array}\right..$$

(56)

Here, ${\rho }_{\mathrm{m}\mathrm{p},k}$ is a constant penalty parameter. MPPI enforces input constraints by clamping sampled inputs that fall outside the admissible range. Assuming the sampled input $V$ follows a multivariate Gaussian distribution centered at the mean input $U$ with covariance $\Sigma$, the sampling distribution is given by

$$\mathbf{V}~q\left(\mathbf{V}|\mathbf{U},\mathbf{\Sigma }\right)=\prod _{k=0}^N\mathcal{N}\left(\mathbf{U},\mathbf{\Sigma }\right).$$

(57)

The optimal control sequence is obtained by solving

$${\mathbf{U}}^{\ast }=\underset{\mathbf{U}}{\mathrm{argmin}} {\mathbb{E}}_{{\mathbb{Q}}_{\mathbf{U},\mathbf{\Sigma }}}\left[J_{\mathrm{m}\mathrm{p}\mathrm{p}\mathrm{i}}\right],$$

(58)

where ${\mathbb{Q}}_{\mathbf{U},\mathbf{\Sigma }}$ is the distribution that is described by $q\left(\mathbf{V}|\mathbf{U},\mathbf{\Sigma }\right)$. According to the variational inference theory, minimizing KL-divergence yields the optimal solution. Under the Law of Large Numbers, the sample-based approximation used in MPPI converges to the true expected value as the number of samples increases. Note, the law does not guarantee convergence to the global optimum. Algorithm 3 shows the MPPI algorithm [49].

$${\mathbf{U}}^{\ast }=\underset{\mathbf{U}}{\mathrm{argmin}} {\mathbb{D}}_{\mathrm{K}\mathrm{L}}\left[{\mathbb{Q}}^{\ast }\left|\right|{\mathbb{Q}}_{\mathbf{U},\mathbf{\Sigma }}\right]$$

(59)

The optimal state sequence ${\mathbf{X}}^{\ast }$ is then obtained from ${\mathbf{U}}^{\ast }$, and the guidance region $D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$ is determined such that it contains ${\mathbf{x}}_{\mathcal{J}}^{\ast }$.

Algorithm 3: Model Predictive Path Integration (MPPI)
	Inputs:
	$\mathrm{Initial} \mathrm{guess} \mathbf{U}$
	$\mathrm{Boundary} \mathrm{condition} {\mathbf{x}}_0$
	Outputs:
	$\mathrm{Optimal} \mathrm{state} \mathbf{X}$$, \mathrm{input} \mathbf{U}$
	Procedure:
1:	$\mathbf{for} n=1,\dots ,K$  // Sampling number
2:	$\mathrm{Sampling} {\mathit{\epsilon }}^n~\mathcal{N}\left(0,\Sigma \right)$
3:	${\mathbf{x}}_0^n\leftarrow {\mathbf{x}}_0$
4:	$S^n\leftarrow 0$
5:	$\mathbf{for} k=0,\dots ,N-1$
6:	${\mathbf{v}}_k^n\leftarrow {\mathbf{u}}_k+{\mathit{\epsilon }}_k^n$
7:	$\mathrm{Saturation} \mathrm{check} {\mathbf{v}}_k^n$
8:	$S^n\leftarrow S^n+\lambda {\mathbf{u}}_k{\Sigma }^{-1}{\mathsf{\epsilon }}_k^n$
9:	${\mathbf{x}}_{k+1}^n={\mathbf{f}}_k\left({\mathbf{x}}_k^n,{\mathbf{v}}_k^n,k\right)$
10:	end for
11:	$S^n\leftarrow J_{\mathrm{m}\mathrm{p}\mathrm{p}\mathrm{i}}\left({\mathbf{X}}^n,{\mathbf{V}}^n\right)+S^n$
12:	end for
13:	${\rho }_{\mathrm{m}\mathrm{p}}\leftarrow \underset{n}{\min }{S}^n$
14:	${\eta }_{\mathrm{m}\mathrm{p}}\leftarrow {\sum }_{n=1}^K\left(\exp \left\{-{\displaystyle \frac{1}{\lambda }}\left(S^n-{\rho }_{\mathrm{m}\mathrm{p}}\right)\right\}\right)$
15:	$w^n\leftarrow {\displaystyle \frac{1}{{\eta }_{\mathrm{m}\mathrm{p}}}}\left(\exp \left\{-{\displaystyle \frac{1}{\lambda }}\left(S^n-{\rho }_{\mathrm{m}\mathrm{p}}\right)\right\}\right)$
16:	$\mathbf{for} k=N,\dots , 0$
17:	${\mathbf{u}}_k\leftarrow {\sum }_{n=1}^K\left(w^n{\mathbf{v}}_k^n\right)$
18:	${\mathbf{x}}_{k+1}={\mathbf{f}}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k,k\right)$
19:	end for
20:	$\mathbf{return} \mathbf{X},\mathbf{U}$

4.3. Integration of $D_{Guide}\left(\mathcal{J}\right)$ into AL-iLQR

Based on the sampled inputs generated by MPPI, we obtain an estimate of the optimal distribution ${\mathbb{Q}}^{\ast }$. The guidance region $D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$ is then integrated into the AL-iLQR framework. The key observation is that AL-iLQR converges efficiently to an optimal solution as long as the trajectory remains within the feasible region. Therefore, it is sufficient to guide the trajectory toward feasibility rather than explicitly solving the global constrained problem. The guidance region is defined as

$$D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)=\bigcup _{k\in \mathcal{J}}{\mathcal{K}}_k,$$

(60)

where

$${\mathcal{K}}_k=\left\{\mathbf{p}\left(k\right)|f_k\left({\mathbf{x}}_k,k\right)\le 0\right\},$$

(61)

$$f_k\left({\mathbf{x}}_k,k\right)={‖\mathbf{p}\left(k\right)-{\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(k\right)‖}^2-r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}^2\left(k\right),$$

(62)

Here, ${\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(k\right)$ corresponds to the MPPI-derived reference position ${\mathbf{p}}^{\ast }\left(k\right)$. The guidance radius $r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(k\right)$, selected to be smaller than the obstacle geometry, is set equal to the collision radius of the controlled vehicle.

Since ${\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(k\right)$ may vary significantly between AL-iLQR iterations—especially in multimodal solution spaces—reusing multipliers from previous iterations can hinder convergence. Therefore, we redefine the augmented Lagrangian as

$${\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},\mathbf{L}\right)=J_{\mathrm{v}\mathrm{a}\mathrm{r}}+J_{\mathrm{c}\mathrm{o}\mathrm{n}}+J_{\mathrm{g}\mathrm{c}\mathrm{o}},$$

(63)

$$J_{\mathrm{g}\mathrm{c}\mathrm{o}}=\sum _{k\in \mathcal{J}}\left({\displaystyle \frac{1}{2}}{\rho }_{\mu ,k}{f}_k^2\right),$$

(64)

$${\rho }_{\mu ,k}=\left\{\begin{array}{ll}{\mu }_{\mathrm{g}\mathrm{c}\mathrm{o},k}\in \mathbb{R}& \mathrm{if} f_k>0\\ 0& \mathrm{otherwise}.\end{array}\right.$$

(65)

The penalty parameter ${\mu }_{\mathrm{g}\mathrm{c}\mathrm{o},k}$ is updated according to

$${\mu }_{\mathrm{g}\mathrm{c}\mathrm{o},k}=\nu {\mu }_{\mathrm{g}\mathrm{c}\mathrm{o},k},$$

(66)

where $\nu$ is chosen such that $\eta <\nu$. Satisfying the guidance constraint in Equation (62) becomes equivalent to satisfying the original collision constraint in Equation (52) in later iterations, once the AL-iLQR multipliers have sufficiently increased. If $\mathcal{J}=\mathrm{\varnothing }$, the guidance constraint is omitted.

When GCO is activated, the guidance constraint is penalized more aggressively than the original constraints, thereby prioritizing feasibility restoration. If the solution later drifts back into infeasible regions, the guidance mechanism is reactivated. Once convergence within the feasible domain is imminent, the guidance constraint is permanently deactivated.

Unlike standard augmented Lagrangian (AL) constraints, the proposed guidance constraint is incorporated directly into the cost function as a penalty term rather than through Lagrange multipliers. From a numerical standpoint, it is treated as an inequality constraint whose associated multiplier is intentionally fixed to zero.

In the proposed scheme, the guidance mechanism $D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$ is integrated into the AL-iLQR optimizer. While the original collision avoidance constraints in Equation (52) are handled using the full AL formulation with Lagrange multipliers $\lambda$, the guidance constraint in Equation (62) is excluded from the AL framework and instead added directly to the AL-iLQR cost function as a simple penalty term. The guidance constraint is introduced as a temporary penalty term to assist the optimization process when the solver encounters a solution that may yield an infeasible local minima during the AL-iLQR iterations. Importantly, the guidance constraint is designed to remain inactive in the vicinity of feasible solutions. Since the optimal trajectory must satisfy the original geometric constraints, the guidance constraint naturally becomes inactive near the final solution. Therefore, the final solution satisfies the Karush–Kuhn–Tucker (KKT) conditions associated with the original optimization problem. In addition, the margin parameter $r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$ ensures that the feasible region around the optimal trajectory remains inside the region where the guidance constraint is inactive.

The proposed framework involves several parameters, such as the guidance margin $r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$, the sampling distribution parameters in MPPI, and the penalty weights used in the augmented Lagrangian formulation. The guidance margin mainly determines the activation region of the guidance mechanism, while the AL-iLQR refinement step ensures that the final trajectory satisfies the original geometric constraints. As a result, the final solution is not highly sensitive to the exact value of this parameter. Similarly, the MPPI sampling parameters primarily influence the exploration behavior during the guidance phase, but once a feasible trajectory is identified, the optimization process proceeds using the standard AL-iLQR framework. The overall guidance algorithm is summarized in Algorithms 4 and 5.

Algorithm 4: Modified AL-iLQR (Our proposed method)
	Inputs:
	$\mathrm{Initial} \mathrm{guess} \mathbf{U}$
	$\mathrm{Boundary} \mathrm{conditions} {\mathbf{x}}_0$
	Outputs:
	$\mathrm{Optimal} \mathrm{state} \mathbf{X}$$, \mathrm{input} \mathbf{U}$$\mathrm{and} \mathrm{multiplier} \mathbf{L}$
	Procedure:
1:	$\mathrm{Predict} \mathbf{X}$$\mathrm{using} \mathbf{U}$$\mathrm{and} {\mathbf{x}}_0$
2:	$\mathrm{Initialize} \mathbf{L}$$, \mathbf{M}$
3:	${\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\leftarrow \mathbf{p}$$// \mathbf{p}\in \mathbf{X}$
4:	$r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\leftarrow r_{\mathrm{F}}$  $\mathrm{where} ‖{\mathbf{p}}_0-{\mathbf{p}}_{\mathrm{G}}‖\ll r_{\mathrm{F}}=\mathrm{const}.$
5:	while
6:	$\mathbf{X},\mathbf{U}\leftarrow \underset{\mathbf{X},\mathbf{U}}{\mathrm{argmin}} {\mathcal{L}}_{\mathbf{M}}\left(\mathbf{X},\mathbf{U},{\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}},r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}},useGCO\right)$ using iLQR
7:	if the convergence conditions are satisfied
8:	Break
9:	end if
10:	${\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$$, r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$$, useGCO\leftarrow \mathrm{CPUF}\left(\mathbf{X},\mathbf{U}\right)$$// \mathrm{Calculating} \mathrm{the} \mathrm{guidance} D_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(\mathcal{J}\right)$
11:	$\mathrm{Update} \mathbf{L}$$, \mathbf{M}$ using Equations (21), (22) and (66)
12:	${\lambda }_{\mathrm{g}\mathrm{c}\mathrm{o}}\leftarrow 0$$// \mathrm{The} \mathrm{multiplier} {\lambda }_{\mathrm{g}\mathrm{c}\mathrm{o}}$ corresponds to Equation (62)
13:	end while
14:	$\mathbf{return} \mathbf{X},\mathbf{U},\mathbf{L}$

Algorithm 5: Constraint Parameters Update for Feasibility (CPUF)
	Inputs:
	$\mathrm{State} \mathbf{X}$$, \mathrm{input} \mathbf{U}$
	Outputs:
	$\mathrm{Constraints} \mathrm{Parameters} {\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$$, r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$$, useGCO$
	Procedure:
1:	${\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\leftarrow \mathbf{p}$$// \mathbf{p}\in \mathbf{X}$
2:	$r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\leftarrow r_{\mathrm{F}}\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{e}‖{\mathbf{p}}_0-{\mathbf{p}}_{\mathrm{G}}‖\ll r_{\mathrm{F}}=\mathrm{c}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}.$
3:	$useGCO\leftarrow \mathrm{f}\mathrm{a}\mathrm{l}\mathrm{s}\mathrm{e}$
4:	$\mathbf{if} \mathcal{J}\ne \mathrm{\varnothing }$
5:	${\mathbf{X}}_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}},{\mathbf{U}}_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\leftarrow \mathrm{M}\mathrm{P}\mathrm{P}\mathrm{I}\left(\mathbf{U},{\mathbf{x}}_0\right)$$// {\mathbf{x}}_0\in \mathbf{X}$
6:	$\mathbf{for} k=0,\dots ,N$
7:	$\mathbf{if} \left|\mathcal{F}\left(k\right)\right|\ge 2$
8:	${\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(k\right)\leftarrow \mathbf{p}\left(k\right)$$// \mathbf{p}\left(k\right)\in {\mathbf{X}}_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}$
9:	$r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}\left(k\right)\leftarrow r_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(=r_{\mathrm{v}\mathrm{e}\mathrm{h}}\right)$
10:	end if
11:	end for
12:	$useGCO\leftarrow \mathrm{t}\mathrm{r}\mathrm{u}\mathrm{e}$
13:	end if
14:	$\mathbf{return} {\mathbf{p}}_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$$, r_{\mathrm{G}\mathrm{u}\mathrm{i}\mathrm{d}\mathrm{e}}$$, useGCO$

5. Simulation Results and Discussion

The effectiveness of the proposed method is verified through numerical simulations. As a preliminary validation, the proposed approach is first applied to a two-dimensional (2D) dynamical system. This simplified setting allows the trajectory optimization behavior to be evaluated on a planar domain while decoupling the complex three-dimensional dynamics associated with UAVs. In particular, the 2D simulation facilitates visualization of the optimization process and clarifies how the solution is deformed during iterative updates when the guidance constraint optimization (GCO) is activated (see Appendix A).

Subsequently, quadrotor simulations are conducted to confirm that the generated trajectories are feasible for path tracking control and result in successful collision avoidance. The quadrotor trajectory optimization problem is formulated as

$$J_{\mathrm{v}\mathrm{a}\mathrm{r}}\left(\mathbf{X},\mathbf{U}\right)=L_N\left({\mathbf{x}}_N\right)+\sum _{k=0}^{N-1}{L}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right),$$

(67)

$$L_N\left({\mathbf{x}}_N\right)={\displaystyle \frac{1}{2}}{\left({\mathbf{x}}_N-{\mathbf{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},N}\right)}^{\mathrm{T}}\mathbf{S}\left({\mathbf{x}}_N-{\mathbf{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},N}\right),$$

(68)

$$L_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)={\displaystyle \frac{1}{2}}{\left({\mathbf{x}}_k-{\mathbf{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},k}\right)}^{\mathrm{T}}\mathbf{Q}\left({\mathbf{x}}_k-{\mathbf{x}}_{\mathrm{r}\mathrm{e}\mathrm{f},k}\right)+{\displaystyle \frac{1}{2}}{\left({\mathbf{u}}_k-{\mathbf{u}}_{\mathrm{r}\mathrm{e}\mathrm{f},k}\right)}^{\mathrm{T}}\mathbf{R}\left({\mathbf{u}}_k-{\mathbf{u}}_{\mathrm{r}\mathrm{e}\mathrm{f},k}\right),$$

(69)

where $\mathbf{S},\mathbf{Q},$ and $\mathbf{R}$ are positive semidefinite weight matrices. To reduce computational cost, the terminal constraints do not include GCO.

In the ALM implementation, a maximum iteration limit is employed instead of checking the KKT conditions in order to simplify the optimization procedure. All simulations were conducted on a laptop equipped with an Intel Core i7-12650H (2.30 GHz) processor, 16 GB RAM, using MATLAB R2024b.

5.1. 2D Trajectory Optimization

This 2D trajectory optimization problem is introduced to provide an intuitive and computationally lightweight validation of the proposed guidance mechanism. By restricting the system dynamics to a planar model, we decouple the core trajectory optimization behavior from the additional complexities introduced by full 3D quadrotor dynamics, such as attitude coupling and thrust vectoring. The purpose of this simplified model is not to replicate realistic UAV dynamics, but to clearly visualize how the proposed guidance constraint (GCO) influences the deformation of trajectories during the iterative optimization process. In particular, the 2D setting allows us to isolate and analyze failure modes related to non-convex geometric constraints and local infeasible minima, which are also present in higher-dimensional systems. Importantly, the same optimization framework, constraint formulation, and guidance logic are retained when extending to the full quadrotor model in Section 5.2. Therefore, the 2D simulation serves as a conceptual validation of the proposed method, while the quadrotor simulation demonstrates its practical applicability under realistic dynamics.

The simulation parameters are summarized in

Table A1. Problem setup for the numerical simulation.

Prediction method	Euler method
$\mathrm{Prediction} \mathrm{period} \Delta t \left[\mathrm{s}\right]$	$0.01$
$\mathrm{The} \mathrm{number} \mathrm{of} \mathrm{prediction} N$	$1000$
$\mathrm{Weight} \mathrm{matrix} \mathbf{Q}$	$\mathrm{diag}\left(5, 5, 1\times {10}^{-8}, 1, 1, 1\times {10}^{-8}\right)$
$\mathrm{Weight} \mathrm{matrix} \mathbf{R}$	$\mathrm{diag}\left(0.1, 0.1\right)$
$\mathrm{Weight} \mathrm{matrix} \mathbf{S}$	$\mathbf{Q}$
Target state ${\mathbf{x}}_{\mathrm{G}}={\left({\mathbf{p}}_{\mathrm{G}}^{\mathrm{T}},{\dot{\mathbf{p}}}_{\mathrm{G}}^{\mathrm{T}},{\mathbf{q}}_{\mathrm{G}}^{\mathrm{T}},{\mathsf{\omega }}_{\mathrm{G}}^{\mathrm{T}},a_{\mathrm{G}}\right)}^{\mathrm{T}}$	${\left(1, 0.4, 0, 0, 0, 0\right)}^{\mathrm{T}}$
Target input ${\mathbf{u}}_{\mathrm{G}}={\left({\mathsf{\omega }}_{\mathrm{G}}^{\mathrm{T}},a_{\mathrm{G}}\right)}^{\mathrm{T}}$	${\left(0, 0\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{guess} {\mathbf{u}}_k$	${\left(0, 0\right)}^{\mathrm{T}}$

,

Table A2. Parameters of the controlled system.

$k_1 \left[1/\mathrm{s}\right]$	$10.0$
$k_2 \left[\text{-}\right]$	$1.00$
$k_3 \left[1/\mathrm{s}\right]$	$0.001$
$k_4 \left[1/\mathrm{m}\right]$	$0.10$
${\mathbf{u}}_{\mathrm{m}\mathrm{a}\mathrm{x}} \left[{\mathrm{m}/\mathrm{s}}^2\right]$	${\left(10, 10\right)}^{\mathrm{T}}$
${\mathbf{u}}_{\mathrm{m}\mathrm{i}\mathrm{n}} \left[{\mathrm{m}/\mathrm{s}}^2\right]$	${\left(0, 0\right)}^{\mathrm{T}}$

and

Table A3. MPPI and AL-iLQR configurations for 2D simulation.

$\mathrm{Tolerance} \mathrm{of} \left|J_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}-J_{\mathrm{p}\mathrm{r}\mathrm{e}}\right|$ in iLQR	$1.0\times {10}^{-18}$
iLQR max iteration	$100$
ALM max iteration	$20$
$\mathrm{Initial} \mathrm{multiplier} {\lambda }_{i,k} \left(i\in \mathcal{G}\cup \mathcal{H}\right)$	$0$
$\mathrm{Multiplier} \mathrm{update} \mathrm{gain} \eta$	$2.2$
$\mathrm{Initial} \mathrm{penalty} {\mu }_{i,k}$	$20$
$\mathrm{Multiplier} \mathrm{update} \mathrm{gain} \mathrm{for} \mathrm{GCO} \nu$	${\eta }^2$
$\mathrm{Initial} \mathrm{penalty} \mathrm{for} \mathrm{GCO} {\mu }_{\mathrm{g}\mathrm{c}\mathrm{o},k}$	$20$
${\rho }_{\mathrm{m}\mathrm{p}}=\left({\rho }_{\mathrm{m}\mathrm{p},1},{\rho }_{\mathrm{m}\mathrm{p},2},\dots ,{\rho }_{\mathrm{m}\mathrm{p},N}\right)$	$\left(1\times {10}^8,\dots ,1\times {10}^8\right)$
$\mathrm{The} \mathrm{number} \mathrm{of} \mathrm{samples} K_{\mathrm{m}\mathrm{p}}$	$500$
$\mathrm{Covariance} \mathrm{matrix} \Sigma$	$\mathrm{diag}\left(100, 100\right)$
$\mathrm{Temperature} \lambda$	$1$
Saturated range in sampling	$\left[-{\mathbf{u}}_{\mathrm{m}\mathrm{a}\mathrm{x}}, {\mathbf{u}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\right]$

, and the positional configuration is identical to that shown in Figure 3. The results obtained using the proposed method under box input constraints and inequality constraints in Equation (52) are presented in Figure 4, Figure 5, Figure A1a and Figure A2a and

Table A4. Numerical optimization results.

	$\mathbf{Cost} {\mathit{J}}_{\mathbf{var}}$	Maximum Constraints Violation
(a) Trial 1	$1364.08$	$1.21\times {10}^{-4}$
(b) Trial 2	$1655.35$	$2.26\times {10}^{-2}$

(a). Although standard MPPI is often regarded as ill-suited for multimodal problems due to its unimodal Gaussian sampling assumption, the numerical results demonstrate that a feasible trajectory can still be obtained while preserving near-optimality. Notably, the SQP solver implemented in MATLAB’s fmincon failed to solve the same problem within several hours.

Figure 4 shows the optimal predicted trajectory as a gradient-colored curve. The resulting trajectory appears non-smooth and seems to converge to a local minimum. Indeed, due to the stochastic nature of MPPI sampling, multiple distinct local solutions were obtained across different trials, as illustrated in Figure A1b, Figure A2b and Figure A3, and Table A4(b). It was shown that the proposed method does not necessarily yield a globally optimal solution. In the MPPI sampling process, soft input constraints ($-{\mathbf{u}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\le \mathbf{u}\le {\mathbf{u}}_{\mathrm{m}\mathrm{a}\mathrm{x}}$) are employed to enable broader exploration of the positional space, without affecting the satisfaction of input constraints in the final optimized solution.

Figure 5 illustrates the evolution of the trajectory throughout the iterative optimization process. In the initial iteration, the standard AL-iLQR yields an infeasible solution. Upon updating the ALM multipliers and penalties, the solver detects the violation set $\mathcal{J}$, which triggers the generation of GCO. In the early stages, the solution remains trapped in the infeasible region because the priority difference between GCO and the original constraints is insufficient. As iterations proceed, GCO is assigned higher priority, allowing the solver to escape the infeasible region and converge to a feasible trajectory. Once the multipliers and penalties associated with the positional constraints become sufficiently large, GCO is deactivated to avoid interfering with the primary cost optimization. If the solution subsequently regresses into an infeasible region, GCO is reactivated to restore feasibility.

5.2. Quadrotor Simulation

5.2.1. Simulation Configuration

Numerical simulations are conducted in an environment designed to induce infeasible trajectories, using a framework intended for practical deployment on real hardware, shown in Figure 1. The quadrotor task configuration is summarized in

Table 1. Problem setup of the quadrotor trajectory optimization.

Prediction method	Euler method
$\mathrm{Gravity} g \left[{\mathrm{m}/\mathrm{s}}^2\right]$	$9.81$
$\mathrm{The} \mathrm{number} \mathrm{of} \mathrm{prediction} N$	$100$
$\mathrm{Weight} \mathrm{matrix} \mathbf{S}$	$\mathrm{diag}\left(1, 1, 10, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1\right)$
$\mathrm{Weight} \mathrm{matrix} \mathbf{Q}$	$\mathrm{diag}\left(1, 1, 100, 1, 1, 1, 1, 1, 1, 1, 10, 10, 10, 10\right)$
$\mathrm{Weight} \mathrm{matrix} \mathbf{R}$	$\mathrm{diag}\left(10, 10, 10, 1\right)$
$\mathrm{Target} \mathrm{state} {\mathbf{x}}_{\mathrm{G}}={\left({\mathbf{p}}_{\mathrm{G}}^{\mathrm{T}},{\dot{\mathbf{p}}}_{\mathrm{G}}^{\mathrm{T}},{\mathbf{q}}_{\mathrm{G}}^{\mathrm{T}},{\mathsf{\omega }}_{\mathrm{G}}^{\mathrm{T}},a_{\mathrm{G}}\right)}^{\mathrm{T}}$	${\left(20, 1, 0.2, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, g\right)}^{\mathrm{T}}$
Target input ${\mathbf{u}}_{\mathrm{G}}={\left({\mathsf{\omega }}_{\mathrm{G}}^{\mathrm{T}},a_{\mathrm{G}}\right)}^{\mathrm{T}}$	${\left(0, 0, 0, g\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{guess} {\mathbf{u}}_k$	${\left(0, 0, 0, g\right)}^{\mathrm{T}}$

, and the quadrotor parameters are adopted from [52], as listed in

Table 2. Model parameters of the quadrotor.

$\mathrm{Mass} \mathrm{of} \mathrm{quadrotor} m \left[\mathrm{kg}\right]$	$1.608$
$\mathrm{Inertia} \mathbf{J} \left[\mathrm{kg}·{\mathrm{m}}^2\right]$	$\left(\begin{array}{ccc}0.0366& 0& 0.0065\\ 0& 0.0327& 0.0046\\ 0.0065& 0.0046& 0.0187\end{array}\right)$
$\mathrm{Length} \mathrm{of} \mathrm{arms} l \left[\mathrm{m}\right]$	$0.216$
$\mathrm{Coefficient} \mathrm{of} \mathrm{rotors} k_r \left[\mathrm{m}\right]$	$1.6\times {10}^{-2}$
$\mathrm{Coefficient} \mathrm{of} \mathrm{air} \mathrm{resistance} k_a \left[\mathrm{N}·\mathrm{m}·\mathrm{s}/\mathrm{rad}\right]$	$1.0\times {10}^{-2}$
$\mathrm{Time} \mathrm{constant} T_r \left[\mathrm{s}\right]$	$0.10$
$\mathrm{Coefficient} b \left[{\mathrm{N}/\mathrm{rpm}}^2\right]$	${\displaystyle \frac{mg}{{5000}^2}}$
$f_{\mathrm{m}\mathrm{a}\mathrm{x}} \left[\mathrm{N}\right]$	$6$
$f_{\mathrm{m}\mathrm{i}\mathrm{n}} \left[\mathrm{N}\right]$	$0.1046$
${\mathsf{\omega }}_{\mathrm{m}\mathrm{a}\mathrm{x}} \left[\mathrm{rad}/\mathrm{s}\right]$	${\left(5, 5, 5\right)}^{\mathrm{T}}$
${\mathsf{\omega }}_{\mathrm{m}\mathrm{i}\mathrm{n}} \left[\mathrm{rad}/\mathrm{s}\right]$	${\left(-5,-5,-5\right)}^{\mathrm{T}}$
$a_{\mathrm{m}\mathrm{a}\mathrm{x}} \left[{\mathrm{m}/\mathrm{s}}^2\right]$	$4f_{\mathrm{m}\mathrm{a}\mathrm{x}}/m$
$a_{\mathrm{m}\mathrm{i}\mathrm{n}} \left[{\mathrm{m}/\mathrm{s}}^2\right]$	$4f_{\mathrm{m}\mathrm{i}\mathrm{n}}/m$
$\mathrm{Time} \mathrm{constant} T_{\omega } \left[\mathrm{s}\right]$	$1.5$
$\mathrm{Time} \mathrm{constant} T_a \left[\mathrm{s}\right]$	$1.5$

.

To reduce computational complexity, the cost function is evaluated by directly subtracting quaternion values, neglecting quaternion error dynamics. In addition, the following constraint is imposed during path planning to ensure consistency between the path planning and path tracking models:

$$\mathbf{0}=\mathsf{\omega }\left(t_1\right)-\left\{\mathsf{\omega }\left(t_0\right)+{\mathbf{J}}^{-1}\left(\mathbf{T}\mathbf{F}\left(t_0\right)-\mathsf{\omega }\left(t_0\right)\times \mathbf{J}\mathsf{\omega }\left(t_0\right)-\mathbf{D}\mathsf{\omega }\left(t_0\right)\right)T_s\right\},$$

(70)

where $T_s$ denotes the control period, and $t_0$ and $t_1$ represent consecutive time steps. For optimization, the collision detection radius is set to $r$, while a reduced boundary of $0.95r$ is used in numerical simulations to incorporate a safety margin (

Table 3. Environments used in the numerical simulations.

Simulation method	Runge–Kutta method
$\mathrm{Simulation} \mathrm{time} \mathrm{period} \Delta t \left[\mathrm{s}\right]$	$0.01$
$\mathrm{Initial} \mathrm{position} {\mathbf{p}}_0 \left[\mathrm{m}\right]$	${\left(1, 1, 0\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{velocity} {\dot{\mathbf{p}}}_0 \left[\mathrm{m}/\mathrm{s}\right]$	${\left(0, 0, 0\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{orientation} {\mathbf{q}}_0 \left[\text{-}\right]$	${\left(1, 0, 0, 0\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{orientation} {\mathsf{\omega }}_0 \left[\mathrm{rad}/\mathrm{s}\right]$	${\left(0, 0, 0\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{thruster} {\mathbf{F}}_0 \left[\mathrm{N}\right]$	${\left({\displaystyle \frac{mg}{4}},{\displaystyle \frac{mg}{4}},{\displaystyle \frac{mg}{4}},{\displaystyle \frac{mg}{4}}\right)}^{\mathrm{T}}$
$\mathrm{Control} \mathrm{frequency} 1/T_s \left[\mathrm{Hz}\right]$	$20$
$\mathrm{Prediction} \mathrm{period} T_f \left[\mathrm{s}\right]$	$5$
$\mathrm{Radius} \mathrm{of} \mathrm{quadrotor} r_{\mathrm{v}\mathrm{e}\mathrm{h}} \left[\mathrm{m}\right]$	$0.36$
$\mathrm{Collision} \mathrm{detection} \mathrm{radius} \mathrm{of} \mathrm{quadrotor} {0.95r}_{\mathrm{v}\mathrm{e}\mathrm{h}} \left[\mathrm{m}\right]$	$0.34$
$\mathrm{Radius} \mathrm{of} \mathrm{obstacle} r_{\mathrm{o}\mathrm{b}\mathrm{s},i} \left[\mathrm{m}\right]$	$0.60$
$\mathrm{Collision} \mathrm{detection} \mathrm{radius} \mathrm{of} \mathrm{obstacle} 0.95r_{\mathrm{o}\mathrm{b}\mathrm{s},i} \left[\mathrm{m}\right]$	$0.57$

). The MPPI and AL-iLQR parameters used for path planning are listed in

Table 4. MPPI and AL-iLQR configurations for path planning.

$\mathrm{Tolerance} \mathrm{of} \left|J_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}-J_{\mathrm{p}\mathrm{r}\mathrm{e}}\right|$ in iLQR	$0.10$
iLQR max iteration	$50$
ALM max iteration	$10$
$\mathrm{Initial} \mathrm{multiplier} {\lambda }_{i,k} \left(i\in \mathcal{G}\cup \mathcal{H}\right)$	$0$
$\mathrm{Multiplier} \mathrm{update} \mathrm{gain} \eta$	$2.2$
$\mathrm{Initial} \mathrm{penalty} {\mu }_{i,k}$	$20$
$\mathrm{Multiplier} \mathrm{update} \mathrm{gain} \mathrm{for} \mathrm{GCO} \nu$	${\eta }^2$
$\mathrm{Initial} \mathrm{penalty} \mathrm{for} \mathrm{GCO} {\mu }_{\mathrm{g}\mathrm{c}\mathrm{o},k}$	$20$
${\rho }_{\mathrm{m}\mathrm{p}}=\left({\rho }_{\mathrm{m}\mathrm{p},1},{\rho }_{\mathrm{m}\mathrm{p},2},\dots ,{\rho }_{\mathrm{m}\mathrm{p},N}\right)$	$\left(1\times {10}^8,\dots ,1\times {10}^8\right)$
$\mathrm{The} \mathrm{number} \mathrm{of} \mathrm{samples} K_{\mathrm{m}\mathrm{p}}$	$100$
$\mathrm{Covariance} \mathrm{matrix} \Sigma$	$\mathrm{diag}\left(1, 1, 1, 1\right)$
$\mathrm{Temperature} \lambda$	$1$

. The penalty associated with GCO is increased more aggressively than the other penalties, satisfying $\eta <\nu$.

The tracking algorithm parameters are summarized in

Table 5. Parameter settings for AL-iLQR-based path tracking.

$\mathrm{The} \mathrm{number} \mathrm{of} \mathrm{prediction} N$	$6$
$\mathrm{Weight} \mathrm{matrix} \mathbf{S}$	$\mathrm{diag}\left(1, 1, 1, b^2, b^2, b^2, b^2\right)$
$\mathrm{Weight} \mathrm{matrix} \mathbf{Q}$	$\mathrm{diag}\left(1, 1, 1, b^2, b^2, b^2, b^2\right)$
$\mathrm{Weight} \mathrm{matrix} \mathbf{R}$	$\mathrm{diag}\left(b^2\times {10}^{-2}, b^2\times {10}^{-2}, b^2\times {10}^{-2}, b^2\times {10}^{-2}\right)$
$\mathrm{Constraints} \mathrm{parameter} a_{\mathrm{r}\mathrm{e}\mathrm{f}} \left[{\mathrm{m}/\mathrm{s}}^2\right]$	Using optimal states in path planning
$\mathrm{Reference} \mathrm{angular} \mathrm{velocity} {\mathsf{\omega }}_{\mathrm{r}\mathrm{e}\mathrm{f}} \left[\mathrm{rad}/\mathrm{s}\right]$	Using optimal states in path planning
$\mathrm{Reference} \mathrm{thrust} {\mathbf{F}}_{\mathrm{r}\mathrm{e}\mathrm{f}} \left[\mathrm{N}\right]$	${\left({\displaystyle \frac{mg}{4}},{\displaystyle \frac{mg}{4}},{\displaystyle \frac{mg}{4}},{\displaystyle \frac{mg}{4}}\right)}^{\mathrm{T}}$
$\mathrm{Initial} \mathrm{guess} {\mathbf{u}}_k \left[\mathrm{rpm}\right]$ for hovering	${\left(5000, 5000, 5000, 5000\right)}^{\mathrm{T}}$
$\mathrm{Tolerance} \mathrm{of} {\mathcal{L}}_{\mathbf{M}}$	$0.10$
iLQR max iteration	$50$
ALM max iteration	$10$
$\mathrm{Initial} \mathrm{multiplier} {\lambda }_{i,k} \left(i\in \mathcal{G}\cup \mathcal{H}\right)$	$0$
$\mathrm{Multiplier} \mathrm{update} \mathrm{gain} \eta$	$2.0$
$\mathrm{Initial} \mathrm{penalty} {\mu }_{i,k}$	$20$

. Standard AL-iLQR was employed to calculate the optimal control inputs for the quadrotor. Importantly, the computational cost of path tracking is negligible relative to that of path planning (Appendix B).

5.2.2. Numerical Simulation

While Section 5.1 examined optimization results at a single time instance, this section considers a continuous transport mission in which the proposed method solves the trajectory optimization problem at every simulation time step. The Lagrange multipliers are re-initialized at each step because changes in the environment and optimal trajectory render multipliers from the previous step ineffective. Although reusing multipliers could reduce computational cost, re-initialization is adopted to improve robustness. In particular, this setup allows us to test whether the method can still generate feasible trajectories when the initial guess is infeasible, which may occur in the presence of modeling errors or dynamic obstacles. Once a feasible trajectory is obtained, the multipliers from the previous step typically provide an accurate initial guess, and the guidance mechanism is rarely activated. In such situations, the solver can operate with the computational efficiency of the standard AL-iLQR framework.

Figure 6 shows the optimal predicted trajectories for path planning at the initial time step. Figure 6a depicts an infeasible trajectory generated by standard AL-iLQR, whereas Figure 6b shows that the proposed method successfully finds a feasible solution. The proposed method successfully found a feasible solution. Figure 7 illustrates the cost evolution during the iterative process corresponding to the result in Figure 6. The red lines indicate the timing of multiplier and penalty updates, and the intervals between these lines correspond to the iLQR iterations. In Figure 7a, the costs increase during the multiplier and penalty updates due to convergence to a local minimum. In Figure 7b, the thin blue regions between the red lines indicate periods of GCO activation. When a potential infeasible solution is detected, GCO is activated to initiate the guidance process. The trajectory effectively bypasses obstacles and is guided toward a feasible region as the multiplier and penalty values increase. Subsequently, GCO is deactivated, and constraint satisfaction improves iteratively. The narrow intervals between the red lines suggest that a local minimum is identified rapidly.

Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12 present the quadrotor control results obtained using the proposed method. Figure 8 compares the computational time per simulation step for (a) standard AL-iLQR and (b) the proposed method. The computational cost decreases as the number of active constraints is reduced. Figure 9 shows the minimum distance between the quadrotor and obstacles, where the red dotted line denotes the collision detection boundary. The results confirm collision-free operation. The black dotted line represents the boundary used during path planning optimization. Figure 10 shows snapshots of the optimized trajectories, demonstrating successful obstacle avoidance despite modeling simplifications in the path planning stage. These simplifications introduce transient fluctuations in the optimal solution, particularly during periods of rapid input variation; however, no infeasible trajectories are observed. The absence of MPPI trajectories in the snapshots indicates that GCO is inactive at those time steps. Figure 11 illustrates the control commands and resulting thruster states, while Figure 12 presents the time histories of state variables, including position and target position.

The numerical simulations demonstrate that the proposed method can reliably detect potentially infeasible solutions and actively guide trajectories toward feasible regions during optimization. Collision avoidance problems are effectively resolved through the proposed framework, and the method naturally accommodates dynamic obstacles owing to its simple and flexible constraint formulation.

6. Conclusions

This paper proposed a practical trajectory optimization framework for improving feasibility in constrained nonlinear optimal control problems for agile UAVs. The key challenge addressed in this study is the tendency of gradient-based trajectory optimization methods, such as AL-iLQR, to converge to infeasible local minima in the presence of highly non-convex geometric constraints arising from obstacle avoidance. To mitigate this problem, we introduced a plugin-style guidance mechanism that integrates Model Predictive Path Integral (MPPI) control into the AL-iLQR iteration process. MPPI is employed as a fast sampling-based method 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 enhances the robustness of trajectory optimization without requiring fundamental modifications to the underlying solver structure. The effectiveness of the proposed approach was demonstrated through numerical simulations involving both simplified two-dimensional dynamics and full quadrotor models. The results showed that the proposed method converges to feasible and dynamically consistent trajectories significantly more reliably than standard AL-iLQR, even in scenarios with multiple obstacles and complex constraint interactions. Furthermore, the proposed guidance mechanism can be activated and deactivated adaptively during the optimization process, enabling efficient real-time computation while preserving solution quality.

It should be emphasized that the proposed method does not aim to provide theoretical guarantees of global optimality. Instead, it is designed as a practical and computationally efficient strategy for improving feasibility and robustness in real-time UAV trajectory optimization. Future work includes theoretical analysis of the guidance mechanism, extension to more complex dynamic environments, and experimental validation on real UAV platforms.