Distributed Formation Planning for Unmanned Aerial Vehicles

Abstract

Formation flying of multiple unmanned aerial vehicles (UAVs) has attracted much attention for its versatility in cooperative tasks. In this paper, a distributed formation planning method is proposed for UAVs. First, we design a path searching algorithm, swarm-A*, which can enhance the cohesion of a swarm, i.e., preventing the disintegration of the swarm when it encounters an obstacle. Then, after waypoint reallocation, a formation trajectory optimization framework is formulated. Smooth formation trajectories for UAVs to travel safely in obstacle-laden environments can be obtained by solving the optimization problem. Next, a tracking controller based on sliding mode control is designed, ensuring that the UAVs follow the planned formation trajectories under dynamic constraints. Finally, numerical simulations and experiments are conducted to validate the effectiveness of the proposed method.

1. Introduction

Unmanned aerial vehicles (UAVs) have gained significant attention because of their numerous advantages, including high mobility, low operational cost, and the ability to access difficult or hazardous environments. These characteristics have led to their widespread application in various fields. However, when a single UAV is tasked with complex operations, its limited capacity in terms of payload, coverage area, and redundancy often restricts working effectiveness. To overcome these challenges, there is a growing need for multiple UAVs to collaborate and complete tasks more efficiently as a swarm. Due to their flexibility and efficiency, UAV swarms can be used for various kinds of cooperative tasks, such as surveillance [1], search [2], rescue [3], exploration [4], cooperative encirclement [5], and object transportation [6,7].

In the domain of UAV swarms, formation flying has emerged as a key strategy for enabling coordinated behavior. It allows UAVs to work together in predefined spatial configurations, thus optimizing resource usage, increasing coverage, and ensuring fault tolerance. The ability to manage and maintain formation not only enhances the operational capabilities of UAV swarms but also improves their overall robustness and efficiency in task execution, making it an essential approach in modern UAV systems.

Planning is an indispensable component of any autonomous system, offering safe and efficient guidance to complete specific tasks. Therefore, formation planning is a critical aspect of UAV swarms. Formation planning refers to the process of determining the optimal movement strategy for UAVs to ensure safe task completion while maintaining the desired formation shape. Collision avoidance between UAVs and formation shape maintenance are the two main challenges of the formation planning problem. Many researchers have devoted attention to this field in recent years.

Extensive research has been carried out on the collision avoidance problem (collision with obstacles and collision between UAVs) in multi-UAV planning. Alonso-Mora et al. [8] used the velocity obstacle method to establish a local obstacle avoidance planning problem for multiple UAVs. Collision avoidance, obstacle avoidance, and motion continuity problems are considered in the cost functions of the proposed optimization problems. Safe and feasible real-time local trajectories are obtained by solving the proposed problems. Zhou et al. [9] presented EGO-swarm, a decentralized approach for autonomous navigation by multiple UAVs using only onboard resources. The planning system is formulated under gradient-based local planning framework, where collision avoidance is realized by formulating the collision risk as a penalty of a nonlinear optimization problem. On that basis, ref. [10] used MINCO instead of a B-spline to parameterize trajectories, thus solving the difficulty of time adjustment when UAVs need to pass through the same area. It also produced smoother trajectories and a lower optimization time. Tordesillas et al. [11] presented MADER, a 3D decentralized and asynchronous trajectory planner for UAVs that generates collision-free trajectories. Collision with other UAVs can be realized by including their committed trajectories as constraints in the optimization and then executing a collision check–recheck scheme. Recently, Toumieh et al. [12] proposed a high-speed, decentralized, and synchronous motion planning framework (HDSM), which generated a time-aware safe corridor (TASC) to guarantee the safety of the UAV trajectories. Zhao et al. [13] introduced a new Theta*–APF method for drone swarm path planning in 3D space; the method reduces the searching time and the path length. Collision avoidance for the agents is realized utilizing repulsive force fields.

Although the above work offers outstanding performance in collision avoidance for UAV swarms, the formation maintenance problem is not taken into account. Existing formation maintenance methods are summarized below.

First, from the perspective of control, a high-precision formation shape can be maintained. Leader–follower control is a hierarchical approach used in UAV swarm operations, where one or several UAVs are designated as leaders, and the others follow these leaders based on predefined rules [14]. This approach depends on the stability and reliability of the leader(s), meaning that disruption of the leader’s communication could impact the entire formation. Therefore, when faced with intricate scenarios and constraints, such methods [15,16,17] need further research and refinement to address all the requirements. Virtual structure methods establish a geometric formation in which the UAVs move together as a cohesive unit. Each UAV preserves its relative position as the formation moves, allowing the entire formation to translate, rotate, or scale as required to meet mission goals or avoid obstacles. These approaches enable precise control over the formation shape and provides an intuitive way to modify the formation in response to changing conditions. However, as the number of UAVs increases, the stability and accuracy of the virtual structure may become extremely difficult to maintain. Consensus control is also a classic and promising formation maintenance approach, enabling the UAVs to agree on specific parameters such as position, velocity, or angle, making the swarm move cohesively. It promotes flexible adaptation to environmental changes and enhances collaborative task performance. Ref. [18,19,20] are all recent works utilizing consensus theory to design formation planning algorithms. However, the effectiveness of consensus algorithms depends heavily on the quality of communication between UAVs and the convergence speed of the algorithms. Other control methods such as fuzzy control and adaptive control [21] can also tackle the formation maintenance problem.

Second, from the perspective of searching and optimization, flexibility and robustness can be improved by treating the formation maintenance requirement as a soft constraint. Nguyen et al. [22] formulated a distributed optimization problem based on dynamic consensus and solved the problem using ADMM (Alternating Direction Method of Multipliers), achieving time-varying formation shape maintenance with inter-robot collision avoidance. They took advantage of MPC-based motion planning approaches to design reference trajectories at each replanning instant. Quan et al. [23] designed a differentiable cost function based on graph theory to evaluate UAV formations. By considering formation similarity, obstacle avoidance, and dynamic feasibility, they realized a balance between UAV formation maintenance and safety during flight. Peng et al. [24] proposed a distributed and synchronous motion planning framework for a formation of multiple UAVs equipped with an active sensing system in an obstacle-based environment using a gradient-based method. They used expanding FOVs to enhance safety in the UAV swarm motion planning task. The planning problem was solved with the distributed particle swarm optimization algorithm. Zhang et al. [25] proposed an online formation planning method for a tethered multirotor UAV cooperative transportation system. An optimization problem was constructed considering asymmetric tension-based swarm reciprocal avoidance, obstacle avoidance, and target transportation. All constraints are represented as soft constraints to realize task requirements. Mikkelsen et al. [26] introduced a distributed planner for rigid formations. They first determined the scaling, rotation, and translation of a base configuration to obtain the desired velocities at each time step for the swarm. The desired velocities of the agents were mapped to a parameter space to guarantee consensus and constraints, and they were then remapped to the velocity space of the agents, ensuring that the robots maintained the shape of their formation. Liu et al. [27] proposed a global formation planning method with obstacle avoidance, which conceptualized robot formations into distinct configurations. The feasible configurations and the transitions between two configurations were represented as vertexes and edges, respectively, to construct an undirected graph where the optimal formation path could be found using searching algorithms.

