The coordinated operation of multiple unmanned aerial vehicles (UAVs), particularly quadrotor drones, represents a significant frontier in aerospace robotics, enabling complex tasks in surveillance, logistics, and search-and-rescue. Among these, the challenge of generating efficient, collision-free trajectories for an entire formation while adhering to strict physical and temporal constraints is paramount. This work addresses the trajectory optimization problem for a formation of multiple quadrotor drones required to reach a target at a specified time while maintaining a rigid geometric configuration. The core innovation lies in a simplification strategy: the multi-agent problem is transformed into a single-agent optimal control problem for a designated leader quadrotor drone. The states and constraints of the following quadrotor drones are effectively converted into additional constraints imposed on the leader’s trajectory. The resulting problem is then solved using the Gauss Pseudo-spectral Method (GPM), yielding a smooth, feasible, and energy-efficient path for the entire formation.
The fundamental model for a single quadrotor drone is based on its nonlinear dynamics. We consider an ‘X’-configuration quadrotor drone. The equations of motion, relating control inputs to translational and rotational accelerations, are given below. Here, $(x, y, z)$ denote the position in an inertial frame, $(\phi, \theta, \psi)$ are the roll, pitch, and yaw (Euler angles), $m$ is the mass, $l$ is the arm length, $g$ is gravitational acceleration, $I_x, I_y, I_z$ are moments of inertia, and $K_i$ are aerodynamic drag coefficients. The control inputs $U_1$ (total thrust), $U_2$ (roll torque), $U_3$ (pitch torque), and $U_4$ (yaw torque) govern the vehicle’s motion.
The nonlinear dynamic model for a quadrotor drone is expressed as:
$$
\begin{aligned}
\ddot{x} &= (\cos\psi\sin\theta\cos\phi + \sin\psi\sin\phi)\frac{U_1}{m} – \frac{K_1 \dot{x}}{m} \\
\ddot{y} &= (\sin\psi\sin\theta\cos\phi – \cos\psi\sin\phi)\frac{U_1}{m} – \frac{K_2 \dot{y}}{m} \\
\ddot{z} &= (\cos\theta\cos\phi)\frac{U_1}{m} – g – \frac{K_3 \dot{z}}{m} \\
\ddot{\phi} &= \frac{l}{I_x}(U_2 – K_4 \dot{\phi}) \\
\ddot{\theta} &= \frac{l}{I_y}(U_3 – K_5 \dot{\theta}) \\
\ddot{\psi} &= \frac{1}{I_z}(U_4 – K_6 \dot{\psi})
\end{aligned}
$$
For formation control, a leader-follower strategy based on graph-theoretic principles is adopted. The entire formation is treated as a virtual rigid body. Consistency algorithms ensure that the follower quadrotor drones maintain predefined relative positions and attitudes with respect to the leader quadrotor drone, effectively locking the formation geometry during flight.
Constraint Analysis and Transformation
The trajectory optimization must satisfy a comprehensive set of constraints. These are first defined for the leader quadrotor drone and then extended to include the followers through transformation.
Leader Quadrotor Drone Constraints
The primary vehicle’s path must obey the following:
1. Boundary Conditions: Initial and final position, velocity, and attitude are specified.
$$
\begin{aligned}
\text{At } t_0: & \quad x(t_0)=x_0, y(t_0)=y_0, z(t_0)=z_0, \quad \phi(t_0)=\phi_0, \theta(t_0)=\theta_0, \psi(t_0)=\psi_0 \\
\text{At } t_f: & \quad x(t_f)=x_f, y(t_f)=y_f, z(t_f)=z_f, \quad \phi(t_f)=\phi_f, \theta(t_f)=\theta_f, \psi(t_f)=\psi_f
\end{aligned}
$$
2. Minimum Turning Radius: Defined by the maximum lateral acceleration (load factor $n_{max}$) and speed $V$.
$$
R_{min}^{\text{leader}} \geq \frac{V^2}{n_{max} \cdot g}
$$
3. Control Saturation: The four control inputs are physically limited.
$$
U_{i,min} \leq U_i \leq U_{i,max}, \quad i=1,2,3,4
$$
4. Obstacle and Terrain Avoidance: The path must remain above terrain $h_{min}(x,y)$ and outside defined no-fly zones (modeled as cylinders).
$$
\begin{aligned}
z(t) &\geq h_{min}(x(t), y(t)) \\
(x(t) – x_j)^2 + (y(t) – y_j)^2 &\geq R_j^2, \quad j=1,…,m
\end{aligned}
$$
Transformation of Follower Quadrotor Drone Constraints
This is the critical step that reduces problem complexity. Each follower quadrotor drone $i$ has a fixed offset $( \Delta x_{i1}, \Delta y_{i1}, \Delta z_{i1} )$ relative to the leader in the formation frame. Therefore, its inertial position is:
$$
\begin{aligned}
x_i(t) &= x_1(t) – \Delta x_{i1} \\
y_i(t) &= y_1(t) – \Delta y_{i1} \\
z_i(t) &= z_1(t) – \Delta z_{i1}
\end{aligned}
$$
Consequently, constraints on followers become constraints on the leader’s position:
1. Follower Terrain Clearance: $z_i(t) \geq h_{min}^i$ transforms to:
$$
z_1(t) \geq h_{min}^i + \Delta z_{i1}
$$
2. Follower Obstacle Avoidance: $(x_i(t) – x_j)^2 + (y_i(t) – y_j)^2 \geq R_{ij}^2$ transforms to:
$$
(x_1(t) – \Delta x_{i1} – x_j)^2 + (y_1(t) – \Delta y_{i1} – y_j)^2 \geq R_{ij}^2
$$
3. Follower Minimum Turning Radius: This requires careful analysis. During a coordinated turn, all quadrotor drones rotate about a common center. The follower’s speed $V_i$ and turn radius $R_i$ relate to the leader’s $(V_1, R_1)$. The follower’s load factor constraint $ \frac{V_i^2}{R_i \cdot g} \leq n_{i,max} $ can be expressed in terms of the leader’s state. Assuming a turn center at the origin for analysis, $R_i = \sqrt{(x_1-\Delta x_{i1})^2 + (y_1-\Delta y_{i1})^2 + (z_1-\Delta z_{i1})^2}$. Using the relation $V_i / V_1 = R_i / R_1$, the constraint becomes:
$$
\frac{ \left[ (x_1-\Delta x_{i1})^2 + (y_1-\Delta y_{i1})^2 + (z_1-\Delta z_{i1})^2 \right]^{3/2} }{R_1^4} \cdot V_1^2 \leq n_{i,max} \cdot g
$$
This is a nonlinear, state-dependent constraint on the leader quadrotor drone’s trajectory.
Optimal Control Problem Formulation
The objective is to guide the formation from start to goal while minimizing the total energy expenditure of the fleet. The performance index is:
$$
J = \min \sum_{i=1}^{N} W_i = \min \sum_{i=1}^{N} \int_{t_0}^{t_f} P_i(t) \, dt
$$
where $P_i(t)$ represents the instantaneous power consumption of the i-th quadrotor drone, often related to the control inputs and velocities. With the constraint transformation, the multi-quadrotor drone trajectory optimization problem is now defined as a single optimal control problem for the leader quadrotor drone:
Find the control history $\mathbf{U}(t) = [U_1, U_2, U_3, U_4]^T$ for $t \in [t_0, t_f]$ that minimizes $J$, subject to the leader dynamics (Eq. 1), the original leader constraints, and the transformed follower constraints (Eq. 2-4). The final time $t_f$ is fixed, imposing a time-of-arrival constraint.
Numerical Solution via Gauss Pseudo-spectral Method (GPM)
Solving the described nonlinear optimal control problem analytically is intractable. The GPM is a direct collocation technique that transcribes the continuous-time problem into a discrete Nonlinear Programming Problem (NLP).
1. Time Domain Transformation: The time interval is mapped to $\tau \in [-1, 1]$.
$$
\tau = \frac{2t}{t_f – t_0} – \frac{t_f + t_0}{t_f – t_0}
$$
2. State and Control Approximation: The state $\mathbf{X}(\tau)$ and control $\mathbf{U}(\tau)$ are approximated using Lagrange interpolating polynomials at Legendre-Gauss (LG) collocation points $\tau_k$, $k=1,…,K$, plus the initial point $\tau_0 = -1$.
$$
\mathbf{X}(\tau) \approx \sum_{i=0}^{K} \mathbf{L}_i(\tau) \mathbf{X}(\tau_i), \quad \mathbf{U}(\tau) \approx \sum_{i=1}^{K} \tilde{\mathbf{L}}_i(\tau) \mathbf{U}(\tau_i)
$$
3. Dynamics Transcription: The derivative of the state approximation at the LG points is equated to the dynamics, converting the differential equation constraints into algebraic constraints.
$$
\sum_{i=0}^{K} D_{ki} \mathbf{X}(\tau_i) – \frac{t_f – t_0}{2} f(\mathbf{X}(\tau_k), \mathbf{U}(\tau_k), \tau_k) = 0, \quad k=1,…,K
$$
where $D_{ki}$ are entries of the differential approximation matrix.
4. Terminal State Constraint: The state at $\tau_f=1$ is not collocated but is approximated via Gauss quadrature.
$$
\mathbf{X}(1) = \mathbf{X}(-1) + \frac{t_f – t_0}{2} \sum_{k=1}^{K} w_k f(\mathbf{X}(\tau_k), \mathbf{U}(\tau_k), \tau_k)
$$
where $w_k$ are the Gauss weights.
5. Cost Function and Constraint Discretization: The integral in the cost function $J$ is approximated using Gauss quadrature. All path constraints (control limits, obstacle avoidance, etc.) are enforced at the collocation points.
The resulting large-scale NLP, with decision variables being the states and controls at collocation points, is solved using robust algorithms like Sequential Quadratic Programming (SQP). This process yields a discrete, optimized trajectory for the leader quadrotor drone, from which the paths for all follower quadrotor drones are immediately derived using their relative offsets.
Simulation Results and Analysis
A scenario involving one leader and five follower quadrotor drones in a triangular formation was simulated. The formation must depart from a specified initial state, navigate a complex 3D environment with mountainous terrain and no-fly zones, and arrive at a target location with a specified final attitude at exactly $t_f = 330$s. The key parameters for the quadrotor drone model are summarized below.
| Parameter | Value | Unit |
|---|---|---|
| Mass, $m$ | 0.087 | kg |
| Arm Length, $l$ | 0.12 | m |
| Moment of Inertia, $I_x$ | 1.3186e-4 | kg·m² |
| Moment of Inertia, $I_y$ | 1.19696e-4 | kg·m² |
| Moment of Inertia, $I_z$ | 1.7998e-4 | kg·m² |
| Drag Coefficients, $K_1, K_2, K_3$ | 0.02 | – |
| Drag Torque Coefficients, $K_4, K_5, K_6$ | 0.1 | – |
| Max Thrust, $U_{1,max}$ | 21.03 | N |
| Max Torque, $U_{2,max}, U_{3,max}$ | 0.67 | N·m |
The terrain was modeled using a combination of sinusoidal functions and Gaussian peaks to create realistic obstacles:
$$
\begin{aligned}
z_{\text{terrain}}(x,y) &= \left| \sin(y + a) + b \cdot \sin(x) + e \cdot \cos(y) + f \cdot \sin(g \cdot \sqrt{x^2 + y^2}) \right| \\
&+ \sum_{n} h_n \exp\left( -\left( \frac{x – x_n}{x_{s_n}} \right)^2 – \left( \frac{y – y_n}{y_{s_n}} \right)^2 \right)
\end{aligned}
$$
The GPM was implemented to generate the optimal trajectory. The resulting 3D path for the leader quadrotor drone successfully avoids all terrain features and no-fly zones. The path exhibits smooth transitions with clear climb phases to surmount ridges and descent phases into valleys, culminating in a final descent to the target altitude. The follower quadrotor drones’ paths, generated via the fixed offset rule, inherently maintain the formation geometry and satisfy all safety constraints due to the leader’s constraint transformation.
| Metric | Result | Status |
|---|---|---|
| Final Arrival Time, $t_f$ | 330.0 s | Met |
| Final Position Error | < 0.1 m | Met |
| Minimum Clearance from Terrain | > 5.0 m | Met |
| Control Inputs | Within Bounds | Met |
| Formation Geometric Error | < 0.05 m | Met |
| Total Estimated Energy Cost, $J$ | Minimized | Achieved |
The attitude and control histories further demonstrate the feasibility of the trajectory. The yaw angle $\psi$ changes smoothly to align the formation’s heading through valleys, while pitch $\theta$ and roll $\phi$ angles show moderate variations necessary for climb/descent and lateral maneuvering, respectively. All control inputs remain within their prescribed saturation limits throughout the flight, confirming the dynamic feasibility of the plan for the quadrotor drone fleet.
Discussion and Conclusion
This work presented a computationally efficient framework for the coordinated trajectory optimization of multiple quadrotor drones in a rigid formation. By leveraging the formation geometry to transform the states and constraints of follower quadrotor drones into a set of augmented constraints for a single leader quadrotor drone, the complex multi-agent optimal control problem is reduced to a more tractable single-agent problem. The high-fidelity, nonlinear dynamics of the quadrotor drone are fully respected in this formulation. The application of the Gauss Pseudo-spectral Method provides an accurate and robust numerical solution, transcribing the problem into a nonlinear program solvable by standard algorithms.
The simulation results validate the approach, showing that the method generates smooth, dynamically feasible, and energy-efficient trajectories. These trajectories guarantee the arrival of the entire quadrotor drone formation at a designated target at a precise time, while ensuring strict adherence to physical limits, obstacle avoidance, and the maintenance of the prescribed formation shape. This capability significantly enhances the operational viability and survivability of autonomous quadrotor drone teams in cluttered environments. Future work may investigate integrating this offline planning method with reactive controllers for dynamic obstacle avoidance and exploring decentralized versions of the constraint transformation paradigm.