Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments

I Introduction

Autonomous aerial swarms can be employed for many systematic and cooperative tasks, such as search and rescue[1], collaborative mapping[2], and package delivery[3]. In some scenarios, it could be desired that the swarm moves according to a specified formation. For example, in [4], robots are required to form and maintain a virtual fence for animal herding tasks.

Extensive research works exist for aerial swarm navigation in formation, but none of them achieves robust formation flight in obstacle-dense environments. In practice, robots are repulsed to deviate from obstacles for safety, while formation imposes tracking targets that may oppose the obstacle avoidance. How to systematically trade off these two conflicting requirements is the key point to accomplish non-colliding formation flights. In the literature[5], consensus-based local control laws are widely used for formation maneuvering. However, the local control scheme is inherently incapable of planning over a prediction horizon, which severely undermines its practicality in complex environments. A group of optimization-based works maintain the formation by enforcing relative position constraints on each agent. Nevertheless, with this strategy, the formation would passively yield to the obstacles in dense scenarios since the surroundings could always defy the positional constraints. To summarize, a swarm trajectory planning method that can effectively manage both formation and obstacle avoidance in dense environments is lacking in the literature.

To bridge the gap, we propose a swarm trajectory optimization method capable of navigating swarms in formation while avoiding obstacles. We model the formation using undirected graphs and define a differentiable Laplacian-based metric that assesses the difference between formation shapes in three-dimensional workspaces. Rather than independently inspecting each agent’s tracking error, our metric quantitatively evaluates the overall performance of formation maintenance, and provides greater flexibility for formation maneuvering by virtue of its invariance to translation, rotation, and scaling. To formulate the trade-offs between formation and obstacle avoidance, we design an unconstrained optimization framework that simultaneously optimizes the trajectories over feasibility cost, collision penalty, and formation similarity error. Benchmark comparisons are carried out with other state-of-the-art methods. Finally, to verify that our method is efficient and practical, extensive experiments are conducted on a real distributed aerial swarm system integrated with the proposed method.

Fig. 1: A swarm of seven quadrotors in a regular hexagon formation is traversing an unknown obstacle-rich area. (a) : A snapshot of the formation flight. (b) : The visualization of the local map and executed trajectories.

We summarize our contributions as:

A differentiable graph-theory-based cost function that quantifies the similarity distance between three-dimensional formations.
A distributed trajectory optimizer that jointly takes formation similarity, obstacle avoidance, and dynamic feasibility into account.
A series of simulations and real-world experiments with a distributed aerial swarm system that validates the efficiency and robustness of our method. The source code will be released for the reference of the community.

Ii Related works

Ii-a Distributed Swarm Trajectory Planning

Extensive works exist for trajectory planning of distributed swarms. The concept of VO (velocity obstacle) is leveraged and generalized by Van Den Berg et al.[6, 7, 8] to accomplish reciprocal collision avoidance for multiple robots. However, the smoothness of the resulting trajectories cannot be guaranteed by VO-based approaches, which greatly impairs the usability on real robot systems.

In order to produce high-quality collision-free trajectories, optimization-based methods are widely introduced in the literature on distributed multicopter swarms[9, 10, 11]. Zhou et al.[12] incorporate Voronoi cell tessellation into a receding horizon QP scheme to prevent collision among the robots while planning. In [13], Chen et al. employ SCP to address the multi-agent planning problem in non-convex space by incrementally tightening the collision constraints. Baca et al.[14] combine MPC with a conflict resolution strategy to ensure mutual collision avoidance for outdoor swarm operations. Nevertheless, the computational load of the above optimation-based methods is large, which could hamper the applicability of the planners in highly dense scenarios.

Recently, Zhou et al.[15] present a distributed autonomous quadrotor swarm system using spatial-temporal trajectory optimization, which generates collision-free motions in dense environments merely in milliseconds. Our swarm trajectory generation scheme is based on this work.

Ii-B Multi-robot Navigation in Formation

Various techniques have been proposed to achieve multi-robot navigation in formation, which include virtual structures[16], navigation functions[17], reactive behaviors[18], and consensus-based local control laws[19]. However, most of the existing methods only consider obstacle-free cases.

A group of works handle the formation navigation in constrained scenarios by designing feedback laws. Han et al.[20] present a formation controller based on complex-valued graph Laplacians. The formation scale is regulated by a leader to perform intended swarm maneuvering, like passing a corridor. In[21], Zhao proposes a leader-follower control law with which the formation can be affinely transformed in responding to the environmental changes. Most control approaches rely on the leader-follower scheme, where the formation parameters are only accessible to the leader.

