Agile target capture with UAV swarm in dense environments

3.1.1. Target state estimation

In the real-time swarm capture operation described in this paper, $$ N $$ pursuer UAVs collaboratively estimate the state of a target. The global pose of the $$ i $$-th pursuer UAV at timestamp $$ t $$ is assumed to be available as $$ (\mathbf{R}_{i, t}, \mathbf{p}_{i, t}) \in SO(3)\times \mathbb{R}^{3} $$ in a common world frame, where $$ \mathbf{R}_{i, t} $$ and $$ \mathbf{p}_{i, t} $$ denote the rotation and translation of the pursuer body frame with respect to that frame. This pose can be provided by motion capture, visual/LiDAR SLAM, or a cooperative localization module; the localization and mapping backend is not the contribution of this paper. To reduce computational load, we estimate only the global position of the target UAV in three dimensions:

$$ \mathbf{z}^{b}_{i, t} $$ indicates the 3D position observation of the target by UAV $$ i $$ in its own attitude at timestamp $$ t $$. With the known pursuer pose, the observation is transformed into the global frame by

where $$ \mathbf{z}^{w}_{i, t} $$ is the global position measurement inferred from the $$ i $$-th pursuer UAV.

All target measurements collected within a short fusion interval are assigned to the same optimization timestamp. This design reduces the impact of small communication delays without requiring strict hardware timestamp synchronization. Due to noise and occasional outliers, observation clustering is performed before graph optimization. Specifically, we need to perform K-means clustering on all observations $$ \{\mathbf{z}^{w}_{i, t}\}_{i=1}^{N} $$ at each time step $$ t $$ to obtain a preliminary target estimate.

Where $$ \{\mathcal{C}^{(k)}_{t}\} $$ denotes the obtained clusters, and $$ \mathcal{I}_{t} $$ represents the set of interior points within the dominant cluster with the largest cardinality. After filtering out observations outside $$ \mathcal{I}_{t} $$, the interior point set undergoes weighted fusion to yield a single stable and robust local measurement at time $$ t $$:

Here, $$ w_{i, t} $$ reflects the confidence level of the $$ i $$-th pursuer observation, derived from the covariance of the observation data. The fused measurement $$ \bar{\mathbf{z}}^{w}_{t} $$ is used as the instantaneous estimate of the target's global position. Although it mitigates the impact of multi-view outliers, it remains subject to residual random errors and is therefore refined by the subsequent PGO module.

Compared to the Kalman filter method, which heavily relies on the target motion model, PGO provides a more robust and convenient tightly coupled method that does not depend on continuous observations of a single node. To achieve a globally optimal consensus target trajectory while avoiding computational overload, we address these challenges by integrating a pose map optimization algorithm with a sliding window approach. The detailed factor graph construction process is shown in Figure 3.

Figure 3. PGO target state estimation. (A) Data acquisition and factor distribution of the active sliding window across four consecutive frames during UAV swarm capture; (B) Factor graph constructed by the PGO method. As the window moves forward, the oldest state is marginalized into the prior residual. The red dashed lines and red triangles represent prior residual information; the yellow dashed lines and yellow triangles represent observation residuals; the black trajectory and black triangles represent motion residuals; and the green triangles represent the smoothing residuals of the optimized trajectory. PGO: Pose graph optimization; UAV: unmanned aerial vehicle.

We construct a graph $$ \mathcal{G}=(\mathcal{V}, \mathcal{E}) $$, where the vertices correspond to the target locations at different timestamps:

Each vertex $$ \mathbf{p}_{t} $$ indicates the target position to be estimated. The edge set $$ \mathcal{E} $$ consists of four types of factors: (1) marginalized prior residual $$ r_p $$ from the previous window; (2) observation residuals aligned with fused measurements; (3) relative motion residuals across multiple timestamps within a sliding window; (4) second-order trajectory smoothing residuals.

The term $$ r_p $$ is the prior factor obtained after marginalizing the oldest target state when the window slides. It preserves information from past measurements and is computed using Schur-complement marginalization^[6].