Third, formation maintenance can also be realized from the perspective of deep reinforcement learning (DRL) [28,29,30,31]. In recent years, significant attention has been focused on DRL, in which deep neural networks are employed to approximate the value function, the policy, or both within reinforcement learning algorithms, allowing agents to effectively manage high-dimensional states. By utilizing DRL, UAVs are capable of independently coordinating their movements to preserve desired formation patterns, avoid collisions, and adapt to changing environments. Through continuous interaction with the environment, DRL methods allow UAVs to learn optimal control strategies, resulting in more efficient and resilient formation control compared to conventional approaches. DRL methods offer benefits such as complex decision-making, long-term reward optimization, and adaptability. However, there exist challenges, including sample inefficiency, potential training instability, the need for hyperparameter tuning, and safety risks during the learning process. Furthermore, the selection of these approaches depends on various factors, including the specific needs of the formation task, available data, computational resources, and safety concerns [14]. However, the high computational demands and long computing time are the main challenges for DRL-based methods.

In summary, different kinds of methods have their own advantages and shortages. A detailed comparison is shown in

Table 1. Comparison of different kinds of formation planning methods.

Method	Computational Complexity	Scalability	Environmental Adaptability
Control-based	relatively low	moderate	relatively low
Search-optimization	moderate	relatively high	relatively high
DRL-based	very high	high	high

. Control-based methods have lower computational demands and higher real-time performance, which make them easier to deploy, yet there is a lack of environmental adaptability because control-based methods rely on accurate environmental models and strict communication among agents. The local minimum problem is also a challenge during intricate obstacle avoidance. Search-optimization-based methods perform well with relatively high scalability and moderate computational complexity when the environment is known (even if it is intricate). This paper mainly considers the formation planning problem in terms of known static environments. DRL-based methods realize much higher scalability and adaptability, but the training process is time-consuming and requires enormous computing power. In summary, control-based methods are suitable for smaller swarms in easier environments; search-optimization methods are good at handling normal formations in environments that allow offline planning; and DRL-based methods are recommended for large-scale formations in unknown and complex environments under the condition of high computational power. Since this paper focuses on the formation planning problem in static environments for medium-scale swarms, the search-optimization method is utilized. Compared with the existing search-optimization-based algorithms, our proposed method simplifies the expression of formation cost using the reference relative vectors instead of the Laplacian matrix used in [23]. The path searching algorithm is also improved to avoid disintegration of the formation. The safe corridor approach is utilized instead of soft constraints to ensure flight safety in [23,24].

In this paper, we introduce the swarm-A* algorithm, which enhances the cohesion of the swarm and prevents disintegration of the formation during the path searching process, and we also present a distributed formation trajectory optimization framework that can balance collision avoidance and formation shape maintenance. In detail, we propose a distributed formation planning approach for UAVs, which can generate collision-free trajectories in environments with static obstacles. The proposed formation planning method mainly consists of swarm path searching and formation trajectory optimization. A sliding mode controller is designed to validate the dynamic feasibility of the trajectories. Simulations and experiments verify the effectiveness and adaptability of the proposed method.

The main contributions of this work are as follows:

• A path searching algorithm that prevents formation disintegration is proposed. A swarm heuristic cost is designed to be applied during the search, and it observably enhances the cohesion of the swarm paths. As a result, the difficulty of solving the optimization problem can be greatly reduced compared to searching without consideration of swarm cohesion.
• We propose a distributed formation trajectory optimization method that takes formation maintenance, obstacle avoidance, and kinematics into account. By solving the optimization problem, smooth rotation and translation of the UAV formation can be realized. The method enables the UAV system to balance between moving in the reference formation shape and avoiding obstacles.
• A series of simulations and a real-world experiment are conducted, validating the effectiveness of our proposed method.

The remaining of this paper is organized as follows. In Section 2, we describe the studied system and formulate the problem. Some basic knowledge used in this paper is also introduced. In Section 3, the proposed formation planning method is described in detail. Section 4 introduces the formation tracking control method. In Section 5, simulations under different circumstances is introduced. Section 6 shows the experimental results and analysis. Finally, Section 7 summarizes the paper and discusses future work.

2. System Description and Problem Formulation

2.1. System Description

In the formation planning task, the UAV formation system under consideration consists of $n(n\ge 3)$ identical UAVs.

In the body-fixed frame of the ith UAV, the attitude is denoted as ${\rho }_i={\left[{\varphi }_i{\theta }_i{\psi }_i\right]}^{\mathrm{T}}$. ${\varphi }_i$, ${\theta }_i$, and ${\psi }_i$ represent the roll, pitch, and yaw, respectively. The dynamics of the ith UAV is given as follows [19,32,33]:

$$\begin{array}{cc}\hfill \ddot{x_i}& ={\displaystyle \frac{(cos{\psi }_{i}sin{\theta }_{i}cos{\varphi }_i+sin{\psi }_{i}sin{\varphi }_i)F_i}{m}}\hfill \end{array}$$

(1a)

$$\begin{array}{cc}\hfill \ddot{y_i}& ={\displaystyle \frac{(sin{\psi }_{i}sin{\theta }_{i}cos{\varphi }_i-cos{\psi }_{i}sin{\varphi }_i)F_i}{m}}\hfill \end{array}$$

(1b)

$$\begin{array}{cc}\hfill \ddot{z_i}& ={\displaystyle \frac{cos{\varphi }_{i}cos{\theta }_i{F}_i}{m}}-g\hfill \end{array}$$

(1c)

$$\begin{array}{cc}\hfill \ddot{{\varphi }_i}& ={\displaystyle \frac{{\tau }_{xi}+\dot{{\theta }_i}\dot{{\psi }_i}\left(I_y-I_z\right)}{I_x}}\hfill \end{array}$$

(1d)

$$\begin{array}{cc}\hfill \ddot{{\theta }_i}& ={\displaystyle \frac{{\tau }_{yi}+\dot{{\varphi }_i}\dot{{\psi }_i}\left(I_z-I_x\right)}{I_y}}\hfill \end{array}$$

(1e)

