In recent years, quadcopter unmanned aerial vehicles (UAVs) have gained significant attention due to their versatility in applications such as aerial photography, search and rescue, and precision agriculture. However, the trajectory tracking control of quadcopters is challenging due to their inherent nonlinearities, underactuation, and susceptibility to external disturbances. This paper addresses the problem of stable trajectory tracking for quadcopters subjected to unknown external disturbances by proposing a control strategy based on disturbance compensation. We design a PID position controller for the outer loop and an incremental nonlinear dynamic inversion (INDI) controller for the inner attitude loop, enhanced with a disturbance estimator to compensate for external perturbations. The effectiveness of the proposed approach is validated through simulations, demonstrating robust performance in the presence of disturbances.
The dynamics of a quadcopter are derived using the Newton-Euler formulation, considering the vehicle as a rigid body with six degrees of freedom. The translational and rotational dynamics are expressed as follows:
$$ \dot{\xi} = v, $$
$$ \dot{v} = -g e_z + \frac{\tau_f}{m} R e_z + \Delta_1, $$
$$ \dot{\Theta} = W \omega, $$
$$ I \dot{\omega} = -\omega \times I \omega + \tau + \Delta_2, $$
where $\xi = (x, y, z)^T$ represents the position in the inertial frame, $v = (v_x, v_y, v_z)^T$ is the velocity, $\Theta = (\phi, \theta, \psi)^T$ denotes the Euler angles, and $\omega = (\omega_x, \omega_y, \omega_z)^T$ is the angular velocity in the body frame. The matrix $R$ is the rotation matrix from the body to the inertial frame, $W$ is the transformation matrix from angular velocities to Euler rates, $I$ is the inertia matrix, $\tau_f$ is the total thrust, $\tau$ is the torque vector, and $\Delta_1$, $\Delta_2$ represent external disturbances affecting the translational and rotational dynamics, respectively.
The control strategy is structured into two main loops: the position loop and the attitude loop. For the position loop, we design a PID controller to track reference trajectories. The position error is defined as $e_1 = \xi – \xi_{\text{ref}}$, and the control law for the virtual force $\tau_f’$ is given by:
$$ \tau_f’ = -K_p e_1 – K_i \int e_1 \, dt – K_d \dot{e}_1 + \ddot{\xi}_{\text{ref}} – \hat{\Delta}_1, $$
where $\hat{\Delta}_1$ is the estimated disturbance obtained from a disturbance observer. The observer is designed based on a higher-order sliding mode approach to ensure accurate estimation under the assumption that the disturbance derivative is bounded by $|\dot{\Delta}_1| \leq L_0$. The observer equations are:
$$ v_0 = -\lambda_2 |z_0 – \xi|^{2/3} \text{sign}(z_0 – \xi) + z_1, \quad \dot{z}_0 = v_0, $$
$$ v_1 = -\lambda_1 |z_1 – v_0|^{1/2} \text{sign}(z_1 – v_0) + z_2, \quad \dot{z}_1 = v_1 + \tau_f’ + z_2, $$
$$ \dot{z}_2 = -\lambda_0 \text{sign}(z_2 – v_1), $$
with parameters $\lambda_0 = 1.1 L_0$, $\lambda_1 = 1.5 L_0^{1/2}$, and $\lambda_2 = 2 L_0^{1/3}$. This disturbance estimator provides robust compensation for external forces acting on the quadcopter.
The desired attitude angles and total thrust are computed from the virtual force $\tau_f’$ to ensure proper orientation during trajectory tracking. The roll ($\phi_{\text{ref}}$), pitch ($\theta_{\text{ref}}$), and thrust ($\tau_f$) commands are derived as:
$$ \phi_{\text{ref}} = \arcsin\left( \frac{\tau_{f1}’ \sin \psi_{\text{ref}} – \tau_{f2}’ \cos \psi_{\text{ref}}}{\sqrt{(\tau_{f1}’)^2 + (\tau_{f2}’)^2 + (\tau_{f3}’ + g)^2}} \right), $$
$$ \theta_{\text{ref}} = \arctan\left( \frac{\tau_{f1}’ \cos \psi_{\text{ref}} + \tau_{f2}’ \sin \psi_{\text{ref}}}{\tau_{f3}’ + g} \right), $$
$$ \tau_f = m \left[ \tau_{f1}’ (\sin \theta \cos \psi \cos \phi + \sin \psi \sin \phi) + \tau_{f2}’ (\sin \theta \sin \psi \cos \phi – \cos \psi \sin \phi) + (\tau_{f3}’ + g) \cos \theta \cos \phi \right]. $$
For the attitude loop, we employ an INDI controller to handle nonlinearities and disturbances. The attitude error dynamics are defined as $e_4 = \Theta – \Theta_{\text{ref}}$ and $e_5 = \dot{\Theta} – \dot{\Theta}_{\text{ref}}$. The INDI control law is derived by linearizing the dynamics around the current state, leading to:
$$ \tau’ = \tau_{f,k} + I (\dot{\omega}_{\text{des}} – \dot{\omega}_{f,k}), $$
where $\dot{\omega}_{\text{des}} = -K_\Theta e_4 – K_\omega e_5$ is the desired angular acceleration, and $\tau_{f,k}$, $\omega_{f,k}$ are the previous control input and filtered angular velocity, respectively. This approach inherently compensates for disturbances by using incremental updates, making it robust against model uncertainties and external torques.
To validate the proposed control strategy, we conduct simulations using parameters representative of a typical quadcopter. The quadcopter parameters and controller gains are summarized in Table 1 and Table 2, respectively.
| Parameter | Value | Unit |
|---|---|---|
| Mass, $m$ | 0.625 | kg |
| Arm length, $l$ | 0.1275 | m |
| Gravity, $g$ | 9.8 | m/s² |
| Thrust coefficient, $k$ | 7.2 × 10⁻⁷ | N·m·s²/rad² |
| Drag coefficient, $c$ | 1.98 × 10⁻⁶ | N·s²/rad² |
| Moment of inertia, $I_x$ | 2.6 × 10⁻³ | kg·m² |
| Moment of inertia, $I_y$ | 2.4 × 10⁻³ | kg·m² |
| Moment of inertia, $I_z$ | 2.6 × 10⁻³ | kg·m² |
| Parameter | Value |
|---|---|
| Proportional gain, $K_p$ | diag(100, 100, 100) |
| Integral gain, $K_i$ | diag(60, 60, 60) |
| Derivative gain, $K_d$ | diag(10, 10, 10) |
| Disturbance bound, $L_0$ | 10 |
| Attitude gain, $K_1$ | diag(200, 200, 200) |
| Euler gain, $K_\Theta$ | diag(400, 400, 400) |
| Angular rate gain, $K_\omega$ | diag(60, 60, 60) |
The simulation results demonstrate the effectiveness of the proposed control strategy. Under disturbance-free conditions, the quadcopter accurately tracks the reference trajectory with position errors converging to within ±0.01 m and attitude errors rapidly approaching zero. When external disturbances are introduced, such as $\Delta_1 = 50(1 + \sin t + \cos t) (I_x, I_y, I_z)^T$ and $\Delta_2 = 0.5(1 + \sin t + \cos t) (I_x, I_y, I_z)^T$, the system maintains stable tracking with minor fluctuations in errors, highlighting the robustness of the disturbance compensation mechanism. The quadcopter’s performance is consistently reliable, affirming the suitability of the approach for real-world applications where unknown disturbances are prevalent.
In conclusion, this paper presents a robust control strategy for trajectory tracking of quadcopter UAVs under unknown external disturbances. By integrating a PID position controller with disturbance compensation and an INDI attitude controller, the system achieves high precision and stability. Future work will focus on extending the controller to handle more complex scenarios, such as aggressive maneuvers and multi-quadcopter coordination, and implementing the strategy on physical quadcopter platforms for experimental validation.