At each timestamp $$ t $$, Observation residual constrains $$ \mathbf{p}_{t} $$ to be close to the fused observation:

The corresponding information matrix $$ \Omega^{u}_{t} $$ is determined by the uncertainty of $$ \bar{\mathbf{z}}^{w}_{t} $$. To encode target motion continuity, we compute a relative displacement measurement between two consecutive fused observations:

Relative motion residual between $$ \mathbf{p}_{i} $$ and $$ \mathbf{p}_{j} $$ is then defined as

The second-order smoothing term constraint primarily constrains the target acceleration, ensuring the estimated value does not deviate excessively. The constraint error term is as follows:

This factor encourages the optimized trajectory to follow the measured inter-frame displacement, while allowing errors to be distributed across the window rather than accumulating at the latest estimate. The information matrix $$ \Omega^{b}_{ij} $$^[6] reflects the confidence of the relative displacement, which can be approximated by the propagated covariance of $$ \bar{\mathbf{z}}^{w}_{t} $$ and $$ \bar{\mathbf{z}}^{w}_{t-1} $$.

After constructing all vertices $$ \mathcal{V} $$ and edges $$ \mathcal{E} $$, they are incorporated into a sliding window pose optimization problem. To further address outliers during the fusion process, we incorporate a robust kernel function $$ \rho(\cdot) $$^[6] into the residual construction.

where $$ s $$ is the squared Mahalanobis residual and $$ \delta $$ is the kernel threshold. By minimizing the following optimization problem, a relatively accurate estimate of the target trajectory state can be obtained.

where $$ \mathcal{T}=\{t-L+1, \dots, t\} $$ denotes a fixed-size sliding window of length $$ L $$, $$ \mathcal{E}_{b} $$ denotes the relative-motion edges inside the window, and $$ \Omega^{a}_{i} $$ is the smoothing information matrix. Upon arrival of a new fusion measurement $$ \bar{\mathbf{z}}^{w}_{t+1} $$, a new vertex and corresponding unary/binary edges are added; the oldest vertex is marginalized into $$ r_p $$ if the window is full, and the optimization problem is solved using the Ceres solver. Where $$ \lambda_1 $$, $$ \lambda_2 $$ and $$ \lambda_3 $$ represent the weights of the observation residual, the relative motion residual and the smoothing residual, respectively, and the specific values will be explained in subsequent experiments. The refined target position estimate is fed into downstream planning and control modules.

3.1.2. Target trajectory prediction

This paper uses Bernstein basis polynomials, called Bézier curves, to describe the target prediction trajectories. nth order Bézier curve is denoted as:

In this context, $$ b_n^i $$ denotes an $$ n^{th} $$ -order Bernstein polynomial basis, as outlined in reference^[3]. The set of control points for the Bézier curve, denoted by $$ [c_0, c_1, \dots, c_n] $$, is a crucial component in the analysis.

The 3D position of the target observed in the global frame at time $$ t $$ is denoted as $$ P_t \in \mathbb{R}^3 $$. Subsequently, a First-In, First-Out (FIFO) queue of length $$ L $$ is established to store the preceding observations and their corresponding timestamps (The value of L is related to the system operating frequency). The queue is denoted as $$ Q_{target}=[q_0, q_1, \dots, q_l] $$, where $$ q_i=\{p_{t_i}, t_i\} $$. $$ Q_{target} $$ contains the time range $$ [t_1, t_l] $$, where $$ t_l $$ equals the current time. Upon the acquisition of a new target observation, a corresponding target prediction trajectory, denoted $$ B(t) $$, is generated through the fitting of preceding observations.

As time elapses, the confidence of prior observations invariably diminishes. Consequently, the queue length L is fixed with precision, while values that fall outside the designated range are discarded. The trajectory is extrapolated to $$ (t_l, t_{pre}] $$ during which the target motion is predicted.

In the capture process, the hyperbolic tangent function $$ \tanh(\cdot) $$ is employed to select the prediction horizon according to the distance $$ d $$ between the center of mass and the current target location:

where $$ t_{step} $$ denotes a preset prediction time step. Figure 4 shows the predicted trajectory of the target in a dense environment.

Figure 4. Trajectory prediction: the red dot on the left side of the picture represents the center of mass of the drone swarm, which is the center for surrounding and capturing the target. The blue curve represents the historical trajectory of the target, and the trajectory is predicted based on the first L historical observations. The red curve represents the expected trajectory. The brown curve represents the target prediction of the previous frame, while the purple curve represents the target prediction of the current frame. By piecing these trajectories together, the central point of the real-time captured trajectory is formed.

3.2.1. Capture queue

At this stage, the UAV swarm position is known in three dimensions. The capture order and circular-formation assignment are planned on the horizontal projection because encirclement mainly constrains the target in the $$ xoy $$ plane; the altitude component is still handled by the 3D trajectory planner. Let $$ P_{i, t}^{xy}\in\mathbb{R}^2 $$ be the horizontal projection of UAV $$ U_i $$, and let the projected centroid be $$ C=\frac{1}{N}\sum_{i=1}^{N}P_{i, t}^{xy} $$. Let the projected target capture point be $$ T^{xy} $$. The capture direction is then defined as $$ \vec{v}=T^{xy}-C $$.

For each UAV $$ U_i $$, define the projected vector with respect to the center of mass: $$ \vec{r_i}=P_{i, t}^{xy}-C $$. Calculate the angle of this vector with respect to the capture direction $$ \vec{v} $$. Let $$ \alpha_i=mod\left(\theta\left(\vec{r_i}\right)-\theta\left(\vec{v}\right), 2\pi\right) $$, where $$ \theta(\vec{u}) $$ denotes the angle of the vector $$ \vec{u} $$ with the x-axis, and takes on the range of $$ [0, 2\pi) $$; here it is ordered in clockwise direction (i.e., the angle is gradually increasing from 0). Sort all $$ \alpha_i $$, and remember that the sorted UAVs are re-assigned the serial number (i.e., the initial capture order) as $$ S=\left(U_1, U_2, \cdots, U_N\right) $$, which satisfies $$ 0\le\alpha_1\le\alpha_2\le\cdots\le\alpha_N\le2\pi $$.

In the context of the center of mass position, if a UAV satisfies $$ \left|\vec{r_i}\right|=0 $$, it can be deduced that said UAV is situated within the “center of mass region.” In order to ensure the continuity of the capture formation, the UAV is inserted between two neighboring UAVs in the circular formation in an orderly manner. All possible insertion sequences I(J) are obtained, and all such candidate sequences are written into a FIFO candidate queue $$ Q_{adj} $$.

If a set of candidate drones satisfies the condition of dense drone angle $$ \left|\Delta\alpha\right|< \delta $$, where $$ \delta $$ is a predefined small threshold, the set of these drones is denoted as J. The objective is to obtain all possible insertion sequences, denoted by $$ \Pi(J) $$, by performing permutations and combinations of all drones in J. A specific permutation, denoted by $$ \sigma(J) $$, is then selected and inserted into the spacing position of the original sequence to form a candidate neighboring sequence. The resultant candidate sequences are then written into a candidate queue, $$ Q_{adj} $$, in a FIFO manner. The formation of the drone swarm capture team is shown in Figure 5 below.

Figure 5. Target capture strategy generation: (A) illustrates the formation of a capture configuration by the drone swarm. The red dot represents the center of mass position of the swarm; (B) shows the capture points for each drone obtained by minimizing the objective function after the capture formation is established, when the surrounding area of the target is relatively open; (C) presents trajectory diagrams for dynamic target capture in dense environments. Curves of varying shades represent the historical trajectories of individual drones, with gray and blue arrows indicating obstacle avoidance factors and inter-drone collision avoidance factors, respectively. (C) also demonstrates how solutions to the objective function during capture alter the initial formation.

The generation of this initial capture queue significantly accelerates the computation of the objective function, ensuring rapid capture decisions can be made for the target.