$$\begin{array}{cc}\hfill \ddot{{\psi }_i}& ={\displaystyle \frac{{\tau }_{zi}+\dot{{\varphi }_i}\dot{{\theta }_i}\left(I_x-I_y\right)}{I_z}}\hfill \end{array}$$

(1f)

where m represents the mass of a UAV; $F_i$ represents the magnitude of thrust; $\tau ={\left[{\tau }_{xi}{\tau }_{yi}{\tau }_{zi}\right]}^{\mathrm{T}}$ is the control torque; $I_x,I_y,$ and $I_z$ are the rotational inertia; and g is the gravitational acceleration. Typically, (1a)–(1c) are the position dynamics, describing the position motion of a UAV; (1d)–(1f) are the attitude dynamics, describing the attitude motion of a UAV.

The obstacle avoidance task in this paper is constrained to the two-dimensional horizontal plane ($xOy$). There are several static obstacles and some unfeasible regions in the environment. The 2D position of the ith UAV in the $xOy$ plane is denoted as $q_i={\left[x_i{y}_i\right]}^{\mathrm{T}}$. The 2D linear velocity and acceleration of the ith UAV in the $xOy$ plane are denoted as $v_i={\left[v_x{v}_y\right]}^{\mathrm{T}}$ and $a_i={\left[a_x{a}_y\right]}^{\mathrm{T}}$, respectively. The acceleration $a_i$ is limited by $a_i\in \mathcal{A}$, where $\mathcal{A}=\left\{{\left[a_x{a}_y\right]}^{\mathrm{T}}\right|a_{min}<a_x<a_{max},a_{min}<a_y<a_{max}.\}$ To better describe the motion of a multirotor UAV, the second-order integrator model is utilized to describe the UAV’s motion in the $xOy$ plane, which is written as

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\hfill \dot{q_i}=v_i\\ \hfill \dot{v_i}=a_i\end{array}\right.\end{array}$$

(2)

2.2. Problem Formulation

Each UAV has its own ID number i; the corresponding starting point is $s_i$, and the target point is $g_i$. The planning task for the each UAV is represented as $〈s_i,g_i〉$; the objective of each UAV is to find a safe, smooth, and feasible trajectory that can take it from the starting point to the goal point. Meanwhile, the UAVs should maintain the reference formation shape.

The reference formation shape $F^r=[F_1^r,F_2^r,\dots F_n^r]$ is defined using relative positions between UAVs. UAV1 is set as the coordinate origin, and all other UAVs’ positions relative to UAV1 can be determined. Hence, it is possible to obtain a set of reference positions of UAVs in the formation, which can be written as

