The control of quadrotor drone systems has been a focal point of research due to their unique capabilities, such as vertical take-off and landing, omnidirectional flight, and autonomous hovering. Among these capabilities, precise trajectory tracking is fundamental for both civilian and military applications, ranging from aerial photography and delivery to surveillance and coordinated missions. While significant progress has been made in controlling a single quadrotor drone, the inherent limitations of a single agent—such as poor robustness and limited task execution scope—have driven the field towards multi-agent systems, or swarms. Controlling a swarm of quadrotor drones introduces complex challenges including inter-agent collision avoidance, communication delays, and maintaining formation stability under dynamic conditions. Addressing these challenges necessitates advanced control algorithms that are not only effective for individual agents but also scalable and robust for coordinated swarm behavior.
Traditional control methods for single quadrotor drones, such as PID control, backstepping control, and sliding mode control, often face limitations like slow convergence, chattering, or high sensitivity to model inaccuracies. Linear Quadratic Regulator (LQR) control, while powerful, requires a linearized model and may not perform optimally under highly nonlinear dynamics or aggressive maneuvers. Geometric control, formulated directly on the special Euclidean group SE(3), offers an elegant and globally almost stable nonlinear tracking solution that avoids the singularities of Euler angles and the ambiguity of quaternions. However, extending this promising framework from a single quadrotor drone to an entire swarm, while accounting for real-world swarm dynamics, remains a critical research gap.
This article presents a comprehensive analysis and a novel solution: an Improved Geometric Control for Swarm Formation (IGCSF). The IGCSF algorithm builds upon the foundation of general geometric tracking control but extends it innovatively to manage a swarm of quadrotor drones. It employs a leader-follower topology combined with a distributed control strategy, explicitly incorporating swarm-specific considerations like separation, cohesion, and migration forces, as well as robustness against communication delays. The primary contributions are threefold: first, the design of the IGCSF controller that significantly enhances swarm trajectory tracking performance compared to baseline geometric and LQR controllers; second, a formal stability analysis proving the asymptotic stability of the entire swarm system under the proposed control laws; and third, extensive simulation validation using an AirSim/Matlab co-simulation environment, demonstrating the algorithm’s superior accuracy, convergence speed, and formation stability under various challenging trajectories and communication constraints.
Dynamical Model of a Quadrotor Drone
Accurate control begins with a precise dynamical model. A quadrotor drone is an underactuated system with six degrees of freedom (position and orientation) controlled by only four independent motor thrusts. The following sections detail the coordinate transformations and the equations of motion.
Coordinate Transformation and Rotation Matrix
The transformation from the body-fixed frame (B) to the inertial world frame (I) is described by a rotation matrix $\mathbf{R} \in SO(3)$, parameterized by the Euler angles $(\phi, \theta, \psi)$ for roll, pitch, and yaw, respectively.
$$ \mathbf{R} = \begin{bmatrix}
\cos\theta \cos\psi & \sin\phi \sin\theta \cos\psi – \cos\phi \sin\psi & \cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi \\
\cos\theta \sin\psi & \sin\phi \sin\theta \sin\psi + \cos\phi \cos\psi & \cos\phi \sin\theta \sin\psi – \sin\phi \cos\psi \\
-\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta
\end{bmatrix} $$
This matrix $\mathbf{R}$ is crucial as it projects vectors (like thrust) from the body frame into the inertial frame.
Translational Dynamics
The translational motion is governed by Newton’s second law. The net force $\mathbf{F}$ acting on the quadrotor drone of mass $m$ is the sum of rotor thrust $\mathbf{F}_s$, gravity $\mathbf{F}_g$, and aerodynamic drag $\mathbf{F}_f$.
$$ \mathbf{F} = \mathbf{F}_s – \mathbf{F}_g – \mathbf{F}_f = m \dot{\mathbf{v}} $$
$$ \mathbf{F}_s = \mathbf{R} \begin{bmatrix} 0 \\ 0 \\ k_s \sum_{i=0}^{3} \omega_i^2 \end{bmatrix}, \quad \mathbf{F}_g = \begin{bmatrix} 0 \\ 0 \\ mg \end{bmatrix}, \quad \mathbf{F}_f = \begin{bmatrix} k_x \dot{x} \\ k_y \dot{y} \\ k_z \dot{z} \end{bmatrix} $$
where $k_s$ is the thrust coefficient, $\omega_i$ is the angular speed of motor $i$, $g$ is gravitational acceleration, and $k_x, k_y, k_z$ are linear drag coefficients. The translational equations in the inertial frame are:
$$ \begin{aligned}
\ddot{x} &= \frac{k_s \sum \omega_i^2}{m} (\cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi) – \frac{k_x \dot{x}}{m} \\
\ddot{y} &= \frac{k_s \sum \omega_i^2}{m} (\cos\phi \sin\theta \sin\psi – \sin\phi \cos\psi) – \frac{k_y \dot{y}}{m} \\
\ddot{z} &= \frac{k_s \sum \omega_i^2}{m} \cos\phi \cos\theta – g – \frac{k_z \dot{z}}{m}
\end{aligned} $$
Rotational Dynamics
The rotational dynamics are derived from the Euler equation. The net moment $\mathbf{M}$ acting on the quadrotor drone is the sum of moments from rotor thrusts $\mathbf{M}_s$, gyroscopic effects $\mathbf{M}_l$ (often neglected for simplicity), and aerodynamic moments $\mathbf{M}_f$.
$$ \mathbf{M} = \mathbf{M}_s + \mathbf{M}_l + \mathbf{M}_f = \mathbf{I} \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times \mathbf{I} \boldsymbol{\omega} $$
where $\mathbf{I} = \text{diag}(I_x, I_y, I_z)$ is the inertia matrix and $\boldsymbol{\omega} = [\dot{\phi}, \dot{\theta}, \dot{\psi}]^T$ is the angular velocity vector. Considering a standard “X” configuration where motors 0 and 2 rotate clockwise, and motors 1 and 3 rotate counter-clockwise, the control moments are:
$$ \mathbf{M}_s = \begin{bmatrix}
M_\phi \\ M_\theta \\ M_\psi
\end{bmatrix} = \begin{bmatrix}
k_s d (\omega_2^2 – \omega_3^2) \\
k_s d (\omega_1^2 – \omega_0^2) \\
k_d (\omega_2^2 + \omega_3^2 – \omega_0^2 – \omega_1^2)
\end{bmatrix} $$
where $d$ is the arm length and $k_d$ is the drag torque coefficient. The resulting rotational equations are:
$$ \begin{aligned}
\ddot{\phi} &= \frac{(I_y – I_z) \dot{\theta} \dot{\psi} + k_s d (\omega_2^2 – \omega_3^2)}{I_x} \\
\ddot{\theta} &= \frac{(I_z – I_x) \dot{\phi} \dot{\psi} + k_s d (\omega_1^2 – \omega_0^2)}{I_y} \\
\ddot{\psi} &= \frac{(I_x – I_y) \dot{\phi} \dot{\theta} + k_d (\omega_2^2 + \omega_3^2 – \omega_0^2 – \omega_1^2)}{I_z}
\end{aligned} $$
| Parameter | Symbol | Value | Unit |
|---|---|---|---|
| Mass | $m$ | 1.000 | kg |
| Gravity | $g$ | 9.810 | m/s² |
| Thrust Coefficient | $k_s$ | 0.1099 | N/(rad/s)² |
| Drag Torque Coefficient | $k_d$ | 0.0402 | N·m/(rad/s)² |
| Arm Length | $d$ | 0.2275 | m |
| Rotor Diameter | $D$ | 0.2286 | m |
| Moment of Inertia (x) | $I_x$ | 0.0081 | kg·m² |
| Moment of Inertia (y) | $I_y$ | 0.0081 | kg·m² |
| Moment of Inertia (z) | $I_z$ | 0.0142 | kg·m² |
IGCSF Controller Design for Quadrotor Drone Swarms
The core of the proposed solution is the Improved Geometric Control for Swarm Formation (IGCSF). This controller operates on two interconnected levels: the low-level geometric tracking control for each individual quadrotor drone, and the high-level distributed swarm control that coordinates multiple agents.
Low-Level Geometric Tracking Control
For a single quadrotor drone, the geometric controller aims to track a desired trajectory defined by position $\mathbf{p}_d(t)$, velocity $\mathbf{v}_d(t)$, acceleration $\ddot{\mathbf{p}}_d(t)$, and yaw angle $\psi_d(t)$. We define the following tracking errors:
$$ \begin{aligned}
\mathbf{e}_p &= \mathbf{p} – \mathbf{p}_d \\
\mathbf{e}_v &= \mathbf{v} – \mathbf{v}_d \\
\mathbf{e}_R &= \frac{1}{2} (\mathbf{R}_d^T \mathbf{R} – \mathbf{R}^T \mathbf{R}_d)^\vee \\
\boldsymbol{e}_\omega &= \boldsymbol{\omega} – \mathbf{R}^T \mathbf{R}_d \boldsymbol{\omega}_d
\end{aligned} $$
where $\mathbf{e}_R$ is the attitude error on $SO(3)$ and $^\vee$ is the vee map. The desired attitude $\mathbf{R}_d = [\mathbf{x}_b, \mathbf{y}_b, \mathbf{z}_b]$ is constructed from the desired acceleration and yaw:
$$ \begin{aligned}
\mathbf{z}_b &= \frac{\ddot{\mathbf{p}}_d + g\mathbf{e}_3}{\|\ddot{\mathbf{p}}_d + g\mathbf{e}_3\|} \\
\mathbf{y}_b &= \mathbf{z}_b \times \begin{bmatrix} \cos\psi_d & \sin\psi_d & 0 \end{bmatrix}^T \\
\mathbf{x}_b &= \mathbf{y}_b \times \mathbf{z}_b
\end{aligned} $$
where $\mathbf{e}_3 = [0, 0, 1]^T$. The control law computes the desired total thrust vector $\mathbf{F}_d$ and moment vector $\mathbf{M}_d$:
$$ \begin{aligned}
\mathbf{F}_d &= (k_p \mathbf{e}_p + k_v \mathbf{e}_v + mg\mathbf{e}_3 – m \ddot{\mathbf{p}}_d) \cdot \mathbf{R} \mathbf{e}_3 \\
\boldsymbol{M}_d &= -k_R \mathbf{e}_R – k_\omega \boldsymbol{e}_\omega + \boldsymbol{\omega} \times \mathbf{I} \boldsymbol{\omega} – \mathbf{I}(\boldsymbol{\omega}^\wedge \mathbf{R}^T \mathbf{R}_d \boldsymbol{\omega}_d – \mathbf{R}^T \mathbf{R}_d \dot{\boldsymbol{\omega}}_d)
\end{aligned} $$
Here, $k_p, k_v, k_R, k_\omega$ are positive definite gain matrices. The control outputs are then transformed into individual motor thrust commands $f_i$ via the allocation matrix:
$$ \begin{bmatrix} f_0 \\ f_1 \\ f_2 \\ f_3 \end{bmatrix} = k_s \rho \omega_{\text{max}}^2 D^4 \begin{bmatrix} 1 & 1 & 1 & 1 \\ -\frac{d}{\sqrt{2}} & \frac{d}{\sqrt{2}} & \frac{d}{\sqrt{2}} & -\frac{d}{\sqrt{2}} \\ \frac{d}{\sqrt{2}} & -\frac{d}{\sqrt{2}} & \frac{d}{\sqrt{2}} & -\frac{d}{\sqrt{2}} \\ k_{fM} & k_{fM} & -k_{fM} & -k_{fM} \end{bmatrix}^{-1} \begin{bmatrix} F_d \\ M_x \\ M_y \\ M_z \end{bmatrix} $$
where $\rho$ is air density, $\omega_{\text{max}}$ is maximum rotor speed, and $k_{fM}$ is the force-to-moment scaling factor.
High-Level Distributed Swarm Control
To coordinate a swarm of $N$ quadrotor drones, we adopt a leader-follower topology with a single leader (agent 0) and multiple followers (agents $i=1,…,N$). The leader tracks the global desired trajectory, while followers maintain a prescribed formation relative to the leader and each other. The distributed control is inspired by Reynolds’ rules: separation, cohesion, and alignment, augmented with a migration force towards the target.
For follower $i$, the interaction force $\boldsymbol{\Upsilon}_i$ from its neighbors $j \in \mathcal{N}_i$ within a perception radius $r_{\text{max}}$ is:
$$ \boldsymbol{\Upsilon}_i = \underbrace{-k_{\text{sep}} \sum_{j \in \mathcal{N}_i} \frac{\mathbf{r}_{ij}}{\|\mathbf{r}_{ij}\|^2}}_{\text{Separation}} + \underbrace{k_{\text{coh}} \sum_{j \in \mathcal{N}_i} \mathbf{r}_{ij}}_{\text{Cohesion}} + \underbrace{k_{\text{mig}} \frac{\mathbf{r}_{\text{mig},i}}{\|\mathbf{r}_{\text{mig},i}\|}}_{\text{Migration}} $$
where $\mathbf{r}_{ij} = \mathbf{p}_j – \mathbf{p}_i$, $\mathbf{r}_{\text{mig},i} = \mathbf{p}_{\text{des},i} – \mathbf{p}_i$, and $k_{\text{sep}}, k_{\text{coh}}, k_{\text{mig}}$ are tuning gains. This interaction force modifies the desired velocity for the low-level geometric controller of follower $i$.
Furthermore, we explicitly model communication delays, a critical practical concern in quadrotor drone swarms. The dynamics of follower $i$ under delayed state information are:
$$ \dot{\mathbf{x}}_i(t) = A \mathbf{x}_i(t) + B \mathbf{u}_i(t) + C \mathbf{h}_i(t) $$
where $\mathbf{h}_i(t)$ represents external disturbances. The distributed formation tracking protocol with time delay $d_t$ is:
$$ \mathbf{u}_i(t) = -K_1 \sum_{j=1}^{N} a_{ij} (\mathbf{x}_j(t-d_t) – \mathbf{x}_i(t-d_t)) + K_2 \mathbf{h}_i(t) $$
Here, $a_{ij}$ are elements of the adjacency matrix defining the communication graph, and $K_1, K_2$ are control gain matrices designed to ensure swarm stability despite delays.
Stability Analysis of the IGCSF Framework
The stability of the entire swarm system under the IGCSF controller is established using Lyapunov theory. We consider a composite Lyapunov candidate function $V = V_1 + V_2$, where $V_1$ addresses the low-level geometric tracking errors and $V_2$ addresses the high-level formation errors under communication constraints.
Low-Level Stability ($V_1$): For the geometric controller, we use an attitude error function on $SO(3)$: $\Psi(\mathbf{R}, \mathbf{R}_d) = \frac{1}{2} \text{tr}(\mathbf{I} – \mathbf{R}_d^T \mathbf{R})$. A suitable Lyapunov function is:
$$ V_1 = \frac{1}{2} k_p \|\mathbf{e}_p\|^2 + \frac{1}{2} m \|\mathbf{e}_v\|^2 + a \mathbf{e}_p \cdot \mathbf{e}_v + \frac{1}{2} \boldsymbol{e}_\omega^T \mathbf{I} \boldsymbol{e}_\omega + k_R \Psi(\mathbf{R}, \mathbf{R}_d) + b \mathbf{e}_R^T \boldsymbol{e}_\omega $$
For appropriately chosen positive gains and constants $a, b$, it can be shown that the time derivative $\dot{V}_1$ is negative definite under the proposed control law, given initial conditions satisfy $\Psi(\mathbf{R}(0), \mathbf{R}_d(0)) < 2$ and $\|\boldsymbol{e}_\omega(0)\|^2 < \frac{2}{\lambda_{\text{min}}(\mathbf{I})} k_R (2 – \Psi(\mathbf{R}(0), \mathbf{R}_d(0)))$. This proves exponential stability of the tracking error dynamics for a single quadrotor drone.
Swarm-Level Stability ($V_2$): For the formation control with delays, consider the formation error $\mathbf{e}_i(t) = \mathbf{x}_i(t) – \mathbf{x}_d(t)$, where $\mathbf{x}_d$ is the leader’s (or target) state. We construct a Lyapunov-Krasovskii functional:
$$ V_2(t) = \mathbf{e}(t)^T (\mathbf{I}_N \otimes P) \mathbf{e}(t) + \int_{t-d_t}^{t} \mathbf{e}^T(s) e^{\gamma(t-s)} (\mathbf{I}_N \otimes Q) \mathbf{e}(s) ds + \int_{-d_t}^{0} \int_{t+\theta}^{t} \dot{\mathbf{e}}^T(s) e^{\gamma(t-s)} (\mathbf{I}_N \otimes Z) \dot{\mathbf{e}}(s) ds d\theta $$
where $\otimes$ denotes the Kronecker product, $P, Q, Z$ are positive definite matrices, and $\gamma > 0$. Under the distributed protocol and by applying the $H_\infty$ performance criterion, we can derive the condition:
$$ \int_0^T \mathbf{e}_i^T(s) \mathbf{e}_i(s) ds < e^{\gamma T} \beta^2 \int_0^T \mathbf{h}_i^T(s) \mathbf{h}_i(s) ds $$
for a finite $\beta > 0$. This inequality proves that the formation error is input-to-state stable with respect to disturbances $\mathbf{h}_i(t)$, and the swarm achieves consensus asymptotically when disturbances vanish, even in the presence of communication delays.
Theorem (Swarm Stability): For the system described by the quadrotor drone dynamics under the IGCSF control law defined in Eqs. (8) and (10), there exist control gains $k_p, k_v, k_R, k_\omega, K_1, K_2$ and constants $a, b, \beta$ such that if the initial attitude error satisfies $\Psi(\mathbf{R}(0), \mathbf{R}_d(0)) \le \beta < 1$, then the zero equilibrium of the combined tracking and formation errors $(\mathbf{e}_p, \mathbf{e}_v, \mathbf{e}_R, \boldsymbol{e}_\omega, \mathbf{e}_i)$ is asymptotically stable.
Simulation Results and Performance Evaluation
The performance of the IGCSF controller was rigorously evaluated through high-fidelity simulations using a co-simulation platform linking AirSim (for realistic physics and visualization) and Matlab/Simulink (for control algorithm implementation). A swarm of one leader and six follower quadrotor drones was tested on three distinct trajectories with increasing difficulty: a 2D circular path (no delay), a 3D cylindrical helix (0.15s delay), and a complex 3D Lemniscate (figure-eight) path (0.3s delay). The IGCSF performance was compared against two baseline controllers: the standard Geometric Controller (GC) and a Linear Quadratic Regulator (LQR) based controller.
| Gain | Symbol | Value |
|---|---|---|
| Position Gain | $k_p$ | 2.0 $\mathbf{I}_3$ |
| Velocity Gain | $k_v$ | 2.0 $\mathbf{I}_3$ |
| Attitude Gain | $k_R$ | 0.4 $\mathbf{I}_3$ |
| Angular Rate Gain | $k_\omega$ | 0.08 $\mathbf{I}_3$ |
| Separation Gain | $k_{\text{sep}}$ | 0.5 |
| Cohesion Gain | $k_{\text{coh}}$ | 0.15 |
| Migration Gain | $k_{\text{mig}}$ | 0.8 |
1. Circular Trajectory (No Communication Delay): This test evaluated basic tracking and formation-keeping. The IGCSF-controlled swarm showed minimal transient error at take-off and maintained a tight formation with near-perfect circular tracking. The average follower position error remained consistently low and stable. In contrast, both GC and LQR controllers exhibited larger initial oscillations and greater steady-state error variance among followers.
2. Cylindrical Helix Trajectory (0.15s Delay): This 3D path tested performance under spatial coupling and moderate delay. The results, visualized in AirSim, demonstrated IGCSF’s superior ability to coordinate the swarm along the ascending spiral. The tracking errors in the X, Y, and Z axes for the leader drone were significantly smaller and converged faster with IGCSF compared to the baselines. The follower drones under IGCSF adapted smoothly to the delayed state information, maintaining a cohesive helical formation, whereas the GC and LQR swarms showed visible phase lag and increased dispersion.
3. Lemniscate Trajectory (0.3s Delay): This challenging path, with its high-curvature turns, rigorously tested the limits of tracking accuracy and swarm stability under significant communication delay. The performance difference was stark. At the high-curvature points (labeled a, b, c, d), both GC and LQR controllers exhibited large spikes in tracking error and severe formation distortion. The IGCSF controller, however, managed these maneuvers effectively, with dramatically reduced error spikes and maintained formation integrity.
| Controller | Avg. Error at Point a (m) | Avg. Error at Point b (m) | Avg. Error at Point c (m) | Avg. Error at Point d (m) | Overall Mean Error (m) |
|---|---|---|---|---|---|
| Geometric Control (GC) | 2.332 | 2.188 | 0.387 | 0.699 | 1.401 |
| LQR Control | 0.758 | 0.620 | 1.662 | 1.593 | 1.158 |
| IGCSF (Proposed) | 0.944 | 0.777 | 0.865 | 1.166 | 0.938 |
The quantitative results in Table 3 are conclusive. Under the most demanding conditions (0.3s delay on a high-curvature path), the proposed IGCSF controller achieved an overall mean tracking error of 0.938 meters. This represents a 33.1% reduction compared to the standard Geometric Controller (1.401 m) and a 19.0% reduction compared to the LQR controller (1.158 m). More importantly, the error distribution for IGCSF is more consistent across all challenging points, indicating robust and predictable swarm behavior, whereas the baselines showed erratic and situation-dependent performance.
Conclusion
This article has presented the design, stability analysis, and validation of an Improved Geometric Control for Swarm Formation (IGCSF) for quadrotor drone trajectory tracking. The proposed framework successfully bridges the gap between sophisticated single-agent geometric control and the practical demands of multi-agent swarm operations. By integrating a distributed leader-follower strategy with Reynolds-inspired swarm interaction forces and explicitly accounting for communication delays, the IGCSF controller ensures not only precise trajectory tracking for each individual quadrotor drone but also robust and stable formation keeping for the entire collective.
The theoretical contribution is substantiated by a formal Lyapunov-based stability proof, guaranteeing asymptotic stability for both the tracking and formation errors. The practical superiority of IGCSF is unequivocally demonstrated through comprehensive simulations in a high-fidelity AirSim/Matlab environment. Compared to standard Geometric Control and LQR methods, IGCSF exhibits faster convergence, smaller steady-state tracking error, and significantly enhanced resilience to communication delays and high-curvature maneuvers. The algorithm’s ability to maintain a stable formation under such conditions marks a substantial advancement toward the deployment of reliable and autonomous quadrotor drone swarms for complex real-world missions. Future work will focus on experimental validation with physical drone platforms and extending the controller to handle dynamic obstacle avoidance in fully unstructured environments.