3.2.2. Generate capture points

In sparse environments, drone swarms can form circular formations with uniform angular spacing to capture targets. However, in dense environments, factors such as obstacles, in-swarm collisions, and rapid target acquisition make strictly uniform formations highly risky. To address this, we allow non-uniform angular spacing while maintaining a common capture radius to extend the capture formation.

Let $$ S=(U_{S(1)}, \dots, U_{S(N)}) $$ denote a candidate capture sequence in the queue $$ Q_{\mathrm{adj}} $$, and Let $$ P $$ denote the predicted value of the target mentioned earlier. For the $$ j $$-th UAV in the sequence $$ S $$, we assign an individual angular variable $$ \theta_j $$ and a shared capture radius $$ R $$, where $$ \Theta = [\theta_1, \dots, \theta_N]^\top $$ collects all angular parameters. In the special case where $$ \theta_j = \theta_0 + 2\pi(j-1)/N $$ degenerates to the uniform circular formation. To rapidly respond to the capture of cluster targets, we constructed a comprehensive cost function that explicitly considers position fitting, tight enclosure, obstacle avoidance, intra-group collision avoidance, and formation regularity. By performing parallel computations across different candidate sequences $$ S $$, we obtained the optimal capture queue and associated capture parameters $$ (\Theta, R) $$.

The desired capture point of the $$ j $$-th UAV on the generalized circular arc is defined as

First, a fitting term measures the deviation between the actual UAV positions and their desired capture points:

To encourage groups to form a compact encirclement around the target while ensuring safe capture, without exceeding the minimum capture radius of $$ R_{\min} $$, we introduce a distance regularization term.

Environmental obstacles are considered through a soft penalty on the distance between capture points and obstacle boundaries. Let $$ d_{j, k} $$ denote the shortest distance from $$ P_j^*(\Theta, R) $$ to the surface of the $$ k $$-th obstacle, and let $$ d_{\mathrm{obs}} $$ be a safety margin. We define a hinge-type penalty function

and accumulate it over all UAV–obstacle pairs:

Similarly, to prevent intra-swarm collisions in the final capture formation, we penalize small pairwise distances between capture points. Let

and let $$ d_{\mathrm{col}} $$ denote a required minimum spacing. We define

and the collision avoidance cost as

To maximize the capture rate of targets and make escape difficult in any direction, we introduced formation rules. Define the angular gaps along the circular formation as

and compare them with the ideal uniform spacing $$ 2\pi/N $$. The regularity cost is given by

When obstacles and collision constraints are inactive, this term encourages the angular parameters to converge back to a nearly uniform circular distribution.

Combining the above components, the dense-environment capture cost for a candidate sequence $$ S $$ is formulated as

where $$ w_{\mathrm{fit}}, w_{\mathrm{dist}}, w_{\mathrm{obs}}, w_{\mathrm{col}}, w_{\mathrm{reg}} $$ are scalar weights that balance tracking fidelity, encirclement tightness, obstacle avoidance, collision avoidance, and formation regularity, respectively. In practice, the signs and magnitudes of these weights are determined empirically, enabling $$ J_{\mathrm{fit}} $$, $$ J_{\mathrm{obs}} $$, and $$ J_{\mathrm{col}} $$ to serve as safety-related penalties, while $$ J_{\mathrm{dist}} $$ and $$ J_{\mathrm{reg}} $$ function as soft rewards for tight and regular capture formation.

For each neighboring candidate $$ S \in Q_{\mathrm{adj}} $$, the optimal formation parameters $$ (\Theta^*, R^*) $$ are obtained by minimizing the dense-environment capture cost above:

Among all candidate sequences $$ S $$, the one achieving the minimum cost

is selected as $$ S^* $$, and the corresponding optimal capture points are

This enables the drone swarm to adapt its formation to local obstacles and inter-aircraft spacing constraints while capturing targets densely and uniformly throughout the environment. The cost functions are solved using the Ceres solver library.

3.3.1. Tracking trajectory planning