$$\begin{array}{c}\hfill F_i^r=\left\{\begin{array}{c}{[0,0]}^{\mathrm{T}},ifi=1\hfill \\ {[x_i^r,y_i^r]}^{\mathrm{T}},ifi\ne 1\hfill \end{array}\right.\end{array}$$

(3)

where ${[x_i^r,y_i^r]}^T$ is the relative position of UAVi ($i>1$) to UAV1. By taking the average of the vectors in $F^r$, the reference position of the formation center is expressed as $F_c^r=\left({\sum }_{i=1}^n{F}_i^r\right)/n$.

2.3. Graph Theory

In this paper, we utilize a directed graph $\mathcal{G}=\{\mathcal{V},\mathcal{E},\mathcal{A}\}$ to describe the communication topology among UAVs. $\mathcal{V}=\{1,2,\dots ,n\}$ is the set of nodes, which represents the n UAVs in the swarm. $\mathcal{E}\subseteq \mathcal{V}\times \mathcal{V}$ denotes the edges between any two nodes. Let $e_{ij}=(i,j)\in \mathcal{E}$ denote that there is a directed path from node i to node j. $\mathcal{A}:\mathcal{E}\to {\mathbf{R}}^{+}$ is a function allocating a weight to each edge; for example, for $e_{ij}=(i,j)\in \mathcal{E}$, there exists $\mathcal{A}\left(e_{ij}\right)=a_{ij}$. If $e_{ij}=(i,j)\notin \mathcal{E}$, then $a_{ij}=0$. The neighbor set of a UAV $i\in \mathcal{V}$ is denoted as $N_i=\{j\in \mathcal{V}|(i,j)\in \mathcal{E}\}$.

3. Formation Planning

The overall framework of the proposed formation planning method is shown in Figure 1. First, the swarm-A* algorithm is designed to search for collision-free discrete waypoints for the center of the formation and for the UAVs. Second, the waypoints are densified, and the numbers of waypoints are equalized. Safe corridors, a set of convex polygons covering the free motion space of the UAVs, are constructed using these processed waypoints. Finally, a nonlinear distributed trajectory optimization problem is presented and solved to obtain safe and smooth trajectories that maintain the reference formation shape. Detailed explanations of the proposed method are provided in the subsections below.

3.1. Path Searching

This subsection presents a path searching method called the swarm-A* algorithm, which aims to obtain discrete collision-free path points for UAVs in consideration of their cohesion. The search space is a two-dimensional grid map that contains free grid squared and occupied grid squares. We define eight available directions in each search iteration.

In terms of formation path searching, conventional algorithms such as A* are not applicable because if we use them to search paths for multiple UAVs sequentially, the UAVs are very likely to bypass obstacles on different sides, which is not conducive to maintaining formation and will lead to a large computational burden, sometimes even to the point of unsolvability, for the subsequent trajectory optimization. Additionally, some large obstacles may lead to communication blockage if UAVs are located on different sides. To overcome this problem, we designed the swarm-A* algorithm, whose principle is presented below.

The swarm-A* algorithm is shown as Algorithm 1. $n_p$ and $n_c$ represent the parent node and the child node, respectively, during the searching process. In Line 2, the algorithm searches for a reference path $p_c$ for the center of the formation using a hybrid A* algorithm [34]. $p_c$ contains both position and orientation information, and its kth waypoint is denoted as $q_c^k={\left[x_c^k{y}_c^k{\omega }_c^k\right]}^{\mathrm{T}}$, where ${\omega }_c^k$ represents the orientation. In Line 12, $n_r$, the closest path point to $n_p$, is found in $p_c$, acting as a reference point. In Line 17, the algorithm judges whether the path passing $n_p$ towards $n_c$ is shorter than the path passing its previous parent node. If so, the parent of $n_c$ is replaced with $n_p$. In Line 21, a node extension formula for swarm-A* is proposed, where $g\left(n_c\right)$ is the cumulative path length from the starting point to the current node $n_p$, $h_1\left(n_c\right)=\sqrt{{(n_c.x-g_i.x)}^2+{(n_c.y-g_i.y)}^2}$ is the heuristic cost between the current node and the goal point, and $h_2\left(n_c\right)=\sqrt{{(n_c.x-n_r.x)}^2+{(n_c.y-n_r.y)}^2}$ is the heuristic cost between the current node and the reference point. ${\alpha }_1$ and ${\alpha }_2$ are the weights of the two heuristic costs, respectively. $f\left(n_c\right)=g\left(n_c\right)+{\alpha }_1{h}_1\left(n_c\right)+{\alpha }_2{h}_2\left(n_c\right)$ is the total cost. A comparison of different values of swarm weight is shown in Figure 2. It can be observed that, as ${\alpha }_2$ increases, the cohesion of the searched paths also improves, i.e., the UAVs’ path become closer to the reference path. When ${\alpha }_2=1$, the paths bypass all obstacles on the same side. It proves that the proposed swarm-A* algorithm is capable of enhancing the cohesion of swarm movements.

Algorithm 1 Swarm-A*

Input:  UAV number n, starting and goal points $\{s_1,s_2,\dots ,s_n\}\{g_1,g_2,\dots ,g_n\}$ Output:  UAV paths $p_c,p_1,p_2,\dots ,p_n$   1: Compute the starting point and goal point of the formation center $s_c=\left({\Sigma }_{i=1}^n{s}_i\right)/n$, $g_c=\left({\Sigma }_{i=1}^n{g}_i\right)/n$   2: Use Hybrid A* algorithm to search for a reference path for the center of the formation, $p_c=HybridAstar(s_c,g_c)$   3: for $i=1\to n$ do   4:       $openlist\leftarrow s_i$;   5:       $closedlist\leftarrow ⌀$;   6:       while $openlist\ne ⌀$ do   7:             Choose $n_p$ with minimal total cost f in $openlist$;   8:             $closedlist\leftarrow n_p$;   9:             if $n_p=g_i$ then 10:                    break; 11:             end if 12:             Find the point $n_r$ in $p_c$ that is closest to $n_p$; 13:             for all $n_c\in neighbors\left(n_p\right)$ do 14:                    if $n_c\in closedlist$ or $n_c$ is unfeasible then 15:                          continue; 16:                    else if $n_c\in openlist$ then 17:                          $Judge(n_c,n_p)$; 18:                    else 19:                          $openlist\leftarrow n_c$; 20:                          $k\left(n_c\right)\leftarrow \left|\right|n_c-n_r{\left|\right|}_2$; 21:                          $f\left(n_c\right)\leftarrow g\left(n_c\right)+{\alpha }_1{h}_1\left(n_c\right)+{\alpha }_2{h}_2\left(n_c\right)$; 22:                    end if 23:              end for 24:       end while 25:       Obtain $p_i$; 26: end for 27: return $p_c,p_1,p_2,\dots p_n$

3.2. Waypoint Reallocation

Due to the diverse starting and goal points of the UAVs, the number of waypoints for each UAV may be different. Since the relative position between UAVs will be calculated at each time step during the trajectory optimization process, the length of the path for each UAV should be the same. Besides, since the reference path (formation center path) will be used to offer a formation rotation angle for UAVs at each time step (Section 4), the length of the reference path should be close to that of the UAVs. To achieve this requirement, the path points are densified by inserting h points equidistantly into the line segments between two adjacent points. An equalization algorithm (Algorithm 2) is proposed to calculate the number h of points that need to be inserted and then insert them into the previously searched paths. The process of Algorithm 2 is as follows.

Algorithm 2 Waypoint reallocation method

Input:  UAV paths 1, waypoint number $l_1,l_2,\dots ,l_n$, waypoint number of the formation center $l_c$, maximum of the inserted point number $i_{max}$ Output:  reallocated paths $p_c,p_1,p_2,\dots p_n$   1: $l_m$ = $max\{l_1,l_2,\dots ,l_n\}$   2: $Extend(p_1,p_2,\dots p_n)$   3: for $i=0\to i_{max}$ do   4:       if $l_m+(l_m-1)\ast i<l_c$ and $l_m+(l_m-1)\ast (i+1)>l_c$ then   5:             if $l_c-(l_m+(l_m-1)\ast i)<l_m+(l_m-1)\ast (i+1)-l_c$ then   6:                    $h=i$   7:             else $h=i+1$   8:             end if   9:       end if 10: end for 11: for $j=1\to h$ do 12:       $Insert(p_j,h)$ 13: end for 14: $l=l_m+h\ast (l_m-1)$ 15: if $l>l_c$ then 16:       $Extend(p_c,l)$ 17: else $Extend(\{p_1,p_2,\dots p_n\},l_c)$ 18: end if 19: return $p_c,p_1,p_2,\dots p_n$

In Line 1, the longest path among the UAVs is chosen, and its waypoint number is denoted as $l_m$. In Line 2, each UAV’s path (except for the longest one) is extended by copying and appending its last waypoint to the end (function $Extend\left(\right)$), thus making all UAVs’ numbers of waypoints equivalent. In Lines 3–10, h is determined by comparing the numbers of waypoints on the reference path and the inserted paths. In Lines 11–18, the UAVs’ paths are densified (function $Insert\left(\right)$), and the length of the reallocated path is denoted as l. After that, the lengths of the reference path and the reallocated path are equalized. $L=max\{l,l_c\}$ represents the final number of waypoints on all discrete paths.

To ensure the clarity and conciseness of the representation, the discrete path point set of UAV i obtained through path searching and waypoint reallocation is denoted as $p_i={\left\{q_i^k\right\}}_0^L$, where $q_i^k={\left[x_i^k{y}_i^k\right]}^{\mathrm{T}}$ represents the position of the kth path point.

3.3. Safe Corridor Generation

Safe corridors are several convex polygons covering the feasible space in the environment that ensures collision-free trajectories. The safe corridor set of the path $p_i$ is represented as $\mathcal{S}\left(p_i\right)=\left\{S_i^k\right|k=1,2,\dots L\}$, where $S_i^k$ represents the safe corridor of the kth waypoint. Note that the safe corridor needs to be sequentially connected, i.e., the safe corridors of two adjacent points must overlap, which is denoted as

$$S_i^k\cap S_i^{k+1}\ne ⌀,\forall k\in \{1,2,\dots L-1\}$$

(4)

In this paper, a rectangular safe corridor is generated around each waypoint by expanding a safe region centered at that point. $S_i^j$ is initialized as the starting point $s_i$. Expansion proceeds in the four cardinal directions $\{+x,-x,+y,-y\}$ until the distance between the corridor’s boundary and nearby obstacles is reduced to a specified safe distance in all directions. This expansion is repeated sequentially for each point in the path $p_i$ until the final corridor for $g_i$ is obtained. Inspired by [35], the connectivity problem is solved by inserting points into the line segments between two adjacent waypoints (the same process as in Section 3.2) of both the UAV paths and the reference path. The number of inserted points is chosen in consideration of the density of obstacles.

Figure 3 demonstrates a safe corridor generation result. The yellow dots represent the original waypoints, and the blue ones represent the inserted waypoints ($h=1$). The dots surrounded by red dashed lines represent the ‘expansion points’. The green rectangles represent the generated safe corridor for the path. During the generation process, if a waypoint $q_i^k$ does not lie within the boundaries of the previously generated safe corridor $S_i^{k-1}$ ($2<k<L$), i.e., $q_i^k\notin S_i^{k-1}$, then it is called an ‘expansion point’. Otherwise, if $q_i^k\in S_i^{k-1}$, then the corridor generation process for the expansion point is skipped, and we suppose that $S_i^k=S_i^{k-1}$. As is shown in Figure 3, after the first corridor $S_i^1$ is constructed, the next waypoint that is not within $S_i^1$ is $p_i^3$. Therefore, $p_i^3$ is defined as an expansion point, whose safe corridor, denoted as $S_i^3$, is then generated, etc. When the last waypoint $p_i^{19}$ receives its corresponding corridor $S_i^{19}$, the generation process is finished.

3.4. Trajectory Optimization

The discrete waypoints are refined into smooth formation trajectories in this subsection. The trajectory of the UAV i obtained through trajectory optimization is denoted as ${\left\{q_i^k,t_i^k\right\}}_0^L$, where $t_k=k\Delta t$ represents the time from the start to the kth trajectory point, $\Delta t$ is the unit time, and L represents the total number of points contained in the optimized trajectory.

First, the formation rotation matrix at each time step is calculated. By combining the reference formation shape with the rotation matrix, the formation cost is determined. Subsequently, a distributed trajectory optimization problem is formulated. Safe and smooth formation trajectories are obtained by solving the optimization problem.

3.4.1. Smoothness Cost

The smoothness cost involves two parts. The first part, $\Delta v_i^k$, describes the difference in linear speed between two adjacent waypoints, i.e., the acceleration of the UAV. The second part, $\Delta a_i^k$, describes the difference in acceleration between two adjacent trajectory segments, i.e., the jerk of the UAV. The smoothness cost is represented by

$$s_i^k={\beta }_1\Delta v_i^k+{\beta }_2\Delta a_i^k$$

(5)

$$\Delta v_i^k=v_i^{k+1}-v_i^k$$

(6)

$$\Delta a_i^k=a_i^{k+1}-a_i^k$$

(7)

where $s_i^k$ is the smoothness cost of UAVi at the kth trajectory point, while ${\beta }_1$ and ${\beta }_2$ are the weighting factors of the two parts of the smoothness cost.

3.4.2. Formation Cost

The formation cost is designed to realize the formation maintenance through the optimization process. First, the calculation of the formation rotation matrix is introduced as follows.

For a two-dimensional vector, we define its rotation matrix R as

$$R=\left[\begin{array}{cc}cos\left(\eta \right)& -sin\left(\eta \right)\\ sin\left(\eta \right)& cos\left(\eta \right)\end{array}\right]$$

(8)

where $\eta$ represents the rotation angle. The counterclockwise rotation of a two-dimensional vector by an angle $\eta$ relative to its original orientation can be achieved by left-multiplying it by the rotation matrix R.

The orientation of the UAV formation is denoted as $\omega$. Let $\omega =0$ when the formation is towards the positive direction of the x-axis in the xOy plane, and $\omega$ gradually increases as the formation rotates counterclockwise. The starting and goal orientation angles of the formation are defined as ${\omega }_s$ and ${\omega }_g$, respectively. To realize the rotation of the moving formation, the previously searched reference path (in Line 2 of Algorithm 1) is utilized to obtain the rotation angle of the formation at each time step, denoted as $p_c={\left\{q_c^k\right\}}_0^L$. The kth waypoint of the reference path is denoted as $q_c^k={\left[x_c^k{y}_c^k{\omega }_c^k\right]}^{\mathrm{T}}$, with ${\omega }_c^1={\omega }_s,{\omega }_c^L={\omega }_g$. Therefore, the rotation matrix of the reference formation at the kth trajectory point can be written as

$$R_k=\left[\begin{array}{cc}cos({\omega }_c^k-{\omega }_s)& -sin({\omega }_c^k-{\omega }_s)\\ sin({\omega }_c^k-{\omega }_s)& cos({\omega }_c^k-{\omega }_s)\end{array}\right]$$

(9)

where ${\omega }_c^k$ represents the orientation angle of the reference path at the kth point. A transformation and rotation process of a triangle reference formation with 3 UAVs is illustrated in Figure 4. The black circles and the red circles represent UAVs and the center of the formation, respectively. The red dashed line represents the reference path (the path taken by formation’s center). It shows that the orientation of the formation’s center is used to describe the rotation of the overall formation. As k increases from 1 to L, the formation’s orientation angle (shown as dark blue arrows) gradually reduces from ${\omega }_s$ to ${\omega }_g$.

After the rotation matrix is obtained, the calculation of the formation cost is explained as follows.

We construct the formation cost of the optimization problem using two values, ${\tilde{F}}_{ic}^k$ and ${\tilde{F}}_{ij}^k$, both representing the deviation between reference and actual relative position vectors:

${\tilde{F}}_{ic}^k$ is the deviation of the relative position vector between each UAV and the formation’s center at the kth trajectory point. Based on the rotation matrix and the reference formation shape, the reference value can be calculated by ${\widehat{F}}_{ic}^k=R_k·(F_i^r-F_c^r)$, where $F_c^r$ represents the reference position of the formation’s center. The actual value is $F_{ic}^k=q_i^k-q_c^k$, where $q_i^k$ and $q_c^k$ represent the k-th waypoint of the i-th UAV’s path and the reference path, respectively. Then,

$${\tilde{F}}_{ic}^k=R_k·(F_i^r-F_c^r)-(q_i^k-q_c^k)$$

(10)

${\tilde{F}}_{ij}^k$ is the deviation of relative position vectors between two UAVs at the kth trajectory point. Only one neighbor $j=i-1(i\ge 2)$ is taken into consideration for UAV i in order to reduce the computational burden. The reference value is ${\widehat{F}}_{ij}^k=R_k·(F_i^r-F_j^r)$, while the actual value is $F_{ij}^k=q_i^k-q_j^k$. Then,

$${\tilde{F}}_{ij}^k=R_k·(F_i^r-F_j^r)-(q_i^k-q_j^k)$$

(11)

In Figure 4, the gray arrows are examples of the two reference vectors mentioned above. ${\widehat{F}}_{21}^k$ represents the reference relative position between UAV1 and UAV2, while ${\widehat{F}}_{2c}^k$ represents the reference relative position between UAV2 and the formation’s center. The sum of the two terms above yields the formation error vector at the kth trajectory point (i.e., at time step $t=k\Delta t$) as follows:

$$m_i^k={\gamma }_1{\tilde{F}}_{ic}^k+{\gamma }_2{\tilde{F}}_{ij}^k$$

(12)

where $m_i^k$ is the formation cost of UAVi at the kth trajectory point, while ${\gamma }_1,{\gamma }_2$ are positive weighting constants. To obtain optimal formation shape maintenance, the objective of the optimization is to minimize the norm of $m_i^k$, i.e., $\parallel m_i^k\parallel \to 0,\forall k\in \{1,2,\dots ,L\}$.

3.4.3. Trajectory Optimization Problem

The proposed trajectory optimization problem is as follows:

$$\begin{array}{cc}\hfill \min & \sum _{k=1}^{L-1}{s}_i^{kT}P{s}_i^k+\sum _{k=1}^L{m}_i^{kT}Q{m}_i^k\hfill \end{array}$$

(13a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& q_i^0=s_i,q_i^L=g_i,\forall i\hfill \end{array}$$

(13b)

$$\begin{array}{cc}\hfill & q_i^{k+1}=\xi (q_i^k,v_i^k),\forall i,k\hfill \end{array}$$

(13c)

$$\begin{array}{cc}\hfill & v_i^{k+1}=\zeta (v_i^k,a_i^k),\forall i,k\hfill \end{array}$$

(13d)

$$\begin{array}{cc}\hfill & q_i^k\in {\mathcal{S}}_i^k,\forall i,k\hfill \end{array}$$

(13e)

$$\begin{array}{cc}\hfill & a_i^k\in \mathcal{A},\forall i,k\hfill \end{array}$$

(13f)

$$\begin{array}{cc}\hfill & ∥q_i^k-q_j^k∥\ge 2R_{safe},\forall i>1,j=\{1,2,...,i-1\},k;\hfill \end{array}$$

(13g)

where $P\in {\mathbf{R}}^{+}$ and $Q\in {\mathbf{R}}^{+}$ are weight constants. The number of the trajectory points is denoted as L. The cost function (13a) involves a smoothness cost ${\sum }_{k=1}^{L-1}{s}_i^{kT}P{s}_i^k$ and a formation cost ${\sum }_{k=1}^L{m}_i^{kT}Q{m}_i^k$, which are represented in quadratic form. The optimization variable is the acceleration of the UAVs. Equations (13b)–(13g) are the hard constraints of the optimization problem. The starting position and goal position of each UAV are limited by (13b), where $i\in \{1,2,\cdots n\}$. The kinematics of a UAV is demonstrated by (13c) and (13d), where $i\in \{1,2,\cdots n\},k\in \{1,2,\cdots L-1\}$. The specific forms of (13c) and (13d) are denoted as

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\hfill q_i^{k+1}=\xi (q_i^k,v_i^k)=q_i^k+v_i^k\Delta t\\ \hfill v_i^{k+1}=\zeta (v_i^k,a_i^k)=v_i^k+a_i^k\Delta t\end{array}\right.\end{array}$$

(14)

The safe corridor constraint is denoted as (13e), where $i\in \{1,2,\cdots n\},k\in \{1,2,\cdots L\}$. Equation (13f) determines the upper bound and lower bound of the control input. Equation (13g) represents the collision avoidance constraint among UAVs, where $i\in \{2,\cdots n\},j\in \{1,2,\cdots ,i-1\},k\in \{1,2,\cdots L\}$. $R_{safe}$ is the collision radius of the UAV. From UAV1 to UAVn, an optimization problem (13a) is constructed and solved sequentially. The prior optimized trajectories are utilized by the latter ones to calculate formation cost and to achieve reciprocal avoidance in the swarm. A directed graph is used to describe the communication topology between UAVs. Take a formation with 4 UAVs as an example (see Figure 5), UAV2 receives the trajectory of UAV1, and UAV3 can receive the trajectories of both UAV1 and UAV2, etc.

Note that the proposed optimization problem (13a) is a non-convex optimization with nonlinear constraints. This may preclude the finding of an optimal solution. However, if a slight deviation from the reference formation shape is acceptable, classic nonlinear optimization techniques can still be utilized to solve the problem and obtain a feasible solution.

4. Formation Tracking Control

This section describes a formation tracking control method to track the generated trajectories (Section 3), aiming to confirm that the planned trajectories in Section 3 satisfy the UAV dynamics and can be executed by UAVs. The framework of the control scheme is shown in Figure 6. Sliding mode control is utilized to design the controller. This method is inspired by a previous study [25].

A UAV can be classified as an under-actuated system because it has four control inputs but six state variables. The horizontal control is closely related to the roll and pitch control. The desired roll angle and the desired pitch angle need to be derived from the horizontal control component, which can be expressed by the following nonlinear equations:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{u}_{xi}=cos{\varphi }_{i}sin{\theta }_{i}cos{\psi }_i+sin{\psi }_{i}sin{\psi }_i\hfill \\ u_{yi}=cos{\varphi }_{i}sin{\theta }_{i}sin{\psi }_i+sin{\psi }_{i}cos{\psi }_i.\hfill \end{array}\right.\end{array}$$

(15)

Solving (15) derives the desired attitude angle, which can be written as

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\varphi }_i^d=arcsin\left(u_{xi}sin{\psi }_i-u_{yi}cos{\psi }_i\right)\hfill \\ {\theta }_i^d=arcsin\left({\displaystyle \frac{u_{xi}cos{\psi }_i-u_{yi}sin{\psi }_i}{cos{\psi }_i^d}}\right)\hfill \end{array}\right.\end{array}$$

(16)

The position control in this paper is designed as follows:

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}{u}_{xi}\hfill & ={\displaystyle \frac{m_i\left({\ddot{x}}_i^d-k_x{s}_{xi}+c_x{\dot{e}}_{xi}\right)}{F_i}}\hfill \\ u_{yi}\hfill & ={\displaystyle \frac{m_i\left({\ddot{y}}_i^d-k_y{s}_{yi}+c_y{\dot{e}}_{yi}\right)}{F_i}}\hfill \\ F_i\hfill & ={\displaystyle \frac{m_i\left({\ddot{z}}_i^d-k_z{s}_{zi}+c_z{\dot{e}}_{zi}-g\right)}{cos{\varphi }_{i}cos{\theta }_i}}\hfill \end{array}\right.\end{array}$$

