The coordinated operation of multiple Unmanned Aerial Vehicles (UAVs) as a cohesive unit, commonly referred to as a drone formation, has garnered significant research interest due to its potential to enhance operational capabilities in complex tasks. Applications span from search and rescue and precision agriculture to aerial light shows and coordinated military missions. The core challenge lies in enabling a drone formation to autonomously and safely navigate from a starting configuration to a target area while adhering to a predefined spatial pattern, avoiding static and dynamic obstacles, and preventing inter-agent collisions, all within the physical limits of the vehicles.
This article addresses the problem of online trajectory planning for a drone formation in cluttered, unknown environments. We propose a novel planning strategy based on Distributed Model Predictive Control (DMPC). The primary contribution is a holistic framework that simultaneously handles multi-agent coordination for formation keeping and trajectory generation towards individual goals, while rigorously accounting for a comprehensive set of constraints: vehicle dynamics, state and input limits, inter-vehicle collision avoidance, and static/dynamic obstacle avoidance. A key innovation is the design of an efficient, on-demand collision avoidance algorithm integrated as a soft constraint within the DMPC optimization, which demonstrates superior computational performance compared to existing methods like the Buffered Voronoi Cell (BVC) approach.
The effectiveness of the proposed DMPC-based trajectory planning strategy is validated through both numerical simulations and real-world flight experiments using a custom-built multi-UAV platform, confirming its practicality and robustness for real-time drone formation control.
1. System Modeling for Drone Formation
We focus on the trajectory planning problem for a drone formation, emphasizing the translational motion in three-dimensional space. For control design, it is common and effective to simplify the complex quadrotor dynamics to a manageable point-mass model, especially when the focus is on high-level path planning and the inner-loop attitude controller is sufficiently fast. We model each UAV in the drone formation as a second-order integrator system. For UAV \(i\) in the formation, its continuous-time kinematics are given by:
$$ \dot{p}_i = v_i $$
$$ \dot{v}_i = a_i $$
where \(p_i = [x_i, y_i, z_i]^T \in \mathbb{R}^3\), \(v_i = [v_{x_i}, v_{y_i}, v_{z_i}]^T \in \mathbb{R}^3\), and \(a_i = [a_{x_i}, a_{y_i}, a_{z_i}]^T \in \mathbb{R}^3\) denote its position, velocity, and acceleration vectors in the inertial frame, respectively. This can be written in state-space form as:
$$ \begin{bmatrix} \dot{p}_i \\ \dot{v}_i \end{bmatrix} = \begin{bmatrix} 0_3 & I_3 \\ 0_3 & 0_3 \end{bmatrix} \begin{bmatrix} p_i \\ v_i \end{bmatrix} + \begin{bmatrix} 0_3 \\ I_3 \end{bmatrix} a_i $$
For digital implementation and optimization within the Model Predictive Control (MPC) framework, we discretize the model with a fixed sampling time \(h\). Using a forward Euler approximation:
$$ \dot{p}_i \approx \frac{p_i[k+1] – p_i[k]}{h}, \quad \dot{v}_i \approx \frac{v_i[k+1] – v_i[k]}{h} $$
the discrete-time linear time-invariant (LTI) model for each agent in the drone formation is obtained:
$$ x_i[k+1] = A_i x_i[k] + B_i u_i[k] $$
where \(x_i[k] = [p_i[k]^T, v_i[k]^T]^T \in \mathbb{R}^6\) is the state vector at time step \(k\), and \(u_i[k] = a_i[k] \in \mathbb{R}^3\) is the control input (acceleration). The system matrices are:
$$ A_i = \begin{bmatrix} I_3 & h I_3 \\ 0_3 & I_3 \end{bmatrix}, \quad B_i = \begin{bmatrix} \frac{h^2}{2} I_3 \\ h I_3 \end{bmatrix} $$
The communication topology within the drone formation is represented by a directed graph \(\mathcal{G} = (\mathcal{V}, \mathcal{E})\), where \(\mathcal{V} = \{1, 2, …, N\}\) is the set of nodes (UAVs), and \(\mathcal{E} \subseteq \mathcal{V} \times \mathcal{V}\) is the set of edges. An edge \((j, i) \in \mathcal{E}\) indicates that UAV \(i\) can receive state information from UAV \(j\). The set of neighbors for UAV \(i\) is denoted as \(\mathcal{N}_i = \{j \in \mathcal{V} \setminus \{i\} : (j, i) \in \mathcal{E}\}\).
2. Problem Formulation for Drone Formation Planning
Consider a drone formation consisting of \(N\) UAVs with the dynamics described above, operating within a bounded workspace \(\mathcal{W} \subset \mathbb{R}^3\). Each UAV \(i\) is assigned a desired final target position \(p_{d,i} \in \mathcal{W}\). The collective objective for the drone formation is threefold: 1) Each UAV should reach its target. 2) The UAVs should establish and maintain a desired geometric formation defined by relative position vectors \(d_{ij,ref}\) for \(j \in \mathcal{N}_i\). 3) All trajectories must be dynamically feasible and satisfy safety constraints.
This leads to the following formal design goals:
- Target Convergence: \(\lim_{k \to \infty} (p_i[k] – p_{d,i}) = 0, \quad \forall i \in \mathcal{V}\).
- Formation Keeping: \(\lim_{k \to \infty} (p_j[k] – p_i[k]) = d_{ij,ref}, \quad \forall j \in \mathcal{N}_i, \forall i \in \mathcal{V}\).
- Inter-Agent Collision Avoidance: \(\| p_j[k] – p_i[k] \| \ge r_{\text{min}}, \quad \forall j \neq i, \forall k \ge 0\), where \(r_{\text{min}} > 0\) is a safety radius.
- Obstacle Avoidance: \(\| p_o[k] – p_i[k] \| \ge r_{\text{obs}}, \quad \forall o \in \mathcal{N}_o, \forall i \in \mathcal{V}, \forall k \ge 0\), where \(\mathcal{N}_o\) is the set of obstacles (static and dynamic) and \(r_{\text{obs}} > 0\) is the obstacle safety radius.
The planning algorithm must generate control inputs that drive the drone formation to satisfy these goals while respecting the inherent constraints on maximum velocity and acceleration (\(v_{\text{max}}, a_{\text{max}}\)) and workspace boundaries.
3. DMPC-Based Trajectory Planning Algorithm Design
We propose a DMPC strategy where each UAV in the drone formation solves a local optimization problem in a receding-horizon fashion, using only information from its neighbors. This decentralized approach enhances scalability and robustness.
3.1 Trajectory Parameterization using B-Splines
To generate smooth trajectories amenable to optimization, we parameterize the predicted position trajectory of each UAV over the prediction horizon \(K\) as a concatenation of \(l\) B-spline curves. A B-spline curve of degree \(k\) in \(\mathbb{R}^3\) is defined as \(B(t) = \sum_{i=0}^{n} P_i B_{i,k}(t)\), where \(P_i \in \mathbb{R}^3\) are control points and \(B_{i,k}(t)\) are the basis functions defined recursively. This parameterization offers several advantages for drone formation planning: it inherently produces \(C^{k-1}\) continuous paths, allows easy enforcement of dynamic constraints via linear constraints on the control points, and provides a compact representation of the trajectory through a finite set of optimization variables \(\mathcal{P}_i = \{P_{i,0}, P_{i,1}, …, P_{i,n}\}\).
3.2 Prediction Model
Based on the discrete model (Eq. 6), the state prediction for UAV \(i\) at discrete time \(k_t\) over a horizon of \(K\) steps is given by:
$$ x_i[k+1 | k_t] = A_i x_i[k | k_t] + B_i u_i[k | k_t], \quad k = 0,…,K-1 $$
with initial condition \(x_i[0 | k_t] = x_i(k_t)\). By stacking predictions, the sequence of future positions \(\mathbf{P}_i = [p_i[1|k_t]^T, …, p_i[K|k_t]^T]^T \in \mathbb{R}^{3K}\) can be expressed as an affine function of the stacked input sequence \(\mathbf{T}_i = [u_i[0|k_t]^T, …, u_i[K-1|k_t]^T]^T \in \mathbb{R}^{3K}\):
$$ \mathbf{P}_i = \mathcal{A}_{0,i} x_i(k_t) + \Lambda_i \mathbf{T}_i $$
where \(\mathcal{A}_{0,i} \in \mathbb{R}^{3K \times 6}\) and \(\Lambda_i \in \mathbb{R}^{3K \times 3K}\) are matrices constructed from \(A_i\) and \(B_i\). This linear relationship is crucial for formulating a quadratic program (QP).
3.3 Local Optimization Problem Construction
At each time step, UAV \(i\) solves the following constrained finite-time optimal control problem (CFTOC):
$$ \min_{\mathbf{T}_i, \alpha_{ij}} J_i(\mathbf{T}_i, \alpha_{ij}) $$
$$ \text{subject to: } \quad \text{Dynamics, Constraints} $$
The cost function \(J_i\) is designed to encode the objectives for the drone formation and is composed of several weighted terms:
1. Target Attraction Cost: Drives the UAV towards its goal. It minimizes the sum of squared distances between predicted positions in the later part of the horizon and the target \(p_{d,i}\).
$$ F_{e,i} = \sum_{k=K-\theta}^{K-1} q_k \| p_i[k|k_t] – p_{d,i} \|^2 $$
Using Eq. (10), this can be written as a quadratic form in \(\mathbf{T}_i\):
$$ F_{e,i} = \mathbf{T}_i^T (\Lambda_i^T \tilde{Q} \Lambda_i) \mathbf{T}_i – 2(\mathbf{P}_{d,i}^T \tilde{Q} \Lambda_i – (\mathcal{A}_{0,i}X_{0,i})^T \tilde{Q} \Lambda_i)\mathbf{T}_i + \text{constant} $$
where \(\tilde{Q}\) is a positive definite weighting matrix.
2. Formation Keeping Cost: Enforces the desired geometric configuration within the drone formation. It penalizes the deviation from the desired relative position with respect to neighbors.
$$ F_{f,i} = \sum_{j \in \mathcal{N}_i} \sum_{k=0}^{K-1} q_f \| (p_i[k|k_t] – p_j[k|k_t]) – d_{ij,ref} \|^2 $$
This term couples the trajectories of neighboring UAVs. In the DMPC scheme, UAV \(i\) uses the assumed or previously communicated predicted trajectories \(\mathbf{P}_j\) of its neighbors \(j \in \mathcal{N}_i\). This cost also translates into a quadratic form in \(\mathbf{T}_i\).
3. Control Smoothness Cost: Promotes smooth trajectories by minimizing the change in acceleration (jerk).
$$ F_{\Theta,i} = \sum_{k=0}^{K-1} q_{\Theta} \| u_i[k|k_t] – u_i[k-1|k_t] \|^2 $$
This term can be written as \(F_{\Theta,i} = \mathbf{T}_i^T (\Theta^T \tilde{S} \Theta) \mathbf{T}_i – 2(\mathbf{U}_{i*}^T \tilde{S} \Theta) \mathbf{T}_i\), where \(\Theta\) is a difference matrix and \(\tilde{S}\) is a weighting matrix.
4. Constraint Violation Penalty: A penalty term on slack variables \(\alpha_{ij} \leq 0\) introduced to soften collision constraints for feasibility, defined as \(F_{v,i} = \zeta \|\alpha_{ij}\|_2^2 + \xi \alpha_{ij}\).
Thus, the total local cost is \(J_i = F_{e,i} + F_{f,i} + F_{\Theta,i} + F_{v,i}\).
3.4 Constraint Formulation
The constraints ensure the safety and feasibility of the drone formation trajectories.
1. State and Input Constraints (Hard): These are linear box constraints on predicted positions, velocities, and accelerations derived from physical limits \(p_{\text{min}}, p_{\text{max}}, v_{\text{max}}, a_{\text{max}}\). Using Eq. (10), they can be expressed as linear inequalities in \(\mathbf{T}_i\):
$$ \mathbf{P}_{\text{min}} – \mathcal{A}_{0,i}x_i(k_t) \leq \Lambda_i \mathbf{T}_i \leq \mathbf{P}_{\text{max}} – \mathcal{A}_{0,i}x_i(k_t) $$
$$ \mathbf{U}_{\text{min}} \leq \mathbf{T}_i \leq \mathbf{U}_{\text{max}} $$
2. Trajectory Continuity Constraints (Hard): Enforced as equality constraints \(A_{eq} \mathbf{T}_i = b_{eq}\) to ensure the B-spline parameterization yields a continuous and smooth path.
3. Collision Avoidance Constraints (Soft/On-Demand): This is a key contribution. Instead of enforcing pairwise distance constraints over the entire horizon (which is computationally heavy and often infeasible), we employ an on-demand strategy. At time \(k_t-1\), UAV \(i\) checks its predicted trajectory against the assumed trajectories of neighbors and known obstacle positions for the first collision time \(k_{c,i}\) within the horizon. If a potential collision is detected (\(\sigma_{ij} = \| p_i[k_{c,i}|k_t-1] – p_j[k_{c,i}|k_t-1] \| \leq r_{\text{min}}\)), a single, linearized avoidance constraint is added to the optimization at time \(k_t\) for the predicted step \(k_{c,i}-1\). The constraint is derived from a linearization of the safety condition:
$$ \omega_{ij}^T p_i[k_{c,i} | k_t] – \alpha_{ij} \sigma_{ij} \geq \upsilon_{ij} $$
where \(\omega_{ij}\) is the linearization gradient. Using Eq. (10), this becomes a linear constraint in \(\mathbf{T}_i\) and the slack variable \(\alpha_{ij}\):
$$ Z_{ij}^T \Lambda_i \mathbf{T}_i – \alpha_{ij} \sigma_{ij} \geq \upsilon_{ij} – Z_{ij}^T \mathcal{A}_{0,i} x_i(k_t) $$
Here, \(Z_{ij}\) is a selector vector. Constraints are only generated for neighbors within a critical distance (e.g., \(3r_{\text{min}}\)), reducing the problem size. The slack variable \(\alpha_{ij}\), penalized by \(F_{v,i}\), ensures recursive feasibility.
This on-demand approach contrasts with methods like BVC, which require defining and constraining within a Voronoi cell at every step. The formulated optimization problem for each UAV in the drone formation is a Quadratic Program (QP) with linear constraints, which can be solved efficiently in real-time.
| Metric | Proposed On-Demand Algorithm | BVC-Based Algorithm |
|---|---|---|
| Average Trajectory Length | ~21.3 m | ~22.0 m |
| Average Speed | ~0.77 m/s | ~0.72 m/s |
| Average Acceleration | ~0.48 m/s² | ~0.66 m/s² |
| Average QP Solve Time | 3.94 ms | 12.91 ms |
| Total Mission Time | 27.66 s | 30.65 s |
4. Simulation Studies and Analysis
To validate the proposed DMPC trajectory planning algorithm for drone formation, extensive numerical simulations were conducted in MATLAB. A scenario with \(N=4\) UAVs and numerous static obstacles was created. The parameters were set as: \(r_{\text{min}} = 0.2 \text{m}\), \(h = 0.2 \text{s}\), prediction horizon \(K = 15\), maximum acceleration \(4 \text{m/s}^2\). The desired formation was a square. The formation error over time, defined as \(\chi(k) = \sum_{i=1}^N \sum_{j \in \mathcal{N}_i} (\| p_j(k) – p_i(k) \| – \| d_{ij,ref} \|)^2\), was used as a metric.
The simulation results demonstrated that the drone formation successfully navigated from start to target positions while maintaining the desired shape. The formation error \(\chi(k)\) converged close to zero after initial transient and small deviations during obstacle avoidance, confirming effective formation keeping. Inter-agent distances remained strictly above \(r_{\text{min}}\), verifying collision-free operation within the drone formation.
A key comparative analysis was performed against the Buffered Voronoi Cell (BVC) method. The performance metrics are summarized in Table 1. The proposed on-demand algorithm consistently outperformed the BVC method: it generated shorter, faster trajectories with lower control effort (acceleration), and most importantly, solved the local QP approximately 3.3 times faster on average. This drastic reduction in computation time (3.94 ms vs. 12.91 ms) is critical for real-time implementation on embedded hardware and highlights a major advantage of our method for scalable drone formation control.
5. Experimental Validation with a Physical Drone Formation
The practical efficacy of the algorithm was tested on a custom-built multi-UAV platform. The drone formation consisted of three quadrotors tasked with forming a triangle and navigating to goals, while a fourth UAV acted as a dynamic obstacle. The test area was a motion capture volume of 5.5m x 4m x 2.5m. Each drone was equipped with an onboard computer (ARM-based) running the DMPC algorithm at 100 Hz, communicating via WiFi. The controller sent acceleration commands to a low-level PX4 flight controller.
The experimental flight paths confirmed that the drone formation successfully planned and executed trajectories towards their goals while maintaining the triangular shape. Crucially, all inter-agent distances and distances to the dynamic obstacle remained above the safety threshold of 0.4 m throughout the flight, as recorded by the motion capture system. The formation error during the experiment showed a rapid initial decrease, a small rise during the dynamic obstacle avoidance maneuver, and a subsequent convergence to a small value (<0.1 m), demonstrating the robustness and real-time capability of the distributed planning framework for a physical drone formation.
| Feature | Description | Benefit for Drone Formation |
|---|---|---|
| DMPC Framework | Decentralized, parallel optimization per agent. | Scalability, robustness to single-point failure, lower communication burden. |
| B-spline Parameterization | Trajectory represented by control points. | Inherent smoothness, easy enforcement of dynamics via linear constraints. |
| On-Demand Collision Avoidance | Linearized constraints added only when imminent collision is predicted. | Drastically reduces computation time vs. all-time constraints (e.g., BVC), increases success rate. |
| Integrated Cost Function | Combines target reaching, formation keeping, and smoothness. | Holistic solution achieving multiple coordinated objectives simultaneously. |
| Soft Constraints via Slack Variables | Relaxed collision constraints with penalty. | Guarantees recursive feasibility of the local QP, essential for real-time operation. |
6. Conclusion and Future Work
This article presented a comprehensive DMPC-based trajectory planning strategy for drone formations operating in complex environments with static and dynamic obstacles. The algorithm successfully integrates several critical aspects: distributed computation for scalability, B-spline-based smooth trajectory generation, a multi-objective cost function for simultaneous goal attainment and formation keeping, and a computationally efficient, on-demand collision and obstacle avoidance mechanism. The superiority of the proposed avoidance strategy over conventional methods like BVC was quantitatively demonstrated in simulation. Finally, the implementation and successful testing on a physical multi-UAV platform confirmed the algorithm’s practicality and effectiveness for real-world drone formation applications.
Future work will focus on incorporating more detailed vehicle dynamics into the prediction model to further enhance trajectory feasibility, extending the algorithm to handle imperfect communication and state estimation, and testing the drone formation in completely unknown environments using onboard sensors like lidar for real-time map building and obstacle detection.