Compared to the leader-follower scheme in[20, 21], fully decentralized strategies possess better scalability and resiliency to partial failures. In[22], Alonso-Mora et al. control a formation of drones to avoid collision by optimally rearranging the desired formation and then planning local trajectories. But since no inter-vehicle coordination exists in the distributed planners, formation maintenance is not conducted in the local planning phase. Zhou et al.[23]

combine virtual structure with potential fields to produce non-colliding trajectories for formation flight. Nevertheless, planning with multiple interacting vector fields is prone to deadlocks. Besides, trajectory optimality is neglected by their method. Parys et al.

[24] use DMPC to tackle the formation preservation by imposing relative position constraints on the swarm. In their framework, coordination among the agents passively breaks once the positional constraints are violated by the obstacles. In contrast, we formulate the overall formation requirement with a differentiable metric, which enables our optimizer to trade off formation and obstacle avoidance collectively and simultaneously.

Iii A Differentiable Formation similarity metric

A formation of $N$ robots is modeled by an undirected graph $G = (V, E)$ , where $V := {1, 2, . . ., N}$ is the set of vertices, and $E \subset V \times V$ is the set of edges. In graph $G$ , the vertex $i$ represents the $i^{t h}$ robot with position vector $p_{i} = [x_{i}, y_{i}, z_{i}] \in R^{3}$ . An edge $e_{i j} \in E$ that connects vertex $i \in V$ and vertex $j \in V$ means the robot $i$ and $j$ can measure the geometric distance between each other. In our work, each robot communicates with all other robots, thus the formation graph $G$ is complete. Each edge of the graph $G$ is associated with a non-negative number as a weight. In this work, the weight of edge $e_{i j}$ is given by

w_{i j} =∥ p_{i} - p_{j} ∥^{2}, (i, j) \in E,

(1)

where $∥ \cdot ∥$ denotes the Euclidean norm. Now the adjacency matrix $A \in R^{N \times N}$ and degree matrix $D \in R^{N \times N}$ of the formation $G$ is determined. Thus, the corresponding Laplacian matrix is given by

L = D - A .

(2)

With the above matrices, the symmetric normalized laplacian matrix of graph $G$ is defined as

^L = D^{- 1 / 2} L D^{- 1 / 2} = I - D^{- 1 / 2} A D^{- 1 / 2},

(3)

where $I \in R^{N \times N}$

is the identity matrix.

As a graph representation matrix, Laplacian contains information about the graph structure[25]. To achieve the desired swarm formation, we propose a formation similarity distance metric as

f=∥^L−^Ldes∥2F=tr{(^L−^Ldes)T(^L−^Ldes)},

(4)

where $t r {\cdot}$ denotes the trace of a matrix, $^L$ is the symmetric normalized Laplacian of the current swarm formation, ${^L}_{d e s}$ is the counterpart of the desired formation. Frobenius norm $∥ \cdot ∥_{F}$ is used in our distance metric. $f$ is natively invariant to translation and rotation of the formation, since the corresponding graph is weighted by the absolute distance between robot positions. Scaling invariance is achieved by normalizing graph Laplacian with the degree matrix in (3).

Fig. 2: Illustration of the formation similarity metric and its gradients. A square formation is desired by the swarm. The surface shows the profile of similarity metric when one UAV moves in the plane and the other three remain still. The minimum suggests the best position for the UAV to form the desired shape. Left side shows the gradients of the similarity metric.

Our metric is analytically differentiable with respect to the position of each robot. For robot $i$ , we use the weights of its $n$ adjacent edges ${e_{i 1}, e_{i 2}, . . .,, e_{i n}}$ to form a weight vector $w_{i} = [w_{i 1}, w_{i 2}, . . .,, w_{i n}]^{T}$

. By chain rule, the gradient of

f

p_{i}

can be written as

\frac{\partial f}{\partial p_{i}} = \frac{\partial w_{i}}{\partial p_{i}} \frac{\partial f}{\partial w_{i}} .

(5)

According to our metric (4), the gradient of $f$ with respect to each weight $w_{i j}$ can be computed as follow

(6)

where

\frac{\partial f}{\partial^L} = \frac{\partial | |^L - {^L}_{d e s} | |_{F}^{2}}{\partial^L} = 2 (^L - {^L}_{d e s}),

