In recent years, the planning and control of multirotor drone formations have attracted extensive research attention. Drone formations hold broad application prospects in fields such as exploration, search and rescue, military operations, and logistics. In many mission scenarios, drone formations need to traverse narrow passages, such as doorways, windows, corridors, or gaps between buildings. Simultaneously, collaborative operations or flight often require maintaining specific formations, like line-abreast, V-shape, or polygonal arrangements. Therefore, enabling a drone formation to safely and rapidly traverse narrow passages while preserving a similar formation configuration is of great significance for practical applications.
The problem of high-speed, safe traversal of narrow passages by a drone formation can be viewed as a challenge of approximate formation transformation and maintenance for corridor-type obstacles, coupled with achieving high-speed flight under kinematic constraints. Existing methods face difficulties in balancing safety, speed, and formation preservation. For instance, vector field methods may struggle with timely correction and oscillations at high speeds, while some trajectory planning approaches do not fully consider the joint optimization of the formation’s overall trajectory. Moreover, efficiently finding a formation configuration that minimizes change while avoiding collisions remains complex. To address these issues, we propose a multi-stage strategy that combines centralized planning for a global reference and safe configuration with distributed optimization for individual drone trajectories.
Our approach consists of three core phases. First, we treat the formation center as a virtual leader and plan a smooth, high-speed trajectory for it that maximizes distance from obstacles, using an improved Mixed-Integer Linear Programming (MILP) formulation. This also generates a safe flight corridor for the entire formation. Second, based on this corridor and the virtual leader’s path, we solve for a safe formation configuration—particularly at the narrowest passage point—that avoids internal and external collisions and minimizes deviation from the initial shape, employing a genetic algorithm. Finally, we distribute this configuration information and use a distributed nonlinear optimization scheme, combined with cooperative time reallocation, to compute kinematically feasible, high-speed trajectories for each drone that adhere to the formation shape as closely as possible.
The main contributions of this work are threefold: 1) An improved MILP algorithm that generates a virtual leader trajectory satisfying kinematic constraints while being fast, smooth, and maximizing safety margins in narrow sections, outperforming basic MILP in safety and speed. 2) A method for generating safe, minimally-altered formation configurations using the safe corridor and a multi-objective genetic search, effectively balancing collision avoidance and formation likeness. 3) A distributed nonlinear optimization and time reallocation algorithm that produces smooth, high-speed trajectories for each drone, respecting kinematic limits and formation constraints.
Our work operates under several assumptions: the geometry of the narrow corridor is known; for simplicity and visualization, we consider formation transformations in the horizontal plane; all drones have identical kinematic constraints; the communication topology among drones is strongly connected with no delays or packet loss; and we focus on static obstacles, excluding dynamic ones.
Virtual Leader Trajectory Planning and Safe Corridor Generation
The first stage centers on planning a reference path for the entire drone formation. We model the formation’s center as a virtual leader. Its kinematic model is discretized over time steps. Let $N$ be the maximum number of waypoints, $t_s$ the fixed time step between them, and for each time index $i \in \{1, \dots, N\}$, we define the virtual leader’s position vector as $\mathbf{P}_i = [p_{i,x}, p_{i,y}, p_{i,z}]^T$, velocity vector $\mathbf{U}_i = [u_{i,x}, u_{i,y}, u_{i,z}]^T$, and acceleration vector $\mathbf{A}_i = [a_{i,x}, a_{i,y}, a_{i,z}]^T$. The discrete kinematics are:
$$ \forall i \in \{2, \dots, N\}, \forall j \in \{x, y, z\}: \quad p_{i,j} = p_{i-1,j} + \frac{u_{i-1,j} + u_{i,j}}{2} t_s $$
$$ \forall i \in \{2, \dots, N-1\}, \forall j \in \{x, y, z\}: \quad a_{i,j} = \frac{u_{i+1,j} – u_{i,j}}{t_s} $$
Start and end conditions are enforced. The start position $\mathbf{P}_{\text{start}}$ and velocity $\mathbf{U}_{\text{start}}$ are fixed. For the end condition, we allow the virtual leader to reach the goal position $\mathbf{P}_{\text{end}}$ and velocity $\mathbf{U}_{\text{end}}$ at any point within a specified late window of the trajectory. Binary variables $b_i \in \{0,1\}$ are introduced such that $b_i=1$ implies $\mathbf{P}_i = \mathbf{P}_{\text{end}}$ and $\mathbf{U}_i = \mathbf{U}_{\text{end}}$. The constraint $\sum_{i=N-M+1}^{N} b_i = 1$ ensures arrival within the last $M$ steps.
Kinematic limits for maximum speed $v_{\text{max}}$ and acceleration $a_{\text{max}}$ are enforced via linear constraints approximating spherical bounds in 3D using inscribed polyhedra. For speed, with $N_s^h$ and $N_s^v$ being the number of facets for horizontal and vertical approximations:
$$ \forall i, \forall p \in \{1,\dots,N_s^h\}, \forall q \in \{1,\dots,N_s^v\}: \quad \sin\left(\frac{2\pi p}{N_s^h}\right) u_{i,x} + \cos\left(\frac{2\pi p}{N_s^h}\right) u_{i,y} + \sin\left(\frac{2\pi q}{N_s^v}\right) u_{i,z} \leq v_{\text{max}} $$
Similar constraints hold for acceleration using $N_a^h$ and $N_a^v$.
Obstacle avoidance is critical. The corridor walls are modeled as inflated rectangles. We introduce a variable $d_{\text{safe}}$ representing the minimum distance between the virtual leader and obstacles throughout the trajectory. For each obstacle $n$ (center $(o_{x,n}, o_{y,n})$, half-length $l_n$, half-width $w_n$, inflated by drone radius $r_u$) and each time step $i$, we use binary variables $Z(n,r,i)$ (where $r \in \{1,2,3,4\}$ denotes the four sides of the rectangle) and big-M constraints to enforce that the leader stays outside at least one side of each obstacle. The constraints ensure that for each $(n,i)$, at least three of the four $Z(n,r,i)$ are zero, meaning the point is on the safe side of one edge. For example, for the left edge of obstacle $n$:
$$ Q \cdot Z(n,1,i) + (o_{x,n} – l_n – r_u – d_{\text{safe}}) \geq p_{i,x} $$
$$ -Q \cdot Z(n,2,i) + (o_{x,n} + l_n + r_u + d_{\text{safe}}) \leq p_{i,x} $$
… and similarly for $y$-dimension edges. The condition $\sum_{r=1}^4 Z(n,r,i) \leq 3$ completes the disjunction.
The objective function for the MILP is designed to promote smoothness, fast arrival, and maximize $d_{\text{safe}}$:
$$ J = \lambda_s J_s + \lambda_{\text{safe}} (-d_{\text{safe}}) + \lambda_f J_f $$
where
$$ J_s = \sum_{i=2}^{N-1} \sum_{j \in \{x,y,z\}} |a_{i,j}|, \quad J_f = \sum_{i=N-M+1}^{N} i \cdot b_i $$
Here, $\lambda_s, \lambda_{\text{safe}}, \lambda_f$ are weighting coefficients. Maximizing $d_{\text{safe}}$ ensures the formation has maximal lateral safety margin at the narrowest point, which is crucial for subsequent formation transformation.
Solving this MILP yields the virtual leader trajectory $\mathbf{P}_i, \mathbf{U}_i, \mathbf{A}_i$, the binary matrices, and the key value $d_{\text{safe}}$. Following this, we compute a safe corridor $\Omega$ as a sequence of circles centered at each $\mathbf{P}_i$ with radii $d_i$, where $d_i$ is the maximum permissible distance such that the point remains outside all obstacles given the fixed leader path. This is found by a second linear optimization minimizing $\sum_i d_i$ subject to constraints similar to the avoidance ones but with $d_i$ as variables and fixed leader positions.
Safe Formation Configuration Generation
Given the safe corridor and $d_{\text{safe}}$, we need to determine how the drone formation should reconfigure to pass through the narrowest section safely while minimizing shape change. Let $N_{\text{UAV}}$ be the number of drones. The initial formation configuration relative to the virtual leader is denoted by vectors $\mathbf{S}_k$ for drone $k$ in a Frenet frame (lateral offset $h$, longitudinal offset $l$). At the narrowest point, we seek target configuration vectors $\mathbf{G}_{k,\text{min}}$ such that all drones’ lateral offsets lie within $[-d_{\text{safe}}, d_{\text{safe}}]$, ensuring they fit within the safe lateral margin.
We formulate the search for $\mathbf{G}_{k,\text{min}}$ as a multi-objective optimization: minimize the total change from $\mathbf{S}_k$ to $\mathbf{G}_{k,\text{min}}$ (sum of Euclidean distances in the Frenet frame) while ensuring that no two drones come within a safety distance $c_d$ (accounting for transformation paths), and that all $\mathbf{G}_{k,\text{min}}$ reside within the allowable rectangular region $[-d_{\text{safe}}, d_{\text{safe}}] \times [0, l_{\text{max}}]$. This is a non-convex problem due to the pairwise distance constraints. We employ a genetic algorithm (GA) to solve it. A chromosome encodes the lateral and longitudinal components of all $\mathbf{G}_{k,\text{min}}$ after discretizing the search space. The fitness function $F$ is:
$$ F = \lambda_d \left( -\sum_{k=1}^{N_{\text{UAV}}} \|\mathbf{G}_{k,\text{min}} – \mathbf{S}_k\| \right) + \lambda_{\text{sa}} \left( -\sum_{k=1}^{N_{\text{UAV}}} \sum_{k’ \neq k} \kappa(\lambda(\mathcal{E}_k, \mathcal{E}_{k’})) \right) $$
where $\mathcal{E}_k$ is the line segment connecting $\mathbf{S}_k$ and $\mathbf{G}_{k,\text{min}}$, $\lambda(\mathcal{E}_k, \mathcal{E}_{k’})$ is the minimum distance between segments, and $\kappa(\cdot)$ is a penalty function that is zero if the distance is above a threshold (related to $c_d$) and negative otherwise. Coefficients $\lambda_d, \lambda_{\text{sa}}$ weight the objectives. The GA runs with a population size, mutation rate, and elitism until convergence or a time limit.
Once $\mathbf{G}_{k,\text{min}}$ and segments $\mathcal{E}_k$ are found, we compute the target configuration $\mathbf{G}_{k,i}$ for each drone $k$ at every virtual leader waypoint $i$. For waypoint $i$, we determine the maximum safe lateral offset $d_{i,\text{safe}}$ by looking back along the trajectory a distance equal to the formation’s maximum longitudinal extent $l_m$ and taking the minimum corridor radius $d_q$ over that segment. Then, if $d_{i,\text{safe}}$ is less than the initial formation’s half-width $h_{\text{max}}$, there is a collision risk, and $\mathbf{G}_{k,i}$ is set as the intersection of segment $\mathcal{E}_k$ with the line $h = d_{i,\text{safe}}$ (or $h = -d_{i,\text{safe}}$ depending on side). If no risk, $\mathbf{G}_{k,i} = \mathbf{S}_k$. This yields a sequence of formation configurations that smoothly transition between the initial shape and the narrowed shape at the tight spot.
| Symbol | Meaning |
|---|---|
| $N_{\text{UAV}}$ | Number of drones in the formation |
| $\mathbf{S}_k$ | Initial configuration position of drone $k$ (Frenet frame) |
| $\mathbf{G}_{k,\text{min}}$ | Target configuration at narrowest point for drone $k$ |
| $\mathcal{E}_k$ | Line segment from $\mathbf{S}_k$ to $\mathbf{G}_{k,\text{min}}$ |
| $c_d$ | Minimum safe distance between drones (Cartesian) |
| $d_{\text{safe}}$ | Lateral safety margin at narrowest point |
| $d_{i,\text{safe}}$ | Lateral safety margin at waypoint $i$ |
| $\lambda_d, \lambda_{\text{sa}}$ | Weights for shape change and safety in GA fitness |
Distributed Trajectory Optimization for Each Drone
With the virtual leader trajectory $\Pi(t)$ (obtained by interpolating $\mathbf{P}_i$) and the target configurations $\mathbf{G}_{k,i}$, we now compute individual drone trajectories in a distributed manner. Each drone solves a local nonlinear optimization problem to generate its trajectory $\mathbf{P}_k(t)$ that tracks its assigned configuration points while satisfying kinematic and collision constraints.
First, we define a mapping $\Psi$ from Frenet frame coordinates $(h, l)$ to Cartesian coordinates $\mathbf{P}$. Given a configuration point $\mathbf{G}_{k,i} = (h_{k,i}, l_{k,i})$ at time $t_i$, the corresponding Cartesian position is:
$$ \mathbf{P}_{k,i} = \Psi(\mathbf{G}_{k,i}) = \Pi(t_i – \frac{l_{k,i}}{v_{\text{ref}}}) + h_{k,i} \begin{bmatrix} -\sin\theta(t_i’) \\ \cos\theta(t_i’) \\ 0 \end{bmatrix} $$
where $t_i’$ is the time adjusted for longitudinal offset, $\theta$ is the heading angle of the virtual leader, and $v_{\text{ref}}$ is a reference speed. For simplicity in optimization, we approximate this mapping linearly around the virtual path.
Each drone $k$ optimizes its discrete trajectory points $\mathbf{P}_{k,i}$ (for $i=1,\dots,N$) with the following objective:
$$ \min_{\mathbf{P}_{k,i}, \delta_i, \epsilon_i} \lambda_o \sum_{i=1}^{N} \|\mathbf{P}_{k,i} – \Psi(\mathbf{G}_{k,i})\|^2 + \lambda_{\text{as}} \sum_{i=2}^{N-1} \|\mathbf{P}_{k,i+1} – 2\mathbf{P}_{k,i} + \mathbf{P}_{k,i-1}\|^2 + \lambda_{\text{vr}} \sum_{i=1}^{N} \delta_i + \lambda_{\text{ar}} \sum_{i=1}^{N} \epsilon_i $$
subject to:
- External collision avoidance: The lateral offset of drone $k$ from the virtual leader path must be within $d_{i,\text{safe}}$: $\| \mathbf{P}_{k,i} – \Pi(t_i) \|_{\text{lat}} \leq d_{i,\text{safe}}$ (simplified as a linear constraint in Frenet coordinates).
- Internal collision avoidance: For any other drone $k’$, the distance between drone $k$’s position and the line segment $\mathcal{E}_{k’}$ (mapped to Cartesian) must be safe. Using scaling factors $\sigma_{\text{min}}$ and $\sigma_{\text{max}}$ to convert Frenet distances to Cartesian, we enforce:
$$ \Gamma(\mathbf{P}_{k,i}, \Psi(\mathbf{G}_{k’,i}), \Psi(\mathbf{G}_{k’,\text{min}})) \geq \frac{\sigma_{\text{min}} c_d – d_{k’}}{2\sigma_{\text{max}}} $$
where $\Gamma$ is point-to-segment distance, and $d_{k’}$ is the minimum distance between segments $\mathcal{E}_k$ and $\mathcal{E}_{k’}$ from the GA. - Kinematic soft constraints: Velocity and acceleration limits are enforced with slack variables $\delta_i \geq 1$, $\epsilon_i \geq 1$:
$$ \|\mathbf{P}_{k,i} – \mathbf{P}_{k,i-1}\| / t_s \leq \delta_i v_{\text{max}}, \quad \|\mathbf{P}_{k,i+1} – 2\mathbf{P}_{k,i} + \mathbf{P}_{k,i-1}\| / t_s^2 \leq \epsilon_i a_{\text{max}} $$ - Initial condition: $\mathbf{P}_{k,1} = \mathbf{P}_{\text{start},k}$.
The weights $\lambda_o, \lambda_{\text{as}}, \lambda_{\text{vr}}, \lambda_{\text{ar}}$ balance trajectory tracking, smoothness, and constraint satisfaction. This is a convex quadratic problem with linear constraints if the internal collision constraint is approximated linearly, which can be solved efficiently with solvers like CLARABEL.
However, the initial time allocation $t_s$ might lead to violations of hard kinematic limits after optimization. Therefore, we perform a cooperative iterative time reallocation. All drones share their trajectory points and current time intervals $\Delta t_{i,q}$ via communication. In each iteration, for any drone that violates speed or acceleration limits at a point, the corresponding time intervals are increased by a small factor $k_e > 1$ (e.g., 1.0001). Drones synchronize by adopting the maximum $\Delta t_{i}$ across the formation for each interval. This process gradually stretches the timeline until all drones satisfy the constraints, ensuring formation synchronization and kinematic feasibility.
The overall distributed approach allows each drone to plan its own trajectory while respecting formation shape and collision constraints, leading to a scalable solution for large drone formations.
Simulation Experiments and Results
We validate our method through simulations in a scenario with a narrow corridor of minimum width 4 m and length 22 m. The drone radius is 0.16 m. The virtual leader starts at (0,0,2) m with velocity (0,3.5,0) m/s and aims for goal (3,40,5) m with velocity (0,3.5,0) m/s. The corridor walls are modeled as six rectangular obstacles inflated by 0.2 m. Parameters: $N=60$, $t_s=0.4$ s, $v_{\text{max}}=4$ m/s, $a_{\text{max}}=0.4$ m/s², $\lambda_s=10$, $\lambda_{\text{safe}}=100$, $\lambda_f=10$.
The improved MILP solved in 5.163 s, yielding $d_{\text{safe}} = 0.88$ m. The virtual leader trajectory reached the goal early (at step 30), demonstrating high-speed capability. Compared to a basic MILP (without maximizing $d_{\text{safe}}$ and with fixed arrival time), our trajectory was more centered in the corridor and safer, as shown in comparative plots (omitted here per guidelines).
For the formation, we considered a quadcopter drone formation ($N_{\text{UAV}}=4$) with initial configuration $\mathbf{S}_k$: (-2,2), (-2,0), (2,0), (2,2) in meters (Frenet frame). The GA parameters: population size 30, generations 500, mutation rate 10%, weights $\lambda_d=\lambda_{\text{sa}}=1$. The GA found $\mathbf{G}_{k,\text{min}}$ approximately as (-0.88,2.0), (-0.88,0.0), (0.84,0), (0.84,2) in 1.499 s. Table below shows GA performance under different parameters, indicating robustness.
| Iterations | Population Size | Mutation Rate | UAV Count | Time (s) | Avg. Config. Error (m) |
|---|---|---|---|---|---|
| 100 | 30 | 10% | 4 | 0.961 | 0.062 |
| 300 | 30 | 10% | 4 | 2.196 | 0.00 |
| 200 | 40 | 10% | 4 | 2.04 | 0.01 |
| 200 | 30 | 5% | 4 | 1.562 | 0.01 |
| 200 | 30 | 20% | 4 | 1.690 | 0.042 |
| 200 | 30 | 10% | 6 | 2.980 | 0.013 |
| 200 | 30 | 10% | 8 | 5.269 | 0.01 |
Subsequently, the distributed nonlinear optimization (with $\lambda_o=10$, $\lambda_{\text{as}}=1$, $\lambda_{\text{vr}}=1$, $\lambda_{\text{ar}}=100$) generated trajectories for each drone. The resulting paths show the formation contracting to a narrow line in the tight section and expanding back to the original shape in open areas, as visualized in trajectory plots. The optimization time per drone was about 10 ms. Speed and acceleration profiles initially had some violations, but after cooperative time reallocation (with $k_e=1.0001$, 20 iterations), all drones satisfied $v_{\text{max}}$ and $a_{\text{max}}$.
We compared our method with a vector field approach from literature and a distributed model predictive control (MPC) method. Our approach ensured no collision with obstacles, whereas the vector field method sometimes led to trajectories overlapping obstacles due to soft constraint limitations. Moreover, in terms of formation likeness, measured by the cumulative sum of mean squared errors between inter-drone distances at each time and those in the initial formation, our method achieved 70.943 m² versus 96.756 m² for the vector field method, indicating better formation preservation. Compared to distributed MPC, our method had lower cumulative shape error (70.943 vs 79.402 m²) and faster arrival time (12.0 s vs 24.0 s), though MPC offers better online computation. These comparisons highlight our method’s advantages in safety, speed, and formation maintenance for narrow corridor traversal.
| Method | Cumulative Formation Error (m²) | Arrival Time (s) | Online Capability |
|---|---|---|---|
| Our Method | 70.943 | 12.0 | Requires pre-planning |
| Vector Field | 96.756 | ~15 | Online |
| Distributed MPC | 79.402 | 24.0 | Online |
Conclusion
In this work, we presented a comprehensive multi-stage strategy for high-speed, safe formation transformation of drone swarms traversing narrow corridors. The key innovation lies in the synergistic combination of centralized planning for a global reference and safe formation configuration with distributed trajectory optimization. The improved MILP generates a virtual leader path that is fast, smooth, and maximizes obstacle distance, providing a robust foundation. The genetic algorithm efficiently finds a minimally altered, collision-free formation configuration for the tightest spot. Finally, the distributed nonlinear optimization with cooperative time reallocation produces kinematically feasible trajectories for each drone that respect formation shape and safety constraints.
Simulations demonstrate that our method enables a drone formation to safely traverse narrow passages at high speed while maintaining a configuration close to the initial one, outperforming existing methods in safety and formation preservation. The approach is scalable, though the centralized MILP and GA may require pre-planning for complex environments. Future work could focus on real-time adaptations using faster trajectory generation techniques and extending to 3D formation transformations with dynamic obstacles. Overall, this work advances the state of the art in coordinated drone formation navigation through constrained spaces, a critical capability for future autonomous swarm operations.