(17)

where $e_{xi}=x_i^d-x_i$, $e_{yi}=y_i^d-y_i$, and $e_{zi}=z_i^d-z_i$ are position tracking errors; $s_{xi}=c_x{e}_{xi}+{\dot{e}}_{xi}$, $s_{yi}=c_y{e}_{yi}+{\dot{e}}_{yi}$, and $s_{zi}=c_z{e}_{zi}+{\dot{e}}_{zi}$ are designed sliding mode surfaces; and ${\mathbf{K}}_p=$ diag$(k_x,k_y,k_z)$ and ${\mathbf{C}}_p=$ diag$(c_x,c_y,c_z)$ are two positive definite gain matrices.

Theorem 1.

For the position dynamics of a UAV (1a)–(1c), if ${\mathbf{K}}_p$ and ${\mathbf{C}}_p$ are positive definite, the position tracking error coordinates $(e_{xi},e_{yi},e_{zi})$ are stable under position control (17).

Proof. 

See Appendix A. □

The procedure for attitude control in this paper is designed as follows:

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}{\tau }_{\varphi i}\hfill & =\left({\ddot{\varphi }}_i^d-k_{\varphi }{s}_{\varphi i}-c_{\varphi }{\dot{e}}_{\varphi i}\right)I_x-\left(I_y-I_z\right){\dot{\theta }}_i{\dot{\psi }}_i\hfill \\ {\tau }_{\theta i}\hfill & =\left({\ddot{\theta }}_i^d-k_{\theta }{s}_{\theta i}-c_{\theta }{\dot{e}}_{\theta i}\right)I_y-(I_z-I_x){\dot{\psi }}_i{\dot{\varphi }}_i\hfill \\ {\tau }_{\psi i}\hfill & =\left({\ddot{\psi }}_i^d-k_{\psi }{s}_{\psi i}-c_{\psi }{\dot{e}}_{\psi i}\right)I_z-\left(I_x-I_y\right){\dot{\varphi }}_i{\dot{\theta }}_i\hfill \end{array}\right.\end{array}$$

