DownwashAware Trajectory Planning for Large Quadrotor Teams
Abstract
We describe a method for formationchange trajectory planning for large quadrotor teams in obstaclerich environments. Our method decomposes the planning problem into two stages: a discrete planner operating on a graph representation of the workspace, and a continuous refinement that converts the nonsmooth graph plan into a set of continuous trajectories, locally optimizing an integralsquaredderivative cost. We account for the downwash effect, allowing safe flight in dense formations. We demonstrate the computational efficiency in simulation with up to 200 robots and the physical plausibility with an experiment with 32 nanoquadrotors. Our approach can compute safe and smooth trajectories for hundreds of quadrotors in dense environments with obstacles in a few minutes.
I Introduction
Trajectory planning is a fundamental problem in multirobot systems. Given a set of robots with known initial locations and a set of goal locations, the task is to find a onetoone goal assignment and a set of continuous functions that move each robot from its start position to its goal, while avoiding collisions and respecting dynamic limits. Trajectory planning is a core subproblem of various applications including searchandrescue, inspection, and delivery. In this work we address the unlabeled case; in the labeled case the goal assignment is given.
A large body of work has addressed this problem with varied discrete and continuous formulations. However, no existing solution simultaneously satisfies the goals of completeness, physical plausibility, optimality in time or energy usage, and good computational performance. In this work, we present a method that attempts to balance these goals.
Our method uses a graphbased planner to compute a solution for a discretized version of the problem, and then refines this solution into smooth trajectories in a separate, decoupled optimization stage. We directly take the downwash effect of quadrotors into account, preserving safety during dense formation flights. Furthermore, our method is complete with respect to the resolution of the discretization, and locally optimal with respect to an energyminimizing integralsquaredderivative objective function. We also present an anytime iterative refinement scheme that improves the trajectories within a given computational budget. We support userspecified smoothness constraints and provide simulations with up to 200 robots and a physical experiment with 32 quadrotors, see Fig. 1.
Ii Related Work
A simple approach to multirobot motion planning is to repurpose a singlerobot planner and represent the Cartesian product of the robots’ configuration spaces as a single large joint configuration space [1]. Robotrobot collisions are represented as configurationspace obstacles. However, the highdimensional search space is computationally infeasible for large teams.
Many works have approached the problem from a graph search perspective [2, 3]. These methods are adept at dealing with mazelike environments and scenarios with high congestion. Some represent the search graph implicitly [4], so they are not always restricted to a predefined set of points in configuration space. However, directly interpreting a graph plan as a trajectory results in a piecewise linear path, requiring the robot to fully stop at each graph vertex to maintain dynamic feasibility. It is possible to use these planners to resolve ordering conflicts and refine the output for execution on robots [5].
Some authors have solved the formation change problem in a continuous setting [6, 7], but methods are often tightly coupled, solving one large optimization problem in which the decision variables define all robots’ trajectories. These approaches are typically demonstrated on smaller teams and do not scale up to the size of team in which we are interested. Others decouple the problem but do not support the level of smoothness in our solution [8], and the authors do not show results on large teams. The method of [9] is computationally fast, but offsets the different trajectories in time, resulting in much longer time durations. Velocity profile methods [10] handle kinodynamic constraints well but are not able to fully exploit free space in the environment. Collisionavoidance approaches [11, 12] let each robot plan its trajectory independently and resolve conflicts in real time when impending collisions are detected. These methods scale well, and their robustness against disturbances is appealing. However, they do not provide any means to optimize the trajectories for objectives such as time or energy use, and they are poorly suited to problems in mazelike environments.
Splinebased refinement of waypoint plans was described in [13] and [14]. Our method builds upon these works by adding support for threedimensional ellipsoidal robots, environmental obstacles, and an anytime refinement stage to further improve the plan after generating an initial set of smooth trajectories. We demonstrate that our iterative refinement produces trajectories with significantly smoother dynamics.
Iii Approach
We start by introducing the robot model, which is required to model the downwash effect. We then formalize the problem statement and outline our approach. In later sections, we will discuss each part of our approach in detail.
Iiia Robot Model
As aerial vehicles, quadrotors have a sixdimensional configuration space. However, as shown in [15], quadrotors are differentially flat in the flat outputs , where is the robot’s position in space and its yaw angle (heading). Differential flatness implies that the control inputs needed to move the robot along a trajectory in the flat outputs are algebraic functions of the flat outputs and a finite number of their derivatives. Furthermore, in many applications, a quadrotor’s yaw angle is unimportant and can be fixed at . We therefore focus our efforts on planning trajectories in threedimensional Euclidean space.
While some multirobot planning work has considered simplified dynamics models such as kinematic agents [5] or doubleintegrators [6], our method produces trajectories with arbitrary smoothness up to a userdefined derivative. This goal is motivated by [15], where it was shown that a continuous fourth derivative of position is necessary for physically plausible quadrotor trajectories, because it ensures that the quadrotor will not be asked to change its motor speeds instantaneously.
Rotorcraft generate a large, fastmoving volume of air underneath their rotors called downwash. The downwash force is large enough to cause a catastrophic loss of stability when one rotorcraft flies underneath another. We model downwash constraints by treating each robot as an axisaligned ellipsoid of radii , illustrated in Fig. 2. Empirical data collected in [16, 17] support this model. The set of points representing a robot at position is given by
(1) 
where . The collisionavoidance constraint between robots located at is given by
(2) 
IiiB Problem Statement
Consider a team of robots in a bounded environment containing convex obstacles . Boundaries of the environment are defined by a convex polytope . The free configuration space for a single robot is thus given by
(3) 
where denotes the Minkowski difference.
We are given a start position for each robot and a set of goal positions . The start and goal inputs must satisfy the collision constraint (2) for all robot pairs. We seek the following:

An assignment of each robot to a goal position , where is a permutation of

The total time duration until the last robot reaches its goal

For each robot , a trajectory where , and must be continuous up to a userspecified parameter :
(4) Additionally, we require that the collisionavoidance constraint (2) is satisfied at all times for all pairs of robots.
In the following, we present an efficient solution to the subclass of problems where all and are positions in an orthogonal grid and obstacles are cubes within that grid.
IiiC Overview
Our approach decomposes the formation change problem into two steps: Discrete Planning and Continuous Refinement. Discrete planning solves the goal assignment problem (generating ) and computes a timed sequence of waypoints for each robot in a graph approximation of the environment. Continuous refinement uses the discrete plan as a starting point to compute a set of smooth trajectories satisfying usersupplied smoothness constraints.
We note that a major benefit of our method is its ability to use different discrete planners. For example, it would be possible to use a discrete planner for planning problems where the goal assignment is fixed apriori, or where robots are split into smaller groups.
Iv Discrete Planning Stage
The discrete planning stage works with a grid discretization of the environment. We assume that the robots’ start and goal locations are vertices of the underlying graph.
Iva Overview
The discrete planning stage computes the goal assignment and a path for each robot composed of a sequence of (time, position) pairs:
(5) 
where , , , and . In between waypoints and , we assume that robot travels on the line segment between and , but we do not make any assumptions about the velocity profile of the robot along that path. We denote this line segment by .
We require that the discrete planner supplies a plan that satisfies the ellipsoid collisionavoidance constraint (2) for all possible identical velocity profiles. We also require all robots to share the same sequence of waypoint times .
In the following, we discuss one specific discrete planner that simultaneously computes the goal assignment and produces waypoint sequences that minimize . This planner operates in a grid environment and assumes fixed timesteps, i.e. is equal for all . Furthermore, we require the grid size to be greater than . A robot can either move to an adjacent grid cell or stay at its current location each step. At all timesteps, and during movements, the planner must ensure that the collision constraints are fulfilled. With fixed timesteps, the number of waypoints corresponds to the time duration of the trajectory. is known as the makespan. Our planner minimizes to produce short trajectories.
IvB Unlabeled Planner
We model unlabeled planning as a variant of the unlabeled MultiAgent PathFinding (MAPF) problem. We are given an undirected connected graph of the environment , where each vertex corresponds to a location in and each edge denotes that there is a linear path in connecting and . Obstacles are implicitly modeled by not including a vertex in for each cell that contains an obstacle. We assume that there exists a vertex corresponding to each start location and that there exists a vertex for each goal location . At each discrete timestep, a robot can either wait at its current vertex or traverse an edge. For the following formulation, we assume that the locations corresponding to the vertices are in a grid world and that and map a vector to its and components, respectively. Our goal is to find paths , such that the following properties hold:

