In recent years, unmanned aerial vehicle systems, particularly quadcopter platforms, have garnered significant attention due to their unique advantages in automation and control. A quadcopter is a small unmanned aircraft equipped with four rotors, capable of vertical take-off and landing and movement in six degrees of freedom. Its simplicity of operation, stealth, and robustness in various environments make it widely applicable in commercial and civil domains. Trajectory tracking control refers to the ability of a quadcopter to follow a reference trajectory in the inertial coordinate system. Under a designed control law, it starts from a given initial state and achieves global uniform asymptotic stability by satisfying position and attitude errors. However, the quadcopter is a typical strongly coupled, nonlinear system often operating in complex environments, making high-precision trajectory tracking crucial.
Current research on quadcopter trajectory tracking has yielded considerable results. For instance, some studies have addressed object detection in videos for small multi-rotor unmanned aerial vehicles using neural network-based multi-level feature recognition and tracking. Others have proposed backstepping-based controllers for quadcopter attitude and position stabilization. Improved artificial potential field methods have been developed for effective trajectory planning, while enhanced linear active disturbance rejection controllers have demonstrated strong anti-interference capabilities. Additionally, adaptive non-singular fast terminal sliding mode control based on RBF neural networks has been designed to mitigate total disturbance effects in systems where accurate models are difficult to establish.
This article presents a novel cascaded dual-closed-loop control strategy for quadcopter trajectory tracking under complex external unknown disturbances. The outer loop addresses position and attitude errors, while the inner loop handles velocity errors. Specifically, model predictive control (MPC) is employed to design the velocity closed-loop controller. The sparrow search algorithm (SSA), known for its rapid convergence and strong robustness, is applied to the MPC rolling optimization process to obtain feasible solutions. To overcome the computational burden of MPC, sliding mode control (SMC) is utilized to design dynamic controllers for position and attitude dynamics. SMC is insensitive to external disturbances, robust, and does not require precise modeling, thus addressing uncertainties in quadcopter external disturbances and modeling challenges. The saturation function replaces the sign function to ensure continuity in the actual system input, effectively reducing high-frequency chattering in SMC. Lyapunov stability theory is used to verify the stability of the proposed SSA-MPC controller. Experimental results demonstrate that the proposed method ensures high-precision trajectory tracking control for a six-degree-of-freedom quadcopter under complex external unknown disturbances and parameter uncertainties, with tracking accuracy significantly superior to traditional single-controller systems.
The quadcopter’s kinematic and dynamic modeling is essential for control design. The kinematic equations describe the relationship between position and velocity, while the dynamic equations account for forces and torques. The Earth-fixed frame $O_E x_E y_E z_E$ and the body-fixed frame $O_B x_B y_B z_B$ are defined, with $O_E$ as a fixed point on Earth and $O_B$ attached to the quadcopter’s center of mass. The position vector is denoted as $\mathbf{X} = (x, y, z)^T$, and the linear velocity vector as $\mathbf{v} = (v_x, v_y, v_z)^T$. The Euler angles $\boldsymbol{\delta} = (\phi, \theta, \psi)^T$ represent roll, pitch, and yaw, respectively, with angular velocities $\boldsymbol{\omega} = (\omega_\phi, \omega_\theta, \omega_\psi)^T$. The full state vector is $\boldsymbol{\eta} = (\mathbf{X}^T, \boldsymbol{\delta}^T)^T = (x, y, z, \phi, \theta, \psi)^T$, and the velocity vector is $\boldsymbol{\vartheta} = (\mathbf{v}^T, \boldsymbol{\omega}^T)^T = (v_x, v_y, v_z, \omega_\phi, \omega_\theta, \omega_\psi)^T$. The kinematic equations are:
$$\dot{\mathbf{X}} = \mathbf{T} \mathbf{v}$$
$$\dot{\boldsymbol{\delta}} = \mathbf{R} \boldsymbol{\omega}$$
where $\mathbf{T}$ is the transformation matrix and $\mathbf{R}$ is the rotation matrix:
$$\mathbf{T} = \begin{pmatrix}
\cos\psi \cos\theta & \cos\psi \sin\theta \sin\phi – \sin\psi \cos\phi & \cos\psi \sin\theta \cos\phi + \sin\psi \sin\phi \\
\sin\psi \cos\theta & \sin\psi \sin\theta \sin\phi + \cos\psi \cos\phi & \sin\psi \sin\theta \cos\phi – \cos\psi \sin\phi \\
-\sin\theta & \cos\theta \sin\phi & \cos\theta \cos\phi
\end{pmatrix}$$
$$\mathbf{R} = \begin{pmatrix}
1 & \sin\phi \tan\theta & \cos\phi \tan\theta \\
0 & \cos\phi & -\sin\phi \\
0 & \sin\phi / \cos\theta & \cos\phi / \cos\theta
\end{pmatrix}$$
The translational dynamics of the quadcopter are derived using the Euler-Lagrange equation:
$$m \ddot{\mathbf{X}} = \mathbf{F}_U + \mathbf{G} + \mathbf{F}_d$$
where $m$ is the mass of the quadcopter, $\mathbf{F}_U = \mathbf{T} \boldsymbol{\rho}$ with $\boldsymbol{\rho} = (0, 0, \sum_{i=1}^4 F_i)^T$ being the thrust from the four motors, $\mathbf{G} = (0, 0, -mg)^T$ is the gravity vector with $g = 9.8 \, \text{m/s}^2$, and $\mathbf{F}_d$ is the external disturbance force. The attitude dynamics are given by:
$$\mathbf{N}(\boldsymbol{\delta}) \ddot{\boldsymbol{\delta}} + \mathbf{C}(\boldsymbol{\delta}, \dot{\boldsymbol{\delta}}) \dot{\boldsymbol{\delta}} = \mathbf{M}_f + \mathbf{M}_d$$
where $\mathbf{M}_f$ and $\mathbf{M}_d$ are the control and disturbance torques, respectively. The inertia matrix $\mathbf{I} = \text{diag}(I_x, I_y, I_z)$, and $\mathbf{N}(\boldsymbol{\delta})$ is defined as:
$$\mathbf{N}(\boldsymbol{\delta}) = \begin{pmatrix}
I_x & 0 & -I_x \sin\theta \\
0 & I_y \cos^2\phi + I_z \sin^2\phi & (I_y – I_z) \cos\phi \sin\phi \cos\theta \\
-I_x \sin\theta & (I_y – I_z) \cos\phi \sin\phi \cos\theta & I_x \sin^2\theta + I_y \sin^2\phi \cos^2\theta + I_z \cos^2\phi \cos^2\theta
\end{pmatrix}$$
The Coriolis term $\mathbf{C}(\boldsymbol{\delta}, \dot{\boldsymbol{\delta}})$ is a complex matrix with elements derived from the Euler-Lagrange formulation. The control torque $\mathbf{M}_f$ is:
$$\mathbf{M}_f = \begin{pmatrix}
b_1 (F_4 – F_2) \\
b_1 (F_1 – F_3) \\
b_2 (F_4 + F_2 – F_3 – F_1)
\end{pmatrix}$$
where $b_1$ and $b_2$ are distances from the motors to the center of mass, and $F_i = h H_i^2$ with $h$ as the propeller torque coefficient and $H_i$ as the rotor speeds.
The trajectory tracking control problem is defined as designing a dual-closed-loop control method for the full six-degree-of-freedom quadcopter with initial trajectory deviations, ensuring high-precision tracking under external disturbances. The desired trajectory and velocity are $\boldsymbol{\eta}_d = (\mathbf{X}_d^T, \boldsymbol{\delta}_d^T)^T$ and $\boldsymbol{\vartheta}_d = (\mathbf{v}_d^T, \boldsymbol{\omega}_d^T)^T$, respectively. The tracking errors are $\boldsymbol{\eta}_e = \boldsymbol{\eta} – \boldsymbol{\eta}_d$ and $\boldsymbol{\vartheta}_e = \boldsymbol{\vartheta} – \boldsymbol{\vartheta}_d$.
For the velocity closed-loop controller, MPC is used. The discrete linearized model is derived by Taylor expansion around the desired trajectory point, neglecting higher-order terms:
$$\boldsymbol{\eta}_e(k+1) = \mathbf{A}_{k,t} \boldsymbol{\eta}(k) + \mathbf{B}_{k,t} \boldsymbol{\vartheta}(k)$$
where $\mathbf{A}_{k,t}$ and $\mathbf{B}_{k,t}$ are Jacobian matrices. The state-space model is defined as $\boldsymbol{\xi}(k|t) = (\boldsymbol{\eta}(k|t)^T, \boldsymbol{\vartheta}(k-1|t)^T)^T$, leading to:
$$\boldsymbol{\xi}(k+1|t) = \tilde{\mathbf{A}}_{k,t} \boldsymbol{\xi}(k|t) + \tilde{\mathbf{B}}_{k,t} \Delta \boldsymbol{\vartheta}(k|t)$$
$$\mathbf{O}(k|t) = \tilde{\mathbf{C}}_{k,t} \boldsymbol{\xi}(k|t)$$
where $\tilde{\mathbf{A}}_{k,t} = \begin{pmatrix} \mathbf{A}_{k,t} & \mathbf{B}_{k,t} \\ \mathbf{0} & \mathbf{I} \end{pmatrix}$, $\tilde{\mathbf{B}}_{k,t} = \begin{pmatrix} \mathbf{B}_{k,t} \\ \mathbf{I} \end{pmatrix}$, and $\tilde{\mathbf{C}}_{k,t} = (\mathbf{C}_{k,t} & \mathbf{0})$. The future output is expressed in matrix form:
$$\mathbf{Y}(t) = \boldsymbol{\Psi}_t \boldsymbol{\xi}(t|t) + \boldsymbol{\Theta}_t \Delta \mathbf{U}(t)$$
where $\mathbf{Y}(t)$ is the output vector, $\boldsymbol{\Psi}_t$ and $\boldsymbol{\Theta}_t$ are constructed from the system matrices, and $\Delta \mathbf{U}(t)$ is the control increment sequence. The objective function for MPC is:
$$J(\boldsymbol{\xi}(t), \boldsymbol{\vartheta}(t-1), \Delta \mathbf{U}(t)) = \sum_{i=1}^{N_p} \| \boldsymbol{\eta}(k+i|t) – \boldsymbol{\eta}_d(k+i|t) \|^2_{\mathbf{Q}} + \sum_{i=1}^{N_c-1} \| \Delta \boldsymbol{\vartheta}(k+i|t) \|^2_{\mathbf{R}}$$
where $\mathbf{Q}$ and $\mathbf{R}$ are weight matrices, $N_p$ is the prediction horizon, and $N_c$ is the control horizon. Constraints include velocity limits, control increments, and output bounds. The optimization problem is transformed into a quadratic programming problem:
$$\min_{\Delta \mathbf{U}(t)} \Delta \mathbf{U}(t)^T \mathbf{H}_t \Delta \mathbf{U}(t) + \mathbf{G}_t^T \Delta \mathbf{U}(t)$$
subject to $\Delta \mathbf{U}_{\min} \leq \Delta \mathbf{U}(k) \leq \Delta \mathbf{U}_{\max}$ and $\mathbf{U}_{\min} \leq \boldsymbol{\vartheta}(t-1) + \sum_{i=t}^k \Delta \mathbf{U}(i) \leq \mathbf{U}_{\max}$ for $k = t, t+1, \dots, t+N_c-1$.
The sparrow search algorithm (SSA) is employed to solve the MPC rolling optimization due to its global convergence and robustness. SSA mimics the foraging behavior of sparrows, with discoverers and followers updating their positions based on fitness. The population position matrix is $\mathbf{X} = [X_{pq}]$ for $p = 1, \dots, n$ (population size) and $q = 1, \dots, d$ (dimension). The discoverer update is:
$$X_{pq}^{g+1} = \begin{cases}
X_{pq}^g \cdot \exp\left(-\frac{p}{\alpha \cdot r_{\max}}\right) & \text{if } R_2 < ST \\
X_{pq}^g + D & \text{if } R_2 \geq ST
\end{cases}$$
where $g$ is the iteration index, $\alpha \in [0,1]$, $R_2 \in [0,1]$ is a random number, $ST$ is the safety threshold (e.g., 0.8), and $D$ is a random number from a normal distribution. Followers update as:
$$X_{pq}^{g+1} = \begin{cases}
D \cdot \exp\left(\frac{X_{\text{worst}}^g – X_{pq}^g}{p^2}\right) & \text{if } p > n/2 \\
X_P^{g+1} + |X_{pq}^g – X_P^{g+1}| \cdot \mathbf{A}^* \cdot \mathbf{L} & \text{if } p \leq n/2
\end{cases}$$
where $X_{\text{worst}}^g$ is the worst position, $X_P^{g+1}$ is the best discoverer position, $\mathbf{A}$ is a random matrix, and $\mathbf{L}$ is a vector of ones. When danger is detected, the position updates as:
$$X_{pq}^{g+1} = \begin{cases}
X_{\text{best}}^g + \beta \cdot |X_{pq}^g – X_{\text{best}}^g| & \text{if } f_p > f_g \\
X_{pq}^g + K \cdot \frac{|X_{pq}^g – X_{\text{worst}}^g|}{(f_p – f_w) + \epsilon} & \text{if } f_p = f_g
\end{cases}$$
where $X_{\text{best}}^g$ is the best position, $\beta$ is a step length, $f_p$ is the current fitness, $f_g$ and $f_w$ are the best and worst fitness values, $K \in [-1,1]$, and $\epsilon$ is a small constant. The SSA-optimized MPC provides the control sequence $\Delta \mathbf{U}^*_t$, and the first element $\Delta \boldsymbol{\vartheta}^*_t$ is applied to the quadcopter system:
$$\boldsymbol{\vartheta}_d(t) = \boldsymbol{\vartheta}_d(t-1) + \Delta \boldsymbol{\vartheta}^*_t$$
Lyapunov stability is proven using the function $F(k) = \min_{\Delta \boldsymbol{\vartheta}} \left( \sum_{i=1}^{N_p} \| \boldsymbol{\eta}(k+i|t) – \boldsymbol{\eta}_d(k+i|t) \|^2_{\mathbf{Q}} + \sum_{i=1}^{N_c-1} \| \Delta \boldsymbol{\vartheta}(k+i|t) \|^2_{\mathbf{R}} \right)$, which is positive definite and whose derivative is negative definite, ensuring system stability.
For the dynamics controller, SMC is designed for both translational and rotational dynamics. The sliding surface is chosen as:
$$\mathbf{s}(t) = \dot{\mathbf{v}}_e(t) + 2 \boldsymbol{\Lambda} \mathbf{v}_e(t) + \boldsymbol{\Lambda}^2 \int_0^t \mathbf{v}_e(\tau) d\tau$$
for translation, and similarly for rotation with $\boldsymbol{\omega}_e(t)$. The derivative is $\dot{\mathbf{s}}(t) = \ddot{\mathbf{v}}_e(t) + 2 \boldsymbol{\Lambda} \dot{\mathbf{v}}_e(t) + \boldsymbol{\Lambda}^2 \mathbf{v}_e(t)$. The control law $\mathbf{u} = \mathbf{u}_{eq} + \mathbf{u}_{sw}$ includes an equivalent control $\mathbf{u}_{eq}$ based on the model and a switching control $\mathbf{u}_{sw} = k_{sw} F(\mathbf{s})$, where $F(\mathbf{s})$ is the saturation function:
$$F(\mathbf{s}) = \begin{cases}
1 & \text{if } \mathbf{s}(t) > \phi’ \\
\frac{\mathbf{s}}{\phi’} & \text{if } |\mathbf{s}(t)| \leq \phi’ \\
-1 & \text{if } \mathbf{s}(t) < -\phi’
\end{cases}$$
with $\phi’$ as the boundary layer thickness. This reduces chattering while maintaining robustness.
Simulation studies validate the proposed SSA-MPC-SMC method for quadcopter trajectory tracking. The quadcopter parameters are: mass $m = 1.44 \, \text{kg}$, moments of inertia $I_x = 0.03 \, \text{kg·m}^2$, $I_y = 0.03 \, \text{kg·m}^2$, $I_z = 0.04 \, \text{kg·m}^2$, prediction horizon $N_p = 5$, control horizon $N_c = 5$. The desired trajectory is a spiral:
$$\boldsymbol{\eta}_d(t) = \begin{pmatrix}
x_d(t) \\
y_d(t) \\
z_d(t) \\
\psi_d(t)
\end{pmatrix} = \begin{pmatrix}
20 \sin(0.2t) \\
20 – 20 \cos(0.2t) \\
t \\
0.2t
\end{pmatrix}$$
with initial conditions $\boldsymbol{\eta}(0) = (0, 5, 0, 0, 0, 0)^T$ and $\boldsymbol{\vartheta}(0) = \mathbf{0}$. Total simulation time is 70 s with a sampling time of 0.01 s.
First, the rolling optimization algorithms SSA, genetic algorithm (GA), and particle swarm optimization (PSO) are compared in MPC. Population size is 20, maximum iterations 50. For GA, crossover probability $P_c = 0.9$, mutation probability $P_m = 0.1$. The results show that SSA-MPC achieves faster convergence and better tracking compared to GA-MPC and PSO-MPC, with lower computational time.
| Algorithm | Convergence Speed | Tracking Error | Computational Time (s) |
|---|---|---|---|
| GA-MPC | Slow | High | ~2× SSA-MPC |
| PSO-MPC | Moderate | Moderate | ~1.2× SSA-MPC |
| SSA-MPC | Fast | Low | Reference |
Next, SSA-MPC-SMC is compared with nonlinear MPC (NMPC) and pure SMC under external disturbances. Time-varying disturbances are applied from 10 s to 60 s:
$$\mathbf{F}_d = \begin{pmatrix}
F_{dx} \\
F_{dy} \\
F_{dz} \\
F_{d\phi} \\
F_{d\theta} \\
F_{d\psi}
\end{pmatrix} = \begin{pmatrix}
0.5 \text{ for } 10 \leq t < 35, -0.5 \text{ for } 35 \leq t < 60 \\
0.2 \cos 5t – 0.2 \sin 5t \\
0.2 \sin 5t \\
0.5 \cos 2t \\
0.5 \sin 3t – 0.1 \cos 2t \\
0.5 \sin 2t
\end{pmatrix}$$
SSA-MPC-SMC demonstrates superior tracking accuracy and robustness, with smaller errors than NMPC and SMC. Additionally, parameter uncertainties are introduced by varying $m$, $I_x$, $I_y$, $I_z$ by ±20% and ±10%. The proposed method maintains stable tracking under combined disturbances and uncertainties, showcasing its robustness.
| Method | Position Error (m) | Attitude Error (rad) | Robustness to Uncertainties |
|---|---|---|---|
| NMPC | High | High | Moderate |
| SMC | Moderate | Moderate | Low |
| SSA-MPC-SMC | Low | Low | High |
The effectiveness of the cascaded controller is further analyzed through Lyapunov-based stability proofs and simulation data. The quadcopter system achieves precise trajectory tracking even in challenging scenarios, highlighting the synergy between SSA-MPC for velocity control and SMC for dynamics control. Future work may focus on real-time implementation and adaptation to more complex environments.
In conclusion, this article presents a novel SSA-MPC-SMC-based cascaded dual-closed-loop control system for quadcopter trajectory tracking. The integration of SSA-optimized MPC for velocity control and improved SMC for dynamics control ensures high precision, robustness, and stability under external disturbances and parameter uncertainties. The proposed method outperforms traditional approaches, making it suitable for practical applications in autonomous quadcopter operations.