Cooperative Safe Trajectory Planning for Quadrotor Swarms

Abstract

In this paper, we propose a novel distributed algorithm based on model predictive control and alternating direction multiplier method (DMPC-ADMM) for cooperative trajectory planning of quadrotor swarms. First, a receding horizon trajectory planning optimization problem is constructed, in which the differential flatness property is used to deal with the nonlinear dynamics of quadrotors while we design a relaxed form of the discrete-time control barrier function (DCBF) constraint to balance feasibility and safety. Then, we decompose the original trajectory planning problem by ADMM and solve it in a fully distributed manner with peer-to-peer communication, which induces the quadrotors within the communication range to reach a consensus on their future trajectories to enhance safety. In addition, an event-triggered mechanism is designed to reduce the communication overhead. The simulation results verify that the trajectories generated by our method are real-time, safe, and smooth. A comprehensive comparison with the centralized strategy and several other distributed strategies in terms of real-time, safety, and feasibility verifies that our method is more suitable for the trajectory planning of large-scale quadrotor swarms.

1. Introduction

In recent years, with the rapid development of communication, computing, and automation technologies, intelligent UAV swarm systems inspired by the behavior of biological swarms have received extensive attention from researchers and practitioners [1]. An intelligent UAV swarm system is a holistic system composed of a group of UAVs capable of accomplishing complex tasks through cooperation and information sharing [2]. UAV swarm systems have advantages in terms of timeliness, economy, and functionality [3]. Consequently, they have been widely used in both civil and military fields [4], such as infrastructure inspection [5], logistics transportation [6], and search and rescue [7]. Among them, trajectory planning, as a crucial and challenging component for UAV swarms to perform tasks, needs to ensure the safety of a smooth trajectory from the initial position to the target position under the dynamics constraints, especially in a confined space. Furthermore, coordination among the UAVs is crucial.

There is increasing research that has emerged to address this safety-critical challenging problem, including potential fields [8], velocity obstacles [9,10], dynamic windows [11], and inter-robot prioritization [12]. However, these classical methods do not pay much attention to the interaction among robots, making them ineffective for more complex environments. Recently, learning-based methods [13,14] have been proposed to consider inter-robot interaction to solve trajectory planning. However, they usually require high-quality training data for generalization and lose the interpretability of the internal decision-making process. The above problems can be overcome by constructing an optimization problem using the MPC framework to handle complex objective functions and constraints explicitly, as well as providing sequences of future state and control input information predictively [15,16,17].

The major challenge of implementing MPC for the trajectory planning of quadrotor swarms lies in the difficulties of guaranteeing the real-time feasibility and safety of the system under various practical constraints. In this paper, we present a novel distributed trajectory planning algorithm for quadrotor swarms based on DMPC-ADMM, referring to Figure 1. Our contributions can be summarized as follows:

• A trajectory planning optimization problem for quadrotor swarms is constructed based on MPC, which uses the differential flatness property to handle the nonlinear dynamics of quadrotors. The dimension of the planning space is reduced compared to directly utilizing the nonlinear model. Additionally, we design a relaxed form of DCBF constraint to balance feasibility and safety. Due to the non-convexity of the DCBF, we linearize the DCBF at each time step and use an iterative convex optimization scheme to improve the solution’s efficiency.
• The high-dimensional optimization problem is decomposed to construct a fully distributed trajectory planning algorithm based on ADMM. In this distributed algorithm, the quadrotors within the communication range reach a consensus on future trajectories through cooperation, thus enhancing the safety of trajectories. To further improve communication efficiency, we design an event-triggered mechanism to reduce the communication overhead.
• The simulation results verify that our method can generate safe and smooth trajectories online under limited communication range, collision avoidance, and dynamic constraints. The comparison with the centralized strategy and several other distributed strategies confirms that our method is more suitable for large-scale quadrotor swarms.

The remainder of this paper is organized as follows. In Section 2, we mainly review the literature on the trajectory planning of UAVs. In Section 3, we introduce the differential flatness and DCBF. The quadrotor swarm trajectory planning problem based on MPC and DCBF is formulated in Section 4. In Section 5, we provide the distributed algorithm to solve the optimization problem via ADMM and design an event-triggered mechanism to reduce the communication overhead. We present the simulation experiments and results in Section 6, followed by concluding remarks in Section 7.

2. Related Work

Ensuring the safety of generated trajectories has consistently been a central issue in the field of multi-robotics, as well as a prerequisite for successful execution of tasks. As noted in [18], the existing literature often adopts Euclidean norms to model distance constraints, which are activated only when the reachable set intersects with the obstacles, resulting in the robot taking action only when it is close to them. To overcome this shortcoming, Ref. [18] combined CBF with MPC to avoid obstacles at an early stage and enhance the safety of the trajectory. The MPC-CBF formulation was also investigated on different platforms, including unmanned aerial vehicles [19,20] and autonomous vehicles [21].

The object of interest in this paper is a group of quadrotors whose underactuation and intrinsic instability make generating safe trajectories challenging. An effective solution involves leveraging the differential flatness property of quadrotors, as introduced in [22], to simplify the optimization problem while preserving the nonlinear dynamics of the quadrotors. This approach was successfully applied to UAV trajectory planning in crowded environments [23], as well as avoiding obstacles [24]. Specifically, Ref. [19] used CBF constraints to ensure trajectory safety for multi-quadrotor systems based on differential flatness. In this paper, we propose a distributed algorithm for the trajectory planning of quadrotor swarms with the MPC-CBF formulation.

UAV swarm trajectory planning is a complex optimization problem involving multiple constraints, variables, and nonlinear effects. Several existing studies on the trajectory planning of UAVs are summarized in Table 1. Centralized methods [24,25,26] have been proposed for the trajectory planning of UAV swarms. Kumar et al. [24] resolved the heterogeneous UAV formation reconfiguration and trajectory planning based on mixed integer quadratic programming (MIQP). Ref. [25] considered the collision-free trajectory generation for multiple UAVs in 3D space as a nonconvex optimization problem, which can be solved by sequential convex programming (SCP). These centralized approaches require the presence of a central node capable of acquiring the state information of the entire system and transmitting the planning results to each UAV. Nevertheless, one of its disadvantages is the large amount of computation, and once the center node fails, the whole system stops working, resulting in poor real-time and scalability. Therefore, the distributed methods have attracted much research attention in recent years. Compared to the centralized ones, each UAV in the distributed framework uses peer-to-peer communication to compute its trajectory without the need for a central node, referring to Figure 2. Hence, the distributed framework is scalable and robust in the face of unexpected node and link failures.

Distributed Model Predictive Control (DMPC) [27] has found successful applications in various networked systems ranging from electric power networks [28] to multi-robot systems [29,30,31]. It is an appealing option for distributed trajectory planning. Borrelli et al. [29] addressed the UAV formation problem based on a decentralized linear MPC approach that guaranteed collision avoidance under constraints. Luis et al. [31] developed a DMPC algorithm for online point-to-point flight trajectory generation in multi-UAV scenarios, which employed on-demand collision avoidance and event-triggered replanning. However, the second-order dynamics did not take into account the characteristics of quadrotors, and the safe distance constraint was approximated by a first-order Taylor expansion of the Euclidean distance. These limitations made it challenging to ensure trajectory safety in confined environments. To solve the quadrotor navigation problem, an online decentralized obstacle avoidance algorithm based on differential flatness and MPC was proposed in [30], where Optimal Reciprocal Collision Avoidance (ORCA) was employed for obstacle avoidance. However, the quadrotors lack cooperation to reach a consensus on their future trajectories, potentially leading to collisions in dense environments.

Table 1. Existing studies on the trajectory planning of UAVs.

References	UAV Model	Methods	Collision Avoidance	Evaluation
Kumar et al. [24]	Differential flatness	MIQP and downwash effect	Yes	Centralized methods: optimal global planning, poor real-time, high computational complexity, poor scalability
Augugliaro et al. [25]	Differential flatness	Discrete planning and continuous refinement	Yes
Preiss et al. [26]	Second-order dynamics	SCP and posteriori vehicle-specific feasibility check	Yes
Borrelli et al. [29]	Second-order dynamics	MILP and inter-vehicle coordination rules	Yes	Distributed methods: good real-time, strong robustness, suitable for large-scale swarms
Arul et al. [30]	Linear flat model	ORCA, downwash effect, and flatness-based feedforward linearization	Yes
Luis et al. [31]	Second-order dynamics	On-demand collision avoidance	Yes

Table 1. Existing studies on the trajectory planning of UAVs.