Each robot starts at its start vertex: .

Each robot ends at its goal vertex: .

At each timestep, each robot either stays at its current position or traverses an edge: : or s.t. and correspond to and .

No robots occupy the same location at the same time (vertex collision): : .

No robots traverse the same edge in opposite directions (edge collision): : or .

Robots obey downwash constraints when stationary (downwash vertex collision): where : .

Robots obey downwash constraints while traversing an edge (downwash edge collision): where : or .
We consider a solution optimal if the makespan is minimal. If only the first five properties are considered and is given, unlabeled MAPF can be solved in polynomial time by reduction to a maximumflow problem in a larger graph, derived from , known as a timeexpanded flowgraph [18]. This graph, denoted by , contains vertices and is constructed such that a flow in represents a solution to the MAPF instance. This maximumflow problem can also be expressed as an Integer Linear Program (ILP) where each edge is modeled as binary variable indicating its flow and the objective is to maximize the flow subject to flow conservation constraints [19]. An ILP formulation allows us to add additional constraints for P6 and P7.
We build the timeexpanded flowgraph as intermediate step to formulate the ILP. Compared to the existing detailed discussions [18, 19, 20], we add additional annotations to some of the edges such that is the set of edges with which is in conflict under the downwash model. For each timestep and vertex we add two vertices and to and create an edge connecting them.
For each timestep and edge , we create a “gadget” connecting , and . As shown in Fig 2(b), the “gadget” disallows agents to swap their positions in one timestep, thus enforcing P5. Furthermore, we connect consecutive timesteps with additional edges (green edges in Fig. 2(c)) to enforce P4. Additionally we add vertices and , which are connected to vertices and respectively. If a maximum flow is computed on this graph, the flow describes a path for each robot, fulfilling P1–P5.
Consider vertices that, if simultaneously occupied, would violate P6. Those vertices map to helper edges and in for all (green edges in Fig. 2(c)). In that case we insert into and into for all . Similarly, consider and that violate P7. These edges map to helper edges and in as part of the gadget for all (blue edges in Fig. 2(c)). As before we insert into for all and vice versa.
For each edge , we introduce a binary variable . The ILP can be formulated as follows:
(6) 
where . The first constraint enforces flow conservation, and thus P3–P5. The second constraint enforces P6–P7. P1 and P2 are implicitly enforced by construction of the flow graph. A solution to the ILP assigns a flow to each edge. We can then easily create the path for each robot by setting for any , and based on the flow in .
In order to find an optimal solution for an unknown , we use a twostep approach. First, we find a lower bound for by ignoring P6 and P7. We search the sequence for a feasible , and then perform a binary search to find the minimal feasible , which we denote as . Because we ignore the downwash constraints, we can check the feasibility in polynomial time using the EdmondsKarp algorithm on the timeexpanded flowgraph. Second, we execute a linear search starting from , solving the fully constrained ILP. In practice, we have found that the lower bound is sufficiently close to the final such that a linear search is faster compared to another modified binary search using the ILP.
V Continuous Refinement Stage
In the continuous refinement stage, we convert the waypoint sequences generated by the discrete planner into smooth trajectories . We use the discrete plan to partition the free space such that each robot solves an independent smooth trajectory optimization problem in a region that is guaranteed to be collisionfree.
Va Spatial Partition
The continuous refinement method begins by finding safe corridors within the free space for each robot. The safe corridor for robot is a sequence of convex polyhedra , such that, if each travels within during time interval , both robotrobot and robotobstacle collision avoidance are guaranteed. For robot in timestep , the safe polyhedron is the intersection of:

halfspaces separating from for