(7)

\frac{\partial^L}{\partial w_{i j}} = - \frac{\partial (D^{- 1 / 2} A D^{- 1 / 2})}{\partial w_{i j}} .

(8)

Then the gradient $\partial f / \partial w_{i}$ can be written as

\partial f / \partial w_{i} = [\partial f / \partial w_{i 1}, \partial f / \partial w_{i 2}, . . ., \partial f / \partial w_{i n}]^{T} .

(9)

As for $\partial w_{i} / \partial p_{i}$ , the Jacobian can be easily derived since the weight function (1) is differentiable. Fig.2 shows a profile of the metric and the gradient for a square formation.

Iv Spatial-temporal trajectory optimization for formation flight

Iv-a Trajectory Representation

In this work, we adopt the MINCO representation [26], a minimum control effort polynomial trajectory class to conduct spatial-temporal deformation of the flat-output trajectory

	$Ξ_{M I N C O} =$	${p(t):[0,TΣ]↦Rm\|c=\textslC(q,T),$		(10)
		$q \in R^{m (M - 1)}, T \in R_{> 0}^{M}},$		(10)

where $c = (c_{1}^{T}, \dots, c_{M}^{T})^{T}$ is the polynomial coefficient, $q = (q_{1}, \dots, q_{M - 1})$ the intermediate points, $T = (T_{1}, \dots, T_{M})^{T}$ the time vector, $\textslC(q,T)$ the parameter mapping constructed from Theorem 2 in [26], and $T_{Σ} = \sum_{i = 1}^{M} T_{i}$ the total time.

A $m$ -dimensional $M$ -piece trajectory $p (t)$ is defined as

p (t) = p_{i} (t - t_{i - 1}), \forall t \in [t_{i - 1}, t_{i}),

(11)

and the $i^{t h}$ piece trajectory is represented by a $N = 5$ degree polynomial

p_{i} (t) = c_{i}^{T} β (t), \forall t \in [0, T_{i}],

(12)

where $c_{i} \in R^{6 \times m}$ is the coefficient matrix, $β (t) = [1, t, \dots, t^{N}]^{T}$ is the natural basis, and $T_{i} = t_{i} - t_{i - 1}$ is the time allocation for the $i^{t h}$ piece.

MINCO is uniquely determined by $(q, T)$ . And the parameter mapping $c=\textslC(q,T)$ converts trajectory representations $(c, T)$ to $(q, T)$ with linear time and space complexity, which allows any second-order continuous cost function $J (c, T)$ to be represented by $˜ J (q, T)$ . Hence, $\partial ˜ J / \partial q$ and $\partial ˜ J / \partial T$ can be efficiently obtained from $\partial J / \partial c$ and $\partial J / \partial T$ , respectively.

Especially, in order to handle the time integral constraints $ψ (p (t), \dots, p^{(3) (t)}) ⪯ 0$ , such as collision avoidance and dynamical feasibility, we transform them into finite-dimensional constraints $ψ ({^p}_{i, j})$ by sampling constraint points ${^p}_{i, j} = p_{i} ((j / κ_{i}) \cdot T_{i})$ on the trajectory.

Iv-B Optimization Problem Formulation

We formulate the trajectory generation for formation flight as an unconstrained optimization problem

min c, T [J_{e}, J_{t}, J_{o}, J_{f}, J_{r}, J_{d}, J_{u}] \cdot λ,

(13)

where $λ$ is the weight vector to trade off each cost function.

Iv-B1 Control Effort $J_{e}$

The third order control input for the $i^{t h}$ piece trajectory and its gradients are written as

J_{e} = \int_{0}^{T_{i}} ∥ p_{i}^{(3)} (t) ∥^{2} d t,

(14)

\frac{\partial J_{e}}{\partial c_{i}} = 2 (\int_{0}^{T_{i}} β^{(3)} (t) β^{(3)} (t)^{T} d t) c_{i},

(15)

\frac{\partial J_{e}}{\partial T_{i}} = c_{i}^{T} β^{(3)} (T_{i}) β^{(3)} (T_{i})^{T} c_{i} .

(16)

Iv-B2 Total Time $J_{t}$

In order to ensure the aggressiveness of the trajectory, we minimize the total time $J_{t} = \sum_{i = 1}^{M} T_{i}$ . The gradients are given by $\partial J_{t} / \partial c = 0$ and $\partial J_{t} / \partial T = 1$ .

