In recent years, the quadrotor drone has emerged as a pivotal platform in various civilian and military applications, ranging from aerial photography and surveillance to search and rescue operations. Its popularity stems from its simple mechanical structure, cost-effectiveness, and high maneuverability. However, the quadrotor drone is an underactuated system with six degrees of freedom and only four control inputs, exhibiting inherent nonlinearities, strong couplings, and sensitivity to external disturbances. These characteristics pose significant challenges for achieving precise trajectory tracking, especially in environments with unknown disturbances such as wind gusts or payload variations. As a quadrotor drone operates, external disturbances can degrade control performance, leading to tracking errors or even instability. Therefore, developing robust control strategies that compensate for disturbances is crucial for enhancing the reliability and autonomy of quadrotor drones.
This article addresses the trajectory tracking problem for a quadrotor drone subjected to unknown external disturbances. I propose a control strategy based on disturbance compensation, which combines a Proportional-Integral-Derivative (PID) controller for the position loop with a disturbance estimator, and an Incremental Nonlinear Dynamic Inversion (INDI) controller for the attitude loop. The position loop PID controller is augmented with a disturbance observer to estimate and compensate for external disturbances, while the attitude loop INDI controller inherently incorporates an implicit disturbance observer, offering robust performance against uncertainties. Through detailed mathematical modeling, controller design, and simulation experiments, I demonstrate that this strategy ensures stable trajectory tracking for the quadrotor drone under disturbance conditions. The goal is to provide a comprehensive framework that balances simplicity and robustness, making it suitable for real-world implementations.
The quadrotor drone’s dynamic model is derived using the Newton-Euler formalism. Consider a quadrotor drone as a rigid body with four rotors arranged in a cross configuration. Let the inertial frame be defined by axes $X_E$, $Y_E$, $Z_E$, and the body frame be attached to the quadrotor drone’s center of mass with axes $X_B$, $Y_B$, $Z_B$. The position of the quadrotor drone in the inertial frame is denoted by $\xi = (x, y, z)^T \in \mathbb{R}^3$, and its velocity is $v = (v_x, v_y, v_z)^T$. The orientation is represented by Euler angles $\Theta = (\phi, \theta, \psi)^T$, where $\phi$, $\theta$, and $\psi$ are the roll, pitch, and yaw angles, respectively. The angular velocity in the body frame is $\omega = (\omega_x, \omega_y, \omega_z)^T$. The translational and rotational dynamics are given by:
$$\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 $g$ is the gravitational acceleration, $e_z = (0, 0, 1)^T$ is the unit vector, $\tau_f$ is the total thrust generated by the four rotors, $m$ is the mass of the quadrotor drone, $R$ is the rotation matrix from the body frame to the inertial frame, $I = \text{diag}(I_x, I_y, I_z)$ is the inertia matrix, $\tau = (\tau_1, \tau_2, \tau_3)^T$ is the torque vector, and $\Delta_1$ and $\Delta_2$ represent lumped external disturbances affecting the translational and rotational dynamics, respectively. The rotation matrix $R$ is:
$$R = \begin{bmatrix}
c_\psi c_\theta & c_\psi s_\theta s_\phi – s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\
s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi – c_\psi s_\phi \\
-s_\theta & c_\theta s_\phi & c_\theta c_\phi
\end{bmatrix},$$
where $c_\cdot$ and $s_\cdot$ denote cosine and sine functions. The matrix $W$ transforms angular velocities to Euler angle rates:
$$W = \begin{bmatrix}
1 & \sin \phi \tan \theta & \cos \phi \tan \theta \\
0 & \cos \phi & -\sin \phi \\
0 & \sin \phi \sec \theta & \cos \phi \sec \theta
\end{bmatrix}.$$
The control inputs for the quadrotor drone are related to rotor speeds. For a standard quadrotor drone, the total thrust $\tau_f$ and torques $\tau$ are expressed as:
$$\tau_f = k (\Omega_1^2 + \Omega_2^2 + \Omega_3^2 + \Omega_4^2),$$
$$\tau_1 = l k (-\Omega_2^2 + \Omega_4^2),$$
$$\tau_2 = l k (-\Omega_1^2 + \Omega_3^2),$$
$$\tau_3 = c (-\Omega_1^2 + \Omega_2^2 – \Omega_3^2 + \Omega_4^2),$$
where $\Omega_i$ are rotor angular speeds, $l$ is the arm length, $k$ is the thrust coefficient, and $c$ is the drag coefficient. However, for controller design, I focus on the high-level dynamics, and the control inputs are $\tau_f$ and $\tau$.
To facilitate controller design, I redefine the system dynamics. Let $x_1 = \xi$, $x_2 = v$, and $x_3 = \tau_f’ + \Delta_1$, where $\tau_f’ = -g e_z + \frac{\tau_f}{m} R e_z$. Then, the translational dynamics become:
$$\dot{x}_1 = x_2,$$
$$\dot{x}_2 = x_3 = \tau_f’ + \Delta_1.$$
Similarly, for rotational dynamics, let $x_4 = \Theta$, $x_5 = W \omega$, then:
$$\dot{x}_4 = x_5,$$
$$\dot{x}_5 = F(x_4, x_5) + \Delta_{2\delta} + \tau’,$$
where $F(x_4, x_5) = -W I^{-1} (W x_5) \times I W^{-1} x_5$, $\Delta_{2\delta} = W I^{-1} \Delta_2$, and $\tau’ = W I^{-1} \tau$. This formulation separates the dynamics into cascaded loops, enabling a hierarchical control approach.
The control objective is to ensure that the quadrotor drone tracks a desired trajectory $(\xi_{\text{ref}}, \psi_{\text{ref}}) = (x_{\text{ref}}, y_{\text{ref}}, z_{\text{ref}}, \psi_{\text{ref}})^T$ despite unknown disturbances $\Delta_1$ and $\Delta_2$. The proposed control strategy is illustrated in the block diagram below, which outlines the overall structure with position and attitude loops.
| Symbol | Description | Unit |
|---|---|---|
| $\xi$ | Position vector in inertial frame | m |
| $v$ | Velocity vector in inertial frame | m/s |
| $\Theta$ | Euler angles (roll, pitch, yaw) | rad |
| $\omega$ | Angular velocity in body frame | rad/s |
| $m$ | Mass of quadrotor drone | kg |
| $I$ | Inertia matrix | kg·m² |
| $g$ | Gravitational acceleration | m/s² |
| $\tau_f$ | Total thrust | N |
| $\tau$ | Torque vector | N·m |
| $\Delta_1, \Delta_2$ | External disturbances | N, N·m |
For the position loop, I design a PID controller enhanced with a disturbance estimator. Define the position error as $e_1 = x_1 – x_{1,\text{ref}}$, where $x_{1,\text{ref}} = \xi_{\text{ref}}$. The error dynamics are:
$$\dot{e}_1 = e_2,$$
$$\dot{e}_2 = \tau_f’ + \Delta_1 – \ddot{x}_{1,\text{ref}}.$$
To achieve $e_1 \to 0$, I propose the control law:
$$\tau_f’ = -K_p e_1 – K_i \int e_1 \, dt – K_d \dot{e}_1 + \ddot{x}_{1,\text{ref}} – \hat{\Delta}_1,$$
where $K_p$, $K_i$, and $K_d$ are positive definite diagonal gain matrices, and $\hat{\Delta}_1$ is the estimate of $\Delta_1$ provided by a disturbance observer. This compensation term $\hat{\Delta}_1$ aims to cancel out the actual disturbance, improving robustness. The stability of this controller can be analyzed by substituting the control law into the error dynamics, yielding a closed-loop system that, under accurate disturbance estimation, reduces to a linear stable system.
The disturbance observer for $\Delta_1$ is designed based on the super-twisting algorithm, which is effective for estimating disturbances with bounded derivatives. Assume that $|\dot{\Delta}_1| \leq L_0$, where $L_0$ is a known positive constant. Define the observer states as $z_0$, $z_1$, and $z_2$, which estimate $x_1$, $x_2$, and $\Delta_1$, respectively. The observer dynamics are:
$$v_0 = -\lambda_2 |z_0 – x_1|^{2/3} \text{sign}(z_0 – x_1) + z_1,$$
$$\dot{z}_0 = v_0,$$
$$v_1 = -\lambda_1 |z_1 – v_0|^{1/2} \text{sign}(z_1 – v_0) + z_2,$$
$$\dot{z}_1 = v_1 + \tau_f’ + z_2,$$
$$\dot{z}_2 = -\lambda_0 \text{sign}(z_2 – v_1),$$
where $\lambda_0$, $\lambda_1$, and $\lambda_2$ are observer gains chosen as $\lambda_0 = 1.1 L_0$, $\lambda_1 = 1.5 L_0^{1/2}$, and $\lambda_2 = 2 L_0^{1/3}$. This observer ensures finite-time convergence of the estimation errors, i.e., $z_0 \to x_1$, $z_1 \to x_2$, and $z_2 \to \Delta_1$, provided that $L_0$ is an upper bound on the disturbance derivative. The estimated disturbance $\hat{\Delta}_1 = z_2$ is then used in the position control law.
Once $\tau_f’$ is computed, the desired attitude angles and total thrust are derived through attitude computation. From the expression $\tau_f’ = -g e_z + \frac{\tau_f}{m} R e_z$, I solve for $\phi_{\text{ref}}$, $\theta_{\text{ref}}$, and $\tau_f$. Let $\tau_f’ = (\tau_{f1}’, \tau_{f2}’, \tau_{f3}’)^T$. Then, the desired roll and pitch angles are:
$$\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),$$
and the total thrust is:
$$\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].$$
These equations provide the reference signals for the attitude loop, ensuring that the quadrotor drone generates the required thrust and orientation to track the desired position trajectory.
For the attitude loop, I employ an Incremental Nonlinear Dynamic Inversion (INDI) controller, which offers robustness against model uncertainties and disturbances. The INDI approach leverages incremental dynamics, reducing reliance on accurate model knowledge. Define the attitude error as $e_4 = \Theta – \Theta_{\text{ref}}$, where $\Theta_{\text{ref}} = (\phi_{\text{ref}}, \theta_{\text{ref}}, \psi_{\text{ref}})^T$. The error dynamics are:
$$\dot{e}_4 = e_5,$$
$$\dot{e}_5 = F(x_4, x_5) + \Delta_{2\delta} + \tau’ – \ddot{\Theta}_{\text{ref}}.$$
In INDI, I consider the incremental form of the dynamics. Let $\tau_d = \Delta_{2\delta}$ represent the lumped disturbance in the attitude loop. From the rotational dynamics, at time step $k$, the angular acceleration is:
$$I \dot{\omega}_k = -\omega_k \times I \omega_k + \tau_k + \tau_{d,k}.$$
Assuming small changes between sampling intervals, the incremental dynamics can be approximated as:
$$\dot{\omega}_{k+1} \approx \dot{\omega}_f_k + I^{-1} (\tau_{k+1} – \tau_{f,k}),$$
where $\dot{\omega}_f_k$ and $\tau_{f,k}$ are filtered values of angular acceleration and control torque from the previous step, respectively. This approximation holds under high sampling rates, where $\omega_{k+1} \times I \omega_{k+1} \approx \omega_f_k \times I \omega_f_k$. The INDI control law is then:
$$\tau’ = \tau_{f,k} + I (\dot{\omega}_{\text{des}} – \dot{\omega}_f_k),$$
where $\dot{\omega}_{\text{des}}$ is the desired angular acceleration, computed using a proportional-derivative (PD) controller on attitude and angular rate errors:
$$\dot{\omega}_{\text{des}} = -K_\Theta e_4 – K_\omega e_\omega,$$
with $e_\omega = \omega – \omega_{\text{des}}$ and $\omega_{\text{des}} = -K_1 e_4$. Here, $K_\Theta$, $K_\omega$, and $K_1$ are diagonal gain matrices. The INDI controller implicitly includes a disturbance observer, as the term $\tau_{f,k}$ encapsulates past disturbances, providing robustness without explicit estimation. This makes the quadrotor drone’s attitude control resilient to external perturbations.
| Parameter | Symbol | Value | Description |
|---|---|---|---|
| Position PID gains | $K_p$ | $\text{diag}(100, 100, 100)$ | Proportional gains for x, y, z |
| $K_i$ | $\text{diag}(60, 60, 60)$ | Integral gains | |
| $K_d$ | $\text{diag}(10, 10, 10)$ | Derivative gains | |
| Disturbance observer gain | $L_0$ | 10 | Upper bound on disturbance derivative |
| Attitude PD gains | $K_1$ | $\text{diag}(200, 200, 200)$ | Gain for desired angular rate |
| $K_\Theta$ | $\text{diag}(400, 400, 400)$ | Proportional gain for attitude error | |
| $K_\omega$ | $\text{diag}(60, 60, 60)$ | Derivative gain for angular rate error |
To validate the proposed control strategy, I conduct simulation experiments using MATLAB/Simulink. The quadrotor drone parameters are chosen to reflect a typical small-scale quadrotor drone, as listed in Table 3. These parameters ensure the simulations are realistic and comparable to practical quadrotor drone systems.
| 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·s²/rad² |
| Drag coefficient, $c$ | 1.98 × 10⁻⁶ | N·s²/rad² |
| Inertia, $I_x$ | 2.6 × 10⁻³ | kg·m² |
| Inertia, $I_y$ | 2.4 × 10⁻³ | kg·m² |
| Inertia, $I_z$ | 2.6 × 10⁻³ | kg·m² |
The reference trajectory is set as a helical path to test the quadrotor drone’s tracking capabilities in three dimensions. The desired position is given by:
$$x_{\text{ref}}(t) = 2 \sin(0.5 t),$$
$$y_{\text{ref}}(t) = 2 \cos(0.5 t),$$
$$z_{\text{ref}}(t) = 0.5 t,$$
$$\psi_{\text{ref}}(t) = 0,$$
for $t \in [0, 20]$ seconds. This trajectory challenges the quadrotor drone with continuous changes in position and orientation. To assess robustness, I introduce external disturbances: $\Delta_1 = 50 (1 + \sin t + \cos t) (1, 1, 1)^T$ N for translational dynamics, and $\Delta_2 = 0.5 (1 + \sin t + \cos t) (1, 1, 1)^T$ N·m for rotational dynamics. These disturbances represent time-varying forces and torques acting on the quadrotor drone, simulating realistic environmental effects.
I compare the performance of the proposed disturbance-compensation-based controller with a standard PID controller without disturbance compensation. The simulation results are analyzed in terms of tracking errors and control efforts. The position tracking error is defined as $e_{\text{pos}} = \| \xi – \xi_{\text{ref}} \|_2$, and the attitude tracking error as $e_{\text{att}} = \| \Theta – \Theta_{\text{ref}} \|_2$. Additionally, I compute integral absolute error (IAE) and integral squared error (ISE) metrics to quantify performance.
| Metric | Proposed Controller (with disturbance) | Standard PID (with disturbance) | Proposed Controller (no disturbance) |
|---|---|---|---|
| IAE for position (m·s) | 0.125 | 1.842 | 0.032 |
| ISE for position (m²·s) | 0.018 | 0.956 | 0.001 |
| IAE for attitude (rad·s) | 0.041 | 0.253 | 0.015 |
| ISE for attitude (rad²·s) | 0.003 | 0.128 | 0.0004 |
| Maximum position error (m) | 0.05 | 0.35 | 0.01 |
| Maximum attitude error (rad) | 0.02 | 0.15 | 0.005 |
The simulation results demonstrate that the proposed control strategy effectively mitigates the impact of disturbances. In the absence of disturbances, both controllers perform well, but the proposed controller shows slightly better accuracy due to its optimized gains. When disturbances are present, the standard PID controller exhibits significant tracking errors, with position errors up to 0.35 m and attitude errors up to 0.15 rad. In contrast, the proposed controller maintains errors within 0.05 m for position and 0.02 rad for attitude, highlighting its robustness. The disturbance observer accurately estimates $\Delta_1$, as shown in Figure 1, where the estimated disturbance closely matches the actual disturbance profile. The INDI controller for the attitude loop also handles $\Delta_2$ effectively, maintaining stable orientation control.
Further analysis involves examining the control inputs. The total thrust $\tau_f$ and torques $\tau$ generated by the proposed controller are smoother and within feasible limits, whereas the standard PID controller produces aggressive control signals that may saturate actuators in real-world quadrotor drones. This underscores the practicality of the disturbance-compensation approach for ensuring safe and efficient operation of quadrotor drones.
The effectiveness of the quadrotor drone’s trajectory tracking can be attributed to the synergistic combination of the disturbance-compensated PID position controller and the INDI attitude controller. The position loop’s disturbance observer rapidly estimates external forces, allowing the PID controller to adjust its output accordingly. This is crucial for a quadrotor drone operating in windy conditions or carrying variable payloads. Meanwhile, the INDI attitude controller leverages incremental dynamics to reject disturbances without requiring explicit model knowledge, making it suitable for quadrotor drones with uncertain parameters. This dual-loop architecture ensures that the quadrotor drone achieves precise tracking while maintaining stability.
To provide deeper insights, I derive the stability conditions for the closed-loop system. For the position loop, substituting the control law into the error dynamics yields:
$$\ddot{e}_1 + K_d \dot{e}_1 + K_p e_1 + K_i \int e_1 \, dt = \Delta_1 – \hat{\Delta}_1.$$
If the disturbance estimation error $\tilde{\Delta}_1 = \Delta_1 – \hat{\Delta}_1$ converges to zero, the system becomes a linear PID controller with guaranteed stability for appropriate gains. Using Lyapunov analysis, one can show that with the super-twisting observer, the estimation error converges in finite time, ensuring bounded tracking errors. Similarly, for the attitude loop, the INDI controller leads to error dynamics:
$$\dot{e}_\omega + K_\omega e_\omega + K_\Theta e_4 = \tilde{\tau}_d,$$
where $\tilde{\tau}_d$ represents residual disturbances after compensation. Since INDI inherently compensates for disturbances through incremental updates, $\tilde{\tau}_d$ is small, and the PD gains stabilize the system. Thus, the overall quadrotor drone control system is robust to disturbances.
In practical applications, the quadrotor drone may face additional challenges such as sensor noise, communication delays, or actuator failures. Future work could extend this control strategy to address these issues. For instance, adaptive techniques could be integrated to tune controller gains online, enhancing the quadrotor drone’s adaptability to changing environments. Moreover, machine learning methods could be employed to improve disturbance prediction, further refining the compensation. Experimental validation on a physical quadrotor drone platform would also be valuable to confirm simulation findings.
In conclusion, this article presents a disturbance-compensation-based control strategy for trajectory tracking of a quadrotor drone. The position loop employs a PID controller with a disturbance observer, while the attitude loop uses an INDI controller. Simulation results validate that this approach ensures accurate tracking under unknown external disturbances, outperforming standard PID control. The quadrotor drone’s performance is characterized by low tracking errors and smooth control inputs, demonstrating the strategy’s robustness and practicality. As quadrotor drones continue to proliferate in diverse fields, such advanced control methods will be essential for reliable autonomous operation. Future research will focus on real-time implementation and extension to multi-agent quadrotor drone systems for cooperative tasks.