(18)

where $e_{\varphi i}={\psi }_i^d-{\psi }_i,e_{\theta i}={\theta }_i^d-{\theta }_i$, and $e_{\psi }i={\psi }_i^d-{\psi }_i$ are attitude tracking errors; $s_{\varphi i}=c_{\varphi }{e}_{\varphi i}+{\dot{e}}_{\varphi i},s_{\theta i}=c_{\theta }{e}_{\theta i}+{\dot{e}}_{\theta i}$, and $s_{\psi i}=c_{\psi }{e}_{\psi i}+{\dot{e}}_{\psi i}$ are designed sliding mode surfaces; and ${\mathbf{K}}_a=$ diag$(k_{\varphi },k_{\theta },k_{\psi })$ and ${\mathbf{C}}_a=$ diag$(c_{\varphi },c_{\theta },c_{\psi })$ are two positive definite gain matrices.

Theorem 2.

For the attitude dynamics of a UAV (1d)–(1f), if ${\mathbf{K}}_a$ and ${\mathbf{C}}_a$ are positive definite, the attitude control procedure (18) can force the attitude tracking errors ($e_{\varphi i}$, $e_{\theta i}$,$e_{\psi i}$) to converge to zero.

Proof. 

The proof of Theorem 2 is similar to that of Theorem 1. The detailed proof can be seen in [25]. □

