Downwash-Aware Trajectory Planning for Large Quadrotor Teams

Downwash-Aware Trajectory Planning for Large Quadrotor Teams

James A. Preiss, Wolfgang Hönig, Nora Ayanian, and Gaurav S. Sukhatme All authors are with the Department of Computer Science, University of Southern California, Los Angeles, CA, USA.Email: {japreiss, whoenig, ayanian, gaurav}@usc.eduThis work was partially supported by the ONR grants N00014-16-1-2907 and N00014-14-1-0734, and the ARL grant W911NF-14-D-0005.

We describe a method for formation-change trajectory planning for large quadrotor teams in obstacle-rich 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 non-smooth graph plan into a set of -continuous trajectories, locally optimizing an integral-squared-derivative 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 nano-quadrotors. 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 multi-robot systems. Given a set of robots with known initial locations and a set of goal locations, the task is to find a one-to-one 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 search-and-rescue, 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 graph-based 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 energy-minimizing integral-squared-derivative objective function. We also present an anytime iterative refinement scheme that improves the trajectories within a given computational budget. We support user-specified smoothness constraints and provide simulations with up to 200 robots and a physical experiment with 32 quadrotors, see Fig. 1.

Fig. 1: Long exposure of 32 Crazyflie nano-quadrotors flying through a wall with windows, viewed from the edge of the wall.

Ii Related Work

A simple approach to multi-robot motion planning is to repurpose a single-robot planner and represent the Cartesian product of the robots’ configuration spaces as a single large joint configuration space [1]. Robot-robot collisions are represented as configuration-space obstacles. However, the high-dimensional 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 maze-like 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. Collision-avoidance 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 maze-like environments.

Spline-based refinement of waypoint plans was described in [13] and [14]. Our method builds upon these works by adding support for three-dimensional 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.

Iii-a Robot Model

As aerial vehicles, quadrotors have a six-dimensional 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 three-dimensional Euclidean space.

While some multi-robot planning work has considered simplified dynamics models such as kinematic agents [5] or double-integrators [6], our method produces trajectories with arbitrary smoothness up to a user-defined 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, fast-moving 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 axis-aligned 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


where . The collision-avoidance constraint between robots located at is given by

Fig. 2: Axis-aligned ellipsoid model of robot volume. Tall height prevents downwash interference between quadrotors.

Iii-B Problem Statement

(b) “Gadget” for flow-graph construction.
(c) with .
Fig. 3: Example flow-graph (Fig. 2(c)) for environment shown in Fig. 2(a) with a single robot. The construction uses a graph “gadget” (Fig. 2(b)) for each edge in . The blue edges are annotated with downwash edge conflicts and the green edges are annotated with downwash vertex conflicts. The bold arrows in Fig. 2(c) show the maximum flow through the network, which can be used to compute the robots’ paths.

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


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 user-specified parameter :


    Additionally, we require that the collision-avoidance 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.

Iii-C 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 user-supplied 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 a-priori, 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.

Iv-a Overview

The discrete planning stage computes the goal assignment  and a path for each robot composed of a sequence of (time, position) pairs:


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 collision-avoidance 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.

Iv-B Unlabeled Planner

We model unlabeled planning as a variant of the unlabeled Multi-Agent Path-Finding (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:

  1. Each robot starts at its start vertex: .

  2. Each robot ends at its goal vertex: .

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

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

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

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

  7. 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 maximum-flow problem in a larger graph, derived from , known as a time-expanded flow-graph [18]. This graph, denoted by , contains vertices and is constructed such that a flow in represents a solution to the MAPF instance. This maximum-flow 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 time-expanded flow-graph 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:


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 two-step 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 Edmonds-Karp algorithm on the time-expanded flow-graph. 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 collision-free.

V-a 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 robot-robot and robot-obstacle collision avoidance are guaranteed. For robot in timestep , the safe polyhedron is the intersection of:

  • half-spaces separating from for

  • half-spaces separating from .

We separate from by finding a separating hyperplane such that:


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:


where is the ellipsoid matrix. Robot-obstacle separating hyperplanes are computed similarly, except we use a different ellipsoid for obstacles to model the fact that downwash is only important for robot-robot 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 hard-margin 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 hard-margin SVM formulation yields a slightly modified version of the typical SVM quadratic program:


We solve a problem of this form for each robot-robot and robot-obstacle half-space 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.

V-B 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


where each is a degree- Bernstein polynomial with coefficients111 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 user-defined continuity level .

V-C 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:


where the are user-chosen 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:


The objective function (11) is a quadratic function of , which can be expressed in the form:


where is a block-diagonal 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:


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 corridor-constrained 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.

Fig. 4: Illustration of discrete plan postprocessing. (a) In timestep , robot arrives at a graph vertex and robot leaves . The separating hyperplane between and (with ellipsoid offset shaded in grey) prevents both robots from planning a trajectory that passes through . (b) Subdivision of discrete plan ensures that this situation cannot occur.
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
TABLE I: Runtime for different examples and safety distances, see Section VI-B. All times are given in seconds.

V-D 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 evenly-spaced points in time to generate a set of points . The number of sample points is a user-specified 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) ellipsoid-weighted 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 user-supplied 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.