In the safe pursuit trajectory planning framework, the tracking trajectory generation module adopts the FAST-tracker framework proposed by^[3]. Because planning differs between multi-machine and single-machine systems, the anti-collision mechanism among clusters must be considered. In multi-UAV cluster cooperative operations, preventing flight-path conflicts is pivotal to ensuring safe flight operations. In this paper, we propose an anti-collision mechanism based on priority ranking and short-term collision prediction that reduces collision risk by coordinating the movement priorities of individual UAVs within the swarm. It does so by coordinating the movement priority of each UAV in the cluster. The fundamental principles of the mechanism are outlined as follows.

The objective of the short-term collision risk detection machine is to detect collisions during agile capture. This machine has been developed to reduce computation time and improve real-time performance. It carries out collision detection for the next short moment (or a short time window) in the UAV prediction path. The specific method is as follows: for the native trajectory to be executed, a sequence of discrete sampling points is generated based on a hybrid A* search and trajectory optimization. This sequence is used as the predicted path. The trajectory of UAV $$ u_i $$ is defined by m-segment polynomials, thereby establishing the set of parameters:

Here, $$ t_k $$ denote the duration of the $$ k^{th} $$ segment, while $$ n_k $$ denotes the polynomial order of the $$ k^{th} $$ segment. The coefficient matrix, $$ C_k=\left[c_x^{\left(k\right)}, c_y^{\left(k\right)}, c_z^{\left(k\right)}\right]\in\mathbb{R}^{\left(n_k+1\right)\times3} $$.

In accordance with the per-segment polynomial that was obtained from the preceding tracking trajectory planning, the relative time of the present moment within each segment is calculated.

where $$ t_{abs} $$ is the absolute time, $$ t_{start} $$ is the trajectory start timestamp, S is the number of trajectory segments, and $$ t_{seg} $$ is the relative time within a segment. Calculate the 3D spatial position of the predicted trajectory:

in this context, $$ P_{odom}=\left[x_{odom}, y_{odom}, z_{odom}\right] $$ denotes the current position of the UAV, and $$ C_j^{\left(s\right)}=\left[c_{x, j}^{\left(s\right)}, c_{y, j}^{\left(s\right)}, c_{z, j}^{\left(s\right)}\right] $$ is the coefficient of the $$ j^{th} $$ term of the $$ s^{th} $$ segment. The sampling points at each moment in the local trajectory are then compared with the extrapolated positions of all other UAVs at the same predicted moment. If the distance is below the preset safety distance, the system determines the potential collision risk.

The prioritization of tasks is contingent upon the target distance. Upon the initiation of collision detection, each UAV is equipped with the capability to receive the state information of the target in real time, concurrently acquiring position information of other UAVs through integrated sensors or communication systems. Each UAV calculates the Euclidean distance between itself and the target and compares it with the distances of other UAVs that may collide, as obtained from the collision detection mechanism described above, to prioritize movement. The drone closest to the target (i.e., with priority 0) will maintain its predetermined trajectory even if a potential collision risk is detected; while other drones will replan to varying degrees according to their priorities. After prioritization, the replanning time of obstacle avoidance drones increases sequentially and equals the smallest time unit multiplied by the priority number. The calculation method for priority is as follows:

in this context, $$ P_J\in\mathbb{R}^3 $$ represents the positional coordinates of the $$ j^{th} $$ UAV, $$ p_{target} $$ denotes the target positional coordinates, and the $$ rank(\cdot) $$ function retrieves the indexed order subsequent to ascending sorting. This method prevents all UAVs from hovering simultaneously during mission execution, thereby ensuring the robustness of cluster-persistent action and target capture.

3.2.2. Yaw coordination and field-of-view-aware planning

The yaw motion of each pursuer UAV plays a critical role in target visibility, trajectory planning, and obstacle perception. Onboard cameras provide a limited horizontal FOV. To balance all these constraints within the limited FOV, this paper proposes a method that enhances tracking trajectories through yaw planning under field-of-view, smoothness, and yaw-rate constraints.

