As someone deeply involved in the field of autonomous systems, I have always been fascinated by the challenge of coordinating multiple aerial vehicles. The image above perfectly captures the spectacular potential of synchronized flight. However, behind such displays—and far more critical for applications like precision surveying, collaborative payload delivery, or strategic missions—lies a complex computational problem: generating optimal trajectories for a drone formation. This task moves beyond simple path planning for a single point-mass; it requires solving a high-dimensional optimal control problem that respects the intricate dynamics of each vehicle, their physical limitations, and the paramount need for safe separation. In my work, I have focused on developing a robust framework for this very purpose. The core challenge is to compute state and control histories for every vehicle in the drone formation that minimize a desired performance index, such as total mission time or energy consumption, while strictly adhering to a web of constraints. This article details my approach, from modeling and problem formulation to the numerical solution strategy and simulation outcomes, for a six-agent quadrotor drone formation.
The foundation of any credible trajectory optimization is an accurate dynamic model. For a quadrotor, the standard approach employs the Newton-Euler formulation. One defines two key coordinate frames: the body-fixed frame \(\{x_B, y_B, z_B\}\) attached to the vehicle’s center of mass, and the inertial world frame \(\{x_W, y_W, z_W\}\). The vehicle’s state is described by 12 variables: position \(\mathbf{p} = [x, y, z]^T\), velocity \(\mathbf{v} = [v_x, v_y, v_z]^T\), attitude (Euler angles) \(\mathbf{\Theta} = [\phi, \theta, \psi]^T\), and body-axis angular rates \(\mathbf{\omega} = [p, q, r]^T\).
Assuming a symmetric and rigid structure, and operating under common simplifications for near-hover or low-speed flight (neglecting aerodynamic effects like ground effect and assuming small attitude angles), the nonlinear dynamics can be significantly simplified. Under the small-angle assumption, the relationship between angular rates and Euler angle derivatives simplifies to \(\mathbf{\omega} \approx \dot{\mathbf{\Theta}}\). The resulting simplified dynamics model, which forms the basis for my optimization, is given by:
$$
\begin{aligned}
\dot{x} &= v_x, \quad \dot{y} = v_y, \quad \dot{z} = v_z, \\
\dot{v}_x &= \frac{U_1}{m} (\cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi), \\
\dot{v}_y &= \frac{U_1}{m} (\cos\phi \sin\theta \sin\psi – \sin\phi \cos\psi), \\
\dot{v}_z &= \frac{U_1}{m} (\cos\phi \cos\theta) – g, \\
\dot{\phi} &= p, \quad \dot{\theta} = q, \quad \dot{\psi} = r, \\
\dot{p} &= \frac{l U_2}{I_{xx}} + q r \frac{I_{yy} – I_{zz}}{I_{xx}}, \\
\dot{q} &= \frac{l U_3}{I_{yy}} + p r \frac{I_{zz} – I_{xx}}{I_{yy}}, \\
\dot{r} &= \frac{U_4}{I_{zz}} + p q \frac{I_{xx} – I_{yy}}{I_{zz}}.
\end{aligned}
$$
Here, \(m\) is the mass, \(g\) is gravitational acceleration, \(l\) is the arm length from the center of mass to each rotor, and \(I_{xx}, I_{yy}, I_{zz}\) are the moments of inertia. The control inputs \(\mathbf{U} = [U_1, U_2, U_3, U_4]^T\) are related to the squared rotational speeds of the four rotors, \(\boldsymbol{\Omega} = [\omega_1^2, \omega_2^2, \omega_3^2, \omega_4^2]^T\), through a linear mapping:
$$
\begin{bmatrix} U_1 \\ U_2 \\ U_3 \\ U_4 \end{bmatrix} =
\begin{bmatrix}
k_F & k_F & k_F & k_F \\
0 & -k_F l & 0 & k_F l \\
k_F l & 0 & -k_F l & 0 \\
k_M & -k_M & k_M & -k_M
\end{bmatrix}
\begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix}.
$$
The parameters \(k_F\) (thrust coefficient) and \(k_M\) (torque coefficient) are determined empirically. For the platform used in my study, these parameters were obtained through dedicated bench tests measuring force and torque against rotor speed. The physical parameters are summarized below:
| Parameter | Symbol | Value | Unit |
|---|---|---|---|
| Mass | \(m\) | 0.625 | kg |
| Arm Length | \(l\) | 0.1275 | m |
| Thrust Coefficient | \(k_F\) | 2.103 × 10⁻⁶ | N/(rad²/s²) |
| Torque Coefficient | \(k_M\) | 2.091 × 10⁻⁸ | Nm/(rad²/s²) |
| Moment of Inertia (x) | \(I_{xx}\) | 2.3 × 10⁻³ | kg·m² |
| Moment of Inertia (y) | \(I_{yy}\) | 2.4 × 10⁻³ | kg·m² |
| Moment of Inertia (z) | \(I_{zz}\) | 2.6 × 10⁻³ | kg·m² |
With a reliable model in hand, the drone formation trajectory optimization problem can be formally stated. For a formation of \(N\) identical quadrotors, the goal is to find the state trajectory \(\mathbf{X}^{(i)}(t)\) and control trajectory \(\mathbf{U}^{(i)}(t)\) for each drone \(i = 1, …, N\) that minimize a cost functional \(J\), subject to differential constraints (the dynamics), boundary conditions, and path constraints.
In my investigation, I considered a drone formation of six vehicles (\(N=6\)). The chosen cost functional was the minimum-time objective, which critically tests the agility and coordination of the entire fleet:
$$
J = t_f.
$$
The boundary conditions specify the initial and final states for each member of the drone formation. For a reconfiguration maneuver, the initial states typically represent a known hovering pattern, and the final states represent a desired target pattern. A generic set is shown below:
| Agent (i) | Initial Position \((x_0, y_0, z_0)\) | Final Position \((x_f, y_f, z_f)\) | Initial/Final Velocity | Initial Attitude |
|---|---|---|---|---|
| 1 | (0, 0, 0) | (30, 30, 30) | (0,0,0) | (0,0,0) |
| 2 | (0, 3, 0) | (30, 33, 30) | (0,0,0) | (0,0,0) |
| 3 | (3, 0, 0) | (33, 30, 30) | (0,0,0) | (0,0,0) |
| 4 | (3, 3, 0) | (33, 33, 30) | (0,0,0) | (0,0,0) |
| 5 | (3, 6, 0) | (33, 36, 30) | (0,0,0) | (0,0,0) |
| 6 | (6, 3, 0) | (36, 33, 30) | (0,0,0) | (0,0,0) |
The final attitude and angular rates were left free, requiring only a stable hover at the destination.
The path constraints are what make the drone formation problem particularly challenging. They ensure the solution is physically feasible and safe. These include:
- State Constraints: Limits on velocity, attitude, and angular rates to reflect operational envelopes and sensor limitations (e.g., \( |v_x| \leq 5 \text{ m/s}, |\phi| \leq \pi/9 \text{ rad} \)).
- Control Input Constraints: Limits on \(U_1, U_2, U_3, U_4\) derived from the maximum achievable rotor speed. From the linear mapping and a maximum \(\omega_{max}^2\), we get:
$$
U_1 \leq 4 k_F \omega_{max}^2, \quad |U_2|, |U_3| \leq \sqrt{2} k_F l \omega_{max}^2, \quad |U_4| \leq 2 k_M \omega_{max}^2.
$$ - Collision Avoidance Constraints: The most critical constraint for a drone formation. A simple yet effective “hard-shell” constraint ensures a minimum separation distance \(d_{min}\) between any two drones \(i\) and \(j\):
$$
|x^{(i)} – x^{(j)}| \geq d_{min}, \quad |y^{(i)} – y^{(j)}| \geq d_{min}, \quad |z^{(i)} – z^{(j)}| \geq d_{min}, \quad \forall i \neq j.
$$
In my study, \(d_{min}\) was set to 0.5 meters, though a larger value is often used in practice.
To solve this complex, constrained optimal control problem, I employed a direct method known as the hp-adaptive pseudospectral method. Traditional direct transcription methods discretize the time horizon into a fixed mesh, converting the infinite-dimensional problem into a finite-dimensional Nonlinear Programming (NLP) problem. Pseudospectral methods are a sophisticated class of direct transcription methods that use global polynomial approximations at carefully chosen collocation points (typically the roots of orthogonal polynomials like Legendre or Chebyshev polynomials) to achieve high accuracy with relatively few nodes.
The standard Gauss pseudospectral method (GPM) discretizes the state and control variables at Legendre-Gauss (LG) points. The dynamics are transcribed into algebraic constraints by ensuring the derivative of the state polynomial approximation matches the dynamics at these collocation points. The cost functional and constraints are similarly discretized. However, a fixed mesh in GPM can be inefficient for problems where the solution has varying smoothness, requiring many nodes to capture rapid variations or discontinuities, which unnecessarily increases the problem size.
This is where the hp-adaptive framework shines for a drone formation problem. The “h” refers to the width of time segments (mesh intervals), and “p” refers to the polynomial degree within each segment. The method starts with a coarse mesh and low polynomial order. After solving an initial NLP, it analyzes the solution’s accuracy, typically by checking the residual of the dynamics differential equation at the midpoint of each segment:
$$
\mathbf{e}_k = \left\| \mathbf{X}_m – \mathbf{X}_0 – \frac{t_f – t_0}{2} \mathbf{f}(\mathbf{X}_m, \mathbf{U}_m, t_m) \right\|,
$$
where \(\mathbf{X}_m, \mathbf{U}_m\) are interpolated midpoint values in segment \(k\). If this error exceeds a specified tolerance \(\epsilon_{tol}\) in a segment, the algorithm decides whether to subdivide the segment (h-refinement) or increase the polynomial order within it (p-refinement). The decision is often based on the local curvature of the state trajectories. Segments with high curvature (suggesting a potential discontinuity or rapid change) are better handled by subdivision, while segments where the solution is smooth but not accurately approximated benefit from a higher-degree polynomial. This adaptive process iterates until all segments meet the error tolerance, yielding an efficient discretization that places computational resources only where they are needed. This is crucial for a drone formation problem, where control inputs might be near their bounds or collision avoidance constraints become active, creating non-smooth features in the optimal solution.
The transcribed NLP problem, characterized by the decision variables (states and controls at all collocation points, plus final time) and the nonlinear constraints (dynamics, boundaries, path constraints), is then solved using a robust large-scale NLP solver like SNOPT, which implements a Sequential Quadratic Programming (SQP) algorithm.
Applying the hp-adaptive pseudospectral method to the six-drone minimum-time reconfiguration problem yielded the following results. The solver converged to a feasible solution with a minimum time \(t_f^* = 7.87\) seconds. The trajectories for all six drones are smooth and satisfy all imposed constraints.
The position trajectories show a coordinated maneuver from the initial grid to the final grid. The control input histories for all drones remain within the specified physical limits throughout the maneuver. Notably, the control profiles are smooth, which is beneficial for real-world flight controller tracking.
Most importantly, the collision avoidance constraints were satisfied. The minimum pairwise distance between any two drones throughout the entire 7.87-second flight was maintained above the 0.5-meter safety threshold. The plot of inter-agent distances shows no violations, confirming the drone formation‘s operational safety. A key observation from the solution is that the performance is often limited by the maximum velocity constraints rather than the control authority, providing a clear guideline for platform selection or mission design.
The successful application of the hp-adaptive pseudospectral method to this six-agent problem demonstrates its power for drone formation trajectory optimization. Its ability to automatically manage the discretization mesh and polynomial order makes it highly efficient and reduces the need for manual tuning. The generated trajectories are not just mathematically optimal but also respect the real-world physics and safety rules of multi-agent flight.
Looking forward, the framework is extensible. The performance index \(J\) could easily be changed to minimize energy consumption, which would involve integrating the control effort over time. More sophisticated collision avoidance models, such as those incorporating velocity or including keep-out zones, can be integrated as additional path constraints. The real promise lies in moving from offline trajectory generation to real-time, receding-horizon implementations. While computationally demanding, the efficiency of the hp-adaptive method brings online trajectory planning for agile drone formation flight closer to reality. The ultimate validation, of course, is flight testing. The trajectories generated by this method provide a high-fidelity reference that can be tracked by onboard feedback controllers, paving the way for advanced, coordinated autonomous missions.