In recent years, the rapid advancement in drone technology has led to widespread adoption of Unmanned Aerial Vehicles (UAVs) in various fields such as surveillance, agriculture, and logistics. Among these, quadrotor UAVs stand out due to their ability to hover, vertical take-off and landing, and agility. However, trajectory tracking for these systems remains challenging due to external disturbances like wind gusts and model uncertainties. This paper addresses these issues by proposing a composite control strategy combining Tube-based Model Predictive Control (Tube-MPC) for position control and Compensation Function Observer-based Model Compensation Control (CFO-MCC) for attitude control. The goal is to achieve precise trajectory tracking and robust disturbance rejection in real-world scenarios.
The dynamics of a quadrotor UAV are inherently nonlinear and underactuated, with six degrees of freedom controlled by only four inputs. To model this system, we use the Newton-Euler approach, defining an inertial frame and a body frame. The position subsystem describes the translational motion, while the attitude subsystem handles rotational dynamics. External disturbances, including aerodynamic drag and random forces, are incorporated as bounded uncertainties. The mathematical model is decoupled into position and attitude subsystems for controller design. For the position subsystem, we consider the dynamics along the x, y, and z axes, each represented as a second-order system with disturbances. The state vector includes position and velocity, and the input is derived from the thrust and orientation. The attitude subsystem involves roll, pitch, and yaw angles, with control moments as inputs. Disturbances are modeled as bounded signals, ensuring robustness in controller design.
For position control, we employ Tube-MPC to handle bounded disturbances and model perturbations. The core idea is to decompose the system into a nominal model and an error dynamics model. The nominal model, free of disturbances, is used for predictive control with constraints, while the error model is stabilized by an auxiliary controller to confine the state errors within a robust invariant set, known as the Tube. This ensures that the actual trajectory remains close to the nominal one despite disturbances. The Tube-MPC formulation involves solving a constrained optimization problem online, minimizing a cost function over a prediction horizon. The cost function penalizes deviations from the reference trajectory and control efforts, with weights tuned for performance. Constraints on states and inputs are tightened using the Tube set to guarantee feasibility. The auxiliary controller, typically a linear feedback law, is designed to make the error system asymptotically stable. The robust invariant set is computed offline, often as a polytope, to capture the worst-case disturbances. This approach significantly enhances the robustness of drone technology in unpredictable environments.
The attitude control utilizes CFO-MCC, which consists of a High-order Differentiator (HOD), a Compensation Function Observer (CFO), and a model compensation control law. The HOD estimates the reference signals and their derivatives, providing smooth trajectories for the controller. The CFO observes the total disturbances, including unmodeled dynamics and external torques, and compensates for them in real-time. The control law combines these estimates with state feedback to achieve fast and accurate tracking. The convergence of HOD and CFO is analyzed using Lyapunov methods, ensuring that estimation errors vanish over time. Parameters for HOD, CFO, and the control law are tuned based on bandwidth considerations, balancing response speed and noise sensitivity. This structure is particularly effective for attitude control due to its simplicity and robustness, making it suitable for implementation in real-time systems on Unmanned Aerial Vehicles.
To validate the proposed control strategy, we conduct numerical simulations using MATLAB/Simulink. The quadrotor parameters, such as mass and moments of inertia, are based on a typical micro-UAV. Disturbances include Gaussian white noise and periodic signals to mimic wind effects. We compare Tube-MPC with traditional MPC, LQR, and PID controllers in scenarios involving take-off, point-to-point flight, curved trajectory tracking, and disturbance rejection. Performance metrics include rise time, overshoot, settling time, and root mean square error (RMSE). The results demonstrate that Tube-MPC outperforms other methods in tracking accuracy and robustness, especially under strong disturbances. For instance, in curved trajectories, Tube-MPC reduces overshoot and phase lag, while in hover tests, it maintains position within a small Tube set despite disturbances.
We also perform real-flight experiments using a ZY-X150 quadrotor platform equipped with an optical motion capture system for high-precision localization. The control algorithms run on a ground station communicating with the UAV via UDP. Experiments evaluate point-to-point flight, circular trajectory tracking, and disturbance rejection using a fan to generate wind. The Tube-MPC controller shows superior performance in terms of response speed and error reduction compared to MPC and the default PX4 controller. Tables summarize key metrics like RMSE and maximum error, highlighting the effectiveness of the proposed method in real-world applications of drone technology.
In conclusion, this work presents a robust control framework for quadrotor UAVs that combines Tube-MPC and CFO-MCC to address trajectory tracking under disturbances. The Tube-MPC ensures constraint satisfaction and disturbance rejection in the position loop, while CFO-MCC provides fast and stable attitude control. Both simulation and experimental results confirm the superiority of this approach in terms of precision and robustness. Future work will focus on adaptive tuning of the Tube set and migration of the position controller to onboard computers for higher control rates. This research contributes to advancing autonomous capabilities in Unmanned Aerial Vehicles, enabling reliable operations in complex environments.
The dynamics of a quadrotor UAV can be expressed using the following equations. Let $\xi = [x, y, z]^T$ represent the position in the inertial frame, and $V = [\mu, v, w]^T$ be the velocity vector. The rotation matrix from the body frame to the inertial frame is given by:
$$R_b^e = \begin{bmatrix}
c_\theta c_\psi & s_\phi s_\theta c_\psi – c_\phi s_\psi & c_\phi s_\theta c_\psi + s_\phi s_\psi \\
c_\theta s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & c_\phi s_\theta s_\psi – s_\phi c_\psi \\
-s_\theta & s_\phi c_\theta & c_\phi c_\theta
\end{bmatrix}$$
where $s_\alpha$ and $c_\alpha$ denote $\sin(\alpha)$ and $\cos(\alpha)$, respectively. The translational dynamics are:
$$\dot{\xi} = V$$
$$\dot{V} = \frac{1}{m} R_b^e F + G + D$$
where $F = [0, 0, u_1]^T$ is the thrust vector, $G = [0, 0, -g]^T$ is gravity, and $D$ represents external disturbances. The rotational dynamics are:
$$\dot{\omega} = J^{-1} (-\omega \times J \omega + \tau + d)$$
$$\dot{\Psi} = W \omega$$
where $\omega = [p, q, r]^T$ is the angular velocity, $\Psi = [\phi, \theta, \psi]^T$ is the Euler angles, $J$ is the inertia matrix, $\tau$ is the control torque, and $d$ is the disturbance torque. The matrix $W$ is:
$$W = \begin{bmatrix}
1 & s_\phi t_\theta & c_\phi t_\theta \\
0 & c_\phi & -s_\phi \\
0 & s_\phi / c_\theta & c_\phi / c_\theta
\end{bmatrix}$$
with $t_\alpha = \tan(\alpha)$. Disturbances are modeled as bounded signals:
$$D = -K_D V + \sigma_D, \quad d = -K_d \omega + \sigma_d$$
where $K_D$ and $K_d$ are diagonal matrices of damping coefficients, and $\sigma_D$, $\sigma_d$ are random bounded disturbances.
For the position controller design, we discretize the system and formulate the Tube-MPC. The nominal system is:
$$x(k+1) = A_d x(k) + B_d u(k)$$
and the error dynamics are:
$$e(k+1) = A_d e(k) + B_d u_e(k) + \sigma(k)$$
where $e(k) = x(k) – \bar{x}(k)$, and $u_e(k) = -K e(k)$ is the auxiliary control. The Tube set $\mathbb{S}$ is computed such that $e(k) \in \mathbb{S}$ for all $k$. The MPC optimization problem is:
$$\min_{U} \sum_{i=1}^{N_p} \|x(k+i|k) – x_d(k+i)\|_Q^2 + \sum_{j=1}^{N_c} \|u(k+j-1|k)\|_R^2$$
subject to:
$$x(k+i|k) \in \bar{\mathbb{X}} \ominus \mathbb{S}, \quad u(k+j-1|k) \in \bar{\mathbb{U}} \ominus K\mathbb{S}$$
where $\bar{\mathbb{X}}$ and $\bar{\mathbb{U}}$ are the tightened constraints, and $\ominus$ denotes the Pontryagin difference.
For attitude control, the CFO-MCC structure is implemented. The HOD for reference tracking is:
$$\dot{v}_1 = v_2 + l_{h1} e_h$$
$$\dot{v}_2 = v_3 + l_{h2} e_h$$
$$\dot{v}_3 = l_{h3} e_h$$
with $e_h = \phi_d – v_1$, and estimates $\hat{\phi}_d = v_1$, $\hat{\dot{\phi}}_d = v_2$, $\hat{\ddot{\phi}}_d = v_3$. The CFO for disturbance estimation is:
$$\dot{z}_1 = z_2 + l_{c1} e_c$$
$$\dot{z}_2 = z_3 + l_{c2} e_c + b u_2$$
$$\dot{z}_3 = l_{c3} e_c$$
with $e_c = \phi – z_1$, and estimates $\hat{\phi} = z_1$, $\hat{p} = z_2$, $\hat{f} = z_3$. The control law is:
$$u_2 = \frac{1}{b} \left[ k_p (\hat{\phi}_d – \hat{\phi}) + k_d (\hat{\dot{\phi}}_d – \hat{p}) + \hat{\ddot{\phi}}_d – \hat{f} \right]$$
Parameters for HOD and CFO are tuned based on bandwidth requirements. For HOD, $l_{h1} = a_h$, $l_{h2} = a_h^2$, $l_{h3} = a_h^3$ with $a_h > 0$. For CFO, $l_{c1} = 3a_c$, $l_{c2} = 3a_c^2$, $l_{c3} = a_c^3$, and $\lambda = a_c / 3$ for stability.
In simulations, the quadrotor parameters are as follows:
| Parameter | Description | Value |
|---|---|---|
| $m$ | Mass (kg) | 0.318 |
| $J_x$ | Moment of inertia ($\text{kg·m}^2$) | $4.5243 \times 10^{-3}$ |
| $J_y$ | Moment of inertia ($\text{kg·m}^2$) | $6.933 \times 10^{-3}$ |
| $J_z$ | Moment of inertia ($\text{kg·m}^2$) | $8.825 \times 10^{-3}$ |
| $K_1$ | Drag coefficient (N·s/m) | 0.602 |
| $K_2$ | Drag coefficient (N·s/m) | 0.602 |
| $K_3$ | Drag coefficient (N·s/m) | 0 |
Disturbances are set as $\sigma_1 = 0.5\sin(0.1t) + \text{rand}(0,0.1)$, etc., to simulate realistic conditions. The Tube-MPC parameters include a prediction horizon $N_p = 10$, control horizon $N_c = 5$, weight matrices $Q = \text{diag}(3,1)$ and $R = 0.1$, and input constraints $[-3.5, 3.5]$. The Tube set $\mathbb{S}$ is computed as $[-0.527, 0.527]$ for the error states.
Simulation results show that Tube-MPC achieves faster response and lower overshoot compared to other methods. For example, in point-to-point flight, the rise time with Tube-MPC is 2.77s, overshoot is 2.5%, and settling time is 2.1s, whereas MPC has higher overshoot. In trajectory tracking, Tube-MPC reduces the RMSE to 0.072m, compared to 0.141m for MPC. Under disturbances, Tube-MPC maintains a position error of 0.0659m, demonstrating its robustness in drone technology applications.
Real-flight experiments further validate the approach. The platform uses a motion capture system for localization at 120Hz, and control algorithms run at 30Hz. In point-to-point tests, Tube-MPC shows improvements in rise time and overshoot. For circular trajectory tracking, the RMSE is 0.072m with Tube-MPC, versus 0.141m with MPC. In disturbance rejection tests with a fan, Tube-MPC achieves an RMSE of 0.0275m, highlighting its effectiveness for Unmanned Aerial Vehicles in windy conditions.
In summary, the integration of Tube-MPC and CFO-MCC provides a comprehensive solution for robust trajectory tracking in quadrotor UAVs. The Tube-MPC handles position control with constraint satisfaction and disturbance invariance, while CFO-MCC ensures rapid and stable attitude control. Future work will explore adaptive mechanisms and onboard implementation to enhance performance in high-maneuverability scenarios. This research underscores the potential of advanced control strategies in advancing autonomous drone technology for practical applications.