5. Simulation

To verify the effectiveness of the proposed method, simulations were conducted in different environments. The simulations were implemented in MATLAB R2024b and Simulink on a laptop with an Intel i7-14650HX @2.20 GHz CPU and 32 GB of RAM. SQP (Sequential Quadratic Programming) was utilized to solve the trajectory optimization problems.

5.1. Simulation Setup

The size of the UAVs’ working space was 50 m × 50 m, which was described by a grid map with a size of 50 × 50. The radius of the UAVs’ collision range was $R_{safe}=0.4$ m, and the acceleration of each UAV was constrained by $a_{min}=-3$ m/s², $a_{max}=3$ m/s². The time interval between two adjacent trajectory points was set to $\Delta t=0.2$ s. The weight coefficients in the cost function were chosen as $P=1,Q=1,{\beta }_1={\beta }_2=1,{\gamma }_1=20,{\gamma }_2=1$. The weight coefficients in the path search were chosen as ${\alpha }_1=1,{\alpha }_2=2.5$. The parameters of the tracking controller were selected as ${\mathbf{C}}_p={\mathbf{C}}_a=diag(8,8,8)$, ${\mathbf{K}}_p=diag(0.1,0.8,0.8)$, and ${\mathbf{K}}_a=diag(10,10,10)$.

5.2. Simulation of Formation Planning

Figure 7 shows the trajectory optimization result of three UAVs maintaining a square formation in an environment with static obstacles. We captured the positions of the UAVs at certain trajectory points, including $k=1,{\displaystyle \frac{L}{6}},{\displaystyle \frac{L}{3}},{\displaystyle \frac{2L}{3}},{\displaystyle \frac{5L}{6}}$, and L. The starting and goal orientation angles of the formation were both ${\displaystyle \frac{\pi }{2}}$. The results showed that the formation could be maintained during the flight. The safety of the UAVs was guaranteed by sacrificing the quality of formation shape maintenance when they moved through the narrow areas between obstacles. After the UAVs passed through the obstacle-rich regions, they quickly regrouped in the reference formation shape. To validate the ability of UAVs to maintain formation with the proposed method, the formation errors are shown in Figure 8. The two subgraphs demonstrate the changes in $\left|\right|F_{ij}^k\left|\right|$ and ${\displaystyle \frac{F_{ij}^k}{\left|\right|F_{ij}^k\left|\right|}}$, which represent the position error and the angle error between UAVs, respectively. It can be seen that the formation errors nearly converged to zero except when the swarm traversed the narrow gap between obstacles and during the subsequent regrouping phase. Figure 9 and Figure 10 are the simulation results in a different environment. The starting and goal orientation angles of the formation were changed into ${\displaystyle \frac{\pi }{2}}$ and $\pi$, respectively. The statistical data of the formation errors were calculated and shown in

Table 2. Error analysis of the simulation results (${\omega }_g={\displaystyle \frac{\pi }{2}},{\omega }_s=\pi$).

	Mean of Position Error (m)	Standard Deviation of Position Error (m)	Mean of Angle Error (Degrees)	Standard Deviation of Angle Error (Degrees)
UAVs 1 and 2	0.0518	0.0798	1.7392	2.7281
UAVs 1 and 3	0.1007	0.1380	1.4751	2.3894
UAVs 1 and 4	0.1201	0.2062	1.6443	2.8327
UAVs 2 and 3	0.0685	0.0945	1.2044	2.1282
UAVs 2 and 4	0.1501	0.2802	1.5088	2.3690
UAVs 3 and 4	0.0788	0.1385	2.5375	4.6287

