A Hierarchical PSMC–LQR Control Framework for Accurate Quadrotor Trajectory Tracking

Abstract

Accurate trajectory tracking of quadrotor UAVs remains challenging due to highly nonlinear dynamics, model uncertainties, and time-varying external disturbances, which make it difficult to achieve both precise position tracking and stable attitude regulation under control constraints. To tackle these coupled problems, this paper develops a hierarchical control framework in which the outer-loop particle swarm optimization (PSO)-compensated model predictive controller (PSMC) adaptively mitigates prediction errors and enhances robustness, while the inner-loop enhanced linear quadratic regulator (LQR), augmented with gain scheduling and control-rate relaxation, accelerates attitude convergence and ensures smooth control actions under varying flight conditions. A Lyapunov-based stability analysis is conducted to ensure closed-loop convergence. Simulation results on a helical reference trajectory show that, compared with the conventional MPC–LQR baseline, the proposed framework reduces the mean tracking errors by more than 13.2%, 17.1%, and 28% in the x-, y-, and z-directions under calm conditions, and by more than 34%, 26.2%, and 46.8% under wind disturbances. These results prove that the proposed hierarchical PSMC–LQR framework achieves superior trajectory tracking accuracy, strong robustness, and high practical implement ability for quadrotor control applications.

1. Introduction

In recent decades, significant progress has been made in the development of quadrotor unmanned aerial vehicles (UAVs). Owing to their low cost, compact size, lightweight structure, high maneuverability, vertical take-off and landing (VTOL) capability, enhanced payload capacity, and ability to operate in harsh environments, UAVs have gained widespread popularity [1]. The quadrotor UAV is a typically underactuated and strongly coupled system with four inputs and six outputs, making it an underactuated system [2]. In the design of its control system, position and attitude dynamics are usually decoupled using a hierarchical control structure, where the outer loop is responsible for position control and the inner loop for attitude stabilization. However, during actual flight, a quadrotor is often subjected to various unknown disturbances, which pose significant challenges to controller design. External disturbances can cause instability or even divergence in the control system, potentially resulting in severe damage to the UAV. Therefore, improving the disturbance rejection capability of a quadrotor is crucial not only for achieving precise trajectory tracking but also for ensuring flight safety [3].

To address the trajectory tracking problem of quadrotor UAVs, several advanced control strategies have been developed, including proportional-integral-derivative (PID) control [4], robust control [5], sliding mode control (SMC) [6,7,8], and backstepping control (BC) [9,10]. However, some of these approaches lack robustness against external disturbances, while others achieve robustness at the expense of control performance. In addition, several robust control approaches have been developed to simultaneously address disturbances and uncertainties [11,12]. These works provide valuable insights into compensation mechanisms under uncertain dynamics. Recent improvements such as adaptive fractional-order sliding modes, disturbance-observer enhanced SMC, and finite-time convergence methods yield better robustness but still often suffer from chattering, limited convergence speed, or sensitivity to gain tuning [13,14]. Hybrid control strategies and learning-based flight control theories, such as neural networks [15] and fuzzy logic [16], have also been developed for quadrotor UAV control. These control methods offer flexibility and adaptability for quadrotor UAV control but suffer from several notable drawbacks. Neural network controllers typically lack formal stability guarantees, require large and representative training datasets, and involve complex tuning of hyperparameters, making them difficult to generalize and validate. Fuzzy logic controllers, on the other hand, heavily rely on expert-defined rules and membership functions, lack rigorous stability analysis, and often exhibit limited tracking precision and high computational cost. These limitations hinder their reliability in high-precision and safety-critical UAV applications [17,18].

MPC has been widely applied to UAV trajectory tracking due to its ability to predict future behavior based on reference information, explicitly handle system constraints, and leverage well-established theoretical frameworks that provide guarantees on closed-loop performance [19]. A common feature in many quadrotor control approaches is the use of a cascaded control architecture, where attitude control (inner loop) is decoupled from position and velocity control (outer loop) [20]. In ref. [21], a strategy combining a thrust-prioritized inner-loop controller with an outer-loop controller that satisfies certain state constraints is proposed; however, it also lacks formal guarantees of stability and convergence. To overcome the limitations of designing controllers for highly nonlinear systems such as quadrotors using linear representations, nonlinear model predictive control (NMPC) strategies have been extensively employed [22]. In ref. [23], NMPC is utilized to avoid obstacles in cluttered environments while executing high-speed trajectories, considering both input and state constraints. In ref. [24], NMPC is compared with differential flatness-based control (DFBC) using an incremental nonlinear dynamic inversion method. Results, with and without an inner-loop controller, show that NMPC achieves better performance than DFBC, at the expense of higher computational cost. While these methods show impressive high-speed tracking performance in real experiments, they lack formal stability guarantees, leading to failures during aggressive maneuvers. Additionally, the feasibility of the NMPC algorithm is not ensured, causing potentially unpredictable and unstable behavior.

LQR has been effectively applied to a wide range of complex systems, such as double-inverted pendulums, fuel cell systems, vibration control systems, and electric vehicles. It has also been widely adopted in robotic trajectory tracking control [25,26,27]. The quadratic cost function of the LQR controller consists of two weighting matrices, namely the Q and R matrices. The Q matrix is associated with the trajectory deviation of the state variables, while the R matrix relates to control effort and actuator saturation. A key challenge in the real-time application of the LQR optimal controller lies in the effective selection of the Q and R weighting matrices [28], which inherently involves trade-offs and is typically tuned through a trial-and-error process.

Despite the significant progress in quadrotor control, three major challenges still hinder accurate trajectory tracking in practical scenarios. First, the inherently nonlinear and strongly coupled dynamics complicate controller design and make linear assumptions insufficient for aggressive or fast maneuvers [29]. Second, external disturbances and model uncertainties, such as aerodynamic effects, unmodeled dynamics, and time-varying flight conditions, can severely degrade tracking performance if not properly compensated [30]. Third, state and input constraints impose additional limitations on controller execution, particularly when rapid response and smooth control allocation are required [31]. These issues motivate the development of a hierarchical control framework that can simultaneously address prediction accuracy, robustness enhancement, and attitude stabilization within a unified structure.

To address the limitations of existing methods, this paper proposes a hierarchical PSMC–LQR control framework. A PSO-based compensation mechanism is integrated into the outer-loop MPC to correct prediction mismatches and enhance robustness against model uncertainties and disturbances. In the inner loop, an LQR controller is augmented with gain scheduling and control-rate relaxation to improve attitude convergence and smooth actuation under varying flight conditions. This hierarchical design leverages the predictive optimization ability of MPC, the adaptive error correction of the PSO-based compensator, and the fast stabilization of the enhanced LQR. In addition, the proposed framework ensures closed-loop stability through Lyapunov-based analysis and maintains moderate computational complexity suitable for practical deployment. The main contributions of this work are summarized as follows:

• A hierarchical control architecture is developed, in which a model predictive controller is coupled with a PSO-based compensator in the outer loop, and an enhanced LQR with gain scheduling and rate relaxation is employed in the inner loop. This structure decouples position and attitude control while improving adaptability to time-varying disturbances.

• A PSO-compensated MPC (PSMC) scheme is designed to correct prediction mismatches and suppress external perturbations. By iteratively optimizing compensation terms based on tracking error feedback, the outer-loop control improves robustness without increasing the MPC horizon or computational load.

• An inner-loop LQR is augmented with gain scheduling and control-rate relaxation to enhance attitude convergence and reduce actuator fluctuation. The former adapts feedback gains online according to the system state, while the latter smooths control updates to avoid excessive rate changes. This combination accelerates tracking response and improves stability under uncertain conditions.

We have organized the paper as follows. In Section 2, the nonlinear dynamics of the quadrotor are introduced, and a discrete-time, control-oriented model is derived for use in predictive control. Section 3 presents the proposed hierarchical PSMC–LQR control framework, including the outer-loop PSMC-based position controller and the inner-loop LQR-based attitude stabilizer. In Section 4, the closed-loop stability of the proposed hierarchical controller is analyzed using a discrete-time Lyapunov framework. In Section 5, extensive simulation results are provided to evaluate tracking performance, disturbance rejection, and computational complexity of the proposed method in comparison with two baseline schemes. In Section 6, concluding remarks are drawn and directions for future work are discussed.

2. System Modeling and Problem

2.1. Quadrotor Dynamics

A quadrotor helicopter is a type of multi-rotor aircraft equipped with four fixed-pitch propellers mounted symmetrically on a cross-shaped frame, as illustrated in Figure 1. Propeller pairs (1, 3) rotate clockwise, while pairs (2, 4) rotate counterclockwise, which ensures that the overall rotational torque is balanced [32]. Additionally, the thrust generated by each propeller is aligned in the same upward direction, contributing to stable lift generation. Based on the structural characteristics of the quadrotor, the following assumptions are made [33]:

1. The quadrotor is modeled as a rigid body with constant mass and moment of inertia.

2. The center of mass of the UAV remains fixed and coincides with the geometric center of the frame.

3. The UAV is subjected only to gravitational force and thrust generated by the propellers, while aerodynamic drag is neglected.

Figure 1.

The body frame and the inertial (Earth) frame, where “E” denotes the origin of the inertial frame.

Based on the Newton–Euler method, the nonlinear dynamic model of the quadrotor UAV is formulated as follows:

$$\left\{\begin{array}{l}\ddot{\varphi }={\displaystyle \frac{I_y-I_z}{I_x}}\dot{\theta }\dot{\psi }+{\displaystyle \frac{I_r}{I_x}}\dot{\theta }{\omega }_{\mathrm{sum}}+{\displaystyle \frac{1}{I_x}}{U}_2\\ \ddot{\theta }={\displaystyle \frac{I_z-I_x}{I_y}}\dot{\varphi }\dot{\psi }-{\displaystyle \frac{I_r}{I_y}}\dot{\varphi }{\omega }_{\mathrm{sum}}+{\displaystyle \frac{1}{I_y}}{U}_3\\ \ddot{\psi }={\displaystyle \frac{I_x-I_y}{I_z}}\dot{\varphi }\dot{\theta }+{\displaystyle \frac{1}{I_z}}{U}_4\\ \ddot{x}=-{\displaystyle \frac{U_1}{m}}\left(\sin \theta \cos \varphi \cos \psi +\sin \varphi \sin \psi \right)\\ \ddot{y}=-{\displaystyle \frac{U_1}{m}}\left(\sin \theta \cos \varphi \sin \psi -\sin \varphi \cos \psi \right)\\ \ddot{z}=-{\displaystyle \frac{U_1}{m}}\cos \theta \cos \varphi +g\end{array}\right.$$ (1)

In the equations, $\varphi ,\theta$, and $\psi$ represent the roll, pitch, and yaw angles of the quadrotor, respectively; $x, y$ and $z$ denote the quadrotor’s position in the three-dimensional Cartesian space. ${I_x,{ I}_y,I}_z$ are the moments of inertia of the UAV, respectively, and $I_r$ is the rotational inertia of the rotor, $g$ denotes the acceleration due to gravity. ${\omega }_{\mathrm{sum}}$ is the algebraic sum of the angular velocities of the four rotors, defined as ${\omega }_{\mathrm{sum}}={\omega }_1-{\omega }_2+{\omega }_3-{\omega }_4$, where ${\omega }_1,{ \omega }_2,{ \omega }_3,{ \omega }_4$ are the rotational speeds of the four propellers. $U_1,{ U}_2,{ U}_3,{ U}_4$ represent the total thrust, roll torque, pitch torque, and yaw torque generated by the UAV, respectively, and are defined as follows:

$$\left\{\begin{array}{l}{U}_1=b\left({\omega }_1^2+{\omega }_2^2+{\omega }_3^2+{\omega }_4^2\right)\\ U_2=lb\left(-{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_1^2+{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_2^2+{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_3^2-{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_4^2\right)\\ U_3=lb\left({\displaystyle \frac{\sqrt{2}}{2}}{\omega }_1^2-{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_2^2+{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_3^2-{\displaystyle \frac{\sqrt{2}}{2}}{\omega }_4^2\right)\\ U_4=d\left({\omega }_1^2+{\omega }_2^2-{\omega }_3^2-{\omega }_4^2\right)\end{array}\right.$$ (2)

where $b$ is the thrust coefficient of the rotor, $d$ is the drag coefficient of the rotor, and $l$ is the distance from the rotor axis to the quadrotor’s center of mass.

2.2. Problem Formulation

To formalize the control problem, consider a quadrotor system represented by a continuous nonlinear state-space model of the form:

$$\dot{x}\left(t\right)=f\left(x\left(t\right),u\left(t\right)\right)+d\left(t\right)$$ (3)

where $x\left(t\right)\in R^6$ is the state vector comprising position and velocity components in the inertial frame, $u\left(t\right)\in R^3$ is the control input (translational force vector), and $d\left(t\right)\in R^6$ represents unknown but bounded external disturbances. In this paper, the external disturbance term $d\left(t\right)$ represents physical effects such as wind gusts, airflow fluctuations, and unmodeled aerodynamic forces that act on the translational dynamics of the UAV. These perturbations are introduced into the system as bounded additive forces and later instantiated as harmonic wind disturbances in Section 5.

Let the desired trajectory be defined by a smooth reference signal:

$${\zeta }_d\left(t\right)={\left[x_d\left(t\right)\begin{array}{ccc}\begin{array}{cc}\begin{array}{cc}\begin{array}{c}{ y}_d\left(t\right)\end{array}& z_d\left(t\right)\end{array}& {\dot{x}}_d\left(t\right)\end{array}& {\dot{y}}_d\left(t\right)& {\dot{z}}_d\left(t\right)\end{array}\right]}^T$$ (4)

The tracking error is defined as:

$$e\left(t\right)=\zeta \left(t\right)-{\zeta }_d\left(t\right)$$ (5)

The discrete-time error dynamics, obtained via zero-order hold discretization with sampling interval $\Delta t$, can be expressed as:

$$e_{k+1}=A_d{e}_k+B_d{u}_k+d_k$$ (6)

where $A_d$ and $B_d$ are the linearized and discretized state-space matrices, and $d_k$ denotes the bounded disturbance at time step $k$. The objective of control is to design a stabilizing controller $u_k$ such that:

• The tracking error $e_k\to 0$ asymptotically or remains bounded within a small neighborhood.

• State and input constraints are satisfied: $x_k\in X$ and $u_k\in U$.

• The controller is suitable for real-time embedded implementation.

Mathematically, the problem is formulated as:

$$\begin{gathered} min\sum _{u_0,\dots u_{N-1}}^{N-1} {∥e_{k+i}∥}^{2}Q+{∥u_{k+i}∥}^{2}R \\ s.t. e_{k+i+1}=A_d{e}_{k+i}+B_d{u}_{k+i}+d_{k+i} \\ x_{k+i}\in \mathcal{X},u_{k+i}\in \mathcal{U},\forall i\in [0,N-1] \end{gathered}$$ (7)

where $Q>0$ and $R>0$ are weight matrices, $\mathcal{X}$ and $\mathcal{U}$ represent feasible state and input sets, and $N$ is the prediction horizon. This formulation serves as the foundation for the development of the outer-loop PSMC and the inner-loop LQR controller, as elaborated in subsequent sections.

The continuous-time quadrotor model is discretized using zero-order hold assumptions to facilitate predictive control design. The detailed linearization and discretization procedure is provided in Appendix A.

3. Hierarchical Control Architecture and Controller Design

3.1. Hierarchical Control Architecture

To achieve real-time and robust trajectory tracking under disturbances and constraints, a hierarchical control architecture is established, consisting of an outer-loop trajectory controller and an inner-loop attitude stabilizer, as illustrated in Figure 2. This cascaded structure follows the principle of time-scale separation: the outer loop operates at a slower update rate to plan motion in the position domain, while the inner loop runs at a faster rate to stabilize attitude dynamics and ensure rapid torque response. Such decomposition not only simplifies controller design for the inherently nonlinear quadrotor system but also enhances robustness and computational efficiency during onboard execution.

Figure 2.

Detailed module-level signal-flow diagram of the hierarchical PSMC–LQR controller.

In this architecture, the outer loop employs a PSO-compensated MPC framework. Its primary task is to compute optimal thrust and attitude references that minimize position-tracking errors while satisfying input and state constraints. Let the position state vector be $\gamma =[x,y,z{]}^T$, and the desired reference trajectory be ${\gamma }_d=[x_d,y_d,z_d{]}^T$. The tracking error is defined as: $e_k={\gamma }_k-{\gamma }_{d,k}$, and the outer loop solves the following quadratic optimization problem at each sampling instant:

$$u_{\mathrm{M}\mathrm{P}\mathrm{C}}=\mathrm{a}\mathrm{r}\mathrm{g} \underset{u}{min} \sum _{i=0}^{N-1} {∥e_{k+i}∥}_Q^2+{∥u_{k+i}∥}_R^2$$ (8)

subject to discrete-time linearized system dynamics and input/state constraints. To enhance adaptability, a PSO-based compensation module is introduced to dynamically adjust the MPC solution, yielding the refined control signal:

$$u_k^{*}=u_{\mathrm{M}\mathrm{P}\mathrm{C}}+\Delta u_{\mathrm{P}\mathrm{S}\mathrm{O}}$$ (9)

This optimized control signal is then transformed through inverse kinematics into the desired attitude command ${\sigma }_d=[{\varphi }_d,{\theta }_d,{\psi }_d{]}^T$, which serves as the reference input to the inner loop. The inner-loop controller utilizes an LQR to stabilize the quadrotor’s rotational dynamics. Its control law is defined as:

$$\tau =-K_{\mathrm{L}\mathrm{Q}\mathrm{R}}(\sigma -{\sigma }_d)$$ (10)

where $\sigma =[\varphi , \theta , \psi {]}^T$ is the measured attitude vector, and $K_{\mathrm{LQR}}$ is the gain matrix designed through optimization and augmented with gain scheduling and control-rate relaxation to adapt to different flight regimes. The inner loop ensures fast attitude convergence, smooth actuation, and effective disturbance rejection, which in turn improves the accuracy and stability of the outer-loop trajectory tracking.

In summary, the proposed hierarchical PSMC–LQR architecture establishes a clear division of control responsibilities:

• The outer loop focuses on trajectory planning, constraint handling, and robustness enhancement through adaptive optimization.

• The inner loop guarantees high-bandwidth attitude stabilization and torque control.

The bidirectional information flow—where the outer loop provides desired attitude and thrusts references, and the inner loop returns stabilized attitude feedback—forms a cooperative closed-loop system. This integration achieves precise position tracking, strong disturbance tolerance, and low-latency computation, making the framework highly suitable for real-time embedded flight control.

3.2. Outer-Loop Controller

To enhance the effectiveness of the outer-loop position controller, which is implemented using a PSMC framework, the PSO algorithm is introduced as an auxiliary adaptive module. The conventional MPC component provides nominal trajectory optimization, while the PSO compensator performs real-time correction to mitigate prediction errors arising from model inaccuracies and unmodeled disturbances. This cooperative design improves the robustness and adaptability of the outer loop without increasing the prediction horizon or tightening the control constraints. Therefore, PSO is discussed separately in the following subsection to elucidate its optimization mechanism.

3.2.1. Particle Swarm Optimization

PSO algorithm, inspired by the foraging behavior of bird flocks [34], enables a disordered population to evolve into an organized system through information sharing among individuals, thereby searching for feasible solutions in the solution space. In PSO, each potential solution to the optimization problem is represented by a particle in the search space. During each iteration, the fitness value of a particle’s position is evaluated based on the objective function. The particle then updates its velocity and trajectory based on both its own best-known position and the global best-known position discovered by the swarm. Let the number of decision variables be D; then each particle can be represented as a D-dimensional vector. The position and velocity of the $ith$ particle are defined as:

$$X_i=\left(x_{i1},x_{i1},\dots ,x_{iD}\right)$$ (11)

$$V_i=\left(v_{i1},v_{i1},\dots ,v_{iD}\right)$$ (12)

The particle velocity update equation is given by:

$$\begin{array}{c}{v}_{id}^{n+1}=\omega \cdot v_{id}^n+c_1\cdot r_1^n\left(Pbes{t}_{id}^n-x_{id}^n\right)\end{array}+c_2\cdot r_2^n\left(Gbes{t}_{id}^n-x_{id}^n\right)$$ (13)

$${\nu }_{min}\le {\nu }_{id}^n\le {\nu }_{max}$$ (14)

The particle position update equation is given by:

$$x_{id}^{n+1}=x_{id}^n+v_{id}^{n+1}$$ (15)

In the particle velocity update equation, $v_{id}^{n+1}$ denotes the velocity of the $ith$ particle in the $dth$ dimension at iteration $n+1$, and ${\nu }_{id}^n$ represents its velocity at iteration $n$. The values ${\nu }_{min}$ and ${\nu }_{max}$ define the lower and upper bounds of the particle velocity, respectively. $Pbes{t}_{id}^n$ refers to the best historical position of the $ith$ particle in the $dth$ dimension up to iteration $n$, while $Gbes{t}_{id}^n$ denotes the global best position of the entire swarm up to iteration $n$.

The swarm consists of $N$ particles, where $i=1, 2,\dots ,N$, and each particle has D decision variables, indexed by $d=1, 2,\cdots ,D$. The parameters $c_1$ and $c_2$ are cognitive and social learning factors, respectively. $c_1$ governs the tendency of a particle to move toward its own best position $Pbest$, while $c_2$ influences movement toward the global best position $Gbest$. The terms $r_1^n$ and $r_2^n$ are random numbers uniformly distributed in the range [0, 1], introduced to provide stochastic behavior in the search process.

The inertia weight $\omega$ regulates the trade-off between global and local exploration capabilities. A larger value of $\omega$ encourages global exploration and helps the particles escape local optima, whereas a smaller value leads to faster convergence and promotes local search. Typically, $\omega$ is selected within the range [0, 1].

In this study, a linearly decreasing strategy is employed to dynamically adjust the inertia weight. A relatively large inertia weight at the early stages of the algorithm enhances the global search capability, while gradually reducing the inertia weight as iterations proceed facilitate faster convergence to the global optimum. The inertia weight is updated according to the following equation:

$$\omega ={\omega }_{max}-{\displaystyle \frac{{\omega }_{max}-{\omega }_{min}}{n_{max}}}n$$ (16)

where ${\omega }_{max}$ denotes the initial inertia weight, ${\omega }_{min}$ represents the final inertia weight, and $n_{max}$ is the maximum number of iterations.

The general procedure of the PSO algorithm is summarized in Algorithm 1.

Algorithm 1. Particle Swarm Optimization Procedure

Input: Objective function $f\left(x\right)$, number of particles $N$, dimension $D$, inertia weight $\omega$, acceleration coefficients $c1$, $c2$, max iterations $T_{max}$     Initialize particle positions $x_i \in R^D$ and velocities $v_i\in R^D$ for $i$= 1 to $N$     Initialize personal bests $p_{best}\leftarrow x_i$, evaluate $f\left(p_{besti}\right)$        Set $g_{best}\leftarrow {argmin}_i·f\left(p_{besti}\right)$ For $t = 1$ to $T_{max}$ DO   For $i = 1$ to $N$   DO         Update velocity $v_i\leftarrow \omega \ast v_i+c_1\ast rand\ast \left(p_{besti}-x_i\right)+c_2\ast rand\ast \left(g_{besti}-x_i\right)$         Update position $x_i \leftarrow x_i + v_i$         Evaluate fitness $f\left(x_i\right)$         If $f\left(x_i\right) < f\left(p_{besti}\right)$ then           $p_{besti} \leftarrow x_i$         If $f\left(p_{besti}\right) < f\left(g_{best}\right)$ then            $g_{best} \leftarrow p_{besti}$         END IF      END IF    END FOR   END FOR return $g_{best}$, $f\left(g_{best}\right)$ Output: Global best position $g_{best}$ and value $f\left(g_{best}\right)$

3.2.2. PSMC Outer-Loop Controller

In this section, a PSMC outer-loop controller is developed by integrating an MPC framework with a PSO-based compensator. The MPC component determines the optimal control input at each time step through a combination of model prediction, rolling horizon optimization, and feedback correction, enabling anticipative and constraint-aware control. To enhance the system’s adaptability to dynamic and uncertain environments, the PSO-based compensator evaluates the current quadrotor state in real time and provides online compensation to the MPC-generated control signals. This integrated design improves the controller’s robustness and tracking accuracy under varying operational conditions. In addition to external disturbances, the quadrotor model may also exhibit parametric uncertainties arising from factors such as unmodeled aerodynamics, actuator imperfections, or deviations in physical parameters. In this work, these uncertainties are treated implicitly as part of the overall disturbance input and are not modeled separately. The robustness of the proposed controller against such effects is reflected through the compensative structure of the outer-loop and validated in the simulation results of Section 5.

For the position dynamics model of a quadrotor, the design can be divided into two components: the first involves generating vertical thrust $U_1\left(t\right)$ through the rotors; the second involves controlling the UAV’s motion in the $x$ and $y$ directions by adjusting the direction of this thrust. The spatial position state equation of the UAV system is defined as $\xi \left(t\right)=\left[x\right(t) \dot{x}(t) y(t) \dot{y}(t) z(t) \dot{z}(t){]}^T$, where $x\left(t\right)$,$y\left(t\right)$ and $z\left(t\right)$ represent the position coordinates of the UAV in the inertial (ground-fixed) frame, and $\dot{x}\left(t\right)$, $\dot{y}\left(t\right)$ and $\dot{z}\left(t\right)$ represent the corresponding velocity components. The motion equations can be reformulated as follows:

$$\dot{\xi }\left(t\right)=f\left(\xi \left(t\right),u_{\xi }\left(t\right)\right)=\left[\begin{array}{c}\dot{x}\left(t\right)\\ {\displaystyle \frac{U_1\left(t\right)}{m}}{u}_x\left(t\right)+{\displaystyle \frac{d_{fx}\left(t\right)}{m}}\\ \dot{y}\left(t\right)\\ {\displaystyle \frac{U_1\left(t\right)}{m}}{u}_y\left(t\right)+{\displaystyle \frac{d_{fy}\left(t\right)}{m}}\\ \dot{z}\left(t\right)\\ -g+{\displaystyle \frac{U_1\left(t\right)}{m}}\left(\mathit{cos}\phi \left(t\right)\mathit{cos}\theta \left(t\right)\right)+{\displaystyle \frac{d_{fz}\left(t\right)}{m}}\end{array}\right]$$ (17)

where

$$\begin{array}{c}{u}_x\left(t\right)=\mathit{cos}\psi \left(t\right)\mathit{sin}\theta \left(t\right)\mathit{cos}\phi \left(t\right)\end{array}+\mathit{sin}\psi \left(t\right)\mathit{sin}\phi \left(t\right)$$ (18)

$$\begin{array}{c}{u}_y\left(t\right)=\mathit{sin}\psi \left(t\right)\mathit{sin}\theta \left(t\right)\mathit{cos}\phi \left(t\right)\end{array}-\mathit{cos}\psi \left(t\right)\mathit{sin}\phi \left(t\right)$$ (19)

For a given reference trajectory, it is assumed to satisfy the following state equation:

$${\dot{\overline{\xi }}}_r\left(t\right)=f\left({\overline{\xi }}_r\left(t\right),u_{\xi r}\left(t\right)\right)$$ (20)

where ${\overline{\xi }}_r\left(t\right)=[x_r\left(t\right) {\dot{x}}_r\left(t\right) y_r\left(t\right) {\dot{y}}_r\left(t\right) z_r\left(t\right) {\dot{z}}_r(t){]}^T$ denotes the reference state vector, and $u_{\xi r}\left(t\right)={\left[\begin{array}{ccc}{u}_{xr}& u_{yr}& U_{1r}\end{array}\right]}^T$ represents the corresponding reference control input vector. The reference inputs are defined as follows:

$$u_{x_r}={\displaystyle \frac{m{\ddot{x}}_r\left(t\right)}{U_1\left(t\right)}}, U_{1_r}\left(t\right)={\displaystyle \frac{m\left({\ddot{z}}_r\left(t\right)+g\right)}{\mathit{cos}\theta \left(t\right)\mathit{cos}\varphi \left(t\right)}}, u_{y_r}={\displaystyle \frac{m{\ddot{y}}_r\left(t\right)}{U_1\left(t\right)}}$$ (21)

To derive the error dynamics of the system, define the state error vector as $\tilde{\overline{\xi }}\left(t\right)=\overline{\xi }\left(t\right)-{\overline{\xi }}_r\left(t\right)$, and the control input error as ${\tilde{u}}_{\xi }\left(t\right)={\tilde{u}}_{\xi }\left(t\right)-{\tilde{u}}_{\xi r}\left(t\right)$. The resulting state-space error dynamics can then be expressed as shown in Equation (17):

$$\dot{\tilde{\overline{\xi }}}\left(t\right)=A\left(t\right)\tilde{\overline{\xi }}\left(t\right)+B\left(t\right){\tilde{u}}_{\xi }\left(t\right)$$ (22)

where $A\left(t\right)$ and $B\left(t\right)$ denote the system Jacobian matrices. To achieve zero-error tracking of the given reference trajectory, an integral term of the state error variable is introduced, and the equation is extended accordingly as follows:

$$x_{\xi }\left(t\right)=\left[\begin{array}{c}\tilde{x}\left(t\right)\\ \tilde{\dot{x}}\left(t\right)\\ \int \tilde{x}\left(t\right)dt\\ \tilde{y}\left(t\right)\\ \tilde{\dot{y}}\left(t\right)\\ \int \tilde{y}\left(t\right)dt\\ \tilde{z}\left(t\right)\\ \tilde{\dot{z}}\left(t\right)\\ \int \tilde{z}\left(t\right)dt\end{array}\right]=\left[\begin{array}{c}x\left(t\right)-x_r\left(t\right)\\ x\left(t\right)-{\dot{x}}_r\left(t\right)\\ \int \left(x\left(t\right)-x_r\left(t\right)\right)dt\\ y\left(t\right)-y_r\left(t\right)\\ \dot{y}\left(t\right)-{\dot{y}}_r\left(t\right)\\ \int \left(y\left(t\right)-y_r\left(t\right)\right)dt\\ z\left(t\right)-z_r\left(t\right)\\ \dot{z}\left(t\right)-{\dot{z}}_r\left(t\right)\\ \int \left(z\left(t\right)-z_r\left(t\right)\right)dt\end{array}\right]$$ (23)

based on the Euler method, the deviation prediction model can be represented as the following discrete-time calibrated system:

$$x_{\xi }\left(k+1\right)=\overline{A}\left(k\right)\cdot x_{\xi }\left(k\right)+\overline{B}\left(k\right)\cdot {\tilde{u}}_{\xi }\left(k\right)$$ (24)

The state variable $x_{\xi }\left(t\right)$ is decomposed into motion control in the z direction and motion control in the x and y directions, i.e.,

$$x_z\left(k+1\right)={\overline{A}}_z\left(k\right)\cdot x_{\xi z}\left(k\right)+{\overline{B}}_z\left(k\right)\cdot {\tilde{u}}_{\xi z}\left(k\right)$$ (25)

$$x_{xy}\left(k+1\right)={\overline{A}}_{xy}\left(k\right)\cdot x_{\xi xy}\left(k\right)+{\overline{B}}_{xy}\left(k\right)\cdot {\tilde{u}}_{\xi xy}\left(k\right)$$ (26)

where

$${\overline{A}}_z=\left[\begin{array}{ccc}1& \Delta t& 0\\ 0& 1& 0\\ \Delta t& 0& 1\end{array}\right], {\overline{B}}_z\left[\begin{array}{c}0\\ {\displaystyle \frac{\Delta t}{m}}\mathit{cos}\theta \left(k\right)\mathit{cos}\phi \left(k\right)\\ 0\end{array}\right]$$ (27)

$${\overline{A}}_{xy}=\left[\begin{array}{cccccc}1& \Delta t& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ \Delta t& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& \Delta t& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& \Delta t& 0& 1\end{array}\right]$$ (28)

$${\overline{B}}_{xy}=\left[\begin{array}{cc}0& 0\\ {\displaystyle \frac{\Delta t}{m}}{U}_1\left(k\right)& 0\\ 0& 0\\ 0& 0\\ 0& {\displaystyle \frac{\Delta t}{m}}{U}_1\left(k\right)\\ 0& 0\end{array}\right]$$ (29)

For the altitude subsystem in the z direction, the cost function of the MPC controller is formulated as shown in Equation (30):

$$J_z={\left[\begin{array}{c}{\widehat{x}}_{\xi z}-{\widehat{x}}_{\xi rz}\end{array}\right]}^T{Q}_z\left[\begin{array}{c}{\widehat{x}}_{\xi z}-{\widehat{x}}_{\xi rz}\end{array}\right]+{\left[{\widehat{\tilde{u}}}_{\xi z}-{\widehat{\tilde{u}}}_{\xi rz}\right]}^T{R}_z\left[{\widehat{\tilde{u}}}_{\xi z}-{\widehat{\tilde{u}}}_{\xi rz}\right]$$ (30)

where $Q_z$ and $R_z$ represent the weighting matrices for the state variables and control inputs, respectively, and are both positive definite diagonal matrices, and $Q_z\in R^{N_z\times N_z}$,${ R}_z\in R^{N_u\times N_u}$, and $N_z$ denotes the prediction horizon and $N_u$ the control horizon. The state prediction vector is defined as ${\widehat{x}}_{\xi z}\triangleq \left[{\widehat{x}}_{\xi z}^T\right(k+1|k)\cdots {\widehat{x}}_{\xi z}^T(k+N_z|k){]}^T$, and the control input vector is defined as ${\widehat{u}}_{\xi z}\triangleq \left[{\widehat{u}}_{\xi z}^T\right(k|k)\cdots {\widehat{u}}_{\xi z}^T(k+N_u-1|k){]}^T$. The predicted discrete-time state trajectory can be computed as follows:

$$\begin{array}{c}{\widehat{x}}_{\xi z}\left(k+1|k\right)=P_Z\left(k|k\right)\cdot x_{\xi z}\left(k|k\right)\end{array}+H_Z\left(k|k\right)\cdot {\widehat{\tilde{u}}}_{\xi z}\left(k|k\right)$$ (31)

where ${\tilde{u}}_{\xi z}\left(k|k\right)=U_1\left(k\right)-U_{1r}\left(k\right)$. The reference deviation state vector and the reference deviation control vector are defined as follows:

$${\widehat{x}}_{\xi rz}=\left[\begin{array}{c}{x}_{\xi rz}\left(k+1\right)|k-x_{\xi rz}\left(k|k\right)\\ ⋮\\ x_{\xi rz}\left(k+N_z-1\right)|k-x_{\xi rz}\left(k|k\right)\end{array}\right]$$ (32)

$${\widehat{u}}_{\xi rz}=\left[\begin{array}{c}{U}_{1r}\left(k|k\right)-U_{1r}\left(k-1|k\right)\\ ⋮\\ U_{1r}\left(k+N_u-1|k\right)-U_{1r}\left(k|k\right)\end{array}\right]$$ (33)

By optimizing the performance index $J_z$, the control input for the deviation model of the altitude controller can be obtained as:

$$\begin{array}{c}{\widehat{\tilde{u}}}_{\xi z}={\left[{H_z}^T{Q}_z{H}_z+R_z\right]}^{-1}·\end{array}\left[{H_z}^T{Q}_z\left({\widehat{x}}_{\xi rz}\left(k\right)-P_z{\widehat{x}}_{\xi z}\left(k\right)\right)+R_z{\widehat{\tilde{u}}}_{\xi rz}\right]$$ (34)

Under the influence of environmental disturbances and noise, the compensation value $\Delta u$ is defined as the particle of the PSO population. The fitness function is designed in the following form:

$$f={\left(e_k\right)}^2+\lambda ·{\left(\Delta U_1\right)}^2+Penalty\left(g\right)$$ (35)

where $e_k=z_r\left(t\right)-z\left(t\right)$ represents the tracking error in the z direction. The term $\lambda$ denotes the weight coefficient of the compensation input, defined as $\lambda ={\lambda }_0·\left(1+{\dot{e}}_k\right)$, which allows for dynamic adjustment of the weight. This design prioritizes error suppression by reducing the weight of the compensation input when the error variation rate is large. $Penalty\left(g\right)$ is the constraint penalty term, defined as follows: $Pendty\left(g\right)=\sum \left(\mathrm{m}\mathrm{a}\mathrm{x}\right(0,{\nu }_z-{\nu }_{zmax}){)}^2+\sum (max\left(0,U_1-U_{1max}\right){)}^2$. To prevent the control compensation from exceeding the maximum performance limits of the UAV. The update rule is defined as:

• If $f_i\left(j\right)<f\left(pbes{t}_i\right)$, then update $pbes{t}_i\leftarrow \Delta u_i\left(j\right)$

• If $f_i\left(j\right)<f\left(gbes{t}_i\right)$, then update $gbes{t}_i\leftarrow \Delta u_i\left(j\right)$

The control compensation for the altitude sub model is iteratively optimized to obtain the optimal compensation input $\Delta u_z$, expressed as:

$${\widehat{\tilde{u}}}_z={\widehat{\tilde{u}}}_{\xi z}+\Delta u_z$$ (36)

Accordingly, the control input $U_1\left(K\right)$ is given by:

$$U_1\left(K\right)=U_{1r}\left(k\right)+{\widehat{\tilde{u}}}_z\left(k|k\right)$$ (37)

Similarly, for the x and y sub-systems, the control inputs $u_x$ and $u_y$ can be derived as follows:

$$\left\{\begin{array}{c}{u}_x(k)=u_{xr}(k)+{\widehat{\tilde{u}}}_{\xi x}+\Delta u_x\\ u_y(k)=u_{yr}(k)+{\widehat{\tilde{u}}}_{\xi y}+\Delta u_y\end{array}\right.$$ (38)

Based on Equations (18) and (19), the reference Euler angles of the quadrotor UAV at the current time can be inversely calculated as follows:

$$\left\{\begin{array}{l}{\varphi }_r\left(k\right)=\mathit{arcsin}(u_x\left(k\right)\mathit{sin}\psi \left(k\right)-u_y\left(k\right)\cos \mathit{\psi }\left(k\right))\\ {\theta }_r\left(k\right)=\mathit{arcsin}(\left(u_x\left(k\right)\mathit{cos}\psi \left(k\right)+{\displaystyle \frac{u_y\left(k\right)\mathit{cos}\psi \left(k\right)}{\mathit{cos}{\varphi }_r\left(k\right)}}\right)\end{array}\right.$$ (39)

The complete decision-making process of the proposed PSMC position controller is presented in Algorithm 2. The method combines an MPC-based nominal control law with a PSO-driven compensation loop for improved tracking robustness under dynamic disturbances.

Algorithm 2. Execution Procedure of the PSMC Position Controller

Input: Current UAV state ${\gamma }_k = {\left[x_k,{ y}_k, z_k\right]}^T$             Desired reference $y_{d,k}$,             System matrices $A, B$             MPC horizons: prediction horizon $N$, control horizon $M$             Weighting matrices $Q, R$             Input and state constraints             PSO parameters Compute tracking error: $e_k \leftarrow y_k- y_{d,k}$ Linearize nonlinear UAV dynamics at operating point $y_k$:              Obtain linear system: $x_{k+1}= A x_k+ B u_k$ Construct standard MPC cost function:              $J\left(u\right) ={\sum }_{i=0}^{n-1}[\Vert e_{k+i}\Vert Q^2 + \Vert u_{k+i}\Vert R^2]$ Formulate MPC constraints:             State limits: $y_{min}\le y_{k+i}\le y_{max}$             Input limits: ${u }_{min}\le u_{k+i} \le u_{max}$                 Terminal condition: optional Solve convex MPC optimization problem to obtain $u_{MPC}= [u_k\dots ,{ u}_{K+M-1}]$ Initialize PSO search around $u_{MPC\left[0\right]}$:              Define particle positions as perturbations $\Delta u \in {\mathbb{R}}^m$ around $u_{MPC\left[0\right]}$ Define PSO objective function:             ${ J}_{PSO\left(\Delta u\right) }= \Vert y_{predk +1} - y_{d,k+1}\Vert Q + \lambda {\Vert \Delta u\Vert }^2$ Run PSO optimizer (see Algorithm 1) to minimize ${ J}_{PSO\left(\Delta u\right) }$              Obtain best particle ${\Delta u}_{opt}$ Compute final control input:             $u_k^{*}\leftarrow u_{MPC\left[0\right]} + {\Delta u}_{opt}$ Apply $u_k^{*}$ to UAV system Wait for next sampling interval and repeat Output: Optimized control input $u_k^{*}$

3.3. LQR Inner-Loop Controller

While the outer-loop controller governs the position-level behavior of the quadrotor, the attitude dynamics require a fast and stable response to ensure accurate tracking and control allocation. To this end, LQR is employed in the inner loop. However, conventional fixed-gain LQR may become suboptimal when the flight state varies significantly. Therefore, gain scheduling is incorporated to adjust the feedback gains according to the real-time operating conditions, and a control-rate relaxation mechanism is adopted to prevent aggressive input changes and reduce actuator saturation. These enhancements improve responsiveness, maintain stability, and reduce control effort under complex flight scenarios.

In this framework, the inner-loop controller stabilizes the rotational dynamics of the quadrotor across the roll, pitch, and yaw axes. Although LQR provides an analytical and optimal solution for linearized attitude regulation, its performance degrades when operating conditions vary or perturbations occur. Therefore, the enhanced design adopted in this work integrates gain scheduling and control-rate relaxation to improve adaptability and ensure smooth actuation. The attitude dynamics of the quadrotor are governed by rigid-body Euler equations. Let $\eta =[\varphi ,\theta ,\psi {]}^T$ denote roll, pitch, and yaw angles, and $\omega =[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$ represent body-frame angular velocities. The rotational dynamics can be modeled as:

$$\dot{\eta }=J\left(\eta \right)\omega$$ (40)

$$I\dot{\omega }=\tau -\omega \times \left(I\omega \right)$$ (41)

where $I$ is the moment of inertia matrix and $\tau$ is the control torque vector. For small-angle maneuvers (hovering or slow transitions), the attitude system can be approximated by linearization around the hover condition ($\eta \approx 0, \omega \approx 0$), resulting in the decoupled form:

$$\left\{\begin{array}{c}\dot{\varphi }={\omega }_x, {\dot{\omega }}_x={\displaystyle \frac{1}{I_x}}{\tau }_{\varphi }\\ \dot{\theta }={\omega }_y, {\dot{\omega }}_y={\displaystyle \frac{1}{I_y}}{\tau }_{\theta }\\ \dot{\psi }={\omega }_z, {\dot{\omega }}_z={\displaystyle \frac{1}{I_z}}{\tau }_{\psi }\end{array}\right.$$ (42)

Each channel can thus be represented as a second-order linear system. For compactness, the general state-space form is written as:

$${\dot{x}}_r=A_r{x}_r+B_r{u}_r$$ (43)

Its state and input vectors are:

$$x_r=\left[\begin{array}{c}\eta -{\eta }_{\mathrm{r}\mathrm{e}\mathrm{f}}\\ \omega -{\omega }_{\mathrm{r}\mathrm{e}\mathrm{f}}\end{array}\right], u_r=\tau =\left[\begin{array}{c}{\tau }_{\varphi }\\ {\tau }_{\theta }\\ {\tau }_{\psi }\end{array}\right]$$ (44)

The matrices $A_r$ and $B_r$ are block-diagonal:

$$A_r=\left[\begin{array}{cc}0& I_3\\ 0& 0\end{array}\right], B_r=\left[\begin{array}{c}0\\ I^{-1}\end{array}\right]$$ (45)

To enhance the smoothness and robustness of the LQR controller under noise or discrete implementation, we introduce a relaxation term penalizing the rate of change in the state, i.e., $\parallel {\dot{x}}_r{\parallel }^2$. The cost function is defined as:

$$J={\int }_0^{\mathrm{\infty }} \left(x_r^{T}Q{x}_r+u_r^{T}R{u}_r+\epsilon \cdot {\dot{x}}_r^T{\dot{x}}_r\right)dt$$ (46)

where $Q>0$ penalizes attitude tracking errors, $R>0$ penalizes control efforts, $\epsilon \ge 0$ is a relaxation factor controlling the sensitivity to rapid state transitions.

Let us denote the auxiliary term:

$${\dot{x}}_r=A_r{x}_r+B_r{u}_r\Rightarrow {\dot{x}}_r^T{\dot{x}}_r=x_r^T{A}_r^T{A}_r{x}_r+2x_r^T{A}_r^T{B}_r{u}_r+u_r^T{B}_r^T{B}_r{u}_r$$ (47)

We substitute it into the cost function:

$$J={\int }_0^{\mathrm{\infty }} \left[x_r^T\left(Q+ϵA_r^T{A}_r\right)x_r+u_r^T\left(R+\epsilon B_r^T{B}_r\right)u_r+2\epsilon x_r^T{A}_r^T{B}_r{u}_r\right]dt$$ (48)

The cross term $x_r^T{A}_r^T{B}_r{u}_r$ can be absorbed using matrix completion or treated as bounded coupling. The modified Riccati equation becomes:

$$A_r^{T}P+P{A}_r-P{B}_r(R+\epsilon B_r^T{B}_r{)}^{-1}{B}_r^{T}P+Q+\epsilon A_r^T{A}_r=0$$ (49)

The optimal control law is:

$$u_r=-K_r{x}_r,K_r=(R+\epsilon B_r^T{B}_r{)}^{-1}{B}_r^{T}P$$ (50)

This relaxed formulation allows tuning of the trade-off between response speed and smoothness and effectively attenuates chattering or high-frequency fluctuations from the outer-loop MPC.

To further improve performance under time-varying dynamics, we introduce a gain scheduling mechanism that dynamically adjusts the weight matrix $Q$ based on the current state energy:

$$Q\left(k\right)=Q_0\cdot \left(1+\rho \cdot \parallel x_r\left(k\right){\parallel }^2\right)$$ (51)

where $Q_0$ is the nominal state penalty, $\rho >0$ is a tuning coefficient. This adaptive weighting increases the control stiffness when the system deviates significantly from the reference, ensuring rapid convergence without sacrificing nominal smoothness. To avoid computational burden, this scheme is implemented as a simple scalar scaling applied to the diagonal entries of $Q$, avoiding full Riccati precomputation at each step.

The LQR controller was tuned following a trial-and-error procedure guided by the physical meaning of the weighting matrices. Specifically, larger diagonal entries of $Q$ penalize deviations in angular states, while larger entries of $R$ penalize excessive control inputs. We first set $Q$ as a diagonal matrix with values in the range [10, 200] for attitude states and [1, 20] for angular rates, and $R$ as a diagonal matrix with values in [0.01, 1.0]. These values were adjusted iteratively until a satisfactory compromise between fast response and limited control effort was obtained. The final selected values are given in Section 5.1.

In addition, gain scheduling was introduced by scaling the diagonal terms of $Q$ with respect to the reference velocity norm. For low-speed flight (<1.0 m/s), the baseline $Q$ was used, while for higher speeds (>3.0 m), the diagonal terms were scaled by a factor of 1.4, with linear interpolation in between. This scheduling allowed the controller to maintain robustness at higher dynamic loads without excessive overshoot.

4. Lyapunov-Based Stability Analysis

To verify the theoretical stability of the proposed hierarchical PSMC–LQR control framework, Lyapunov stability analysis is conducted in this section. Based on the control laws derived in Section 3, the outer-loop PSMC–MPC generates desired attitude angles and thrust commands, while the inner-loop LQR controller stabilizes the attitude dynamics through torque regulation. To rigorously link the proposed control laws to the system’s stability, the composite error dynamics of the quadrotor are first formulated.

Let $e_p=[e_x,e_y,e_z{]}^T$ denote the position tracking error, and $e_a=[e_{\varphi },e_{\theta },e_{\psi }{]}^T$ denote the attitude tracking error. According to Equations (9) and (10), the closed-loop error dynamics can be expressed as:

$$\dot{e}=A_e+B_e∆u_{PSO}$$ (52)

where $e=[e_p^T,e_a^T{]}^T$ represents the composite error vector, $A_e$ and $B_e$ are the system matrices, and $\Delta u_{\mathrm{P}\mathrm{S}\mathrm{O}}$ is the bounded adaptive term introduced by the PSO compensator.

To facilitate the stability proof, we define a Lyapunov candidate function as:

$$V={\displaystyle \frac{1}{2}}{e}_p^T{\mathrm{p}}_1{\mathrm{e}}_p+{\displaystyle \frac{1}{2}}{e}_a^T{\mathrm{p}}_1{e}_a$$ (53)

where $P_1,{ P}_2>0$ are symmetric positive-definite matrices corresponding to the position and attitude subsystems, respectively.

Taking the time derivative of $V$ yields:

$$\dot{V}=e_p^T{P}_1{\dot{e}}_p+e_a^T{P}_2{\dot{e}}_a=e^T(P{A}_e+A_e^{T}P)e+2e_p^T{P}_1{B}_e\Delta u_{\mathrm{P}\mathrm{S}\mathrm{O}}$$ (54)

The first term in Equation (54) corresponds to the nominal linearized dynamics of the cascaded system, while the second term represents the perturbation caused by the PSO adaptive compensation.

To guarantee convergence, the feedback gain matrices of both the PSMC and LQR controllers are designed such that:

$$P{A}_e+A_e^{T}P=-Q$$ (55)

where $Q$ is a symmetric positive-definite matrix. Hence, the Lyapunov derivative becomes:

$$\dot{V}=-e^{T}Qe+2e_p^T{P}_1{B}_e\Delta u_{\mathrm{P}\mathrm{S}\mathrm{O}}$$ (56)

Since the PSO-based adaptive term $\Delta u_{\mathrm{P}\mathrm{S}\mathrm{O}}$ is bounded and continuously adjusted to minimize the MPC cost, it satisfies:

$$|2e_p^T{P}_1{B}_e\Delta u_{\mathrm{P}\mathrm{S}\mathrm{O}}|\le \epsilon \parallel e_p{\parallel }^2$$ (57)

where $\epsilon >0$ is a sufficiently small constant. Substituting this inequality yields:

$$\dot{V}\le -({\lambda }_{min}\left(Q\right)-\epsilon )\parallel e{\parallel }^2$$ (58)

Therefore, as long as ${\lambda }_{min}\left(Q\right)>\epsilon$, $V$ is strictly decreasing, ensuring that the equilibrium point $e=0$ is asymptotically stable. This result confirms that the proposed PSMC–LQR framework guarantees closed-loop stability while enhancing robustness and adaptability.

5. Simulation and Discussion

To validate the effectiveness of the proposed control framework, this section presents a series of representative simulation experiments that evaluate the quadrotor’s trajectory tracking accuracy and robustness against external disturbances. In addition to performance evaluation, a detailed computational complexity analysis is also conducted to compare the algorithmic efficiency of the proposed PSMC-LQR controller with two baseline methods. This comprehensive assessment aims to demonstrate both the control effectiveness and real-time feasibility of the proposed scheme.

All simulations were implemented in Python 3.10.13 using the nonlinear quadrotor model introduced in Section 2. The controller sampling time was set to 20 ms, and the total simulation duration was fixed at 20 s for all test scenarios. A three-dimensional helical trajectory was used to evaluate position–attitude tracking performance under coupled dynamics. To emulate realistic perturbations, time-varying sinusoidal disturbances were applied along the x-, y-, and z-axes. The tracking performance is evaluated using four commonly adopted metrics: mean absolute error (MAE), root mean square error (RMSE), maximum tracking error (MaxErr), and control effort. Their explicit definitions and calculation formulas are provided in Section 5.3 and are not repeated here for conciseness.

To ensure rationality and fairness of comparison, all evaluated controllers shared the same quadrotor dynamics, initial conditions, sampling period, state constraints, and disturbance settings. We let the system be defined as $\dot{x}=f\left(x,u\right), x\left(0\right)=x_0,{ T}_s=constant$, and the same reference trajectory was used for all configurations. In addition, the weighting matrices of the baseline MPC and LQR, $Q_p, R_p$ and $Q_a,{ R}_a$, were fixed across all compared methods unless one of the associated modules was intentionally deactivated in the ablation setting.

Parameter optimality was ensured through iterative tuning under the baseline architecture (MPC–LQR), where a grid-based adjustment of Q–R ratios and constraint bounds was performed to minimize the steady-state tracking error and control effort. Once the baseline weights achieved convergence and no further improvement was observed, the same parameter set was inherited by the other variants to eliminate bias from re-tuning. As a result, performance differences among methods arose solely from the activation of individual modules rather than from inconsistent parameter choices.

5.1. Trajectory Tracking Performance Evaluation in Calm Air Conditions

This subsection evaluates the performance of the proposed PSMC-LQR controller in tracking a helical trajectory under disturbance-free conditions. The helical trajectory and its parameters, which are specified below, are newly defined for this experiment and differ from those used in previous sections. The controller’s tracking performance is analyzed in terms of precision, error behavior, and response stability to demonstrate its effectiveness in ideal conditions.

This example first verifies the trajectory tracking performance of the quadrotor UAV in the absence of external disturbances. The parameters used in the simulation experiments are listed in Table 1 for the UAV model. The reference trajectories for the UAV in the x, y, and z directions are defined as $x_{ref}=5\mathit{sin}({\displaystyle \frac{\pi t}{5}})$,${ y}_{ref}=5\mathit{cos}({\displaystyle \frac{\pi t}{5}})$,${ z}_{ref}=0.5t$, and ${\psi }_{ref}=0$. The initial position and attitude of the quadrotor are given by $(x_0 y_0 z_0{)}^T=(0 0 0{)}^{T}m; ({\varphi }_0 {\theta }_0 {\psi }_0{)}^T=(0 0 0{)}^{T}rad$. In all the simulations reported in this paper the desired yaw angle is kept constant (zero) to simplify the outer-loop/inner-loop decoupling and to isolate the performance of the position controller under structured translational disturbances. This is a common experimental choice in trajectory tracking comparisons where yaw control is orthogonal to position-control design.

Table 1.

Quadrotor Model Parameters.

Parameter	Value
Mass	$1.0 \mathrm{k}\mathrm{g}$
Gravitational acceleration	$9.8 \mathrm{m}/{\mathrm{s}}^2$
Moment of inertia about the X-axis	$4\times {10}^{-3} {\mathrm{k}\mathrm{g}\mathrm{m}}^2$
Moment of inertia about the Y-axis	$4\times {10}^{-3} {\mathrm{k}\mathrm{g}\mathrm{m}}^2$
Moment of inertia about the Z-axis	$8.4\times {10}^{-3} {\mathrm{k}\mathrm{g}\mathrm{m}}^2$

To ensure reliable performance, all controller parameters were determined through iterative simulations and parametric analysis to balance trajectory accuracy, robustness, and computational efficiency.

For the PSO-based compensator, the parameters were selected to guarantee rapid convergence within each sampling period while maintaining real-time feasibility. The particle number and maximum iteration count were set to $i=10$ and $n_{\mathrm{m}\mathrm{a}\mathrm{x}}=8$, respectively. Comparative simulations showed that increasing either parameter beyond these values provided less than 3% improvement in tracking accuracy but caused more than 20% growth in computation time. The inertia weight range was defined as ${\omega }_{\mathrm{m}\mathrm{a}\mathrm{x}}=0.9$ and ${\omega }_{\mathrm{m}\mathrm{i}\mathrm{n}}=0.4$, while the acceleration coefficients were set to $c_1=0.8$ and $c_2=0.8$, providing a balanced exploration–exploitation capability and stable convergence under model uncertainties. The weighting factor was assigned as ${\lambda }_0=0.7$ to regulate the amplitude of the correction term, avoiding excessive adjustment during strong disturbances.

For the MPC controller, the sampling interval was set to $\Delta t=0.02 \mathrm{s}$, consistent with the outer-loop update rate. The weighting matrices were defined as $Q_z=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(40,1)$, $R_z=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(1\right)$, $Q_{xy}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(40,40,1,1)$, and $R_{xy}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(1,1)$. These values were tuned through iterative trials to emphasize position accuracy while maintaining smooth control inputs. Parameter sweeps revealed that larger $Q/R$ ratios improved steady-state precision but reduced robustness to modeling errors, whereas the selected configuration achieved the best trade-off between accuracy and stability.

For the inner-loop LQR controller, the sampling period was set to $\Delta T=0.02 \mathrm{s}$, and the weighting matrices were chosen as $Q=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(200,200,2,2)$ and $R=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(1,1,1)$. Simulation-based parametric studies showed that increasing the $Q/R$ ratio accelerated response but induced overshoot, while smaller ratios slowed stabilization. The adopted values provided the most balanced transient performance with limited control effort.

All parameters were finalized after multiple rounds of simulation-based iteration and sensitivity verification. Moderate perturbations (±20%) in these values did not significantly affect tracking accuracy or system stability, confirming the robustness of the selected configuration.

The simulation results are illustrated in Figure 3, Figure 4 and Figure 5. Specifically, Figure 3 depicts the UAV’s tracking performance along a helical trajectory, while Figure 4 shows the trajectory tracking curves along the x, y, and z axes and Figure 5 illustrates the tracking error curves in the three directions. In the figures, “MPC-MPC” denotes that both the outer and inner control loops adopt the MPC control strategy; “MPC-LQR” refers to the use of MPC in the outer loop and LQR in the inner loop; “PSMC-LQR” represents the novel control approach proposed in this study; and “PD Control” refers to a conventional proportional–derivative controller applied to both the outer and inner loops, serving as a representative of traditional control techniques. As observed from Figure 3, under ideal conditions without any external disturbances, all three control strategies enable the quadrotor UAV to accurately follow the desired trajectory, indicating that each control scheme possesses a certain degree of effectiveness in basic trajectory tracking performance.

Figure 3.

Helical Trajectory Tracking Results.

Figure 4.

Tracking performance for different control strategies under ideal conditions. (a) Tracking performance along X axes. (b) Tracking performance along Y axes. (c) Tracking performance along Z axes.

Figure 5.

Tracking error curves under different control strategies. (a) Tracking error along X axes. (b) Tracking error along Y axes. (c) Tracking error along Z axes.

As shown in Figure 3, under nominal conditions without external disturbances, all four control strategies enable the quadrotor UAV to follow the desired trajectory to some extent, demonstrating basic tracking feasibility. However, a closer inspection of Figure 4 reveals that the PD Control scheme exhibits the largest tracking deviation and oscillation due to its limited adaptability and lack of predictive capability. The MPC–MPC approach, serving as the baseline, achieves relatively stable tracking but still suffers from noticeable overshoot, while the MPC–LQR scheme exhibits minor undershoot. In contrast, the proposed PSMC–LQR controller achieves the smoothest and most accurate trajectory tracking by effectively suppressing overshoot and compensating for undershoot, and more importantly, it significantly outperforms the conventional PD Control scheme, highlighting its superiority over traditional control methods. Furthermore, the tracking error curves in Figure 5 confirm these findings quantitatively: the PSMC–LQR method yields the smallest steady-state error and lowest variance, followed by MPC–LQR and MPC–MPC, whereas the PD Control exhibits the largest cumulative tracking error, further verifying the robustness and precision advantages of the proposed hierarchical framework.

The experimental results indicate that the control scheme using MPC for both the outer and inner loops exhibits lower tracking accuracy compared to the combined MPC-LQR control method. The mean and variance of the tracking errors in each direction are presented in Table 2. Furthermore, in comparison with the MPC-LQR strategy, the proposed PSMC-LQR control method reduces tracking error by more than 13.2% in the x-direction, over 17.1% in the y-direction, and over 28% in the z-direction. In addition, when compared with the conventional PD control, which exhibits significantly larger errors and higher variances due to its lack of predictive and adaptive capabilities, the PSMC–LQR strategy demonstrates markedly superior accuracy and robustness. These results verify that the proposed hierarchical framework effectively suppresses disturbances, reduces steady-state deviations, and ensures smooth and precise trajectory tracking.

Table 2.

Tracking Error Statistics under Wind-Free Conditions.

Metric	MPC-MPC	MPC-LQR	PSMC-LQR	PD Control
X-Error Variance	0.03586	0.03400	0.03067	0.05741
Y-Error Variance	0.47491	0.46824	0.46974	0.47777
Z-Error Variance	0.000031	0.000028	0.000025	0.000104
X-Mean Error	0.1117	0.0775	0.0673	0.1852
Y-Mean Error	0.2150	0.1746	0.1447	0.2719
Z-Mean Error	0.04334	0.03638	0.02621	0.7455

Figure 6 depicts the rotational speed responses of the four motors (ω1, ω2, ω3, ω4) of the UAV in a wind-free environment. At the initial moment (t = 0 s), the motors exhibit transient speed fluctuations, which originate from the UAV’s attitude-stabilizing process. The flight control system drives the motors rapidly adjust output torques, aiming to counteract initial attitude deviations and establish a stable flight posture. Within the first 2.5 s, the speeds of the four motors show complex yet coordinated variations. This transient behavior reflects the dynamic interaction between the motor control subsystem and the UAV’s inertial dynamics. After approximately 2.5 s, the speeds of all motors converge to a consistent stable value (around 300 rad/s). This steady—state convergence indicates that the control algorithm effectively suppresses transient disturbances in wind—free conditions, enabling the UAV to maintain a stable flight state with balanced motor outputs. Such results verify the basic performance of the motor control strategy: in a calm environment without external wind interference, the system can quickly stabilize the UAV’s attitude and maintain constant motor speeds, providing a reliable reference for trajectory tracking accuracy.

Figure 6.

Time histories of the four motor speeds under calm conditions.

5.2. Trajectory Tracking Performance Evaluation in Wind Disturbance Conditions

This subsection evaluates the robustness of the proposed PSMC-LQR controller under time-varying external disturbances. By applying simulated wind forces in the x, y, and z directions, we emulate complex real-world conditions that UAVs typically encounter during outdoor missions. The same helical trajectory and controller configurations are retained to ensure experimental consistency. The performance of the three control methods is analyzed in terms of tracking precision, error resilience, and response stability.

To evaluate the robustness of the proposed controller against structured environmental perturbations, harmonic disturbances were introduced along the UAV’s translational axes. These disturbances were modeled as deterministic sinusoidal signals with distinct amplitudes and periods, representing periodic wind gusts or aerodynamic oscillations. This study focused on harmonic excitations as structured disturbances to clearly expose the disturbance rejection capabilities of the controllers, and the selection rationale was: (i) harmonic disturbances enable controlled and repeatable tests across different controllers; (ii) the amplitudes were chosen to represent light-to-moderate gusts that are typical for small quadrotors. For reproducibility, a short table detailing the magnitudes and units of relevant parameters is included here. External disturbances in the simulations were modeled as deterministic harmonic signals applied to translational dynamics: $F_x\left(t\right)=A_x\mathrm{s}\mathrm{i}\mathrm{n}(2\pi f_{x}t+{\varphi }_x)$, $F_y\left(t\right)=A_y\mathrm{s}\mathrm{i}\mathrm{n}(2\pi f_{y}t+{\varphi }_y)$, $F_z\left(t\right)=A_z\mathrm{s}\mathrm{i}\mathrm{n}(2\pi f_{z}t+{\varphi }_z)$, where $A\{x,y,z\}$,$f\{x,y,z\}$ and $\varphi \{x,y,z\}$ denote amplitude, frequency and phase, respectively. The specific values used in the experiments reported in this paper are summarized in Table 3 below.

Table 3.

Wind disturbance parameters used in simulations.

Disturbance	$\mathbf{Amplitude} \mathit{A}$(m)	$\mathbf{Frequency} \mathit{f}$(Hz)	$\mathbf{Phase} \mathit{\varphi }$(rad)
$F_x$	5	$2/15$	0
$F_y$	4	$2/15$	0.5$\pi$
$F_z$	2	0.2	0.5$\pi$

Figure 7 presents the spiral trajectory tracking results of the UAV under wind disturbances. Figure 8 shows the trajectory tracking curves in the x, y, and z directions under the same disturbance conditions, while Figure 9 depicts the corresponding tracking error curves in each direction. From the experimental results in Figure 7, Figure 8 and Figure 9, it is evident that the introduction of external disturbances leads to noticeable oscillations and deviations in the UAV’s flight trajectory. Among the four tested strategies, the PD Control exhibits the most significant tracking deviation due to its lack of predictive and adaptive capabilities. The MPC–MPC scheme maintains basic tracking performance but shows clear oscillatory behavior, while the MPC–LQR method provides improved stability yet still suffers from minor steady-state errors. In contrast, the proposed PSMC–LQR controller achieves the most stable and accurate trajectory tracking under wind disturbances, effectively suppressing oscillations and reducing trajectory deviations. These results clearly demonstrate the superior capability of the PSMC-LQR strategy in mitigating external disturbances and enhancing trajectory tracking performance for quadrotors.

Figure 7.

Helical Trajectory Tracking Results Under Wind Disturbance.

Figure 8.

Trajectory tracking results under Wind Disturbance. (a) Tracking results along X axes. (b) Tracking results along Y axes. (c) Tracking results along Z axes.

Figure 9.

Tracking error curves under Wind Disturbance. (a) Tracking errors along X axes. (b) Tracking errors along Y axes. (c) Tracking errors along Z axes.

The experimental results demonstrate that the proposed PSMC-LQR control method significantly improves trajectory tracking accuracy compared to both the hierarchical MPC control and the combined MPC-LQR control strategies. The mean and variance of the tracking errors in each direction under wind disturbance are presented in Table 4. Specifically, the PSMC-LQR method reduces the average tracking error in the x-direction by more than 34%, in the y-direction by over 26.2%, and in the z-direction by more than 46.8%. Despite wind disturbances, the system maintains stable flight. Furthermore, compared with the traditional PD Control, which exhibits large deviations and slow error recovery under external disturbances, the proposed framework demonstrates significantly enhanced disturbance rejection and robust trajectory tracking capability. These findings confirm that the PSMC–LQR strategy effectively suppresses wind-induced oscillations and ensures stable flight performance even in challenging conditions.

Table 4.

Tracking Error Statistics under Wind Disturbance.

Metric	MPC-MPC	MPC-LQR	PSMC-LQR	PD Control
X-Error Variance	0.55657	0.53966	0.14670	0.55657
Y-Error Variance	1.02599	0.90950	0.64590	1.35781
Z-Error Variance	0.00984	0.000045	0.000030	0.000126
X-Mean Error	0.6063	0.5109	0.3370	0.6103
Y-Mean Error	0.7352	0.6464	0.4768	0.7970
Z-Mean Error	0.0748	0.0938	0.0395	0.0743

Figure 10 illustrates the motor rotational speed characteristics of the UAV under wind-disturbed conditions. At t = 0 s, the speed deviations among the four motors are more significant compared to the wind—free case, directly reflecting the impact of wind on the UAV’s initial aerodynamic balance. In the transient phase (t < 2.5 s), the speeds of ω1–ω4 fluctuate violently: the wind disrupts the UAV’s attitude, and the flight control system triggers rapid motor speed adjustments to compensate for wind—induced disturbances (e.g., side-wind-caused lateral forces, gust-induced pitch changes). From 2.5 s to 20 s, although the motor speeds do not fully converge to an absolute constant value like in the wind—free scenario, they maintain relatively stable oscillations around a mean level (approximately 280–320 rad/s). This phenomenon demonstrates the adaptive regulation capability of the control system: facing continuous wind interferences, the algorithm dynamically allocates motor outputs to balance the UAV’s attitude and trajectory, rather than pursuing strict single—point speed stability. The minor speed oscillations are the result of the control system continuously correcting deviations caused by wind changes (such as wind speed fluctuations, direction shifts). These data highlight the robustness of the motor control scheme in complex environments: even under wind disturbances, the system can adjust motor speeds in real-time to ensure the UAV’s flight stability, laying a foundation for subsequent trajectory tracking tasks under dynamic wind conditions.

Figure 10.

Time histories of the four motor speeds under wind disturbance.

5.3. Ablation Analysis of Component Contributions

To further verify the contribution of each component in the proposed control framework, an ablation study was conducted under the same reference trajectory and disturbance settings. By selectively enabling or disabling the PSO-based compensator, gain scheduling, and control-rate relaxation, the individual and combined effects of each module could be quantitatively assessed. This design allows us to identify how each mechanism contributes to the overall tracking accuracy, robustness, and control effort.

To rigorously evaluate the role of each functional module in the proposed control architecture, an ablation study was performed using the same helical trajectory and disturbance conditions as in Section 6. Six controller variants were compared. The first is the baseline MPC–LQR, which employs an MPC-based outer loop and an LQR-based inner loop without any enhancement. The second adds the PSO-based compensator to the outer loop. The third incorporates gain scheduling in the inner loop, while the fourth enables only the rate relaxation mechanism. The fifth applies both gain scheduling and rate relaxation without PSO. The sixth corresponds to the full PSMC–LQR configuration, in which the PSO compensator, gain scheduling, and rate relaxation operate jointly.

Each configuration was evaluated with $N$ = 10 Monte Carlo runs. In each run, the harmonic disturbances described in Section 5.2 were applied with randomized initial phases, and the same initial state and reference trajectory were used across configurations. The performance metrics reported for each run are: per-axis mean absolute error ${MAE}_x$, ${MAE}_y$, ${MAE}_z$, overall RMSE, maximum tracking error, control effort (mean absolute actuator command), and computational load (average and maximum per-step computation time in milliseconds). For a quantity $q\left(t\right)$ computed over t = 1,…,T, $MAE$ and $RMSE$ were computed as:

$${MAE}_x={\displaystyle \frac{1}{T}}\sum _{t=1}^T\left|x\left(t\right)-x_{ref}\left(t\right)\right|$$ (59)

$${MAE}_y={\displaystyle \frac{1}{T}}\sum _{t=1}^T\left|y\left(t\right)-y_{ref}\left(t\right)\right|$$ (60)

$${MAE}_z={\displaystyle \frac{1}{T}}\sum _{t=1}^T\left|z\left(t\right)-z_{ref}\left(t\right)\right|$$ (61)

$$MaxErr=\underset{t}{max}\sqrt{(x-x_{ref}{)}^2+(y-y_{ref}{)}^2+(z-z_{ref}{)}^2}$$ (62)

$$RMSE=\sqrt{{\displaystyle \frac{1}{T}}\sum _{t=1}^T\left((x-x_{ref}{)}^2+(y-y_{ref}{)}^2+(z-z_{ref}{)}^2\right)}$$ (63)

$$\stackrel{-}{|u|}={\displaystyle \frac{1}{T}}\sum _{t=1}^T\parallel u\left(t\right){\parallel }_1$$ (64)

The numerical values reported in Table 5 were computed using the performance definitions in Equations (54)–(58). For each configuration, the mean absolute tracking errors along the x-, y- and z-axes (${MAE}_x$, ${MAE}_y$, ${MAE}_z$), the overall root mean square error ($RMSE$), the maximum instantaneous deviation ($MaxErr$), and the control effort were averaged over the Monte Carlo runs. These metrics provide complementary views of trajectory accuracy, robustness to perturbations, and actuation efficiency, allowing a fair comparison of the individual contributions from PSO compensation, gain scheduling, and rate relaxation.

Table 5.

Comparison of Tracking Error and Control Performance in Ablation Experiments.

Method	${\mathit{M}\mathit{A}\mathit{E}}_{\mathit{x}}$	${\mathit{M}\mathit{A}\mathit{E}}_{\mathit{y}}$	${\mathit{M}\mathit{A}\mathit{E}}_{\mathit{z}}$	$\mathit{R}\mathit{M}\mathit{S}\mathit{E}$	$\mathit{M}\mathit{a}\mathit{x}\mathit{E}\mathit{r}\mathit{r}$	Control Effort
MPC–LQR (baseline)	12.48	14.67	21.80	39.86	85.19	43.52
MPC–LQR + PSO	12.52	14.63	21.88	39.96	85.32	43.61
MPC–LQR + Scheduling only	12.40	14.72	22.32	40.17	84.89	43.81
MPC–LQR + Relaxation only	12.10	12.82	5.93	28.31	70.70	22.14
MPC–LQR (inner-only)	12.10	12.81	5.94	28.31	70.68	22.14
PSMC–LQR (full)	12.06	12.78	5.91	28.25	70.51	22.12

The results in Table 5 indicate that each of the three components contributes to a distinct aspect of the closed-loop performance. The PSO-based outer-loop compensation slightly reduces steady-state deviations but does not substantially improve control expenditure on its own—this aligns with its core role of mitigating prediction mismatch in the outer loop, where it primarily refines position prediction accuracy rather than optimizing actuation efficiency. Inner-loop gain scheduling and rate relaxation, when enabled individually, yield noticeable improvements in vertical tracking accuracy and actuation smoothness under wind perturbations: specifically, gain scheduling enhances attitude convergence to adapt to varying flight conditions, while rate relaxation substantially decreases control expenditure and suppresses overshoot. Notably, the full PSMC–LQR configuration consistently achieves the lowest $RMSE$ and $MaxErr$ across all runs, while preserving one of the lowest control efforts among the tested settings. When all components operate in synergy, the proposed framework achieves the optimal overall balance in tracking precision, disturbance rejection and control smoothness. This confirms that the combined action of prediction-based compensation, scheduled feedback gains, and rate constraints leads to a more robust and accurate trajectory tracking control framework than any partial variant.

5.4. Computational Complexity Analysis

To quantitatively assess the computational characteristics of the proposed PSMC-LQR framework, a comparative complexity analysis is performed across three control architectures: the baseline MPC-MPC, the hybrid MPC-LQR, and the proposed PSO-augmented MPC-LQR scheme. The analysis focuses on per-step computational overhead, primarily considering the dominant operations involved—namely, the quadratic programming (QP) cost associated with MPC optimization and the metaheuristic search complexity introduced by the PSO-based compensation module.

MPC-MPC: At each time step, two sequential MPC optimizations are solved—first for trajectory generation and then again for attitude control. Assuming each MPC solver uses a standard quadratic programming (QP) solver with complexity $O\left(n^3\right)$ for control horizon length $n$, the total complexity per step is approximately:

$$T_{MPC\text{-}MPC}=2\cdot O\left(n^3\right)$$

MPC-LQR: Here only the outer-loop MPC is solved (complexity $O\left(n^3\right)$, while the inner-loop LQR uses fixed-gain feedback with constant cost $O\left(n^2\right)$. Hence:

$$T_{MPC\text{-}LQR}=O\left(n^3\right)+O\left(n^2\right)\approx O\left(n^3\right)$$

PSMC-LQR: Combines the outer MPC solve $O\left(n^3\right)$ with a bounded PSO search step. PSO complexity is $O(Np\cdot Tp\cdot m)$, where $Np$ is particle count, $Tp$ iteration count, and mmm control dimension. Since $Np$ and $Tp$ are kept small (e.g., $Np\le 20$, $Tp\le 5$), PSO’s overhead is approximately $O\left(1\right)$ relative to MPC. Thus:

$$T_{PMPC\text{-}LQR}=O\left(n^3\right)+O\left(1\right)\approx O\left(n^3\right)$$

In summary, all three methods share the same cubic computational complexity in the MPC portion. Although PSMC-LQR adds a PSO-based adjustment step, the additional time is marginal compared to the MPC solve and remains constant per step. Importantly, simulation and embedded test-bed results confirm that PSMC-LQR achieves at least 50% improvement in trajectory tracking accuracy without increasing control cycle time or exceeding hardware real-time constraints.

6. Conclusions

This paper presented a hierarchical control framework for quadrotor trajectory tracking that combines a nonlinear model predictive controller with a PSO-based compensator in the outer loop (PSMC) and an LQR controller with rate relaxation and gain scheduling in the inner loop. The main motivation was to achieve robust real-time tracking performance under disturbances while retaining computational feasibility for embedded deployment. A discrete-time Lyapunov analysis was also conducted to support the stability of the proposed architecture. Through extensive simulations on helical trajectories with and without wind disturbances, the proposed PSMC–LQR achieved consistently better performance compared with the MPC–LQR and MPC–MPC baselines. In calm conditions, the mean tracking errors in the x, y, and z directions were reduced by more than 13.2%, 17.1%, and 28%, respectively. Under wind disturbances the reductions were more than 34%, 26.2%, and 46.8%, respectively. These results indicate that the hybrid use of MPC anticipation and PSO-based adaptability yields a good compromise between robustness and accuracy. The inclusion of rate relaxation and gain scheduling in the LQR inner loop further contributed to smoother attitude responses and reduced overshoot. The proposed control architecture highlights a practical way to integrate heuristic optimization (PSO) with model-based control (MPC + LQR). This demonstrates that embedding lightweight metaheuristics into MPC frameworks can significantly improve disturbance rejection while keeping the computational complexity within a range suitable for real-time implementation. The approach can be extended to other underactuated aerial vehicles or ground robots facing similar disturbance-prone environments. The current study is limited to simulation experiments with fixed yaw and structured harmonic disturbances. No onboard implementation results or computational benchmarks on embedded processors are reported at this stage. Moreover, only one reference trajectory (a helix) was tested; additional paths such as aggressive maneuvers or obstacle-rich trajectories could provide a broader evaluation.

Our next steps will include hardware-in-the-loop and onboard tests to quantify real-time computational load; extension of the framework to fully coupled yaw dynamics and variable yaw scenarios; experiments under stochastic turbulence models such as Dryden or von Kármán spectra; and exploration of adaptive or learning-based alternatives to PSO to further enhance online adaptability. These directions are expected to strengthen both the theoretical contribution and the practical applicability of the proposed method.

Appendix A

To facilitate the design of the outer-loop PSMC, the nonlinear dynamics of the quadrotor system are linearized around a time-varying reference trajectory and discretized using zero-order hold assumptions. This appendix provides the detailed mathematical procedure used for obtaining the linear discrete-time state-space model.

The full state vector is defined as:

$$x={\left[\begin{array}{cccccccccccc}{p}_x& p_y& p_z& v_x& v_y& v_z& \varphi & \theta & \psi & {\omega }_x& {\omega }_y& {\omega }_z\end{array}\right]}^T$$ (A1)

where $p_i$ are positions, $v_i$ are linear velocities, $\left(\varphi ,\theta ,\psi \right)$ are Euler angles, and ${\omega }_i$ are angular rates.

The translational motion of the quadrotor can be expressed as:

$$\dot{p}=v$$ (A2)

$$m\dot{v}=R\left(\varphi ,\theta ,\psi \right){\left[\begin{array}{ccc}0& 0& T\end{array}\right]}^T-mg{\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$$ (A3)

where $m$ is the quadrotor mass, $T$ is total thrust, $g$ is gravity, and $R(\cdot )$ is the body-to-inertial rotation matrix.

Linearization is performed using first-order Taylor expansion around the current operating point $(x_{\mathrm{r}\mathrm{e}\mathrm{f}},u_{\mathrm{r}\mathrm{e}\mathrm{f}})$:

$$\delta \dot{x}=A_c\delta x+B_c\delta u$$ (A4)

where $A_c={\left.{\displaystyle \frac{\partial f}{\partial x}}\right|}_{\left(x_{\mathrm{r}\mathrm{e}\mathrm{f}},u_{\mathrm{r}\mathrm{e}\mathrm{f}}\right)}$, $B_c={\left.{\displaystyle \frac{\partial f}{\partial u}}\right|}_{(x_{\mathrm{r}\mathrm{e}\mathrm{f}},u_{\mathrm{r}\mathrm{e}\mathrm{f}})}$, $\delta x=x-x_{\mathrm{r}\mathrm{e}\mathrm{f}}$, $\delta u=u-u_{\mathrm{r}\mathrm{e}\mathrm{f}}$. Taking partial derivatives, the nonzero blocks of $A_c$ include:

$${\displaystyle \frac{\partial \dot{p}}{\partial v}}=I_3, {\displaystyle \frac{\partial \dot{v}}{\partial \varphi ,\theta }}\approx {\displaystyle \frac{T}{m}}\cdot {\displaystyle \frac{\partial R}{\partial \varphi ,\theta }}$$ (A5)

where the approximation assumes small angles and thrusts $T$ near hover. The matrix $B_c$ includes:

$${\displaystyle \frac{\partial \dot{v}}{\partial T}}={\displaystyle \frac{1}{m}}R\cdot e_3$$ (A6)

where $e_3=[\begin{array}{ccc}0& 0& 1\end{array}{]}^T$. The continuous-time linear system is discretized using first-order matrix exponential:

$$A_d=e^{A_c\Delta t}\approx I+A_c\Delta t, B_d={\int }_0^{\Delta t} e^{A_c\tau }d\tau \cdot B_c\approx B_c\Delta t$$ (A7)

where $\Delta t$ is the sampling time. This approximation holds well for fast-sampled quadrotor systems. The resulting discrete-time linear model

$$x_{k+1}=A_d{x}_k+B_d{u}_k$$ (A8)

is used by the outer-loop MPC for trajectory prediction and control optimization. This model is re-linearized at each sampling instant along the reference trajectory.

Author Contributions

Conceptualization, S.C.; methodology, S.C.; software, S.C.; validation, S.C., Y.S. and Y.Z.; formal analysis, Y.S.; investigation, Y.F. and Y.Q.; resources, X.Z. and D.H.; data curation, Y.Q.; writing—original draft preparation, S.C.; writing—review and editing, Y.F. and Y.Z.; visualization, Y.Q. and Y.F.; supervision, X.Z. and D.H.; project administration, X.Z.; funding acquisition, D.H. All authors have read and agreed to the published version of the manuscript.

Institutional Review Board Statement

No applicable.

Informed Consent Statement

No applicable.

Data Availability Statement

Data are contained within the article.

Conflicts of Interest

The authors declare no conflicts of interest.

Funding Statement

This work was supported by the Fundamental Research Funds for the Central Universities, Civil Aviation University of China, under Grant 25CAFUC03022 (“Cooperative Trajectory Planning of Heterogeneous UAVs in Post-Disaster Rescue Scenarios”), and by the National Natural Science Foundation of China under Grant 6247071842 (“Research on Safe Flight Coordination Methods for Large-Scale Dense UAV Operations”).