halfspaces separating from .
We separate from by finding a separating hyperplane such that:
(7) 
While this hyperplane separates and , it does not account for the robot ellipsoids. Without loss of generality, suppose the hyperplanes are given in the normalized form where . Then we accomodate the ellipsoids by shifting each hyperplane according to its normal vector:
(8) 
where is the ellipsoid matrix. Robotobstacle separating hyperplanes are computed similarly, except we use a different ellipsoid for obstacles to model the fact that downwash is only important for robotrobot interactions, and we shift the hyperplanes such that they touch the obstacles.
In our implementation, we require that the obstacles are bounded convex polytopes described by vertex lists. Line segments are also convex polytopes described by vertex lists. Computing a separating hyperplane between two disjoint convex polytopes and , where denotes the convex hull, can be posed as an instance of the hardmargin support vector machine (SVM) problem [21]. However, the ellipsoid robot shape alters the problem: for a separating hyperplane with unit normal vector , the minimal safe margin is . Incorporating this constraint in the standard hardmargin SVM formulation yields a slightly modified version of the typical SVM quadratic program:
(9) 
We solve a problem of this form for each robotrobot and robotobstacle halfspace to yield the safe polyhedron in the form of a set of linear inequalities. Note that the safe polyhedra need not be bounded and that in general. In fact, the overlap between consecutive allows the smooth trajectories to deviate significantly from the discrete plans, which is an advantage when the discrete plan is far from optimal.
VB Bézier Trajectory basis
After computing safe corridors, we plan a smooth trajectory for each robot, contained within the robot’s safe corridor. We represent these trajectories as piecewise polynomials with one piece per time interval . Piecewise polynomials are widely used for trajectory planning: with an appropriate choice of degree and number of pieces, they can represent arbitrarily complex trajectories with an arbitrary number of continuous derivatives.
We denote the piece of robot ’s piecewise polynomial trajectory as . We wish to constrain to lie within the safe polyhedron . However, when working in the standard monomial basis, i.e. when the decision variables are the in the expression
bounding the polynomial inside a convex polyhedron is not a convex constraint. Instead, we formulate trajectories as Bézier curves. A degree Bézier curve is defined by a sequence of control points and a fixed set of Bernstein polynomials, such that
(10) 
where each is a degree Bernstein polynomial with coefficients^{1}^{1}1 The canonical Bernstein polynomials are defined over the time interval , but they are easily modified to span our desired time interval. given in [22]. The curve begins at and ends at . In between, it does not pass through the intervening control points, but rather is guaranteed to lie in the convex hull of all control points. Thus, when using Bézier control points as decision variables instead of monomial coefficients, constraining the control points to lie inside a safe polyhedron guarantees that the resulting polynomial will lie inside the polyhedron also. We define as a piece, degree Bézier curve and denote the control point of as . The degree parameter must be sufficiently high to ensure continuity at the userdefined continuity level .
VC Optimization Problem
The set of Bézier curves that lie within a given safe corridor describes a family of feasible solutions to a single robot’s planning problem. We select an optimal trajectory by minimizing a weighted combination of the integrated squared derivatives:
(11) 
where the are userchosen weights on the derivatives. A typical choice in our experiments is to penalize acceleration and snap equally. As an input to the trajectory optimization stage, we require the user to supply an initial guess of the duration of each timestep, such that .
Our decision variable consists of all control points for concatenated together:
(12) 
The objective function (11) is a quadratic function of , which can be expressed in the form:
(13) 
where is a blockdiagonal matrix transforming control points into polynomial coefficients, and the formula for is given in [23]. The start and goal position constraints, as well as the continuity constraints between successive polynomial pieces, can be expressed as linear equalities. Thus, we solve the quadratic program:
(14) 
It is important to note that this quadratic program may not always have a solution due to our conservative assumptions regarding velocity profiles. In these cases, we fall back on a solution that follows the discrete plan exactly, coming to a complete stop at corners. Details of this solution are given in [13].
The corridorconstrained Bézier formulation presents one notable shortcoming: for a given safe polyhedron , there exist degree polynomials that lie inside the polyhedron but cannot be expressed as a Bézier curve with control points that are contained within . Empirical exploration of Bézier curves suggests that this problem is most significant when the desired trajectory is near the faces of the polyhedron rather than the center. Further research is needed to characterize this issue more precisely.
Discrete  Continuous  

