Unmanned Aerial Vehicle (UAV) technology faces significant challenges in trajectory tracking due to external disturbances and model uncertainties during flight operations. This research presents a novel composite control strategy integrating Tube-based Model Predictive Control (Tube-MPC) for position tracking and Compensation Function Observer-based Model Compensation Control (CFO-MCC) for attitude stabilization. The framework addresses bounded disturbances through robust invariant sets while compensating for model perturbations in real-time.
Quadrotor UAV Dynamic Modeling
The quadrotor dynamics are decoupled into position ($\mathbf{\xi} = [x, y, z]^T$) and attitude ($\mathbf{\Theta} = [\phi, \theta, \psi]^T$) subsystems. The inertial frame ($O_e$) and body frame ($O_b$) dynamics follow Newton-Euler formulations:
$$
\begin{cases}
\dot{\mathbf{\xi}} = \mathbf{v} \\
m\dot{\mathbf{v}} = \mathbf{R}_b^e \mathbf{F} + \mathbf{G} + \mathbf{D} \\
\dot{\mathbf{\Theta}} = \mathbf{W}\mathbf{\omega} \\
\mathbf{J}\dot{\mathbf{\omega}} = -\mathbf{\omega} \times \mathbf{J}\mathbf{\omega} + \mathbf{\tau} + \mathbf{d}
\end{cases}
$$
Where $\mathbf{R}_b^e$ is the rotation matrix, $\mathbf{F} = [0,0,u_1]^T$ denotes thrust, $\mathbf{G} = [0,0,-mg]^T$ represents gravity, and $\mathbf{D}, \mathbf{d}$ model disturbances. Disturbance bounds satisfy:
$$
\|\mathbf{\sigma}_D\| \leq \epsilon_D, \quad \|\mathbf{\sigma}_d\| \leq \epsilon_d
$$
Table 1 summarizes key physical parameters for drone technology implementation:
| Parameter | Description | Value |
|---|---|---|
| $m$ | Mass (kg) | 0.318 |
| $J_x$ | $x$-axis inertia (kg·m²) | $4.524 \times 10^{-3}$ |
| $J_y$ | $y$-axis inertia (kg·m²) | $6.933 \times 10^{-3}$ |
| $K_1,K_2$ | Drag coefficients (N·s/m) | 0.602 |
Controller Design
Position Control via Tube-MPC
Decompose position dynamics into nominal ($\mathbf{x}$) and error ($\mathbf{e}$) systems:
$$
\begin{cases}
\mathbf{x}(k+1) = \mathbf{A}_d\mathbf{x}(k) + \mathbf{B}_d u_x(k) \\
\mathbf{e}(k+1) = \mathbf{A}_d\mathbf{e}(k) + \mathbf{B}_d u_x^*(k) + \mathbf{\sigma}(k)
\end{cases}
$$
The control law combines nominal MPC and auxiliary feedback:
$$
u_x(k) = u_x^{\text{MPC}}(k) – \mathbf{K}\mathbf{e}(k)
$$
where $\mathbf{K}$ stabilizes $(\mathbf{A}_d – \mathbf{B}_d\mathbf{K})$. The robust invariant set $\mathbb{S}$ satisfies:
$$
\mathbf{A}_{\text{cl}}\mathbb{S} \oplus \mathbb{E}_D \subseteq \mathbb{S}, \quad \mathbf{A}_{\text{cl}} = \mathbf{A}_d – \mathbf{B}_d\mathbf{K}
$$
Table 2 lists Tube-MPC parameters for Unmanned Aerial Vehicle control:
| Parameter | Description | Value |
|---|---|---|
| $N_p$ | Prediction horizon | 10 |
| $N_c$ | Control horizon | 5 |
| $\mathbb{S}$ | Robust invariant set | $[-0.527, 0.527]$ |
Attitude Control via CFO-MCC
The structure includes:
- High-Order Differentiator (HOD): Tracks reference derivatives
- Compensation Function Observer (CFO): Estimates total disturbances
- Model Compensation Law: Provides robust tracking
Roll channel dynamics with disturbance $f_\phi$:
$$
\ddot{\phi} = b_\phi u_2 + f_\phi(\phi, p, \sigma_4)
$$
HOD extracts reference derivatives ($\hat{\dot{\phi}}_d, \hat{\ddot{\phi}}_d$) with bandwidth $a_h$:
$$
\begin{cases}
\dot{v}_1 = v_2 + l_{h1}(\phi_d – v_1) \\
\dot{v}_2 = v_3 + l_{h2}(\phi_d – v_1) \\
\dot{v}_3 = l_{h3}(\phi_d – v_1)
\end{cases}
$$
CFO estimates states and disturbances with bandwidth $a_c$:
$$
\begin{cases}
\dot{z}_1 = z_2 + l_{c1}(\phi – z_1) \\
\dot{z}_2 = z_3 + l_{c2}(\phi – z_1) + b_\phi u_2 \\
\dot{z}_3 = l_{c3}(\phi – z_1)
\end{cases}
$$
Simulation Experiments
Tests compare Tube-MPC against MPC, LQR, and PID controllers under disturbances:
$$
\mathbf{\sigma} = \begin{bmatrix} 0.5\sin(0.1t) + \eta \\ 0.5\sin(0.1t + \pi/3) + \eta \\ 0.2\sin(0.1t + \pi/7) + \eta \end{bmatrix}, \quad \eta \sim \mathcal{N}(0, 0.1)
$$
Trajectory tracking performance metrics:
| Controller | RMSE (m) | Max Overshoot (%) | Settling Time (s) |
|---|---|---|---|
| Tube-MPC | 0.072 | 2.5 | 2.10 |
| MPC | 0.141 | 14.6 | 4.43 |
| PID | 0.693 | 13.3 | 9.57 |
Tube-MPC reduces trajectory tracking error by 49% compared to MPC during turns and maintains position within 0.0659m under 3.5m/s wind disturbances.
Flight Tests
Real-world validation used ZY-X150 drones (350g mass, 150mm wheelbase) with optical positioning. Key results:
Position Holding Accuracy
| Controller | Max Error (m) | Disturbance Rejection (m) |
|---|---|---|
| Tube-MPC | 0.112 | 0.0659 |
| MPC | 0.232 | 0.1748 |
| PX4 Baseline | 0.991 | 0.2641 |
Circular Trajectory Tracking
Tube-MPC achieved 0.072m RMSE at 0.3rad/s angular velocity, outperforming MPC by 49% in maximum turn error.
Conclusion
This work demonstrates that Tube-MPC/CFO-MCC integration significantly enhances Unmanned Aerial Vehicle trajectory tracking under bounded disturbances. Key advantages include:
- 48% reduction in tracking error versus conventional MPC
- Disturbance rejection within 0.0659m under 3.5m/s winds
- Real-time computational feasibility for embedded systems
The framework advances drone technology for applications requiring precise motion control in uncertain environments. Future work will address high-speed maneuver limitations through adaptive Tube sets and onboard implementation.