Before entering the capture phase, one UAV, denoted by $$ U_i $$, is assigned to follow the real target and provide an accurate estimate of its motion. At the switching time $$ t_0 $$ from tracking to capture, we use the terminal state of $$ U_i $$ to initialize the yaw planning. Let $$ P(t_0) $$ be the estimated target position and $$ Q_j^* $$ be the position of $$ U_i $$. The initial yaw of the tracking UAV is chosen such that its camera FOV is centered on the target:

where $$ d(\psi) $$ denotes the unit viewing direction of the camera in the horizontal plane associated with yaw $$ \psi $$. For the remaining UAVs, the initial yaw is set to align with their outward radial directions (defined below), so that they immediately contribute to outward perception around the swarm boundary. This initialization ensures a seamless transition from the tracking stage to the capture stage, while preserving at least one reliable line of sight to the real target.

For each pursuer UAV $$ U_i $$, the trajectory planner described earlier provides a time-parameterized positional trajectory $$ p_i(t) $$. At each discrete sampling instant along this trajectory, we define a radial “outward” unit vector relative to the group center of mass $$ C(t_k) $$ as

The yaw angle of UAV $$ u_i $$ at time $$ t_k $$ is denoted by $$ \psi_i(t_k) $$, and its corresponding camera boresight direction in the horizontal plane is $$ d(\psi_i(t_k)) $$. To make each UAV’s FOV preferentially “look outward”, we introduce an alignment cost over the planning horizon $$ \mathcal{T} = \{t_1, \dots, t_K\} $$:

To ensure that the swarm does not lose sight of the target. Let $$ P(t_k) $$ be the estimated target position at time $$ t_k $$, and define the line-of-sight unit vector from $$ U_i $$ to the target as

The angular separation between the camera boresight and the line of sight is

The target is inside the horizontal FOV if $$ \gamma_i(t_k)\le \varphi_{\max} $$, where $$ \varphi_{\max} = \tfrac{1}{2}\varphi_{\mathrm{FOV}} - \Delta\varphi $$ includes a safety margin $$ \Delta\varphi $$ for modeling errors and occlusion.

To encode the requirement that “at least $$ N_{FOV} $$ drones must observe the target”, we introduce a soft visibility penalty mechanism. At each time step, we select the optimal observing drone and impose a penalty for violating the FOV constraint:

Where, $$ rank^{N_{FOV}} $$ denotes the $$ N_{FOV} $$ number in this ascending sequence.

If at least $$ N_{FOV} $$ UAVs satisfy $$ \gamma_i(t_k)\le \varphi_{\max} $$, then $$ \eta(t_k)\le 0 $$ and the corresponding term vanishes. Otherwise, $$ J_{\mathrm{vis}} $$ grows quadratically with the smallest FOV violation, which drives the optimizer to rotate one or more UAVs toward the target until visibility is re-established.

To avoid abrupt and aggressive yaw motions, we further impose a smoothness term on the yaw trajectories. Let $$ \Delta t_k = t_k - t_{k-1} $$ and define the discrete yaw increment of $$ U_i $$ as

The yaw smoothness cost over the planning horizon is given by

This term penalizes large step-to-step changes in yaw angle, leading to continuous and physically feasible yaw trajectories that are more compatible with the attitude dynamics and onboard gimbal constraints. In implementation, we also limit the yaw increment by the maximum yaw rate $$ \dot{\psi}_{\max} $$:

This rate limit prevents the FOV from snapping instantaneously between two directions.

Collecting all yaw angles over the horizon into a single decision vector,

The yaw planning problem is formulated as the following nonlinear least-squares problem:

Where $$ \lambda_{\mathrm{vis}} $$ and $$ \lambda_{\mathrm{sm}} $$ represent the weights of the visualization constraint and the smoothing term constraint, respectively. The optimization problem is solved using Ceres. In each iteration, residuals are linearized around the current yaw estimate. The resulting yaw trajectory is then passed to the lower-level controller, along with position and velocity references.