In recent years, quadrotor unmanned aerial vehicles (UAVs) have gained significant attention due to their versatility in applications such as surveillance, agriculture, and logistics. However, achieving precise trajectory tracking in the presence of uncertainties and external disturbances remains a critical challenge. This paper addresses these issues by proposing an integrated framework for trajectory planning and robust control. We focus on minimizing the fourth-order derivative of the trajectory to ensure smooth motion and develop a composite control strategy that combines an Extended State Observer (ESO) with a Kalman Filter (KF) to enhance disturbance rejection. The effectiveness of our approach is validated through experimental studies on a quadrotor platform, demonstrating improved tracking performance under adverse conditions.
The dynamics of a quadrotor are inherently nonlinear and susceptible to factors like parameter variations and wind gusts. To tackle this, we first establish a mathematical model for the position subsystem of the quadrotor. Consider the translational dynamics derived from Newton’s second law, which can be expressed as:
$$ \ddot{x} = \frac{f}{m} (\cos\psi \sin\theta \cos\phi + \sin\psi \sin\phi) + d_x $$
$$ \ddot{y} = \frac{f}{m} (\sin\psi \sin\theta \cos\phi – \cos\psi \sin\phi) + d_y $$
$$ \ddot{z} = -g + \frac{f}{m} \cos\phi \cos\theta + d_z $$
Here, \(x, y, z\) represent the position coordinates in the inertial frame, \(\phi, \theta, \psi\) denote the roll, pitch, and yaw angles, \(g\) is the gravitational acceleration, \(m\) is the mass of the quadrotor, \(f\) is the total thrust, and \(d_x, d_y, d_z\) account for lumped disturbances including uncertainties and external forces. By defining the position vector \(\mathbf{P} = [x, y, z]^T\) and the control input vector \(\mathbf{U} = [u_x, u_y, u_z]^T\), where:
$$ u_x = \frac{f}{m} (\cos\psi \sin\theta \cos\phi + \sin\psi \sin\phi) $$
$$ u_y = \frac{f}{m} (\sin\psi \sin\theta \cos\phi – \cos\psi \sin\phi) $$
$$ u_z = -g + \frac{f}{m} \cos\phi \cos\theta $$
the system can be simplified to a second-order form:
$$ \ddot{\mathbf{P}} = \mathbf{U} + \mathbf{d} $$
where \(\mathbf{d} = [d_x, d_y, d_z]^T\) represents the lumped disturbances. We assume that these disturbances are slowly time-varying and bounded, which is a reasonable assumption in practical scenarios for a quadrotor.
Trajectory planning is crucial for generating feasible paths that the quadrotor can follow without collisions. Our method focuses on minimizing the fourth-order derivative of the trajectory to ensure smoothness and reduce abrupt changes. Given a set of waypoints, the trajectory is divided into \(k\) segments, each represented by a polynomial of degree \(n\). For instance, the desired trajectory along the X-axis is expressed as:
$$ x_d(t) =
\begin{cases}
[1, t, \ldots, t^n] \sigma_1 & t_0 \leq t \leq t_1 \\
[1, t, \ldots, t^n] \sigma_2 & t_1 \leq t \leq t_2 \\
\vdots \\
[1, t, \ldots, t^n] \sigma_k & t_{k-1} \leq t \leq t_k
\end{cases} $$
where \(\sigma_i = [\sigma_{i0}, \sigma_{i1}, \ldots, \sigma_{in}]^T\) is the parameter vector for the \(i\)-th segment, and \(n\) is chosen to be at least 7 to ensure continuity of higher-order derivatives. The optimization problem is formulated to minimize the integral of the square of the fourth-order derivative:
$$ \min \int_0^T (x_d^{(4)}(t))^2 dt = \min \sum_{i=1}^k \int_{t_{i-1}}^{t_i} (x_d^{(4)}(t))^2 dt = \min \sigma^T \mathbf{Q} \sigma $$
subject to equality constraints that ensure the trajectory passes through specified waypoints and maintains continuity at segment junctions. The matrix \(\mathbf{Q}\) is constructed from block matrices \(\mathbf{Q}_i\), which are derived from the polynomial basis functions. The constraints can be written as \(\mathbf{A}_{eq} \sigma = \mathbf{b}_{eq}\), where \(\mathbf{A}_{eq}\) includes conditions for position, velocity, and acceleration at waypoints. This approach generates smooth trajectories that are essential for stable quadrotor flight.
For trajectory tracking control, we design a composite strategy that leverages the strengths of ESO and KF to handle disturbances and measurement noise. Considering the position subsystem along one axis (e.g., X-axis), the system model with measurement noise is:
$$ \dot{\mathbf{\Xi}} = \mathbf{A} \mathbf{\Xi} + \mathbf{B}_u u_x + \mathbf{B}_d d_x $$
$$ y = \mathbf{C} \mathbf{\Xi} + v $$
where \(\mathbf{\Xi} = [x_1, x_2]^T = [x, \dot{x}]^T\), \(\mathbf{A} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}\), \(\mathbf{B}_u = [0, 1]^T\), \(\mathbf{B}_d = [0, 1]^T\), \(\mathbf{C} = [1, 0]\), and \(v\) is the measurement noise. By treating the disturbance \(d_x\) as an extended state, \(x_3 = d_x\), we obtain an augmented system:
$$ \dot{\mathbf{\Xi}}_a = \mathbf{A}_a \mathbf{\Xi}_a + \mathbf{B}_{u,a} u_x + \mathbf{G} h $$
$$ y = \mathbf{C}_a \mathbf{\Xi}_a + v $$
where \(\mathbf{\Xi}_a = [\mathbf{\Xi}^T, x_3]^T\), \(\mathbf{A}_a = \begin{bmatrix} \mathbf{A} & \mathbf{B}_d \\ \mathbf{0} & 0 \end{bmatrix}\), \(\mathbf{B}_{u,a} = [\mathbf{B}_u^T, 0]^T\), \(\mathbf{G} = [0, 0, 1]^T\), \(\mathbf{C}_a = [\mathbf{C}, 0]\), and \(h = \dot{d}_x\). The ESO is designed as:
$$ \dot{\mathbf{Z}} = \mathbf{A}_a \mathbf{Z} + \mathbf{B}_{u,a} u_x + \mathbf{K}_{eso} (y – \mathbf{C}_a \mathbf{Z}) $$
where \(\mathbf{Z} = [z_1, z_2, z_3]^T\) estimates \(\mathbf{\Xi}_a\), and \(\mathbf{K}_{eso} = \text{diag}(k_1, k_2, k_3)\) is the observer gain matrix. To balance noise suppression and dynamic performance, we integrate a KF in parallel:
$$ \dot{\mathbf{Z}} = \mathbf{A}_a \mathbf{Z} + \mathbf{B}_{u,a} u_x + \mathbf{K}_{eso} (\mathbf{C} \hat{\mathbf{\Xi}} – \mathbf{C}_a \mathbf{Z}) $$
$$ \dot{\hat{\mathbf{\Xi}}} = \mathbf{A} \hat{\mathbf{\Xi}} + \mathbf{B}_u u_x + \mathbf{B}_d z_3 + \mathbf{K}_{kf} (y – \mathbf{C} \hat{\mathbf{\Xi}}) $$
The Kalman gain \(\mathbf{K}_{kf}\) is computed recursively using the Riccati equation to minimize estimation error covariance. The control law for trajectory tracking is then formulated as:
$$ u_x = -K_p (z_1 – x_d) – K_d (z_2 – \dot{x}_d) – z_3 + \ddot{x}_d $$
where \(K_p\) and \(K_d\) are positive gains, and \(x_d, \dot{x}_d, \ddot{x}_d\) are the desired trajectory and its derivatives. This composite controller ensures that the tracking error converges to a bounded region despite disturbances and noise, as proven through Lyapunov analysis.
Experimental validation was conducted using a Crazyflie 2.1 quadrotor platform equipped with a high-precision motion capture system. The quadrotor was tasked to follow a predefined trajectory while subjected to disturbances. We compared the performance of our PD+ESO+KF controller against PID and PD+ESO controllers with different gain settings. The trajectory consisted of multiple waypoints in 3D space, with constraints on maximum velocity and acceleration to ensure feasibility. The table below summarizes the controller parameters used in the experiments:
| Controller Type | Parameters |
|---|---|
| PID | \(K_p = 4.5, K_d = 5, K_i = 1\) for X/Y; \(K_p = 5, K_d = 3\) for Z |
| PD+ESO (Small Gains) | Poles at \(-3, -3, -4\) for X, Y, Z |
| PD+ESO (Large Gains) | Poles at \(-6, -6, -7\) for X, Y, Z |
| PD+ESO+KF | Poles at \(-6, -6, -7\); KF tuned with \(R=0.01, Q=10-50\) |
The results demonstrated that the PD+ESO+KF controller achieved superior tracking performance. For instance, along the X-axis, the average tracking error was reduced by over 35.8% compared to PD+ESO with small gains, and the maximum error decreased by more than 26.7%. The standard deviation of the error also improved, indicating better consistency. The following table provides a detailed comparison of tracking errors for different controllers:
| Controller | Axis | Average Error (m) | Maximum Error (m) | Standard Deviation (m) |
|---|---|---|---|---|
| PID | X | 0.0070 | 0.0165 | 0.0079 |
| PID | Y | 0.0067 | 0.0181 | 0.0083 |
| PID | Z | 0.0587 | 0.0157 | 0.1139 |
| PD+ESO (Small Gains) | X | 0.0053 | 0.0231 | 0.0071 |
| PD+ESO (Small Gains) | Y | 0.0048 | 0.0187 | 0.0060 |
| PD+ESO (Small Gains) | Z | 0.0184 | 0.0125 | 0.0649 |
| PD+ESO (Large Gains) | X | 0.0047 | 0.0253 | 0.0066 |
| PD+ESO (Large Gains) | Y | 0.0035 | 0.0125 | 0.0043 |
| PD+ESO (Large Gains) | Z | 0.0134 | 0.0077 | 0.0579 |
| PD+ESO+KF | X | 0.0034 | 0.0160 | 0.0040 |
| PD+ESO+KF | Y | 0.0033 | 0.0137 | 0.0040 |
| PD+ESO+KF | Z | 0.0143 | 0.0080 | 0.0599 |
These results highlight the effectiveness of our approach in enhancing the robustness and accuracy of quadrotor trajectory tracking. The integration of ESO and KF allows for rapid disturbance estimation while mitigating the effects of sensor noise, which is critical for real-world applications. Future work will extend this framework to multi-quadrotor systems for coordinated missions, further exploring the challenges of swarm dynamics and communication constraints.
In conclusion, this paper presents a holistic solution for trajectory planning and anti-disturbance control of quadrotor UAVs. The trajectory planning method ensures smooth and feasible paths, while the composite control strategy robustly handles uncertainties and external disturbances. Experimental validation on a physical quadrotor platform confirms the superiority of our method over conventional approaches, paving the way for more reliable autonomous operations in complex environments.