References	UAV Model	Methods	Collision Avoidance	Evaluation
Kumar et al. [24]	Differential flatness	MIQP and downwash effect	Yes	Centralized methods: optimal global planning, poor real-time, high computational complexity, poor scalability
Augugliaro et al. [25]	Differential flatness	Discrete planning and continuous refinement	Yes
Preiss et al. [26]	Second-order dynamics	SCP and posteriori vehicle-specific feasibility check	Yes
Borrelli et al. [29]	Second-order dynamics	MILP and inter-vehicle coordination rules	Yes	Distributed methods: good real-time, strong robustness, suitable for large-scale swarms
Arul et al. [30]	Linear flat model	ORCA, downwash effect, and flatness-based feedforward linearization	Yes
Luis et al. [31]	Second-order dynamics	On-demand collision avoidance	Yes

Distributed optimization theory and its applications have gained increasing attention in recent years. The alternating direction multiplier method (ADMM) [32], as one of them, has been proven to show significant advantages in terms of convergence speed and computational efficiency in multi-robot task scenarios [33]. A series of examples combining MPC and ADMM approaches for multiple robots [3,34,35,36] illustrate the effectiveness and scalability of such a distributed framework. For instance, Ref. [34] proposed a distributed MPC method utilizing ADMM decomposition to coordinate the control problem of waterborne AGVs. Chen et al. [35] employed MPC and ADMM to achieve formation navigation of multiple vessels under environmental perturbations. In [3], a flocking control framework based on MPC and ADMM was presented to enable multi-vehicle systems to track desired trajectories while considering limited communication distances, collision avoidance, and bounded speed and control inputs. Compared to the methods above, we address a more complex quadrotor scenario that takes into account the quadrotor’s dynamics. Moreover, we utilize the linearized DCBF instead of the Euclidean norms and incorporate MPC to ensure the real-time generation of safe trajectories.

3. Preliminaries

In this section, we present an overview of differential flatness and DCBF. We highlight the linear flat model of the quadrotor and a relaxed form of the DCBF constraint. In addition, the main symbols used in this paper are defined in Abbreviations.

3.1. Differential Flatness and Quadrotor Dynamics

A nonlinear system $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$ is considered differentially flat if it can be described using a set of differentially independent variables $\mathit{\zeta }\in {\mathbb{R}}^m$ called flat output, i.e., the state and input of the system can be expressed as algebraic functions of the flat output and its finite-order derivatives [37]. The definition is as follows:

$$\mathit{\zeta }=\Phi (\mathbf{x},\mathbf{u},\dot{\mathbf{u}},\dots ,{\mathbf{u}}^{\left(p\right)}),$$

(1)

$$\mathbf{x}={\Psi }_{\mathbf{x}}(\mathit{\zeta },\dot{\mathit{\zeta }},\dots ,{\mathit{\zeta }}^{(q-1)}),$$

(2)

$$\mathbf{u}={\Psi }_{\mathbf{u}}(\mathit{\zeta },\dot{\mathit{\zeta }},\dots ,{\mathit{\zeta }}^{\left(q\right)}),$$

(3)

where $\Phi$, ${\Psi }_{\mathbf{x}}$, and ${\Psi }_{\mathbf{u}}$ are smooth functions. Both ${\Psi }_{\mathbf{x}}$ and ${\Psi }_{\mathbf{u}}$ are also called endogenous transformations of the system. Here, p and q are the maximum orders of the derivatives of $\mathbf{u}$ and $\mathit{\zeta }$ required to describe the system. The quadrotor has been shown to have the differential flatness property [22] with a flat output $\mathit{\zeta }={[x,y,z,\psi ]}^T$.

Consider a quadrotor with its control input $\mathbf{u}=(T,{\varphi }_{cmd},{\theta }_{cmd},{\dot{\psi }}_{cmd})$, where T is the commanded thrust, ${\varphi }_{cmd}$ and ${\theta }_{cmd}$ are the commanded roll and pitch angles, and ${\dot{\psi }}_{cmd}$ is the commanded yaw rate. The state $\mathbf{x}=(x,y,z,\dot{x},\dot{y},\dot{z},\varphi ,\theta ,\psi )$ includes position, velocity, and roll, pitch, and yaw angles. As in [38], the inner-loop attitude dynamics are written as follows:

$$\begin{array}{c}\dot{\varphi }=\frac{1}{{\tau }_{\varphi }}(k_{\varphi }{\varphi }_{cmd}-\varphi ),\\ \dot{\theta }=\frac{1}{{\tau }_{\theta }}(k_{\theta }{\theta }_{cmd}-\theta ),\\ \dot{\psi }={\dot{\psi }}_{cmd},\end{array}$$

(4)

where $k_{\varphi },k_{\theta }$ and ${\tau }_{\varphi },{\tau }_{\theta }$ are the gains and time constants of the roll and pitch angles, respectively. Then the following equation gives the relationship between $T,\varphi ,\theta$ and flat output $\mathit{\zeta }$.

$$\begin{array}{cc}\hfill T& =m\frac{\ddot{z}+g}{cos\varphi cos\theta },\hfill \\ \hfill \varphi & =arctan\left(\frac{\ddot{x}sin\psi -\ddot{y}cos\psi }{\ddot{z}+g}cos\theta \right),\hfill \\ \hfill \theta & =arctan\left(\frac{\ddot{x}cos\psi +\ddot{y}sin\psi }{\ddot{z}+g}\right).\hfill \end{array}$$

(5)

From (4) and (5), we can then compute $\mathbf{u}$ in terms of flat output $\mathit{\zeta }$ and its derivatives. Hence, we simplify the quadrotor trajectory generation process by seeking a sufficiently smooth flat output trajectory within a reduced planning space dimension.

Hagenmeyer et al. [37] introduced the notion of exact feedforward linearization based on differential flatness. The differential flat system $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$ can be transformed into an equivalent linear discrete-time flat model that can provide the benefit of reducing computation overload. The linear flat model of the quadrotor [38] is shown as:

$$\begin{array}{cc}\hfill & {\mathbf{z}}^{k+1}=\mathbf{A}{\mathbf{z}}^k+\mathbf{B}{\mathbf{v}}^k,\hfill \\ \hfill & {\mathit{\zeta }}^k=\mathbf{C}{\mathbf{z}}^k,\hfill \end{array}$$

(6)

where