. It can be observed that the mean values of both position error and angle error were very small, indicating that the formation was well maintained. Additionally, the standard deviations of the errors were relatively low, suggesting that the formation remained stable over time with minimal fluctuations in individual agents’ deviations. This demonstrates the robustness of the planned trajectories in maintaining formation integrity. The above simulation results prove that the proposed method has the ability to balance between safety and formation maintenance. The adaptability of the proposed formation planning method to different environments was also verified through the simulation results.

5.3. Simulation of Formation Tracking Control

In this subsection, the controller designed in Section 4 is utilized to track the formation trajectories shown in Figure 9. As is shown in Figure 11a, the control results drawn in blue lines track the desired trajectories well. The tracking errors, illustrated in Figure 11b, also prove that the tracking performance is quite satisfactory. It also indicates that the trajectories generated by the proposed method satisfy the dynamic constraints of quadrotor UAVs.

6. Experiment

The UAVs utilized in the experiment are Crazyflies (https://www.bitcraze.io/products/crazyflie-2-1/, accessed on 10 April 2025). The Crazyflie is a light, versatile, open-source quadrotor platform. The position measurements were supported by the OptiTrack (https://www.optitrack.com/, accessed on 10 April 2025) motion capture system. The proposed distributed formation planning was realized and solved using MATLAB R2024b. The control commands were broadcast using the Crazyradio PA (https://www.bitcraze.io/products/crazyradio-pa/, accessed on 10 April 2025) data transmission module through a data relay laptop running Ubuntu 20.04.

Four Crazyflies were considered in a planar obstacle environment. The reference formation and shape was a square with a side length of 1 m. The task of the UAVs was to pass by two obstacles, which were located at ${[3.7,-1.8]}^{\mathrm{T}}$ and ${[0.75,0.7]}^{\mathrm{T}}$, respectively. To guarantee the safety of the flight, the cuboid obstacles were expanded in the xOy plane, i.e., the obstacles are modeled as cylinders with a radius of 1m. The starting and goal positions of the four UAVs were

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\hfill s_1={[1.0,-1.8]}^{\mathrm{T}}\\ \hfill s_2={[2.0,-1.8]}^{\mathrm{T}}\\ \hfill s_3={[2.0,-2.8]}^{\mathrm{T}}\\ \hfill s_4={[1.0,-2.8]}^{\mathrm{T}}\end{array}\right.\left\{\begin{array}{c}\hfill g_1={[2.25,2.2]}^{\mathrm{T}}\\ \hfill g_2={[3.25,2.2]}^{\mathrm{T}}\\ \hfill g_3={[3.25,1.2]}^{\mathrm{T}}\\ \hfill g_4={[2.25,1.2]}^{\mathrm{T}}\end{array}\right.\end{array}$$

(19)

The parameters in the experiment were selected as

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}\hfill a_{min}& =-2{\mathrm{m}/\mathrm{s}}^2\hfill \\ \hfill a_{max}& =2{\mathrm{m}/\mathrm{s}}^2\hfill \\ \hfill \Delta t& =0.25\mathrm{s}\hfill \\ \hfill R_{safe}& =0.3\mathrm{m}\hfill \end{array}\right.\left\{\begin{array}{cc}\hfill P& =1\hfill \\ \hfill Q& =1\hfill \\ \hfill {\beta }_1& ={\beta }_2=1\hfill \\ \hfill {\gamma }_1& ={\gamma }_2=1\hfill \end{array}\right.\left\{\begin{array}{cc}\hfill {\alpha }_1& =1\hfill \\ \hfill {\alpha }_2& =2.5\hfill \end{array}\right.\end{array}$$

(20)

Figure 12 shows snapshots at t = 0, 23, and 50 s during the real-world experiment. The Crazyflies are marked with flashing blue lights, and the colored cubes are the obstacles (modeled as cylinders). The red dashed lines represent the square formation of the UAVs during the flight, and the yellow arrows illustrate the reference path of the formation’s center. It can be seen that four UAVs maintaining a square formation were able to move along the reference path from the starting points (the upper right corner in the scene) to the goal points (the bottom left corner in the scene). Collisions with the two obstacles and collisions between UAVs were avoided. The reference formation shape was maintained well, and the formation scale was automatically transformed to pass through the narrow gap between obstacles. Figure 13 illustrates the UAV position data collected during the experiment. Note that the figure is displayed upside down from reality to better show the process of moving from the starting points to the goal points of the UAVs. The blue dots represent the four UAVs. The light blue lines and the red line represent the trajectories of the UAVs and the reference path, respectively. Obstacles are drawn in gray. It was found that the UAVs took off and formed an initial reference square formation at 0 s. As the UAVs drew closer to the obstacles during the flight, the formation shape was compressed because of the obstacle avoidance consciousness of each individual UAV (e.g., at 23 s). After that, the UAVs regrouped into the reference formation and finally land at 50 s.

7. Conclusions and Future Work

This paper proposed a distributed formation planning method for multiple UAVs in environments with static obstacles. The proposed method consists of swarm path searching and distributed trajectory optimization. We designed a path searching method named swarm-A* to find discrete, collision-free UAV paths that prevent formation disintegration when encountering an obstacle. The main result is a distributed trajectory optimization that is constructed and solved to transform the paths into smooth formation trajectories, with safe flight corridors being the safety constraints. A rotation matrix is applied to realize rotation of the whole formation, and the relative position vectors between the UAV and the reference points are utilized to build the cost function of the optimization. A tracking controller is designed to track the generated formation trajectories, confirming that the trajectories satisfy quadrotor dynamics. According to the simulation and real-world experiment results, the UAV swarm can travel in an obstacle-containing environment safely with flexible formations, realizing a balance between obstacle avoidance and formation shape maintenance. Smooth rotation and transformation of the UAV formation can be achieved. In summary, the main novel aspects of this paper are (1) the swarm-A* algorithm, which enhances the cohesion of the swarm and prevents disintegration of the formation during the path searching process, and (2) a distributed formation trajectory optimization framework that can balance collision avoidance and formation shape maintenance.

In future work, the proposed method can be enhanced by addressing the following. First, the method can only handle static obstacles. Real-time planning will be considered to handle moving obstacles, improving adaptability to different environments. Second, our current method is validated only in a planar setting. Extending it to full 3D space with altitude control will enable application to more complex aerial missions. Third, future work could explore adaptive strategies inspired by driving behavior research [36,37]. Such techniques could provide enhancement for UAV formations to handle multi-agent interactions, uncertainty, and individual differences better. Moreover, another promising research interest is to combine the proposed formation planning method with the theory of affine formation (in [19,20]) to achieve better performance in formation variation.