Example  Grid Size  
USC  32  61  0.3  15  2.3  19  17  39  9  2.7  0.6  2.1  19  
0.9  17  17  32  
Maze50  50  441  0.3  26  8.6  51  26  60  43  8.6  2.8  5.8  57  
0.9  67  26  76  
Sort200  200  1320  0.3  19  101  438  19  541  94  36  20  16  239  
0.9  615  19  722 
VD Iterative Refinement
Solving (14) for each robot converts the discrete plan into a set of smooth trajectories that are locally optimal given the spatial decomposition. However, these trajectories are not globally optimal. In our experiments, we found that the smooth trajectories sometimes lie quite far away from the original discrete plan. Motivated by this observation, we implement an iterative refinement stage where we use the smooth trajectories to define a new spatial decomposition, and use the same optimization method to solve for a new set of smooth trajectories.
For time interval , we sample at evenlyspaced points in time to generate a set of points . The number of sample points is a userspecified parameter, set to in our experiments. We then compute the separating hyperplanes as before, except we separate from instead of from . This problem is also a (slightly larger) ellipsoidweighted support vector machine instance. While the sample points are not a complete description of , is guaranteed to be linearly separable from for , because the polynomial pieces lie inside their respective disjoint polyhedra , .
These new safe corridors are roughly “centered” on the smooth trajectories, rather than on the discrete plan. Intuitively, iterative refinement provides a chance for the smooth trajectories to move further towards a local optimum that was not feasible under the original spatial decomposition.
Iterative refinement can be classified as an anytime algorithm. If a solution is needed quickly, the original set of can be obtained in a few seconds. If the budget of computational time is larger, iterative refinement can be repeated until the quadratic program cost (13) converges.
The usersupplied timestep duration directly affects the magnitudes of dynamic quantities such as acceleration and snap that are constrained by the robot’s actuation limits. In the case that the final refined trajectories violate some constraint, we can apply a uniform temporal scaling to all trajectories. For quadrotors, as the temporal scaling goes to infinity, the actuator commands are guaranteed to approach a hover state [15], so kinodynamically feasible trajectories can always be found.
VE Discrete Postprocessing
Our gridbased MAPF discrete planner produces waypoints that require some postprocessing to ensure that they satisfy the collision constraints (2) under arbitrary velocity profiles. In particular, we must deal with the case when one robot arrives at a vertex in the same timestep when another robot leaves . This situation creates a conflict where neither robot’s smooth trajectory can pass through , as illustrated in Fig. 4. We ensure that this situation cannot happen by dividing each discrete line segment in half. In the subdivided discrete plan, odd timesteps exit a graphvertex waypoint and arrive at a segmentmidpoint waypoint, while even timesteps exit a segmentmidpoint waypoint and arrive at a graphvertex waypoint. Under this subdivision, the conflict cannot occur.
In our experiments, we noticed that the continuous trajectories typically experience peak acceleration at and due to the requirement of accelerating to/from a complete stop. We add an additional wait state at the beginning and end of the discrete plans to reduce the acceleration peak.
Vi Experiments
We implement the discrete planner in C++ using the Boost Graph library for maximum flow computation and Gurobi 7.0 as the ILP solver. The continuous refinement stage is implemented in Matlab. We convert adjacent gridcell obstacles into larger boxes using a greedy algorithm. To compute separating hyperplanes for the safe corridors, our method requires solving small ellipsoidweighted SVM problems. For these problems, we use the CVXGEN package [24] to generate C code optimized for the exact quadratic program specification (9). The perrobot trajectory optimization quadratic programs (14) are solved using Matlab’s quadprog solver. Since these problems are independent, this stage can take advantage of up to additional processor cores.
In our experiments, we use a polynomial degree and enforce continuity up to the fourth derivative (). We evaluate our method in simulation and on the Crazyswarm — a swarm of nanoquadrotors [25].
Via Downwash Characterization
In order to determine the ellpsoid radii , we executed several flight experiments. For , we fly two quadrotors directly on top of each other and record the average position error of both quadrotors at for varying distances between the quadrotors. We noticed that high controller gains lead to very low position errors even in this case, but can cause fatal crashes when the quadrotors are close. We determined to be a safe vertical distance. For the horizontal direction, we use . We set to a sphere of radius based on the size of the Crazyflie quadrotor.
ViB Runtime Evaluation
We execute our implementation on a PC running Ubuntu 16.04, with a Xeon E52630 CPU and RAM. This CPU has 10 physical cores, which improves the execution runtime for the continuous portion significantly. We compute plans for three example problems for 32 to 200 robots navigating in obstaclerich environments. Table I summarizes the problems and breaks down the observed computation time into component parts. For the discrete step we report the runtime to find (), the runtime to solve the ILP with known (), and the total time for the discrete solver to find paths for each robot (). For the continuous step we report , the runtime for the first iteration (), and the total time (). For the first iteration, we report the time for finding the hyperplanes () and the time for solving the quadratic program ().
To investigate the effect of the robot ellipsoid size on computation time, we try each example with two ellipsoid heights: corresponding to our experimental results, and as an arbitrary larger safety distance. These necessitate safety margins of one and three empty grid cells, respectively, in the discrete planner. We notice that the choice of has little impact on the performance because there is enough slack in the examples to achieve a specific makespan even with higher safety distances. Furthermore, the estimated lower bound for is very close to the actual lowest possible in our examples, and the runtime for the discrete solver is dominated by solving the ILP.
In the continuous portion, the balance between computing separating hyperplanes and solving the perrobot quadratic programs depends on the size of the problem. For all experiments, an initial solution is found in less than one minute. In these examples, we executed a total of six refinement iterations, which was enough for the quadratic program cost (13) to converge in all of our experiments.
One of the examples (“USC”) is discussed in more detail in the next section. The supplemental material contains animated simulations for all examples.
ViC Flight Test
We discuss the different steps of our approach on a concrete task with 32 quadrotors. In this task, the quadrotors begin in a grid in the plane, fly through a wall with three holes, and form the letters “USC” in the air.
The discrete planner plans on a grid of side length and finds a solution of timesteps in 40 seconds. The continuous planner needs three seconds to find the first set of smooth trajectories and finishes six iterations of refinement after 19 seconds. Fig. 7 shows the effect of iterative refinement on the dynamics properties of the trajectories . For each iteration, we take the maximum acceleration and angular velocity over all robots for the duration of the trajectories. Iterative refinement results in trajectories with significantly smoother dynamics. This effect is also qualitatively visible when plotting a subset of the trajectories, as shown in Fig. 6. The final set of 32 trajectories is shown in Fig. 4(a).
We use a swarm of Crazyflie 2.0 nanoquadrotors to execute the trajectories in a space with a physical barrier with windows. The space is in size and equipped with a VICON motion capture system with 24 cameras. We upload the planned trajectories to the quadrotors before takeoff, and use the Crazyswarm infrastructure [25] to execute the trajectories. State estimation and control run onboard the quadrotor, and the motion capture system information is broadcast to the UAVs for localization. Figure 4(b) shows a snapshot of the execution when the quadrotors reached their final state. The executed trajectories can be visualized with longexposure photography, as shown in Fig. 1. The supplemental video shows the full trajectory execution.
Vii Conclusion
We presented a trajectory planning method for large quadrotor teams. Our approach is downwashaware and thus creates plans where robots can safely fly in close proximity to each other. We plan trajectories using two independent stages, a discrete stage and a continuous stage. The presented discrete planner finds a goal assignment for each robot and a path such that the makespan is minimized while avoiding collisions and respecting downwash constraints. The continuous stage decouples each robot’s trajectory planning, allowing easy parallelization and improving performance for large teams. The twostage architecture supports the use of different discrete multiagent planners, for example planners where each robot has an assigned goal or taskspecific planners. Iterative refinement offers a usercontrolled tradeoff between trajectory quality and computation time.
Our approach can compute safe and arbitrarily smooth trajectories for hundreds of quadrotors in dense environments with obstacles in a few minutes. The trajectory plan outputs have been tested and executed safely in numerous trials on a team of 32 quadrotors.
In future work, we plan to generalize our method to support arbitrary environments and start and goal locations that are not limited to an underlying grid, by exploring different discrete planning algorithms. We also plan to investigate performance improvements in both discrete and continuous stages.
References
 [1] S. M. LaValle, Planning algorithms. Cambridge Univ. Press, 2006.
 [2] G. Wagner and H. Choset, “M*: A complete multirobot path planning algorithm with performance bounds,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2011, pp. 3260–3267.
 [3] G. Sharon, R. Stern, A. Felner, and N. R. Sturtevant, “Conflictbased search for optimal multiagent pathfinding,” Artificial Intelligence, vol. 219, pp. 40–66, 2015.
 [4] K. Solovey, O. Salzman, and D. Halperin, “Finding a needle in an exponential haystack: Discrete RRT for exploration of implicit roadmaps in multirobot motion planning,” in Algorithmic Foundations of Robotics XI. Springer, 2015, pp. 591–607.
 [5] W. Hönig, T. K. S. Kumar, H. Ma, S. Koenig, and N. Ayanian, “Formation change for robot groups in occluded environments,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2016, pp. 4836–4842.
 [6] F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collisionfree trajectories for a quadrocopter fleet: A sequential convex programming approach,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2012, pp. 1917–1922.
 [7] D. Mellinger, A. Kushleyev, and V. Kumar, “Mixedinteger quadratic program trajectory generation for heterogeneous quadrotor teams,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2012, pp. 477–483.
 [8] Y. Chen, M. Cutler, and J. P. How, “Decoupled multiagent path planning via incremental sequential convex programming,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2015, pp. 5954–5961.
 [9] M. Turpin, K. Mohta, N. Michael, and V. Kumar, “Goal assignment and trajectory planning for large teams of aerial robots,” in Robotics: Science and Systems, 2013.
 [10] J. Peng and S. Akella, “Coordinating multiple robots with kinodynamic constraints along specified paths,” I. J. Robotics Res., vol. 24, no. 4, pp. 295–310, 2005.
 [11] D. Bareiss and J. van den Berg, “Reciprocal collision avoidance for robots with linear dynamics using LQRobstacles,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2013, pp. 3847–3853.
 [12] J. AlonsoMora, A. Breitenmoser, M. Rufli, P. Beardsley, and R. Siegwart, “Optimal reciprocal collision avoidance for multiple nonholonomic robots,” in Distributed Autonomous Robotic Systems. Springer, 2013, pp. 203–216.
 [13] S. Tang and V. Kumar, “Safe and complete trajectory generation for robot teams with higherorder dynamics,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2016, pp. 1894–1901.
 [14] M. E. Flores, “Realtime trajectory generation for constrained nonlinear dynamical systems using nonuniform rational bspline basis functions,” Ph.D. dissertation, California Institute of Technology, 2007.
 [15] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2011, pp. 2520–2525.
 [16] D. Yeo, E. Shrestha, D. A. Paley, and E. Atkins, “An empirical model of rotorcraft UAV downwash model for disturbance localization and avoidance,” in AIAA Atmospheric Flight Mechanics Conference, 2015.
 [17] N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, “The GRASP multiple microUAV testbed,” IEEE Robotics & Automation Magazine, vol. 17, no. 3, pp. 56–65, 2010.
 [18] J. Yu and S. M. LaValle, “Multiagent path planning and network flow,” CoRR, vol. abs/1204.5717, 2012.
 [19] ——, “Planning optimal paths for multiple robots on graphs,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2013, pp. 3612–3617.
 [20] H. Ma and S. Koenig, “Optimal target assignment and path finding for teams of agents,” in Int. Conf. on Autonomous Agents & Multiagent Systems (AAMAS), 2016, pp. 1144–1152.
 [21] S. R. Gunn, “Support vector machines for classification and regression,” University of Southhampton, Tech. Rep., 1998.
 [22] K. I. Joy, “Bernstein polynomials,” OnLine Geometric Modeling Notes, vol. 13, 2000.
 [23] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for quadrotor flight,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2013, pp. 649–666.
 [24] J. Mattingley and S. Boyd, “CVXGEN: a code generator for embedded convex optimization,” Optimization and Engineering, vol. 13, no. 1, pp. 1–27, 2012.
 [25] J. A. Preiss, W. Hönig, G. S. Sukhatme, and N. Ayanian, “Crazyswarm: A large nanoquadcopter swarm,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2017.