$$\begin{array}{cc}\hfill & \mathbf{A}=\left[\begin{array}{cccc}{\mathit{I}}_{3\times 3}& {\mathit{I}}_{3\times 3}{T}_s& {\mathit{I}}_{3\times 3}\frac{T_s^2}{2}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{I}}_{3\times 3}{T}_s& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& 1\end{array}\right],\mathbf{B}=\left[\begin{array}{cc}{\mathit{I}}_{3\times 3}\frac{T_s^3}{6}& {\mathbf{0}}_{3\times 1}\\ {\mathit{I}}_{3\times 3}\frac{T_s^2}{2}& {\mathbf{0}}_{3\times 1}\\ {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{1\times 3}& T_s\end{array}\right],\hfill \\ \hfill & \mathbf{C}=\left[\begin{array}{cccc}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& 1\end{array}\right],\hfill \end{array}$$

and $\mathbf{z}={[x,y,z,\dot{x},\dot{y},\dot{z},\ddot{x},\ddot{y},\ddot{z},\psi ]}^T$ denotes the flat state of the quadrotor, including position, velocity, acceleration, and yaw angle. $\mathbf{v}={[\stackrel{⃛}{x},\stackrel{⃛}{y},\stackrel{⃛}{z},\dot{\psi }]}^T$ denotes the flat input, including the third-order derivatives of the position and the yaw rate. In this paper, we consider (6) as the quadrotor dynamics model to generate a smooth collision-free trajectory.

3.2. Discrete-Time CBF

Consider a discrete-time dynamical system as

$${\mathbf{x}}^{k+1}=f({\mathbf{x}}^k,{\mathbf{u}}^k),$$

(7)

where ${\mathbf{x}}^k\in \mathcal{X}\subset {\mathbb{R}}^n$ denotes the state of the system at time k, ${\mathbf{u}}^k\in \mathcal{U}\subset {\mathbb{R}}^m$ is the control input, and f is a continuous dynamics function. Obstacle avoidance requires the invariance of a trajectory with respect to a safe, connected set. Specifically, if the system (7) is safe with respect to a set $\mathcal{C}$, then any trajectory starting inside the set $\mathcal{C}$ will remain inside it. The set $\mathcal{C}$ is defined to be a superlevel set of the continuously differentiable functions $h:\mathcal{X}\to \mathbb{R}$.

$$\mathcal{C}=\{\mathbf{x}\in \mathcal{X}\subset {\mathbb{R}}^n:h\left(\mathbf{x}\right)\ge 0\}.$$

(8)

Here, $\mathcal{C}$ is called a safety set. The function h is a discrete-time CBF (DCBF) [18] if the following condition (9) is satisfied.

$$\exists {\mathbf{u}}^k\mathrm{s}.\mathrm{t}.\Delta h({\mathbf{x}}^k,{\mathbf{u}}^k)\ge -\gamma h\left({\mathbf{x}}^k\right),0<\gamma \le 1,$$

(9)

where $\Delta h({\mathbf{x}}^k,{\mathbf{u}}^k):=h\left({\mathbf{x}}^{k+1}\right)-h\left({\mathbf{x}}^k\right)$, and $\gamma$ is a hyperparameter. The constraint (9) implies $h\left({\mathbf{x}}^{k+1}\right)\ge (1-\gamma )h\left({\mathbf{x}}^k\right)$, i.e., the lower bound of $h\left(\mathbf{x}\right)$ decays exponentially at time k with the rate $1-\gamma$. Incorporating a valid DCBF constraint (9) into an optimization problem ensures the safety of the generated trajectories. If $\gamma$ decreases, the system becomes more capable of avoiding obstacles, but it might lead to an unfeasible problem. On the other hand, if $\gamma$ increases, the problem is more likely to be feasible, but the trajectory rapidly approaches the boundary of the safe set $\partial \mathcal{C}$, making the system unsafe. Therefore, the fixed $\gamma$ is challenging to adapt to complex and changing environments; see [18] for details. To better balance feasibility and safety, we design a relaxed form of the DCBF constraint as follows:

$$h({\mathbf{x}}^{k+1})-(1-\gamma )h({\mathbf{x}}^k)+{\omega }^k\ge 0.$$

(10)

Here, the slack variable ${\omega }^k\in \mathbb{R}$ will be adaptively optimized along with other variables in the optimization problem. The collision-free trajectory needs to consider both the neighbors and the obstacles in the external environment, so two types of safety constraints are added. One is the $DCB{C}_{ij}$ constraint between quadrotor i and quadrotor j, and the other is the $DCB{C}_{io}$ constraint between quadrotor i and obstacle $o\in \mathcal{O}$. The formulation of $DCB{C}_{ij}$ and $DCB{C}_{io}$ are shown in the next section.

4. Problem Formulation

In this section, we present an undirected adjacency graph representing communication among quadrotors and then formulate a trajectory planning optimization problem based on MPC and DCBF.

4.1. Proximity Network

To realize cooperative trajectory planning, quadrotors need to communicate and exchange information with each other. In this paper, we use an undirected proximity graph ${\mathcal{G}}^k=(\mathcal{V},{\mathcal{E}}^k)$ to represent the communication topology of quadrotors at time k, where $\mathcal{V}:=\{1,2,\dots ,N\}$ is the set of vertices and ${\mathcal{E}}^k\in \mathcal{V}\times \mathcal{V}$ is the set of edges. In graph ${\mathcal{G}}^k$, vertex i stands for quadrotor i. The edge ${\mathcal{E}}_{ij}^k$ stands for a communication link between quadrotor i and quadrotor j at time k, as

$${\mathcal{E}}^k=\{(i,j)\mid d_{ij}^k<{\Delta }^{\mathrm{dect}},i,j\in \mathcal{V},i\ne j\},$$

(11)

where $d_{ij}^k=\parallel {\mathbf{p}}_i-{\mathbf{p}}_j\parallel$ and ${\Delta }^{\mathrm{dect}}>0$ is the maximum distance that quadrotors can communicate. At time k, the neighbors of quadrotor i are defined as ${\mathcal{N}}_i^k\triangleq \{j\mid (i,j)\in {\mathcal{E}}^k\}$. The graph ${\mathcal{G}}^k$ is time-varying due to the movement of the quadrotors. Therefore, ${\mathcal{N}}_i^k$ may be ∅, i.e., quadrotor i does not communicate with any other quadrotors at time k.

4.2. Trajectory Planning Based on MPC and DCBF

Cooperative trajectory planning for quadrotor swarms is a multi-variable and multi-constraint optimization problem. Combining MPC and DCBF, we aim to generate a safe and smooth trajectory under collision avoidance and dynamics constraints. Consider a swarm system with N quadrotors in a shared workspace $\mathcal{W}\in {\mathbb{R}}^3$. This finite-time optimization problem with constraints at time k within a prediction horizon H can be formulated as follows:

$$\begin{array}{cc}\hfill \underset{{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1}}{min}& \sum _{i=1}^N{J}_i^k({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k})+J_i^k\left({\mathit{\omega }}_i^{0:H-1}\right)\hfill \end{array}$$

(12a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& {\mathbf{z}}_i^{k+t+1\mid k}=A{\mathbf{z}}_i^{k+t\mid k}+B{\mathbf{v}}_i^{k+t\mid k},\hfill \end{array}$$

(12b)

$$\begin{array}{cc}\hfill & {\mathit{\zeta }}_i^{k+t\mid k}=C{\mathbf{z}}_i^{k+t\mid k},\hfill \end{array}$$

(12c)

$$\begin{array}{cc}\hfill & {\mathbf{z}}_i^{k+t\mid k}\in \Omega ,\hfill \end{array}$$

(12d)

$$\begin{array}{cc}\hfill & {\mathbf{z}}_i^{k\mid k}={\mathbf{z}}_i^k,\hfill \end{array}$$

(12e)

$$\begin{array}{cc}\hfill & DCB{C}_{ij}^{k+t\mid k}\ge 0,\forall j\in {\mathcal{N}}_i,\hfill \end{array}$$

(12f)

$$\begin{array}{cc}\hfill & DCB{C}_{io}^{k+t\mid k}\ge 0,\forall o\in \mathcal{O},\hfill \end{array}$$

(12g)

$$\begin{array}{cc}\hfill & \forall t\in \{0,1,\dots ,H-1\},\hfill \end{array}$$

(12h)

where N is the number of quadrotors, and $k+t\mid k$ denotes the prediction at time k for the state at time $k+t$. The cost function (12a) is composed of two parts along the prediction horizon H, i.e., the cost $J_i^k({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k})$ with respect to the variables ${\mathbf{z}}_i^{k:k+H\mid k}$ and ${\mathbf{v}}_i^{k:k+H-1\mid k}$, and the additional cost $J_i^k({\mathit{\omega }}_i^{0:H-1})$ with respect to the slack variables ${\mathit{\omega }}_i^{0:H-1}$. (12b) and (12c) are the linear flat model of the quadrotor described by (6), while (12d) and (12e) denote the system dynamics constraints and initial state conditions, respectively. To ensure the safety of the trajectory, (12f) and (12g) give two types of DCBF constraints defined by (10). The specific forms of the cost function and constraints in the optimization are described in detail below.

(1)

Cost $J_i^k({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k})$: The cost function consists of three parts, including the terminal cost $p({\mathbf{z}}_i^{k+H\mid k})$, the stage cost $q({\mathbf{z}}_i^{k+t\mid k},{\mathbf{v}}_i^{k+t\mid k})$, and the input change rate cost $r({\mathbf{v}}_i^{k+t\mid k},{\mathbf{v}}_i^{k+t-1\mid k})$, as shown below:

$$\begin{array}{c}\hfill J_i^k({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k})=p\left({\mathbf{z}}_i^{k+H\mid k}\right)+\sum _{t=0}^{H-1}q({\mathbf{z}}_i^{k+t\mid k},{\mathbf{v}}_i^{k+t\mid k})+\sum _{t=1}^{H-1}r({\mathbf{v}}_i^{k+t\mid k},{\mathbf{v}}_i^{k+t-1\mid k}),\end{array}$$

(13)

$$p\left({\mathbf{z}}_i^{k+H\mid k}\right)={\parallel {\mathbf{z}}_i^{k+H\mid k}-{\mathbf{z}}_i^d\parallel }_P^2,$$

(14)

$$q({\mathbf{z}}_i^{k+t\mid k},{\mathbf{v}}_i^{k+t\mid k})=\parallel {\mathbf{z}}_i^{k+t\mid k}-{\mathbf{z}}_i^d{\parallel }_Q^2+{\parallel {\mathbf{v}}_i^{k+t\mid k}\parallel }_R^2,$$

(15)

$$r({\mathbf{v}}_i^{k+t\mid k},{\mathbf{v}}_i^{k+t-1\mid k})={\parallel {\mathbf{v}}_i^{k+t\mid k}-{\mathbf{v}}_i^{k+t-1\mid k}\parallel }_S^2,$$

(16)

where P, Q, R, and S denote the weight matrices of the corresponding parts, respectively. ${\mathbf{z}}_i^d$ is the target state of quadrotor i. From Equations (14)–(16), it can be seen that this cost penalizes the deviation of the predicted state from the target state, the size of the input, and the size of the input variations along the prediction horizon H. Therefore, the objective of the optimization problem is to enable the quadrotors to rapidly approach the target state while minimizing input size and its variation.

(2)

Cost $J_i^k\left({\mathit{\omega }}_i^{0:H-1}\right)$: The additional cost function $J_i^k\left({\mathit{\omega }}_i^{0:H-1}\right)$ is to drive the slack variables ${\mathit{\omega }}_i^{0:H-1}=[{\omega }_{ij}^{0:H-1},{\omega }_{io}^{0:H-1}]$ close to 0 to ensure the safety of the generated trajectories as follows:

$$J_i^k\left({\mathit{\omega }}_i^{0:H-1}\right)=\sum _{t=0}^{H-1}\alpha {\left({\omega }_{ij}^t\right)}^2+\alpha {\left({\omega }_{io}^t\right)}^2,$$

(17)

where $\alpha$ is a weighting coefficient. It is advisable to set $\alpha$ to a large value to prevent excessive relaxation of the DCBF constraints, which is also verified in the simulation experiments. In addition, the cost function $J_i^k\left({\mathit{\omega }}_i^{0:H-1}\right)$ can be tuned for different performance.

(3)

DCBF Constraints: To generate collision-free trajectories, (12f) and (12g) provide safety constraints $DCB{C}_{ij}^{k+t\mid k}$ and $DCB{C}_{io}^{k+t\mid k}$ in the optimization problem to ensure the forward invariance of the corresponding safety set $\mathcal{C}$. The formulation is as follows:

$$\begin{array}{cc}\hfill DCB{C}_{ij}^{k+t\mid k}& \triangleq h_{ij}({\mathit{\zeta }}_i^{k+t+1\mid k},{\mathit{\zeta }}_j^{k+t+1\mid k})-(1-{\gamma }_{ij})h_{ij}({\mathit{\zeta }}_i^{k+t\mid k},{\mathit{\zeta }}_j^{k+t\mid k})+{\omega }_{ij}^t\ge 0,\forall j\in {\mathcal{N}}_i,\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill DCB{C}_{io}^{k+t\mid k}& \triangleq h_{io}({\mathit{\zeta }}_i^{k+t+1\mid k},{\mathbf{p}}_o)-(1-{\gamma }_{io})h_{io}({\mathit{\zeta }}_i^{k+t\mid k},{\mathbf{p}}_o)+{\omega }_{io}^t\ge 0,\forall o\in \mathcal{O}.\hfill \end{array}$$

(19)

We model each quadrotor i as a closed rigid sphere with radius $r_i$ and each obstacle o as a closed ellipsoid with semi-major axis $(a_o,b_o,c_o)$. Similar to [18], the corresponding h-functions are given by

$$h_{ij}({\mathit{\zeta }}_i^{k+t\mid k},{\mathit{\zeta }}_j^{k+t\mid k})=\parallel {\mathbf{p}}_i^{k+t\mid k}-{\mathbf{p}}_j^{k+t\mid k}\parallel -r_i-r_j\ge 0,$$

(20)

$$h_{io}({\mathit{\zeta }}_i^{k+t\mid k},{\mathbf{p}}_o)={\parallel {\mathbf{p}}_i^{k+t\mid k}-{\mathbf{p}}_o\parallel }_W-1\ge 0,$$

(21)

where ${\mathbf{p}}_i^{k+t\mid k}$ represents the position of quadrotor i and is a component of the output ${\mathit{\zeta }}_i^{k+t\mid k}$. Correspondingly, ${\mathbf{p}}_o$ is the position of obstacle o. (20) implies that the sphere representing quadrotor i does not intersect the sphere representing quadrotor j, see Figure 3 for details. Similarly, (21) can be interpreted as approximating the obstacle o as an enlarged ellipsoid to check whether the position of quadrotor i is inside it [16], and $W=diag(1/{(a_o+r_i)}^2,1/{(b_o+r_i)}^2,1/{(c_o+r_i)}^2)$. Since the DCBF is nonconvex, the real-time computation of problem (12) is challenging when it has a large prediction horizon H. Therefore, we linearize the DCBF around the result of the previous iteration at each time step using a first-order Taylor expansion as follows:

$$\begin{array}{cc}\hfill & h_{ij}({\mathit{\zeta }}_i^{k+t\mid k},{\mathit{\zeta }}_j^{k+t\mid k})={\mathit{\eta }}_{ij}^T({\mathbf{p}}_i^{k+t\mid k}-{\mathbf{p}}_j^{k+t\mid k})-r_i-r_j\ge 0,\hfill \\ \hfill & h_{io}({\mathit{\zeta }}_i^{k+t\mid k},{\mathbf{p}}_o)={\mathit{\eta }}_{io}^{T}W({\mathbf{p}}_i^{k+t\mid k}-{\mathbf{p}}_o)-1\ge 0,\hfill \\ \hfill & {\mathit{\eta }}_{ij}=\frac{{\widehat{\mathbf{p}}}_i^{k+t\mid k}-{\widehat{\mathbf{p}}}_j^{k+t\mid k}}{\parallel {\widehat{\mathbf{p}}}_i^{k+t\mid k}-{\widehat{\mathbf{p}}}_j^{k+t\mid k}\parallel },{\mathit{\eta }}_{io}=\frac{{\widehat{\mathbf{p}}}_i^{k+t\mid k}-{\mathbf{p}}_o}{\parallel {\widehat{\mathbf{p}}}_i^{k+t\mid k}-{\mathbf{p}}_o\parallel },\hfill \end{array}$$

(22)

where ${\widehat{\mathbf{p}}}_i^{k+t\mid k}$ is the result of the previous iteration for quadrotor i. Then, we can solve problem (12) with an iterative convex optimization. The detailed steps of the solution are described in the following section.

Remark 1.

In this paper, we consider that the yaw direction of the quadrotor is fixed as $\psi =0$ in the desired trajectory, and the focus is on the spatial position of the quadrotor.

The above is the optimization problem for trajectory planning. We will introduce a fully distributed trajectory planning algorithm for efficiently computing future trajectories for all quadrotors in a swarm, as explained in the following section.

5. DMPC-ADMM Based Trajectory Planning

In this section, we reformulate the trajectory planning (12) for quadrotor swarms based on ADMM, converting it into a fully distributed framework. This approach decomposes the original high-dimensional optimization problem into N low-dimensional optimization subproblems, allowing each quadrotor i to compute its optimal trajectory in parallel to speed up the online solution of the problem. The focus lies in determining the communication information and mode to coordinate the quadrotors in order to avoid collision among them.

5.1. General ADMM Formulation

Here, we present a brief overview of ADMM. For more details, the readers can refer to [32]. ADMM is an iterative algorithm for solving distributed optimization problems. It decomposes the original problem into several subproblems and solves them by updating the multipliers and alternating iterations. The standard ADMM considers the following optimization problem with equation constraints.

$$\underset{\mathbf{x},\mathbf{z}}{min}f\left(\mathbf{x}\right)+g\left(\mathbf{z}\right)\mathrm{s}.\mathrm{t}.A\mathbf{x}+B\mathbf{z}=c,$$

(23)

where $\mathbf{x}\in {\mathbb{R}}^{n1}$ and $\mathbf{z}\in {\mathbb{R}}^{n2}$ are the original decision variables. $f:{\mathbb{R}}^{n_1}\to \mathbb{R}$, $g:{\mathbb{R}}^{n_2}\to \mathbb{R}$, $\mathbf{A}\in {\mathbb{R}}^{n_3\times n_1}$, $\mathbf{B}\in {\mathbb{R}}^{n_3\times n_2}$, and $\mathbf{c}\in {\mathbb{R}}^{n_3}$. ADMM utilizes the augmented Lagrangian function with an additional quadratic penalty term to obtain better convergence. (24) is the augmented Lagrangian function of problem (23). The steps of ADMM iteratively solving (23) are described in Algorithm 1 until the predefined stopping iteration condition is satisfied.

$${\mathcal{L}}_{\rho }(\mathbf{x},\mathbf{z},\mathit{\lambda })=f\left(\mathbf{x}\right)+g\left(\mathbf{z}\right)+{\mathit{\lambda }}^T(A\mathbf{x}+B\mathbf{z}-\mathbf{c})+\frac{\rho }{2}{\parallel A\mathbf{x}+B\mathbf{z}-\mathbf{c}\parallel }_2^2,$$

(24)

where $\mathit{\lambda }\in {\mathbb{R}}^{n_3}$ is the Lagrange multiplier and $\rho >0$ is the penalty coefficient. Although $\rho$ is independent of the convergence of the optimization problem (23), it affects the convergence rate. And we can obtain a suitable $\rho$ by sufficient experiments. The next subsection describes how to reformulate the trajectory planning problem (12) as an adaptation of the structure of problem (23) to solve it in a distributed manner.

5.2. Problem Decomposition Based on ADMM

For trajectory planning of quadrotor swarms, the only coupling among quadrotors is represented by the collision avoidance constraints (18). The purpose of interactions among quadrotors is to tell the neighbors about their own future trajectories so that they can avoid collision in advance. In such scenarios, quadrotors need to agree on safe trajectories within the confined space. Motivated by [36], each quadrotor maintains communication with its neighbors while computing trajectories by introducing duplicates of its neighbors’ trajectories. Specifically, we use ${\mathbf{w}}_i$ to represent the duplicate of the trajectory ${\mathit{\zeta }}_i$, and ${\mathbf{w}}_{i\to j}$ is the duplicate of quadrotor i for the trajectory ${\mathit{\zeta }}_j$ of quadrotor j, which can also be interpreted as the desired trajectory proposed by quadrotor i for quadrotor j. Then the control barrier function $h_{ij}({\mathit{\zeta }}_i^{k+t\mid k},{\mathit{\zeta }}_j^{k+t\mid k})\ge 0$ can be reformulated as

$$\begin{array}{cc}\hfill & {\overline{h}}_{ij}({\mathbf{w}}_i^{k+t\mid k},{\mathbf{w}}_{i\to j}^{k+t\mid k})\ge 0,\hfill \\ \hfill & {\mathbf{w}}_i^{k+t\mid k}={\mathit{\zeta }}_i^{k+t\mid k},{\mathbf{w}}_{i\to j}^{k+t\mid k}={\mathit{\zeta }}_j^{k+t\mid k},\forall j\in {\mathcal{N}}_i.\hfill \end{array}$$

(25)

It can be seen that using ${\mathbf{w}}_i$ and ${\mathbf{w}}_{i\to j}$, the coupling among quadrotors is successfully decoupled. Here, we use $\mathcal{I}$ to denote an indicator function, defined as

$${\mathcal{I}}_A\left(x\right):=\left\{\begin{array}{cc}0\hfill & \mathrm{if}x\in A,\hfill \\ \infty \hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(26)

Compared to the problem (23), we replace the cost function of the optimization problem (12) with $f({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1})+g({\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k})$ as follows:

$$\begin{array}{cc}\hfill f({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1})& =\sum _{i=1}^N[J_i^k({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k})+J_i^k\left({\mathit{\omega }}_i^{0:H-1}\right)\hfill \\ \hfill & + {\mathcal{I}}_{\Phi }({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k})],\hfill \end{array}$$

(27)

$$g({\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k})=\sum _{i=1}^N\left[\sum _{j\in {\mathcal{N}}_i}{\mathcal{I}}_{DCB{C}_{ij}\ge 0}({\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k})\right],$$

(28)

where the set $\Phi$ denotes all the constraints in (12) except for inter-quadrotor collision avoidance. So, the optimization problem (12) can be transformed into the form as follows:

$$\begin{array}{cc}\hfill \underset{{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k},{\mathit{\omega }}_i^{0:H-1}}{min}& f({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1})+g({\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k})\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{w}}_i^{k+t\mid k}={\mathit{\zeta }}_i^{k+t\mid k},{\mathbf{w}}_{i\to j}^{k+t\mid k}={\mathit{\zeta }}_j^{k+t\mid k},\hfill \\ \hfill & \forall t\in \{0,1,\dots ,H-1\},\forall j\in {\mathcal{N}}_i.\hfill \end{array}$$

(29)

Obviously, (29) is an optimization problem with equation constraints, whose augmented Lagrangian form is given in (30), where ${\mathit{\lambda }}_i$ and ${\mathit{\lambda }}_{i\to j}$ are the corresponding dual variables and $\rho$ is the penalty coefficient. This allows us to solve this optimization problem according to Algorithm 1, iteratively updating ${\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1},{\mathbf{w}}_i^{k:k+H\mid k}$, ${\mathbf{w}}_{i\to j}^{k:k+H\mid k}$, ${\mathit{\lambda }}_i^{k:k+H\mid k}$, and ${\mathit{\lambda }}_{i\to j}^{k:k+H\mid k}$ sequentially until the stopping condition is satisfied or the maximum number of iterations is reached. Each quadrotor i computes its trajectory while maintaining communication with its neighbors to exchange future trajectories of the prediction horizon H.

$$\begin{array}{cc}\hfill {\mathcal{L}}_{\rho }=& f({\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1})+g({\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k})+\sum _{i=1}^N(M_i+\sum _{j\in {\mathcal{N}}_i}{M}_{i\to j})\hfill \\ \hfill =& \sum _{i=1}^N[{\mathcal{L}}_{\rho ,i}({\mathit{\zeta }}_i^{k+H\mid k},{\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1},{\mathbf{w}}_i^{k:k+H\mid k},{\mathit{\lambda }}_i^{k:k+H\mid k})\hfill \\ \hfill & +\sum _{j\in {\mathcal{N}}_i}{\mathcal{L}}_{\rho ,i\to j}({\mathit{\zeta }}_j^{k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k},{\mathit{\lambda }}_{i\to j}^{k:k+H\mid k})]\hfill \\ \hfill =& \sum _{i=1}^N[{\mathcal{L}}_{\rho ,i}({\mathit{\zeta }}_i^{k:k+H\mid k},{\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\mathit{\omega }}_i^{0:H-1},{\mathbf{w}}_i^{k:k+H\mid k},{\mathit{\lambda }}_i^{k:k+H\mid k})\hfill \\ \hfill & +\sum _{j\in {\mathcal{N}}_i}{\mathcal{L}}_{\rho ,j\to i}({\mathit{\zeta }}_i^{k:k+H\mid k},{\mathbf{w}}_{j\to i}^{k:k+H\mid k},{\mathit{\lambda }}_{j\to i}^{k:k+H\mid k})]\hfill \end{array}$$

(30)

Algorithm 1 ADMM.

$\begin{array}{cc}\mathbf{repeat}\hfill & 1:\mathbf{x}\leftarrow arg\underset{\mathbf{x}}{min}{\mathcal{L}}_{\rho }(\mathbf{x},\mathbf{z},\mathit{\lambda })\hfill \\ \hfill & 2:\mathbf{z}\leftarrow arg\underset{\mathbf{z}}{min}{\mathcal{L}}_{\rho }(\mathbf{x},\mathbf{z},\mathit{\lambda })\hfill \\ \hfill & 3:\mathit{\lambda }\leftarrow \mathit{\lambda }+\rho \frac{\partial }{\partial \mathit{\lambda }}{\mathcal{L}}_{\rho }(\mathbf{x},\mathbf{z},\mathit{\lambda })\hfill \end{array}$ $\begin{array}{c}\mathbf{until}\mathrm{satisfaction}\mathrm{of}\mathrm{a}\mathrm{stopping}\mathrm{criterion}\hfill \end{array}$

The last equation in (30) holds because a bi-directional interaction is assumed, i.e., $j\in {\mathcal{N}}_i\iff i\in {\mathcal{N}}_j$. So, we can flip the indexes i and j. $M_i$ and $M_{i\to j}$ in (30) are defined as follows:

$$\begin{array}{cc}\hfill & M_i=\sum _{t=0}^H{\mathit{\lambda }}_i^T({\mathit{\zeta }}_i^{k+t\mid k}-{\mathbf{w}}_i^{k+t\mid k})+\frac{\rho }{2}{\parallel {\mathit{\zeta }}_i^{k+t\mid k}-{\mathbf{w}}_i^{k+t\mid k}\parallel }_2^2,\hfill \\ \hfill & M_{i\to j}=\sum _{t=0}^H{\mathit{\lambda }}_{i\to j}^T({\mathit{\zeta }}_j^{k+t\mid k}-{\mathbf{w}}_{i\to j}^{k+t\mid k})+\frac{\rho }{2}{\parallel {\mathit{\zeta }}_j^{k+t\mid k}-{\mathbf{w}}_{i\to j}^{k+t\mid k}\parallel }_2^2\hfill \end{array}$$

(31)

According to Algorithm 1, we separate the Lagrangian function (30) into two subproblems. We first minimize ${\mathcal{L}}_{\rho }$ over the local variables ${\mathbf{v}}_i^{k:k+H-1\mid k}$ and ${\mathit{\omega }}_i^{0:H-1}$, and then minimize it over the global variables ${\mathbf{w}}_i^{k:k+H\mid k}$ and ${\{{\mathbf{w}}_{i\to j}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i}$. Algorithm 2 describes the steps of the distributed trajectory planning algorithm based on DMPC-ADMM, containing the steps of prediction, coordination, and mediation, as well as two communications, all of which are executed iteratively on the quadrotors in parallel. The prediction step ensures that each quadrotor i computes a trajectory ${\mathit{\zeta }}_i^{k:k+H\mid k}$ of prediction horizon H under the constraints represented by $\Phi$. The trajectory should be close to the target position while not deviating too much from its collision-free trajectory ${\mathbf{w}}_i^{k:k+H\mid k}$ and the desired trajectories ${\{{\mathbf{w}}_{j\to i}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i}$ that its neighbors propose. Each quadrotor i then exchanges the updated trajectory ${\mathit{\zeta }}_i^{k:k+H\mid k}$ with its neighbors in the first round of communication. In the coordination step, each quadrotor i updates $({\mathbf{w}}_i^{k:k+H\mid k},{\{{\mathbf{w}}_{i\to j}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i})$. This is done by coordinating with its neighbors to avoid collision while staying as close as possible to the trajectory ${\mathit{\zeta }}_i^{k:k+H\mid k}$ computed in the prediction step. In the mediation step, the Lagrange multipliers $({\mathit{\lambda }}_i^{k:k+H\mid k},{\{{\mathit{\lambda }}_{i\to j}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i})$ that accumulate the deviation between the trajectories computed in the prediction step and the coordination step are updated, further facilitating the quadrotors to reach a consensus on the trajectories of prediction horizon H. Finally, the desired Lagrange multipliers ${\{{\mathit{\lambda }}_{i\to j}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i}$ and trajectories ${\{{\mathbf{w}}_{i\to j}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i}$ are exchanged with its neighbors in the second round of communication. The iteration is finished when the stopping iteration condition (35) is satisfied or the maximum number of iterations $l_{max}$ is reached. The first control input ${\mathbf{v}}_i^k$ is selected from ${\mathbf{v}}_i^{k:k+H\mid k}$ to guide the controller at time $k+1$. Then, this algorithm repeats in this manner until all the quadrotors have reached the target positions.

Algorithm 2 Distributed algorithm based on DMPC-ADMM.

1: $\forall i\in \{1,\dots ,N\}, \mathbf{Initialize}{\mathbf{z}}_i^0,k=0$. 2: while target positions not reached do 3:     for $\forall i\in \{1,\dots ,N\}$ in parallel do 4:        Initialize ${\mathit{\lambda }}_i^{k:k+H\mid k},{\mathbf{w}}_i^{k:k+H\mid k},{\{{\mathit{\lambda }}_{i\to j}^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i}$. 5:        Set $l=0$ and update ${\mathcal{N}}_i$. 6:        while $l<l_{max}$ or stopping criterion (35) is not satisfied do 7:            1. prediction: update ${\mathit{\zeta }}_i^{k:k+H\mid k}$ with $$\begin{array}{cc}\hfill \underset{{\mathbf{v}}_i^{k:k+H\mid k},{\omega }_i^{0:H-1}}{argmin}& {\mathcal{L}}_{\rho ,i}({\mathit{\zeta }}_i^{k:k+H\mid k},{\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\omega }_i^{0:H-1},{\mathbf{w}}_i^{k:k+H\mid k},{\mathit{\lambda }}_i^{k:k+H\mid k})\hfill \\ & +\sum _{j\in {\mathcal{N}}_i}{\mathcal{L}}_{\rho ,j\to i}({\mathit{\zeta }}_i^{k:k+H\mid k},{\mathbf{w}}_{j\to i}^{k+t\mid k},{\mathit{\lambda }}_{j\to i}^{k:k+H\mid k})\hfill \end{array}$$ (32) 8:            2. communication 1: 9:               send ${\mathit{\zeta }}_i^{k:k+H\mid k}$ to $j\in {\mathcal{N}}_i$; 10:             receive $\left\{{\mathit{\zeta }}_j^{k:k+H\mid k}\right\}$ from $j\in {\mathcal{N}}_i$. 11:          3. coordination: 12:             update $({\mathbf{w}}_i^{k:k+H\mid k},{\left\{{\mathbf{w}}_{i\to j}^{k:k+H\mid k}\right\}}_{j\in {\mathcal{N}}_i})$ with $$\begin{array}{cc}\hfill \underset{{\mathbf{w}}_i^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k}}{argmin}& {\mathcal{L}}_{\rho ,i}({\mathit{\zeta }}_i^{k:k+H\mid k},{\mathbf{z}}_i^{k:k+H\mid k},{\mathbf{v}}_i^{k:k+H-1\mid k},{\omega }_i^{0:H-1},{\mathbf{w}}_i^{k:k+H\mid k},{\mathit{\lambda }}_i^{k:k+H\mid k})\hfill \\ & +\sum _{j\in {\mathcal{N}}_i}{\mathcal{L}}_{\rho ,i\to j}({\mathit{\zeta }}_j^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k},{\mathit{\lambda }}_{i\to j}^{k:k+H\mid k})\hfill \end{array}$$ (33) 13:          4. mediation: 14:             update $({\mathit{\lambda }}_i^{k:k+H\mid k},{\left\{{\mathit{\lambda }}_{i\to j}^{k:k+H\mid k}\right\}}_{j\in {\mathcal{N}}_i})$ with $$\begin{array}{c}{\mathit{\lambda }}_i^{k:k+H\mid k}\leftarrow {\mathit{\lambda }}_i^{k:k+H\mid k}+\rho ({\mathit{\zeta }}_i^{k:k+H\mid k}-{\mathbf{w}}_i^{k:k+H\mid k}),\hfill \\ {\mathit{\lambda }}_{i\to j}^{k:k+H\mid k}\leftarrow {\mathit{\lambda }}_{i\to j}^{k:k+H\mid k}+\rho ({\mathit{\zeta }}_j^{k:k+H\mid k}-{\mathbf{w}}_{i\to j}^{k:k+H\mid k}),\forall j\in {\mathcal{N}}_i.\hfill \end{array}$$ 15:          5. communication 2: 16:             send $({\mathit{\lambda }}_{i\to j}^{k:k+H\mid k},{\mathbf{w}}_{i\to j}^{k:k+H\mid k})$ to $j\in {\mathcal{N}}_i$; 17:             receive $\{{\mathit{\lambda }}_{j\to i}^{k:k+H\mid k},{\mathbf{w}}_{j\to i}^{k:k+H\mid k}\}$ from $j\in {\mathcal{N}}_i$. 18:      end while 19:      Select the first control input ${\mathbf{v}}_i^k$ from ${\mathbf{v}}_i^{k:k+H-1\mid k}$. 20:      Update state ${\mathbf{z}}^{k+1}=A{\mathbf{z}}^k+B{\mathbf{v}}^k,{\mathit{\zeta }}_i^{k+1}=C{\mathbf{z}}_i^{k+1}$. 21:   end for 22:   $k\leftarrow k+1\mathrm{and}l\leftarrow l+1$. 23: end while

In (22), we give a specific formulation of the linearized DCBF such that (29) is a convex problem with constraints. Here, we solve (32) and (33) in an iterative manner to approximate the optimal solution; see Algorithms 3 and 4. In Algorithm 3, the updated output ${\overline{\mathit{\zeta }}}_{i,d}^{k:k+H\mid k}$ is passed between iterations allowing for the linearization of $DCB{C}_{io}$. The iteration is finished when the convergence criterion (34) is satisfied or the maximum number of iterations $d_{max}$ is reached. The updated optimal ${\mathit{\zeta }}_i^{k:k+H\mid k}$ is then exchanged with its neighbors in the first round of communication. Algorithm 4 follows the same principle as Algorithm 3, so it is not explained further here.

$$∥{\mathit{\zeta }}_{i,d}^{*,k:k+H\mid k}-{\overline{\mathit{\zeta }}}_{i,d}^{k:k+H\mid k}∥\le {ϵ}_{abs},$$

(34)

where ${ϵ}_{abs}>0$ is constant, (34) implies that the iteration of Algorithm 3 is finished when the absolute value of the change in output ${\overline{\mathit{\zeta }}}_{i,d}^{k:k+H\mid k}$ is less than ${ϵ}_{abs}$.

Algorithm 3 Iterative convex optimization of (32).

1: Set initial guess ${\overline{\mathit{\zeta }}}_{i,0}^{k:k+H\mid k}$. 2: Initialize$d=0$. 3: while$d<d_{max}$ or convergence criteria (34) is not satisfied do 4:      Linearize safety constraints $DCB{C}_{io}$ (12g) with ${\overline{\mathit{\zeta }}}_{i,d}^{k:k+H\mid k}$. 5:      Solve a convex optimization problem with constraints and obtain the optimal value of state ${\mathit{\zeta }}_{i,d}^{*,k:k+H\mid k}$. 6:     Update ${\overline{\mathit{\zeta }}}_{i,d+1}^{k:k+H\mid k}={\mathit{\zeta }}_{i,d}^{*,k:k+H\mid k}$. 7:     $d\leftarrow d+1.$ 8: end while 9: Update ${\mathit{\zeta }}_i^{k:k+H\mid k}$ = ${\mathit{\zeta }}_{i,d}^{*,k:k+H\mid k}$.

Algorithm 4 Iterative convex optimization of (33).

1: Set initial guess ${\overline{\mathbf{w}}}_{i,0}^{k:k+H\mid k}$, ${\overline{\mathbf{w}}}_{i\to j,0}^{k:k+H\mid k}$. 2: Initialize$d=0$. 3: while$d<d_{max}$ or convergence criteria is not satisfied do 4:      Linearize safety constraints $DCB{C}_{ij}$ (12f) with ${\overline{\mathbf{w}}}_{i,d}^{k:k+H\mid k}$, ${\overline{\mathbf{w}}}_{i\to j,d}^{k:k+H\mid k}$. 5:      Solve a convex optimization problem with constraints and obtain the optimal value of state ${\mathbf{w}}_{i,d}^{*,k:k+H\mid k}$, ${\mathbf{w}}_{i\to j,d}^{*,k:k+H\mid k}$. 6:     Update ${\overline{\mathbf{w}}}_{i,d+1}^{k:k+H\mid k}={\mathbf{w}}_{i,d}^{*,k:k+H\mid k}$, ${\overline{\mathbf{w}}}_{i\to j,d+1}^{k:k+H\mid k}={\mathbf{w}}_{i\to j,d}^{*,k:k+H\mid k}$. 7:     $d\leftarrow d+1.$ 8: end while 9: Update ${\mathbf{w}}_i^{k:k+H\mid k}={\mathbf{w}}_{i,d}^{*,k:k+H\mid k}$, ${\mathbf{w}}_{i\to j}^{k:k+H\mid k}={\mathbf{w}}_{i\to j,d}^{*,k:k+H\mid k}$.

Remark 2.

For the initialization of Algorithm 2, we use the optimal result of the previous loop of DMPC-ADMM. Likewise, we use the optimal result of the previous iteration as the initial guess in Algorithms 3 and 4. This hot-start strategy can speed up the convergence of the algorithm, especially in slowly changing scenarios.

Each quadrotor i plans its trajectory ${\mathit{\zeta }}_i^{k:k+H\mid k}$ by taking into account its optimal trajectory ${\mathbf{w}}_i^{k:k+H\mid k}$ and the desired trajectories ${\{{\mathbf{w}}_{j\to i}^{k:k+H\mid k}\}}_{j\in {\mathcal{N}}_i}$ that are proposed by its neighbors. Thus, the stopping iteration condition for each MPC-ADMM loop can be designed in the following form:

$$\begin{array}{cc}\hfill & ∥{\mathit{\zeta }}_i^{k:k+H\mid k}-{\mathbf{w}}_i^{k:k+H\mid k}∥\le {ϵ}_1,\hfill \\ \hfill & ∥{\mathit{\zeta }}_i^{k:k+H\mid k}-\frac{1}{nu{m}_i^{nbrs}}\sum _{j\in N_i}{\mathbf{w}}_{j\to i}^{k:k+H\mid k}∥\le {ϵ}_2,\hfill \end{array}$$

(35)

where ${ϵ}_1$ and ${ϵ}_2$ represent tolerable deviation thresholds, respectively, and $nu{m}_i^{nbrs}$ is the number of neighbors of quadrotor i. With sufficient iterations, the quadrotor i will agree with its neighbors on the trajectories of prediction horizon H. However, extensive simulations show that the algorithm reaches an accepted result after a certain number of iterations. To reduce the computational burden, we present an empirical value of $l_{max}$ that allows us to compute a sub-optimal result.

5.3. Event-Triggered Mechanism

In general, the maximum communicable distance ${\Delta }^{dect}$ is much greater than the distance that quadrotors can fly within the prediction horizon H. Consequently, Algorithm 2 exhibits substantial redundancy in inter-quadrotor communication, especially in scenarios involving many quadrotors. Inspired by [39,40], we design an event-triggered mechanism that initiates communication only when a specific trigger condition is met. The critical element of the event-triggered mechanism lies in the design of the event detector. The content and operational mode of the event detector determines the functioning of the event-triggered mechanism, subsequently influencing the communication frequency of the system, as shown in Figure 4.

Considering a scenario in which two quadrotors are widely separated, there is no risk of collision within the prediction horizon H, so they need not maintain communication while generating trajectories. Within the distributed framework, at time k, quadrotor i listens to the position of its neighbor $j\in {\mathcal{N}}_i$ to determine whether the event is triggered. We design the trigger function of the event detector as follows:

$$E({\mathbf{p}}_i^k,{\mathbf{p}}_j^k)=\parallel {\mathbf{p}}_i^k-{\mathbf{p}}_j^k\parallel -2H{T}_s{v}_{max}-r_i-r_j,j\in {\mathcal{N}}_i,$$

(36)

where ${\mathbf{p}}_i^k$, ${\mathbf{p}}_j^k$ are the positions of quadrotor i and its neighbor $j\in {\mathcal{N}}_i$ at time k, $T_s$ is the time step, and $v_{max}$ is the maximum velocity of the quadrotor. The trigger condition holds when the value of the trigger function (36) is negative. We then incorporate this event-triggered mechanism into Algorithm 2, which reduces communication resources and the number of collision avoidance constraints $DCB{C}_{ij}\ge 0$.

Remark 3.

The optimization problem may become infeasible when the flight space of the quadrotors is highly competitive. In this case, we can slow down the quadrotors quickly, and after a few time steps, the problem becomes feasible again.

6. Simulation Experiments and Results

In this section, we describe and evaluate the implementation of our method in simulation experiments. The simulation results validate the efficiency of our method. And we use the RflySim platform [41] to validate the proposed method. The platform provides quadrotor dynamics that are almost indistinguishable from actual scenario flights. The trajectories of all quadrotors are generated by our method, and the PID controller is used to track the trajectories. A video demonstration on the RflySim platform can be found at https://www.bilibili.com/video/BV11u4y1w7HU (accessed on 15 January 2024).

6.1. Experimental Setup

All the quadrotors have the same dynamics model, and we apply a box constraint set $\Omega$ on the flat state as is done in [42] for trajectory generation, as shown in (37). The parameters used in the simulation are shown in

Table 2. Parameters setting.

Parameters	Values	Parameters	Values
H	15	P	$50·{\mathcal{I}}_{10}$
$T_s$	0.08 s	Q	$50·{\mathcal{I}}_{10}$
${\gamma }_{io},{\gamma }_{ij}$	$0.6$	R	$1·{\mathcal{I}}_4$
r	0.2 m	S	$1·{\mathcal{I}}_4$
${\Delta }^{\mathrm{dect}}$	20 m	${ϵ}_{abs},{ϵ}_1,{ϵ}_2$	0.01
$\rho$	1	$d_{max}$	50
$\alpha$	1 × 10${}^8$	$l_{max}$	20

. Here, we define two scenarios, one for obstacle avoidance flight in complex environments (Scenario 1) and one for exchanging positions flight (Scenario 2). We provide a performance comparison of our method with centralized MPC (CMPC), constant velocity MPC (CVMPC, treat the quadrotor as a constant velocity model), and distributed MPC (DMPC, use the future trajectories computed by its neighbors at the previous time as collision avoidance constraints) [16] for trajectory planning. The only difference among these methods is the coordination strategies, where all parameters are identical. All methods are solved using OSQP [43] with the modeling language Yalmip [44]. We use a Windows desktop with Intel Core i7-11700K (CPU 3.6 GHz) running MATLAB for all computations. In the simulation, we assume that the environmental information is known, the communication packets are not lost, and there are no external perturbations.

$$\Omega =\{\mathbf{z}\in {\mathbb{R}}^{10}\mid -3\le \dot{x},\dot{y},\dot{z}\le 3;-1\le \ddot{x},\ddot{y},\ddot{z}\le 1;\psi \in [-\pi ,\pi ]\}.$$

(37)

6.2. Performance Comparison of Different Methods

We compare the performance of our method with CMPC, CVMPC, and DMPC in two scenarios. For Scenario 1 with ten quadrotors, Figure 5, Figure 6 and Figure 7 show the simulation results using our method and CMPC, respectively. Figure 8 and Figure 9 show the simulation results of the eight quadrotors in Scenario 2 using the four methods. From the velocity variations of three of the quadrotors, we observe that our method generates smoother trajectories compared to CVMPC and DMPC, and the statistics of the distance among quadrotors show that our method is safer. Moreover, the performance of the trajectories generated by our method is not much different from that of CMPC.

For further comparison, we report the trajectory length (minimum, maximum, mean value, and standard deviation to compare cooperativeness), the minimum distance $d_{ij,min}$ among quadrotors, the minimum distance $d_{io,min}$ between quadrotors and obstacles, as well as the average computation time of the four methods in

Table 3. Comparison of the simulation results of the four methods for ten quadrotors cross a finite space with obstacles. The safe distance among quadrotors is $d_{ij,safe}=0.4\mathrm{m}$ and the safe distance between quadrotors and obstacles is $d_{io,safe}=0.2\mathrm{m}$.

Coordination Strategy	Trajectory Length (m)				${\mathit{d}}_{\mathit{ij},\mathit{min}}$ (m)	${\mathit{d}}_{\mathit{io},\mathit{min}}$ (m)	Av. comp_time (ms)
	min	max	av.	std.
CMPC	44.09	44.41	44.28	0.12	1.04	0.22	546.6
CVMPC	44.11	44.87	44.44	0.22	1.22	0.15	57.6
DMPC	44.18	44.67	44.42	0.19	1.12	0.17	47.1
MPC-ADMM (ours)	44.09	44.55	44.30	0.14	1.13	0.21	7.7

and

Table 4. Comparison of the simulation results of the four methods for eight quadrotors exchanging positions flight. The safe distance among quadrotors is $d_{ij,safe}=0.4\mathrm{m}$.

Coordination Strategy	Trajectory Length (m)				${\mathit{d}}_{\mathit{ij},\mathit{min}}$ (m)	Av. comp_time (ms)
	min	max	av.	std.
CMPC	12.14	12.41	12.22	0.09	0.45	293.7
CVMPC	12.17	12.68	12.45	0.20	0.15	73.7
DMPC	12.08	13.07	12.45	0.30	0.28	74.8
MPC-ADMM (ours)	12.06	12.62	12.30	0.21	0.48	16.1

. It can be observed that CVMPC and DMPC have a minimum distance lower than the safe distance due to the lack of coordination and take more time than our method due to the need for a greater number of iterations. Instead, our method and CMPC can achieve safe navigation, and the computation time of our method is much less than that of CMPC.

In addition,

Table 5. Comparison of the four methods in terms of average computation time (Av), collision probability (Cp), and feasibility (Fea) in Scenario 2.

Coordination Strategy	2 Quadrotors			4 Quadrotors			8 Quadrotors			16 Quadrotors
	Av (ms)	Cp (%)	Fea	Av (ms)	Cp (%)	Fea	Av (ms)	Cp (%)	Fea	Av (ms)	Cp (%)	Fea
CMPC	12.2	0	Yes	61.9	0	Yes	293.7	0	Yes	1301.9	3	Yes
CVMPC	24.1	0	Yes	47.5	13	Yes	73.7	56	No	104.1	85	No
DMPC	23.6	0	Yes	40.2	0	Yes	74.8	21	Yes	101.3	59	No
MPC-ADMM (ours)	3.5	0	Yes	6.0	0	Yes	16.1	2	Yes	44.4	8	Yes

shows the comparison of the four methods in terms of average computation time, collision probability, and feasibility in Scenario 2. For each method, we generate trajectories of the quadrotors under 50 random initial and target states. We define the method as infeasible when the collision probability is greater than $50\%$. We observe that CMPC suffers from a large computational burden, while our method performs well at a much lower computational cost. The collision probability of our method and CMPC stays below $10\%$ as the number of quadrotors increases. Instead, due to the deviation of trajectory information, CVMPC and DMPC become infeasible with a significant increase in collision probability, especially for CVMPC. Therefore, our method scales well with the number of quadrotors. Figure 10 illustrates the trajectories of two, four, and sixteen quadrotors for exchanging positions in flight.

6.3. Evaluation of Event-Triggered Mechanism

To verify the effectiveness of the event-triggered mechanism, we compare the performance of the algorithm before and after adding the event-triggered mechanism under a uniform spatial distribution of different numbers of quadrotors in Scenario 1. Define a metric $COMM$ as the communication cost and the communication ratio as the ratio of the communication cost after adding the event-triggered mechanism to the original communication cost, as shown in (38). Figure 11 shows a significant reduction in communication ratio and computation time after adding the event-triggered mechanism. Therefore, the event-triggered mechanism greatly improves the performance of the distributed algorithm.

$$\begin{array}{cc}\hfill & COMM=\sum _{i=1}^N\sum _{j=1}^{N}com{m}_{ij},\hfill \\ \hfill & com{m}_{ij}=\left\{\begin{array}{cc}1,\hfill & i\mathrm{and}j\mathrm{communicate},\hfill \\ 0,\hfill & \mathrm{otherwise}.\hfill \end{array}\right.\hfill \end{array}$$

(38)

6.4. Evaluation of Algorithm Robustness

We compare the sensitivity of our algorithm for different hyperparameters $\gamma$, maximum velocity $v_{max}$, and maximum acceleration $a_{max}$ of the quadrotors in

Table 6. Performance comparison of our algorithm for eight quadrotors exchanging positions flight at different hyperparameter ${\gamma }_{ij}$, maximum velocity $v_{max}$ (m/s) and maximum acceleration $a_{max}(\mathrm{m}/{\mathrm{s}}^2)$. The safe distance among quadrotors is $d_{ij,safe}=0.4\mathrm{m}$.

Parameters	Trajectory Length (m)				${\mathit{d}}_{\mathit{ij},\mathit{min}}$ (m)	Av. comp_time (ms)
	min	max	av.	std.
${\gamma }_{ij}=0.4,v_{max}=3,a_{max}=1$	12.07	12.58	12.30	0.20	0.62	16.4
${\gamma }_{ij}=0.6,v_{max}=3,a_{max}=1$	12.06	12.62	12.30	0.21	0.48	16.1
${\gamma }_{ij}=0.8,v_{max}=3,a_{max}=1$	12.05	12.43	12.24	0.14	0.47	14.4
${\gamma }_{ij}=1.0,v_{max}=3,a_{max}=1$	12.03	12.49	12.18	0.15	0.41	10.2
${\gamma }_{ij}=0.6,v_{max}=5,a_{max}=1$	12.07	12.83	12.32	0.25	0.56	15.4
${\gamma }_{ij}=0.6,v_{max}=7,a_{max}=1$	12.09	12.89	12.33	0.27	0.51	16.7
${\gamma }_{ij}=0.6,v_{max}=5,a_{max}=2$	12.12	13.12	12.57	0.35	0.53	16.2
${\gamma }_{ij}=0.6,v_{max}=5,a_{max}=4$	12.67	13.27	12.95	0.25	0.60	12.7

. It can be seen that the length of the trajectories does not vary much, and the minimum distance $d_{ij,mim}$ among quadrotors is always greater than the safe distance. Please note that as $\gamma$ increases, $d_{ij,mim}$ gets closer to the safe distance, and the computation time gets shorter, which is consistent with the explanation in Section 3. In addition, the computation time of our algorithm is kept short. Therefore, our method has good robustness and can be applied to different scenarios.

7. Conclusions

In this paper, we propose a fully distributed algorithm for cooperative trajectory planning of quadrotor swarms based on DMPC-ADMM, which employs differential flatness property to handle the complex dynamics of the quadrotor. Additionally, we design a relaxed form of DCBF constraint to balance feasibility and safety. Due to the non-convexity of the DCBF, we linearize the DCBF at each time step and use an iterative convex optimization scheme to solve it. Simulation results show that our method can generate safe and smooth trajectories while satisfying dynamics constraints. Compared with the centralized strategy and several other distributed strategies in terms of computation time, safety, and feasibility, our method is more suitable for the trajectory planning of large-scale quadrotor swarms. Furthermore, the effect of the designed event-triggered mechanism for reducing the communication overhead is also verified.

In future work, we will improve the event-triggered mechanism to enhance inter-quadrotor communication efficiency. It is also worth exploring how to improve the robustness of the algorithm considering the presence of uncertainties in practice, such as perceptual uncertainty, communication packet loss, and external perturbations. Additionally, considering that the trajectory planning framework presented in this paper is currently implemented synchronously, limiting its flexibility, we will develop an asynchronous implementation.