V-E Discrete Postprocessing

Our grid-based 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 graph-vertex waypoint and arrive at a segment-midpoint waypoint, while even timesteps exit a segment-midpoint waypoint and arrive at a graph-vertex 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 grid-cell obstacles into larger boxes using a greedy algorithm. To compute separating hyperplanes for the safe corridors, our method requires solving small ellipsoid-weighted SVM problems. For these problems, we use the CVXGEN package [24] to generate C code optimized for the exact quadratic program specification (9). The per-robot 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 nano-quadrotors [25].

(a) Full 32-robot trajectory plan after six iterations of refinement. The start and end positions are marked by squares and filled circles, respectively. The obstacles are not shown for clarity.
(b) Picture of the final configuration after the test flight. A video is available as supplemental material.
Fig. 5: Formation change example where quadrotors fly from an plane grid formation to a goal configuration spelling “USC” while avoiding obstacles.
(a) 1 iteration
(b) 2 iterations
(c) 6 iterations
Fig. 6: Subset of results for the example shown in Fig. 5 after different numbers of refinement iterations. Fine lines represent the discrete plans ; heavy curves represent the continuous trajectories . The remaining 28 robots are hidden for clarity.

Vi-a 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.

Vi-B Runtime Evaluation

We execute our implementation on a PC running Ubuntu 16.04, with a Xeon E5-2630 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 obstacle-rich 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 per-robot 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.

Vi-C 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 nano-quadrotors 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 long-exposure photography, as shown in Fig. 1. The supplemental video shows the full trajectory execution.

Fig. 7: Illustration of worst-case acceleration and angular velocity over all robots in “USC” example during six iterative refinement cycles. Left: peak acceleration was reduced from   to   . Right: peak angular velocity was reduced from   to   .

Vii Conclusion

We presented a trajectory planning method for large quadrotor teams. Our approach is downwash-aware 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 two-stage architecture supports the use of different discrete multi-agent planners, for example planners where each robot has an assigned goal or task-specific planners. Iterative refinement offers a user-controlled 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.


  • [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, “Conflict-based search for optimal multi-agent 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 multi-robot 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 collision-free 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, “Mixed-integer 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 LQR-obstacles,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2013, pp. 3847–3853.
  • [12] J. Alonso-Mora, A. Breitenmoser, M. Rufli, P. Beardsley, and R. Siegwart, “Optimal reciprocal collision avoidance for multiple non-holonomic 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 higher-order dynamics,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2016, pp. 1894–1901.
  • [14] M. E. Flores, “Real-time trajectory generation for constrained nonlinear dynamical systems using non-uniform rational b-spline 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 micro-UAV testbed,” IEEE Robotics & Automation Magazine, vol. 17, no. 3, pp. 56–65, 2010.
  • [18] J. Yu and S. M. LaValle, “Multi-agent 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,” On-Line 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 nano-quadcopter swarm,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2017.
Comments 0
Request Comment
You are adding the first comment!
How to quickly get a good reply:
  • Give credit where it’s due by listing out the positive aspects of a paper before getting into which changes should be made.
  • Be specific in your critique, and provide supporting evidence with appropriate references to substantiate general statements.
  • Your comment should inspire ideas to flow and help the author improves the paper.

The better we are at sharing our knowledge with each other, the faster we move forward.
The feedback must be of minimum 40 characters and the title a minimum of 5 characters
Add comment
Loading ...
This is a comment super asjknd jkasnjk adsnkj
The feedback must be of minumum 40 characters
The feedback must be of minumum 40 characters

You are asking your first question!
How to quickly get a good answer:
  • Keep your question short and to the point
  • Check for grammar or spelling errors.
  • Phrase it like a question
Test description