Iv-B3 Obstacle Avoidance $J_{o}$

Inspired by[27], obstacle avoidance penalty $J_{o}$ is computed using Euclidean Signed Distance Field (ESDF). The constraint points which are close to the obstacles are selected by

ψo(^pi,j)={dthr−d(^pi,j),ifd(^pi,j)<dthr,0,ifd(^pi,j)≥dthr,

(17)

where $d_{t h r}$ is the safety threshold and $d ({^p}_{i, j})$ is the distance between the considered point and the closest obstacle around it. Then the obstacle avoidance penalty is obtained by computing the weighted sum of sampled constraint function:

Jo=Tiκiκi∑j=0¯ωjmax{ψo(^pi,j),0}3,

(18)

where $κ_{i}$ is the sample number on the $i^{t h}$ piece and $({¯ ω}_{0}, {¯ ω}_{1}, \dots, {¯ ω}_{κ_{i} - 1}, {¯ ω}_{κ_{i}}) = (1 / 2, 1, \dots, 1, 1 / 2)$ are the orthogonal coefficients following the trapezoidal rule [28]. The gradients of $J_{o}$ w.r.t $c_{i}$ and $T_{i}$ are detailed as

\frac{\partial J_{o}}{\partial c_{i}} = \frac{\partial J_{o}}{\partial ψ_{o}} \frac{\partial ψ_{o}}{\partial c_{i}},

(19)

\frac{\partial J_{o}}{\partial T_{i}} = \frac{J_{o}}{T_{i}} + \frac{\partial J_{o}}{\partial ψ_{o}} \frac{\partial ψ_{o}}{\partial t} \frac{\partial t}{\partial T_{i}},

(20)

\frac{\partial t}{\partial T_{i}} = \frac{j}{κ_{i}}, t = \frac{j}{κ_{i}} T_{i},

(21)

where $t$ is the relative time on the piece. For the case that $d ({^p}_{i, j}) < d_{t h r}$ , the gradients are given by

\frac{\partial ψ_{o}}{\partial c_{i}} = - β (t) \nabla d^{T}, \frac{\partial ψ_{o}}{\partial t} = - \nabla d^{T} ˙ p (t),

(22)

where $\nabla d$ is the gradient of ESDF in ${^p}_{i, j}$ . Otherwise, the gradients become $\partial ψ_{o} / \partial c_{i} = 0, \partial ψ_{o} / \partial t = 0$ .

Iv-B4 Swarm Formation Similarity $J_{f}$

In Sec.III, we design a differentiable metric to quantify the similarity distance between swarm formations. In optimization, the similarity error between the current formation and the desired formation is measured by $ψ_{f} = f (p (t), \sum_{Φ} p_{ϕ} (τ))$ , where $f (\cdot)$ is detailed in (4) and $Φ$ represents the collection of other agents.

Since $J_{f}$ involves the trajectories of other agents, we need to deal with both the relative time $t = j T_{j} / κ_{i}$ of the own trajectory and the global timestamp $τ = T_{1} + . . . + T_{i - 1} + j T_{j} / κ_{i}$ of others’ trajectories. $J_{f}$ considers the preceding time $T_{l}$ for any $1 \leq l \leq i$ and is formulated as

J_{f} = \frac{T_{l}}{κ_{i}} κ_{i} \sum j = 0 {¯ ω}_{j} max {ψ_{f} (p (t), \sum Φ p_{ϕ} (τ)), 0}^{3} .

(23)

The gradients of $J_{f}$ w.r.t $c_{i}$ and $T_{l}$ are computed as

\frac{\partial J_{f}}{\partial c_{i}} = \frac{\partial J_{f}}{\partial ψ_{f}} \frac{\partial ψ_{f}}{\partial c_{i}},

(24)

\frac{\partial J_{f}}{\partial T_{l}} = \frac{J_{f}}{T_{l}} + \frac{\partial J_{f}}{\partial ψ_{f}} \frac{\partial ψ_{f}}{\partial T_{l}} .

(25)

To derive $\partial ψ_{f} / \partial T_{l}$ , $ψ_{f}$ need to be differentiated by $t$ and $τ$ :

\frac{\partial ψ_{f}}{\partial T_{l}} = \frac{\partial ψ_{f}}{\partial t} \frac{\partial t}{\partial T_{l}} + \frac{\partial ψ_{f}}{\partial τ} \frac{\partial τ}{\partial T_{l}},

(26)

\frac{\partial t}{\partial T_{l}} = {\begin{matrix} \frac{j}{κ_{i}}, & l = i, 0, & l < i, \end{matrix} \frac{\partial τ}{\partial T_{l}} {\begin{matrix} \frac{j}{κ_{i}}, & l = i, 1, & l < i . \end{matrix}

(27)

The gradients of $ψ_{f}$ w.r.t $c_{i}$ , $t$ and $τ$ are given by

\frac{\partial ψ_{f}}{\partial c_{i}} = \frac{\partial ψ_{f}}{\partial p (t)} \frac{\partial p (t)}{\partial c_{i}},

(28)

\frac{\partial ψ_{f}}{\partial t} = \frac{\partial ψ_{f}}{\partial p (t)} \frac{\partial p (t)}{\partial t} = \frac{\partial ψ_{f}}{\partial p (t)} ˙ p (t),

(29)

\frac{\partial ψ_{f}}{\partial τ} = \sum Φ \frac{\partial ψ_{f}}{\partial p_{ϕ} (τ)} \frac{\partial p_{ϕ} (τ)}{\partial τ} = \sum Φ \frac{\partial ψ_{f}}{\partial p_{ϕ} (τ)} {˙ p}_{ϕ} (τ),

(30)

where the gradient of $ψ_{f}$ to $p (t)$ and $p_{ϕ} (τ)$ is detailed in (5).

Iv-B5 Swarm Reciprocal Avoidance $J_{r}$

We penalize the constraint points which are close to other agents’ trajectories at global timestamp $τ$ . Thus, the cost function of swarm reciprocal avoidance is defined as

J_{r} = Φ \sum ϕ = 1 \frac{T_{l}}{κ_{i}} κ_{i} \sum j = 0 {¯ ω}_{j} max {ψ_{r_{ϕ}} (p (t), τ), 0}^{3},

(31)

ψ_{r_{ϕ}} (p (t), τ) = D_{r}^{2} - d (p (t), p_{ϕ} (τ))^{2},

(32)

d (p (t), p_{ϕ} (τ)) =∥ E^{1 / 2} (p (t) - p_{ϕ} (τ)) ∥,

(33)

where $Φ$ represents the collection of other agents, $D_{r}$ is the clearance between each agent, and $E = d i a g (1, 1, 1 / c)$ transforms Euclidean distance into ellipsoidal distance.

The gradients of $J_{r}$ w.r.t $c_{i}$ and $T_{l}$ are the same as (24) and (25), and $\partial ψ_{r_{ϕ}} / \partial T_{l}$ is the same as (26). When $D_{r}^{2} \geq d (p (t), p_{ϕ} (τ))^{2}$ , the gradients of $ψ_{r_{ϕ}}$ w.r.t $c_{i}$ , $t$ and $τ$ are

\frac{\partial ψ_{r_{ϕ}}}{\partial c_{i}} = - 2 β (t) (p (t) - p_{ϕ} (τ))^{T} E,

(34)

\frac{\partial ψ_{r_{ϕ}}}{\partial t} = - 2 (p (t) - p_{ϕ} (τ))^{T} E ˙ p (t),

(35)

\frac{\partial ψ_{r_{ϕ}}}{\partial τ} = 2 (p (t) - p_{ϕ} (τ))^{T} E_{ϕ} (t) .

(36)

Iv-B6 Dynamical Feasibility $J_{d}$

We limit the maximum value of velocity, acceleration, and jerk to guarantee that the trajectory can be executed by the agent. Readers can refer to [15] for more details.

Iv-B7 Uniform Distribution of Constraint Points $J_{u}$

The constraint points are expected to be space-uniform. Non-uniform constraint points may skip some small-sized obstacles, which could diminish the safety of the resulting trajectory. Therefore, the uniform distribution penalty

J_{u}

is optimized to prevent constraint points from gathering in certain locations. Readers can refer to [15] for more details.

Fig. 3: Benchmark comparison between Turpin’s method (the upper row) and our method (the lower row). With Turpin’s method, robots could take non-colliding routes that actually impairs the formation. In contrast, with our method, robots make more reasonable decisions to hold the formation. Position error $e_{d i s t}$ of each formation is shown in the figure.

Scenario	Method	success rate(%)	$e_{d i s t}$ (m)	$e_{s i m}$
	Zhou’s[23]	65	1.47	0.0162
Sparse	Turpin’s[29]	85	1.08	0.0066
	Ours	100	0.77	0.0032
	Zhou’s[23]	15	-	-
Medium	Turpin’s[29]	75	1.55	0.0162
	Ours	100	0.90	0.0045
	Zhou’s[23]	0	-	-
Dense	Turpin’s[29]	60	2.01	0.0278
	Ours	95	1.25	0.0107

TABLE I: Formation Navigation Methods Comparison

V Benchmark

To demonstrate the efficiency and robustness of our method, benchmark comparisons are conducted with cutting-edge formation control methods. We compare our work with Zhou’s method[23] and Turpin’s method[29]. We implement the formation control method in Turpin’s work and adapt it to the dense environments by adding our own obstacle avoidance strategy. However, unlike our work, in [23] and [29], changing the scale and rotation of the formation is not permitted during the flight. Hence, to evaluate the performance fairly, a new indicator of overall position error is proposed for formation flight.

Inspired by [30], in order to assess the overall position error $e_{d i s t}$ between the current formation $F^{c}$ and the desired one $F^{d}$ , we solve the following nonlinear optimization problem to find the best similarity transformation ( $S i m (3)$ transformation) that aligns $F^{c}$ with $F^{d}$ :

e_{d i s t} = minimize R, t, s n \sum i = 1 | | p_{i}^{d} - (s R p_{i}^{c} + % t) | |^{2} .

(37)

Notations $p_{i}^{d}$ and $p_{i}^{c}$ represent the position of $i^{t h}$ robot in formation $F^{d}$ and $F^{c}$ , respectively. The $S i m (3)$ transformation is composed of a rotation $R \in S O (3)$ , a translation $t \in R^{3}$ and a scale expansion $s \in R_{+}$ . By optimizing the transformation in (37) and applying to formations, the influence of scaling and rotation is squeezed out, so that all the formations can be equitably rated by measuring the position error w.r.t the desired formation. The larger the error $e_{d i s t}$ , the more that $F^{c}$ deviates from the desired shape $F^{d}$ . Besides the position error $e_{d i s t}$ , methods are also compared over the success rate and the formation similarity error $e_{s i m}$ we proposed in Sec.III.

We simulate seven drones flying in a regular hexagon formation from one side of an obstacle-rich map to another side with a velocity limit of 0.5 $m / s$ . The cluttered area is of 30 $\times$ 15m size, and three obstacle densities are tested for comparison. Parameters are finely tuned for the best performance of each compared method.

Fig. 4: Composite snapshots of a regular tetrahedron formation passing through a corridor. The swarm flies through the narrow space from right to left. The blue line segments show the outline of the shape.

The result is summarized in Tab.I. It states that the success rate of Zhou’s method[23] is unsatisfactory in the presence of dense obstacles. The multiple interacting potential fields used in their work tend to generate local minima near the corridors. Thus, drones are often trapped in deadlocks. Turpin’s method[29] maintains formation by assigning desired relative positions to each pair of agents and then minimizing the relative error. However, these constraints are barely satisfied in cluttered scenarios, which raises the difficulty of finding feasible solutions and results in the lower success rates in Tab.I. Furthermore, Turpin’s strategy focuses on the individual tracking error instead of the overall formation shape, which could result in unfavorable trajectories for formation flights, as shown in Fig.3.

In Tab.I, our method achieves better performance in terms of position error $e_{d i s t}$ and formation similarity error $e_{s i m}$ . Moreover, our success rate is promising even with complex surroundings, since the scaling and rotational invariance of the proposed metric provides more flexibility for the planner to generate motion plans.

Vi Real world and simulation experiments

Fig. 5: A large-scale heart-shaped formation consisting of ten quadrotors traverses an unknown dense environment from the left side to right side. Snapshots (a)-(e) are uniformly sampled in the cluttered area. Colored curves show the executed trajectories of the formation flight and the thick red line segments represent the outline of the shape.

Vi-a Implementation Details

Our method is integrated with an autonomous distributed aerial swarm system as shown in Fig.6. The swarm shares trajectories through a broadcast network, which is the only connection among all the quadrotors.

Fig. 6: System architecture of our distributed aerial swarm.

Each quadrotor is equipped with an Intel RealSense D435²²2https://www.intelrealsense.com/depth-camera-d435/

stereo camera for imagery and depth sensing. In addition, software modules including state estimation, environment perception, trajectory planning, and flight control are all running with an onboard computer Xavier NX

³³3https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-xavier-nx/ in real-time.

In both real-world and simulation experiments, we generate the local trajectory by solving (13

) per second and run collision check at a frequency of 100Hz. The unconstrained optimization problem is solved by an open-source library LBFGS-Lite

⁴⁴4https://github.com/ZJU-FAST-Lab/LBFGS-Lite. All simulations are run on a desktop with an Intel i9-9900K CPU in real-time.

Vi-B Real-world Experiments

Real-world experiments are designed to validate the feasibility and robustness of our method. In the first experiment, as shown in Fig.1, a 2-D hexagon formation consisting of seven quadrotors successfully traverses a obstacle-rich area without any collision. Twelve cylinder obstacles with a diameter of 0.3m are placed in the area. This test demonstrates that our method is able to maintain the formation for large-scale swarms in unknown complex environments.

In the second experiment, as shown in Fig.4, four quadrotors in a 3-D regular tetrahedron formation manage to pass through a narrow corridor safely. During the flight, the swarm adaptively rotates and compresses the formation shape in responding to the environmental changes. This test proves that the scaling and rotational invariance provides more flexibility for formation flights in constrained spaces.

Vi-C Simulation Experiment

In order to testify the effectiveness of our method with large-scale irregular formations, we design a heart-shaped formation consisting of ten quadrotors. A cluttered area of $80 \times 20 m$ with 300 cylinder obstacles and 80 circular obstacles is set up in simulation. As depicted in Fig.5, the swarm successfully avoids the obstacles and the desired formation is well preserved during the flight.

Vii Conclusions and Future Work

In this paper, we present a distributed swarm trajectory optimization method for formation flight in dense environments. A novel metric is proposed to measure the formation similarity, which is incorporated with a spatial-temporal optimization framework to generate swarm trajectories. The solid performance of our method in simulations and real-world experiments validates its practicality and efficiency.

In the future, we will be committed to further improving the robustness of our method. The challenges arise when the communication ranges of some robots are unpredictably narrowed. Also, in highly constrained environments, task reassignment among the robots could be necessary to resolve deadlocks timely. Finally, we hope to provide a complete solution of formation navigation in dense environments for the robotics community.

References

[1] L. Marconi, C. Melchiorri, M. Beetz, D. Pangercic, R. Siegwart, S. Leutenegger, R. Carloni, S. Stramigioli, H. Bruyninckx, P. Doherty, A. Kleiner, V. Lippiello, A. Finzi, B. Siciliano, A. Sala, and N. Tomatis, “The sherpa project: Smart collaboration between humans and ground-aerial robots for improving rescuing activities in alpine environments,” in 2012 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2012, pp. 1–4.
[2] N. Mahdoui, V. Frémont, and E. Natalizio, “Communicating multi-uav system for cooperative slam-based exploration,” Journal of Intelligent & Robotic Systems, vol. 98, no. 2, pp. 325–343, 2020.
[3] K. Dorling, J. Heinrichs, G. G. Messier, and S. Magierowski, “Vehicle routing problems for drone delivery,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 47, no. 1, pp. 70–85, 2016.
[4] A. Jahn, R. J. Alitappeh, D. Saldaña, L. C. Pimenta, A. G. Santos, and M. F. Campos, “Distributed multi-robot coordination for dynamic perimeter surveillance in uncertain environments,” in 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 273–278.
[5] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” Automatica, vol. 53, pp. 424–440, 2015.
[6] J. Van Den Berg, S. J. Guy, M. Lin, and D. Manocha, “Reciprocal n-body collision avoidance,” in Robotics research. Springer, 2011, pp. 3–19.
[7] J. Van Den Berg, J. Snape, S. J. Guy, and D. Manocha, “Reciprocal collision avoidance with acceleration-velocity obstacles,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 3475–3482.
[8] D. Bareiss and J. Van den Berg, “Reciprocal collision avoidance for robots with linear dynamics using lqr-obstacles,” in 2013 IEEE International Conference on Robotics and Automation. IEEE, 2013, pp. 3847–3853.
[9] S. H. Arul and D. Manocha, “Dcad: Decentralized collision avoidance with dynamics constraints for agile quadrotor swarms,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 1191–1198, 2020.
[10] C. E. Luis and A. P. Schoellig, “Trajectory generation for multiagent point-to-point transitions via distributed model predictive control,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 375–382, 2019.
[11] J. Park, J. Kim, I. Jang, and H. J. Kim, “Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 434–440.
[12] D. Zhou, Z. Wang, S. Bandyopadhyay, and M. Schwager, “Fast, on-line collision avoidance for dynamic vehicles using buffered voronoi cells,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 1047–1054, 2017.
[13] Y. Chen, M. Cutler, and J. P. How, “Decoupled multiagent path planning via incremental sequential convex programming,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 5954–5961.
[14] T. Baca, D. Hert, G. Loianno, M. Saska, and V. Kumar, “Model predictive trajectory tracking and collision avoidance for reliable outdoor deployment of unmanned aerial vehicles,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 6753–6760.
[15] X. Zhou, Z. Wang, X. Wen, J. Zhu, C. Xu, and F. Gao, “Decentralized spatial-temporal trajectory planning for multicopter swarms,” arXiv preprint arXiv:, 2021.
[16] M. A. Lewis and K.-H. Tan, “High precision formation control of mobile robots using virtual structures,” Autonomous robots, vol. 4, no. 4, pp. 387–403, 1997.
[17] M. C. De Gennaro and A. Jadbabaie, “Formation control for a cooperative multi-agent system using decentralized navigation functions,” in 2006 American Control Conference. IEEE, 2006, pp. 6–pp.
[18] T. Balch and R. C. Arkin, “Behavior-based formation control for multirobot teams,” IEEE transactions on robotics and automation, vol. 14, no. 6, pp. 926–939, 1998.
[19] Z. Lin, W. Ding, G. Yan, C. Yu, and A. Giua, “Leader–follower formation via complex laplacian,” Automatica, vol. 49, no. 6, pp. 1900–1906, 2013.
[20] Z. Han, L. Wang, and Z. Lin, “Local formation control strategies with undetermined and determined formation scales for co-leader vehicle networks,” in 52nd IEEE Conference on Decision and Control. IEEE, 2013, pp. 7339–7344.
[21] S. Zhao, “Affine formation maneuver control of multiagent systems,” IEEE Transactions on Automatic Control, vol. 63, no. 12, pp. 4140–4155, 2018.
[22] J. Alonso-Mora, E. Montijano, M. Schwager, and D. Rus, “Distributed multi-robot formation control among obstacles: A geometric and optimization approach with consensus,” in 2016 IEEE international conference on robotics and automation (ICRA). IEEE, 2016, pp. 5356–5363.
[23] D. Zhou, Z. Wang, and M. Schwager, “Agile coordination and assistive collision avoidance for quadrotor swarms using virtual structures,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 916–923, 2018.
[24] R. Van Parys and G. Pipeleers, “Distributed model predictive formation control with inter-vehicle collision avoidance,” in 2017 11th Asian Control Conference (ASCC). IEEE, 2017, pp. 2399–2404.
[25] M. Tantardini, F. Ieva, L. Tajoli, and C. Piccardi, “Comparing methods for comparing networks,” Scientific Reports, vol. 9, 11 2019.
[26] Z. Wang, X. Zhou, C. Xu, and F. Gao, “Geometrically constrained trajectory optimization for multicopters,” arXiv preprint arXiv:, 2021.
[27] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3529–3536, 2019.
[28] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery, Numerical Recipes 3rd Edition: The Art of Scientific Computing, 3rd ed. USA: Cambridge University Press, 2007.
[29] M. Turpin, N. Michael, and V. Kumar, “Trajectory design and control for aggressive formation flight with quadrotors,” Autonomous Robots, vol. 33, no. 1, pp. 143–156, 2012.
[30] P. C. Lusk, X. Cai, S. Wadhwania, A. Paris, K. Fathian, and J. P. How, “A distributed pipeline for scalable, deconflicted formation flying,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 5213–5220, 2020.

Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments

Authors

EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments

Fast Trajectory Planning for Multiple Quadrotors using Relative Safe Flight Corridor

A Distributed Pipeline for Scalable, Deconflicted Formation Flying

Swarm Formation Morphing for Congestion Aware Collision Avoidance

Optimization-Based Collision Avoidance

EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors

E^2Coop: Energy Efficient and Cooperative Obstacle Detection and Avoidance for UAV Swarms

Code Repositories

Swarm-Formation

I Introduction

Ii Related works