Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments
For aerial swarms, navigation in a prescribed formation is widely practiced in various scenarios. However, the associated planning strategies typically lack the capability of avoiding obstacles in cluttered environments. To address this deficiency, we present an optimization-based method that ensures collision-free trajectory generation for formation flight. In this paper, a novel differentiable metric is proposed to quantify the overall similarity distance between formations. We then formulate this metric into an optimization framework, which achieves spatial-temporal planning using polynomial trajectories. Minimization over collision penalty is also incorporated into the framework, so that formation preservation and obstacle avoidance can be handled simultaneously. To validate the efficiency of our method, we conduct benchmark comparisons with other cutting-edge works. Integrated with an autonomous distributed aerial swarm system, the proposed method demonstrates its efficiency and robustness in real-world experiments with obstacle-rich surroundings. We will release the source code for the reference of the community.READ FULL TEXT VIEW PDF
This paper presents a decentralized and asynchronous systematic solution...
This paper presents a new trajectory planning method for multiple quadro...
Reliance on external localization infrastructure and centralized coordin...
The focus of this work is to present a novel methodology for optimal
This paper presents a novel method for reformulating non-differentiable
Gradient-based planners are widely used for quadrotor local planning, in...
Energy efficiency is of critical importance to trajectory planning for U...
Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments
Autonomous aerial swarms can be employed for many systematic and cooperative tasks, such as search and rescue, collaborative mapping, and package delivery. In some scenarios, it could be desired that the swarm moves according to a specified formation. For example, in , 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, 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.
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.
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. incorporate Voronoi cell tessellation into a receding horizon QP scheme to prevent collision among the robots while planning. In , 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. 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. 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.
Various techniques have been proposed to achieve multi-robot navigation in formation, which include virtual structures, navigation functions, reactive behaviors, and consensus-based local control laws. 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. 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, 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, 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.
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. 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.
A formation of robots is modeled by an undirected graph , where is the set of vertices, and is the set of edges. In graph , the vertex represents the robot with position vector . An edge that connects vertex and vertex means the robot and can measure the geometric distance between each other. In our work, each robot communicates with all other robots, thus the formation graph is complete. Each edge of the graph is associated with a non-negative number as a weight. In this work, the weight of edge is given by
where denotes the Euclidean norm. Now the adjacency matrix and degree matrix of the formation is determined. Thus, the corresponding Laplacian matrix is given by
With the above matrices, the symmetric normalized laplacian matrix of graph is defined as
is the identity matrix.
As a graph representation matrix, Laplacian contains information about the graph structure. To achieve the desired swarm formation, we propose a formation similarity distance metric as
where denotes the trace of a matrix, is the symmetric normalized Laplacian of the current swarm formation, is the counterpart of the desired formation. Frobenius norm is used in our distance metric. 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).
Our metric is analytically differentiable with respect to the position of each robot. For robot , we use the weights of its adjacent edges to form a weight vector
. By chain rule, the gradient ofto can be written as
According to our metric (4), the gradient of with respect to each weight can be computed as follow
Then the gradient can be written as
In this work, we adopt the MINCO representation , a minimum control effort polynomial trajectory class to conduct spatial-temporal deformation of the flat-output trajectory
where is the polynomial coefficient, the intermediate points, the time vector, the parameter mapping constructed from Theorem 2 in , and the total time.
A -dimensional -piece trajectory is defined as
and the piece trajectory is represented by a degree polynomial
where is the coefficient matrix, is the natural basis, and is the time allocation for the piece.
MINCO is uniquely determined by . And the parameter mapping converts trajectory representations to with linear time and space complexity, which allows any second-order continuous cost function to be represented by . Hence, and can be efficiently obtained from and , respectively.
Especially, in order to handle the time integral constraints , such as collision avoidance and dynamical feasibility, we transform them into finite-dimensional constraints by sampling constraint points on the trajectory.
We formulate the trajectory generation for formation flight as an unconstrained optimization problem
where is the weight vector to trade off each cost function.
The third order control input for the piece trajectory and its gradients are written as
In order to ensure the aggressiveness of the trajectory, we minimize the total time . The gradients are given by and .
Inspired by, obstacle avoidance penalty is computed using Euclidean Signed Distance Field (ESDF). The constraint points which are close to the obstacles are selected by
where is the safety threshold and 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:
where is the sample number on the piece and are the orthogonal coefficients following the trapezoidal rule . The gradients of w.r.t and are detailed as
where is the relative time on the piece. For the case that , the gradients are given by
where is the gradient of ESDF in . Otherwise, the gradients become .
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 , where is detailed in (4) and represents the collection of other agents.
Since involves the trajectories of other agents, we need to deal with both the relative time of the own trajectory and the global timestamp of others’ trajectories. considers the preceding time for any and is formulated as
The gradients of w.r.t and are computed as
To derive , need to be differentiated by and :
The gradients of w.r.t , and are given by
where the gradient of to and is detailed in (5).
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
where represents the collection of other agents, is the clearance between each agent, and transforms Euclidean distance into ellipsoidal distance.
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  for more details.
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 penaltyis optimized to prevent constraint points from gathering in certain locations. Readers can refer to  for more details.
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 and Turpin’s method. 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  and , 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 , in order to assess the overall position error between the current formation and the desired one , we solve the following nonlinear optimization problem to find the best similarity transformation ( transformation) that aligns with :
Notations and represent the position of robot in formation and , respectively. The transformation is composed of a rotation , a translation and a scale expansion . 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 , the more that deviates from the desired shape . Besides the position error , methods are also compared over the success rate and the formation similarity error 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. The cluttered area is of 3015m size, and three obstacle densities are tested for comparison. Parameters are finely tuned for the best performance of each compared method.
The result is summarized in Tab.I. It states that the success rate of Zhou’s method 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 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 and formation similarity error . 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.
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.
Each quadrotor is equipped with an Intel RealSense D435222https://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 NX333https://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-Lite444https://github.com/ZJU-FAST-Lab/LBFGS-Lite. All simulations are run on a desktop with an Intel i9-9900K CPU in real-time.
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.
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 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.
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.