In the burgeoning field of autonomous robotics, aerial robots are quickly becoming a useful platform for firefighting, police, search-and-rescue, surveillance, and product delivery. In the deployment of large fleets of such robots, trajectory plans must satisfy the competing criteria of safety and performance. Specifically, the safety requirement of avoiding vehicle-to-vehicle collisions and the performance requirement of minimizing time-in-flight are considered. As the number of robots increases, so too does the complexity of satisfying these requirements, warranting the development of computationally tractable methods to this end.
A wide class of traditional single-agent motion planning methods rely on discretization of the state space and definition of a related state-transition graph [svestka1998]. Optimal feasible paths are then found through a graph search [dijkstra1959, hart1968, wang2012, koenig2002, koenig2004, stentz1993] or other combinatorial solvers [lavalle2006]. While these methods can solve the multi-agent planning problem e.g. as in [turpin2014ar, sharon2015conflict, adler2015, solovey2015, honig2018], they become computationally intractable quickly as the number of agents increases, leading to an exponential growth of the search space dimensionality [erdmann1986, lavalle2006]. Some methods have been explored which reduce the search space dimensionality [lavalle2006, wagner2012, wagner2015, solovey2015finding], but are unable to sufficiently reduce the complexity for large numbers of agents [turpin2014]. Other centralized planning approaches based on sequential mixed-integer linear optimization [schouwenaars2001mixed, richards2002aircraft, ayuso2016], sequential convex programming [gglr2012, chen2015], semidefinite programming [frazzoli2001], formation space-based velocity planning [kloder2006], or path improvement using revolving areas [solomon2018motion] can work well for relatively small teams but do not scale well to large teams due to high computational complexity.
A complementary approach for motion planning is to use local decentralized feedback control laws to satisfy collision avoidance and dynamic constraints [warren1990multiple, fiorini1998, berg2008, berg2011, guy2009, guy2010, cap2014, cap2015, panagou2020]. These require real-time sense-and-avoid capabilities, increasing the system cost and complexity. In general, these schemes lead to difficulties maintaining optimal or near-optimal performance and avoiding undesirable deadlock situations.
It was recently shown by [turpin2014] that when the robots are interchangeable, combining the assignment and planning problems facilitates finding collision-free trajectories. Therein a concurrent assignment and trajectory planning algorithm was proposed which tractably gave collision-free trajectories for large robot teams for sufficiently spaced start and goal locations. The trajectories generated by that approach are globally optimal with respect to a total squared distance metric and under the assumption of synchronized robot motion, which resulted in trajectories that can be significantly suboptimal in terms of total time-in-motion and may violate minimum velocity constraints associated with certain aerial robots. Additionally, it was asserted that coupling the assignment and trajectory planning was critical to avoid overwhelming computational complexity.
To overcome these shortcomings, in this work a similar problem setup and centralized planning setting to [turpin2014] is considered, but allows non-simultaneous trajectory end times and attempts to minimizes the total time-in-motion via a strategy with partial coupling between goal assignment and trajectory planning. The proposed algorithm designs piecewise polynomial-in-time trajectories with physical feasibility and computational tractability guarantees; physical systems track the generated trajectories without colliding with each other, while the polynomial formulation enables key calculations such as the point-of-minimum-clearance to be executed via well-studied and efficient root finding algorithms. It is demonstrated that the proposed method significantly reduces total time-in-motion relative to [turpin2014] with only modest additional computational expense which becomes negligible for large numbers of agents. The authors’ prior work [Gravell2018] presented a similar approach, but required a simple kinematic model which assumed an unrealistic ability to instantaneously change positions vertically and instantaneously change velocities horizontally; these assumptions are removed in the present work. Further, more extensive simulations and an experimental implementation on a multi-robot testbed are provided.
The remainder of the paper is organized as follows. First a variation of the trajectory planning problems given in [turpin2014, Gravell2018] is defined, then a strategy in which goal assignment is performed based on initial trajectory plans is proposed, followed by a refinement step to resolve potential collisions using either time delays or altitude assignment. Throughout, constraints on the magnitude of time derivatives of position (speed, acceleration, jerk, etc.) are honored. Finally, simulated and physical experiments on a many-member quadrotor platform are presented that illustrate the effectiveness of the algorithms.
This work begins by establishing mathematical and notational preliminaries; where applicable, the notation of [turpin2014] is followed. Unlike the authors’ previous work [Gravell2018] which restricted agents to 2-dimensional planes and assumed instantaneous switching between these planes, now a fully 3-dimensional Euclidean space is considered and accompanying trajectory generation algorithms developed which support implementation on a physical robotic system. To be clear, in this work all objects and geometry with physical extent reside within a common 3-dimensional Euclidean space. The set of integers between and positive integer is denoted by , and the identity matrix is denoted by . The following symbols are used for certain operators and objects:
|Logical “and"||Logical “or"|
|Set intersection||Set union|
|Empty set||Minkowski sum|
Vectors in are column vectors unless otherwise specified. The Euclidean norm of such a vector is denoted as and defined as . Consider the scenario where agents begin at start locations and move towards
goal locations in a 3-dimensional Euclidean space with a fixed origin and coordinates measured in a Cartesian coordinate system. The first two coordinates of this space are referred to as “horizontal” and the third coordinate as “vertical”.
The agent position is given by with the horizontal and vertical parts denoted as and respectively. The first four derivatives of position with respect to time are called velocity, acceleration, jerk, and snap. These and higher-order derivatives are collectively referred to as time derivatives. Derivatives with respect to time are notated either by dots or by numbers enclosed by parentheses above the variable, e.g. is velocity and is acceleration of agent .
The collision volume of agent is represented by a finite cylinder of radius and height . Each cylinder is centered at . The cylinders represent the safe collision volume around an agent; a collision occurs if and only if two cylinders intersect. The cylinders have orientations which remain fixed with the axial direction parallel to the world vertical direction (a “vertical” cylinder). The variable height of the cylinders relative to the radius is useful for modeling phenomena such as downwash from the rotors of a quadrotor vehicle. Let the largest radius and height of all vehicles be and .
The start location and goal location are given by and respectively. The horizontal ground plane is the 2-dimensional set . The agents operate in a region :
where is a vertical cylinder with radius whose horizontal coordinates of the center are on the origin and with vertical extent from the ground to positive infinity. An altitude at height is the subset of within which any agent at that height is contained i.e. a 3-dimensional horizontal slab volume:
Define the goal matrix as
Define the boolean assignment matrix , which assigns agents to goals, as
Therefore row of , denoted as , gives the goal location assigned to agent . All agents are assigned to goals in a one-to-one mapping so
A polynomial of scalar with degree , with coefficients , is given by
Minimizing a polynomial over a finite domain interval is a straightforward and computationally efficient procedure which follows from Fermat’s theorem for stationary points from differential calculus. A description is provided in Algo. 5 for completeness since it will be called at various points throughout this work. Maximization is accomplished by the same algorithm by passing a negated polynomial. Also, both minima and maxima can be found concurrently at little additional computational expense by a simple modification to the algorithm.
Similarly, finding the intervals of a finite domain interval over which a polynomial evaluates within a range of values i.e. finding the set
is also straightforward and efficient. The procedure begins by finding domain intervals where the polynomial evaluates above the lower limit and below the upper limit (Alg. 6), then intersects those domain intervals with eachother and the prescribed interval (Alg. 7). Note that Alg. 6 works identically for finding intervals where a polynomial is below an upper value by simply reversing inequalities.
The trajectory planning problem and the proposed solution techniques are now introduced.
3 Trajectory Planning Problem
The trajectory planning problem requires finding instances of -dimensional trajectories which guide agents from start to goal locations. The trajectories are given agent-wise by
and must satisfy the initial and terminal conditions
Agents are considered to be quadrotors, whose center position dynamics linearized about the hover configuration are modeled as a quadruple integrator in horizontal directions (due to the rolling action which must precede lateral acceleration) and a double integrator in the vertical direction [mellinger2011]:
where and are control inputs. The dynamics are not used explicitly in terms of designing control inputs, but rather are used to motivate the choice of trajectory form, namely piecewise polynomials of a particular order.
By choosing a whole number sufficiently high and imposing constraints on the norm of time derivatives, actuator constraints are honored. The particular choice for in the case of quadrotors is established in Section 3.1.1. These constraints are encoded in a vector with and applied as
Define the global start and end times for which motion may occur over all agents:
Ensure collision avoidance by requiring the collision volumes of all agent pairs to be disjoint during the period of possible motion:
Like the previous work in [Gravell2018], the proposed method aims to minimize the total, or equivalently average, time-in-flight of all agents. This is a useful cost metric for many applications e.g. product delivery and emergency response. Therefore, the optimization problem seeks trajectories and goal assignment that minimize total time-in-motion:
Assumptions: The following assumptions are explicitly imposed as part of the problem formulation:
Any assignment of agents to goals is permissible.
The collision volume of each agent is the set of points contained in cylinder .
The effect of any dynamics model mis-specification, imperfect state knowledge, actuation error, and external disturbance are small enough such that the true physical extent of each agent is always fully contained inside the collision volume .
Continuity and satisfaction of upper bound constraints on time derivatives of position is sufficient to ensure actuator constraints are honored.
The region in (1) is devoid of any obstacles other than the agents themselves.
The region in (1) has infinite positive vertical extent.
All start and goal locations are fixed on a common ground plane and are spaced at least apart:
The modeling assumption of no uncontrolled obstacles in the operating space is not altogether unreasonable when considering the nearly empty airspace encountered at altitudes above tree tops, buildings, etc. in typical real-world flight scenarios. The use of cylindrical collision volumes renders orientation of each quadrotor irrelevant for the purpose of trajectory planning.
The solution to the global problem in (13) is ultimately not obtained exactly, but rather a suboptimal solution is found using (13) to guide generation of trajectories and goal assignment by the approach proposed in the following subsections. The strategy for finding an approximate solution to this problem proceeds by temporarily ignoring the clearance requirements (3) which effectively reduces the domain of trajectories under consideration to the ground plane, choosing a function form for trajectories (piecewise polynomials) to reduce the problem to goal assignment, generating horizontal trajectories, then constructing vertical trajectories using refinement techniques which detect and resolve collisions. As a result, these trajectories will be shown to be feasible (e.g. collision-free) and computable after a finite number of operations by construction.
As the trajectory generation procedure based on piecewise polynomial functions is used throughout the goal assignment and collision resolution phases, the trajectory generation scheme is described next.
3.1 Trajectory Generation
The trajectory design is motivated by the observation that minimum-time trajectories along a long straight line with maximum speed constraints will naturally partition into three segments; acceleration, constant (max) speed, and deceleration. A similar idea has previously been suggested for point-to-point robot trajectory planning under the name “Linear Segments with Parabolic Blends" [spong2008]. This idea is generalized to higher-order acceleration (blend) segments. During the acceleration segments, one or more time derivatives of order 2 and higher will be pushed to a constraint maximum, and during the constant max speed segment the higher order time derivatives will be zero. Although physical models involving friction (i.e. higher fidelity models than that assumed in (10)) theoretically allow only asymptotic approach of the maximum speed under actuation constraints e.g. the exponential approach of the speed of a particle in gravitational free-fall to a terminal speed, in practice it was found that the polynomial trajectories were sufficient for reference tracking.
This work does not attempt to optimize control effort during the acceleration segments since the control effort expended during the constant speed segment dominates e.g. due to air friction and by virtue of the relative duration of this segment over long horizontal paths. If deemed necessary, techniques such as minimum-snap trajectory design via quadratic programming [mellinger2011] could be utilized to further decrease the control effort, possibly at the expense of trajectory duration and computational burden. Any techniques which return polynomial-in-time trajectory segments are fully compatible with the the remainder of the proposed method.
This work also restricts trajectories to strictly piecewise vertical and horizontal straight-line paths, which permits simplified trajectory planning and collision resolution by treating trajectories as single-dimensional polynomials of time multiplied by a constant unit heading vector. A pair of tuples and are used and (11) is used with set to either or depending on whether is horizontal or vertical at .
The acceleration segments are individualized polynomials scaled from a base polynomial. The base polynomial is calculated only once at the beginning of the overall routine. Particular whole trajectories are generated by joining acceleration and constant speed segments. Generation of the base polynomial and individualized polynomials are described in the subsequent two subsections.
3.1.1 Base polynomial
Recalling the definition of a polynomial of degree in (6) and the whole number which represents the number of time derivatives on which constraints will be enforced, let . It is evident that a given tuple of initial and terminal time derivative conditions ( total point constraints) uniquely specifies a polynomial of degree so long as the problem is well-posed i.e. if a certain coefficient matrix is invertible. To ensure continuity of position and time derivatives at the endpoints, specify constraints at and constraints at . Due to the assumption on the dynamics in (10), by choosing reference trajectories which are piecewise polynomial with degree at least 4 and 2 respectively, open-loop control with sufficient control effort and the absence of disturbances would give perfect tracking. It is also desirable to make the segment transitions smooth to avoid discontinuous control signals. Choosing degree 9 would allow the specification of 5 endpoint time derivative constraints: position, speed, acceleration, jerk, and snap. However, to reduce the computational storage requirement for the trajectories during implementation on actual hardware and reduce computational effort during centralized trajectory planning, a degree of 7 is used. It was found that the difference between the degree 7 and 9 polynomials was extremely slight and in practice the reference tracking error was dominated by other noise sources. For comparison, degree 1 polynomials represent constant speed trajectories; this was effectively the approach taken in the authors’ previous work [Gravell2018]. The procedure for calculating the base polynomial is as follows:
Form the vector of endpoint conditions
Form the matrix of coefficients as
where . This follows from simple differentiation of polynomials and matching coefficients according to the endpoint constraints. As an example, for and one has
Solve the system of linear equations to obtain the vector of polynomial coefficients .
In this framework, other polynomial bases such as the orthogonal polynomials of Chebyshev or Legendre could be used to improve the conditioning of the matrix [Mellinger2012]
i.e. to encourage the singular values of thematrix to remain clustered around unity and ensure numerical stability of the solution to ; for ever-higher degree polynomials the conditioning of the matrix in the monomial basis degrades. However, for simplicity, monomials are used since the error was found to be manageable on the problem instances encountered, i.e. for degree polynomials. If position and the first time derivatives are at , the first coefficients are also zero, which is evident from the partial diagonal structure of . Indeed, it is desirable to create an acceleration polynomial which has , , , and some higher-order time derivatives zero at both endpoints i.e.
Although this procedure will always generate a polynomial which satisfies the endpoint constraints, the behavior between the endpoints is governed by the duration . In particular, there is a unique setting of which ensures that both the position and velocity monotonically increase from the initial to terminal points, thus ensuring that the endpoints are where the minimum and maximum position and speed occur over the segment. This setting is
With this choice, as an additional benefit, the polynomial degree is reduced by 1 i.e. = 0. Although proving these facts for arbitrary degree polynomials is difficult, it is now shown that at least for , which is the case of interest in this work, that the given setting of in (18) gives the desired behavior. Assuming, without loss of generality, that , , , , and by (18) set . Solving for the coefficients of the position polynomial obtain
and differentiating, the acceleration is
which is nonnegative for all and thus on the interval . Thus the velocity monotonically increases from 0 and so does the position, as desired. Attempting to show this for any other setting of will fail; a proof of this fact is left to future work, noting that a product-of-squares argument (as here) is insufficient to prove a setting of gives an acceleration which is somewhere negative.
It is emphasized that the base polynomial only needs to be calculated once at the beginning of the overall routine and can be scaled and translated (in time) as necessary for each particular trajectory. The base polynomials for vertical and horizontal trajectories are calculated separately to account for differing actuation constraints in each direction. In each case, a unit path length and terminal speed equal to the max speed (, , , ) are used. This results in the polynomials and .
3.1.2 Individualized polynomials
Once the base polynomials for an acceleration segment have been found, a piecewise polynomial (sub)trajectory may be generated which connects any two points , with a straight line path, subject to the time derivative constraints. Let the distance between , be . The whole piecewise polynomial trajectory for agent with pieces has the form
where is a unit heading vector. In this work, this heading will either be horizontal or vertical where are dummy constants satisfying . Also, in this work these trajectories are comprised of 2- or 3-segment subtrajectories and 1-segment stationary wait segments. For notational compactness, let
represent a polynomial trajectory segment which encodes a polynomial, a heading, and a time interval.
Accordingly, the norm of the time derivatives has the simplified form
It will be useful to keep in mind the spatial and temporal scaling formulas for polynomials:
from which it follows that the derivatives satisfy:
First, temporal scaling is applied to the acceleration segment in order to ensure the terminal speed is the agent max speed so that
with equality ensured exactly at . The (absolute) maximum time derivative of the base polynomial is computed via Algorithm 5 with the interval . The scale factor is found as then temporal scaling is applied as
which achieves the proper scaling of speed per (29) and preserves the path length traversed. Next, scaling is applied to the acceleration segment in order to satisfy constraints on the higher time derivatives which are denoted by so that
with equality ensured in at least one derivative at one time. This minimizes the time taken to traverse the path by taking full advantage of the available time derivatives. The (absolute) maximum time derivatives of the base polynomial are computed via repeated applications of Algorithm 5 with the interval . Once the (absolute) maximum time derivatives have been identified, scale factors associated with satisfying each time derivative constraint are found by
The maximum of these scale factors is the only one that is needed to ensure all constraints are satisfied, so take . The scaling is then applied by
which compresses the trajectory temporally and stretches it spatially in equal proportions such that the terminal speed remains the same, per (30), while honoring all higher order time derivative constraints, per (29).
Next, a determination of whether a middle constant speed segment is needed is made. This is accomplished by comparing the path length needed by the acceleration segment to reach max speed and (half) the actual path length between the physical endpoints i.e. if then a constant speed segment is needed. This segment is trivial to calculate; it is simply a constant maximum speed segment whose duration is simply . On the other hand, if then no constant speed segment is needed and the acceleration segments must be scaled again to reduce their path length, in which case the maximum speed will not be attained.
The process continues with a spatial stretch in order to fit the path length exactly:
This has the effect of strictly decreasing the time derivatives per (28) since the scale factor is less than 1. Then new scale factors are calculated similarly to (34) and a temporal stretch is applied to further optimize the trajectory by making full use of the available “capacity” of higher order time derivatives.:
The end result of this entire procedure is a piecewise polynomial (sub)trajectory with 2 or 3 pieces with the first time derivatives continuous and satisfies all initial, terminal, and range constraints. See Fig. 1 for an illustrative example.
3.2 Goal Assignment
Having described the piecewise polynomial trajectory generation procedure, it is now possible to reduce the problem in (13) to one of a linear assignment (combinatorial) goal assignment problem by fixing the functional form of the trajectories. As in [Gravell2018], if the collision avoidance constraint is ignored (3), an argument from the calculus of variations shows that trajectories which minimize the integral of , which is the time-in-motion, follow straight line paths and achieve the highest average speed possible while satisfying the boundary conditions and position derivative constraints. Thus the problem reduces to simply connecting each start to each goal with minimum-time trajectories on straight line paths, computing the time-in-motion incurred by each trajectory, and finding the goal assignment which minimizes total time-in-motion. If these minimum-time trajectories are replaced with constant velocities, as in [Gravell2018], the problem amounts to minimizing the total non-squared distance. Unlike [Gravell2018], motivated by Section 3.1, we now replace these minimum-time trajectories with piecewise polynomial trajectories, where the expression of the cost in terms of distance is more complicated and is driven by the size of the constraints on the time derivatives (which determine the base polynomial) relative to the distances.
Therefore, the optimal assignment is given by
where the cost matrix encodes the cost of assigning agent to goal . In accordance with (13), contains the values of the time-in-motion taken by agent to travel to goal along a straight line. These times are found by calculating polynomial segment trajectories for agent moving from start to goal by the procedure described earlier:
Due to the exceptionally simple form of the piecewise polynomial trajectories, calculating the trajectories for each start-goal pair remains computationally tractable compared with the simplified case of constant velocity trajectories. This problem may be efficiently solved to optimality with a finite number of iterations using the well-known Hungarian algorithm [kuhn1955, munkres1957], which runs in time. Alternate algorithms such as the auction algorithm could also be used with the same time complexity, but with the benefit of parallelization [bertsekas1989, bertsekas1991]. After solving the optimal assignment, the presumptive horizontal trajectories for each agent are simply chosen as those from the cost matrix generation which are selected by the optimal assignment.
A comparison with the C-CAPT algorithm of [turpin2014], which uses a cost function of the distance traveled squared, is given in [Gravell2018]. The main disadvantages of the C-CAPT algorithm are that the speed of agents is limited due to the requirement of agents to start and arrive at goals at the same time, as well as a minimum separation spacing between starts and between goals of . The advantage of allowing asynchronous goal arrival is highly dependent on the distribution of the start and goal locations; when some trajectory lengths are much larger than others, the ability to arrive earlier than other agents significantly improves utilization of the available actuation resources, e.g. speed. For many practical applications the service area includes goal locations which are both near and far from the start locations, which necessitates some agents to travel much longer than others, regardless of the goal assignment, so the advantage is substantial. The results of Section 6 demonstrate this advantage quantitatively, despite the minor degradation in flight times due to collision detection and resolution, which are discussed next.
4 Collision detection
Here the advantage of piecewise polynomial trajectories on straight line paths becomes apparent as the global minimum distance between any pair of agents across their entire trajectories becomes extremely easy and fast to compute. Additionally, the cylindrical collision volume representation synergizes with the restriction that paths are only vertical or horizontal and makes collision checking especially convenient and computationally efficient. Collisions at an instant of time are detected exactly by simply checking if both the radial separation is less than the sum of the radii and the vertical separation is less than the sum of the half-heights. Mathematically, the following equivalent conditions of collision between agents and hold:
For a pair of points moving on straight-line paths whose positions are polynomials in time, the procedure in Alg. 1 is used to find the minimum separation distance.
Consequently, Alg. 8 is used to check for a collision between a pair of agents for a single pair of polynomial segment trajectories. The algorithm uses a default return of “false” and terminates immediately whenever “true” is returned; this “short-circuiting” dramatically improves computational speed. First, it is checked whether the intersection of the time intervals for the segments is nonempty; otherwise the segments are never active at the same time and no collision could occur. Then it is determined whether both agents are moving vertically, both horizontally, or one of each. Based on this, it is checked if (39) or (40) are satisfied, and if so Alg. 1 is used to obtain the relevant minimum separation distance and that distance is used in (39) or (40) to determine the presence of a collision.
Now segment pair collision detection is used to detect collisions between all pairs of full composite trajectories of the polynomial segment type described earlier using Alg. 2. The paths start and end at the start and goal locations on the ground and reside entirely within the planar region with infinite vertical extent passing through the line segment joining the start and goal i.e. . A “short-circuit" of the full polynomial segment collision check is then accomplished by first doing a computationally cheap check which helps quickly guarantee safety of many trajectory segment pairs. If the minimum distance between the two line segments of the trajectory pair is greater than the sum of agent radii, then a collision is impossible since there is no configuration of the agent centers within the assumed planar regions which gives an intersection since (39) is impossible to satisfy. If the minimum distance between the two line segments of the trajectory pair is not greater than the sum of agent radii, then collision detection using Alg. 8 is run. Using this fast preliminary check is critical to obtaining usable performance since, in all but the most highly congested scenarios, this check catches a large portion of segment pairs which are far apart spatially. The segment collision check is repeated for all polynomial trajectory segment pairs (for a single pair of agents). As soon as a collision is detected on a single pair of trajectory segments, the pair of agents is flagged as having a collision and the check progresses to the next pair of agents without finishing checking all remaining segments of the current pair of agents (another “short-circuit”). This process is repeated for each pair of agents, resulting in a symmetric boolean matrix of collision flags which can be represented by an upper triangular matrix or flattened vector to reduce the storage space by half. With exact collision results for the entire group of agents and trajectories in hand, the proposed methodology advances on to resolving the detected collisions.
5 Collision resolution
The overall trajectory generation proceeds by using the general collision detection scheme described in the previous section to determine which agents collide assuming they are all in the same altitude. After single-altitude collisions are detected, they are resolved by inserting vertical trajectories and time delays and/or additional altitudes.
5.1 Collision resolution via time delay
One way to resolve collisions is to send all agents first to a high holding altitude , then after some delay times have agents descend vertically down, then move horizontally in a single traversal altitude , then finally descend to the ground altitude at the goal location. In this scheme, a maximum of three altitudes are needed with a total height of above the ground plane. By construction, given sufficient delay time on each agent that eventually all agents can complete their trajectories without colliding, since in the worst case an agent can simply wait in the holding altitude until all other agents have completed their trajectories and landed. See Fig. 2 for an illustration of this idea in the case when two identical agents must exchange positions. Although such a troublesome goal assignment would never be chosen by the goal assignment procedure in Sec. 3.2 since the reversal of the assignment gives a lower cost, it is conceptually useful simply to illustrate the ability of time delay to resolve collisions. The image shows a side-view with dashed lines representing paths and the table shows a sequence of positions that the agents pass through at generalized times along the linear paths.
The reason to have agents wait in a high holding altitude rather than on the ground is simply that the start and goal locations of two agents may be within a colliding distance of eachother. If the somewhat weak restriction is imposed that
then the possibility of landing on top of another agent waiting on the ground is avoided and the holding altitude is unnecessary and agents can wait on the ground i.e. set so that the holding and ground altitudes coincide. In either case, the proposed method works the same way.
Choosing the height of the traversal and holding altitudes as and ensures agents in different altitudes cannot collide. The traversal and holding altitudes are thus and .
Each full trajectory is made up of 4 or 5 subtrajectories which are generated according to the procedure in Sec. 3.1 which have 2 or 3 polynomial segments each:
Vertical ascent from :
Stationary wait in for time :
Vertical descent from :
Horizontal movement within :
Vertical descent from :
In the case that agents wait on the ground, the subtrajectory in step 1 can be skipped and the agents ascend rather than descend in step 3.
The trajectory generation problem is now reduced to finding the set of time delays whose sum is minimum and also resolve all collisions while adhering to the trajectory generation framework described earlier:
. This problem is nonconvex due to the collision avoidance constraint and has continuous decision variables, so a discretization scheme is used as an effective heuristic. The heuristic begins by ordering the agents randomly, then for each agent the associated delay time is increased by an incrementuntil collisions with all agents whose time delays have been fixed are resolved. This is repeated until each agent’s delay time has been established. This procedure is expressed in Alg. 3.
Although this heuristic is not assured to find the global minimum of the problem in (47), the solutions found are empirically good and importantly are guaranteed to be found after a finite number of computations. To see this, consider the case depicted in Fig. 2 where each agent proceeds one-by-one; the first agent descends from the holding altitude and completes its full trajectory while all remaining agents remain in the holding altitude, then the second agent does the same and so forth until all agents have landed.
Let for be the times taken by each agent to execute trajectory segments 3, 4, 5 in (44), (45), (46), with ordered from greatest to least. Then an upper bound on the number of increments of time delay increase for any other agent is since for any greater time delay a collision is not possible, as explained earlier in the discussion of Fig. 2. Applying this argument iteratively shows that an upper bound on the number of increments for is , and thus an upper bound on the total number of increments is
which is clearly . In practice, many fewer increments are required than this conservative upper bound. The ordering of agents could likely be further improved i.e. according to some metric such as shortest time in horizontal flight, but it was found that random ordering gave good results.
5.2 Collision resolution via altitude assignment
Another way to resolve collisions is by finding an assignment to a set of altitudes and sending agents on trajectories that move horizontally only in these altitudes. The altitudes are given sufficient vertical separation to ensure clearance between agents in different altitudes regardless of horizontal position. Additional wait time and holding altitudes are introduced to resolve potential secondary collisions induced by the primary collision resolution.
There are traversal altitudes for and holding altitudes which are inserted between traversal altitudes and indexed to match the traversal altitudes, although in general . In this scheme, a maximum of traversal altitudes and holding altitudes are needed in addition to the ground altitude.
Define the boolean altitude assignment matrix , which assigns agents to altitudes, as
Therefore in row of , denoted as , the index where gives the altitude assigned to agent . Alternatively, in column of the indices where give the agents assigned to altitude . All agents are assigned to altitudes in a one-to-many mapping, so
where is an diagonal matrix whose entry is the integer number of agents assigned to altitude .
Each full trajectory is made up of 4 or 6 subtrajectories which are generated according to the procedure in Sec. 3.1 which have 2 or 3 polynomial segments each:
Vertical ascent from :
Stationary wait in until global time :
Horizontal movement within :
Vertical descent from :
Stationary wait in for time :
Vertical descent from :
where the final 3 subtrajectories may be collapsed to a single vertical descent from .
Similarly to the collision resolution via time delays, the trajectory generation problem is now reduced to finding the altitude assignment and set of time delays which minimize the sum of flight times and also resolve all collisions while adhering to the trajectory generation framework described earlier:
5.2.1 Primary collisions
Primary collisions are those resulting from two agents moving horizontally in a shared altitude. These are resolved by altitude assignment. As a heuristic for minimizing the sum of flight times, one might seek to minimize the number of altitudes required so that time spent in vertical motion is minimized. However even finding the optimal altitude assignment which minimizes the number of altitudes is a hard nonconvex combinatorial problem, so a similar procedure as in collision resolution via time delays is used to find the altitude assignment . Agents are prioritized randomly, then each agent is assigned the lowest altitude possible that resolves primary collisions with all previously assigned agents. If no such altitude exists, a new one is created at a height above the previous highest altitude by a vertical spacing of . This is repeated for all agents. By construction, such an assignment guarantees that there will be no collisions during the horizontal movements. Alg. 4 documents this procedure using mathematical notation.
5.2.2 Secondary collisions
Although altitude assignment resolves primary collisions, the possibility remains of secondary collisions during the vertical descent movements down towards the goals on the ground. These are easily detected by the same collision detection scheme in Sec. 4. Secondary collisions are exhaustively partitioned into two types of collision: exit and entrance collisions. In practice, it was found that these secondary collisions were exceedingly rare, but nevertheless must be prevented.
In an exit collision, a descending agent is struck by another agent moving horizontally in the same traversal altitude. To resolve this, a simple enlargement of the collision radius is used. The longest time that any agent could take to exit its altitude is calculated; this is easily accomplished by generating a trajectory which descends vertically downwards by (the spacing between two altitudes). This captures the effect of all position derivative constraints imposed on the agents. This is also conservative since some agents may not have to come to a full stop at the altitude below; some agents will continue descending and accelerating which would reduce the time taken to exit the altitude, but this is ignored for simplicity. Next, the greatest distance that the fastest agent would traverse horizontally moving at maximum speed over the time is calculated. Then the collision radii of all agents are increased by . Thus by using the same collision detection scheme in Sec. 4 it is ensured that agents maintain an additional horizontal clearance of at all times, which by construction ensures that exit collisions are impossible.
Unfortunately this procedure requires the collision radii to be increased by an amount proportional to the maximum speed of the agents, but for agents with high maximum acceleration relative to the maximum speed, such as quadrotors, the detriment is not too severe. The enlargement of the collision radii is performed as the first step of the overall collision resolution, prior to finding the altitude assignment to resolve primary collisions.
In an entrance collision, a descending agent enters a lower traversal altitude at the same time as another agent is moving horizontally underneath. To resolve an entrance collision, a holding altitude is placed between the descending agent’s traversal altitude and the next lowest traversal altitude (if one does not already exist). This gives the descending agent a place to wait while the other agent moves out of the way. Once the holding altitude has been inserted, new trajectories are generated and the entire check must begin again from the point where the altitude assignment was made. In particular, the offending descending agent is made to come to a full stop and wait in its (newly inserted) holding altitude. If an entrance collision still exists with this agent, delay time is added according to the same scheme as in Section 5.1. Again, by construction, given sufficient delay time all the possible collisions with agents at lower heights will be resolved since those agents can all land. Agents in the lowest traversal altitude will clearly not encounter this type of secondary collision, and so can complete their trajectories without collision. Arguing inductively, since the lowest agents have collision-free trajectories, and entrance collisions can be resolved for agents in each successively higher altitude, all entrance collisions can be resolved. Since there are a finite number of altitudes, it also follows that the time delays required are also finite. The roles of each agent in an entrance collision are distinguished by the collision detection algorithm simply by noting the heading vector of each agent. As a final remark, in the worst case traversal altitudes and holding altitudes are needed, and thus by construction the computations terminate in finite time.
To conclude the algorithmic development, Figure 3 gives a broad description of all the steps involved in the method and their relationships. Evaluation of the proposed schemes is presented next, both in computer simulations and in deployment on a physical testbed.
6 Simulation results
Both the computer simulations and physical experiments were based on the Crazyswarm, a hardware and software platform that serves as a research testbed for quadrotor autonomy, which is described in more detail in Section 7. Throughout the simulations, the vehicle parameters used for trajectory planning were chosen to match the actual Crazyswarm platform used in the physical experiments. The Crazyflie quadrotor vehicles had a nominal outer diameter of 14 cm and height of 4 cm, while the diameter and height of the cylinders used for trajectory planning were enlarged to 30cm and 40cm respectively; see Section 7 for the rationale of this enlargement. The kinematic constraints imposed during trajectory generation for all simulations are listed in Table 1. These correspond to conservative values computed by scaling down the most aggressive values the physical Crazyswarm platform could experience without significant tracking error.
For the time delay increase rule, used by both algorithms discussed in Sections 5.1 and 5.2, an addition rule with an increment of was used, which was found empirically to strike a nice balance between computation time and quality of solutions.
In order to analyze the performance of the proposed algorithms, Monte Carlo trials were performed with start and goal locations generated randomly with uniform probability over a square of side length. In all trials all agents were identical so that collision volume dimensions were , and position derivatives were for all . The number of agents and the area density were varied, where is defined as the ratio of the summed area of all agents’ projection onto the ground to the area on the ground that any projection could occupy:
To ensure initial and terminal configurations were noncolliding, a minimum start-start and goal-goal separation distance of was imposed. This led to an upper bound on the density, which occurs when the start locations are hexagonally close packed; proofs of this fact date back to Lagrange in 1773 with the first universally accepted proof delivered by Toth in 1942 [Toth1942]. For a separation of the upper limit of density is and for a separation of as in [turpin2014] the limit is . For reference, a typical area density encountered in commercial aircraft traffic management is on the order of [air2016]. For applications involving many unmanned aerial robots the traffic is considerably more dense, so simulations were performed over a wide range of densities.
6.1 Time delay distribution
Fig. 4 shows a histogram of time delays using the proposed time delay collision resolution method for agents and a high density of for a single random Monte Carlo problem instance described in Section 6. This shows that, even when start and goal locations are very dense, most agents have zero or low-valued time delays.
6.2 Number of altitudes required
Using altitude assignment, the number of altitudes required to resolve collisions was studied. Fig. 5 shows the number of flight altitudes (altitudes other than the ground) as a function of area density. As expected, the number of altitudes required grew as the density increased as a result of more potential collisions, but only a few altitudes were required even for highly dense scenarios.
6.3 Normalized flight times
To analyze the relative degradation in flight times due to avoiding collisions (altitude changes and time delays), the flight time data are normalized by dividing by the average time spent in horizontal motion for each trial. The time spent in horizontal motion can be viewed as unavoidable, since this is the minimum time which must be spent to reach the goals even if collisions were ignored.
6.3.1 Effect of collision resolution
From Fig. 6, for this class of random scenarios, it is evident that as the density increases, the total time taken increases as a larger portion of the time is spent moving vertically and waiting. From the zoomed portion, it is evident that the average induced degradation is manageable, being virtually negligible at low agent densities and peaking at around worse than the lower bound at an agent density of which represents a highly congested scenario as can be seen in Figure 8.
From Fig. 7 similar trends are observed as in Fig. 6 using the time delay collision resolution method, but with less time spent waiting, more time spent in vertical motion, and less time spent in total with the average induced degradation again virtually negligible at low agent densities and peaking at around worse than the lower bound at a density of .
In Figures 8 and 9 example trajectories generated by the proposed algorithm are shown. Figure 8 gives a visualization of the scale of the area density, while Figure 9 demonstrates the ability of the proposed algorithm to plan trajectories for a large number of vehicles navigating between arbitrary locations.
6.3.2 Performance relative to alternate methods
For the purpose of comparing the proposed method to alternate methods e.g. that of [turpin2014], define the characteristic time which is the time an agent would take to traverse the longest horizontal straight-line path within the space, which for a square space has length . For a given trajectory plan, denote the time spent by agent in horizontal motion and in waiting respectively as and . Also define the characteristic normalized time in horizontal motion and in waiting as
For simplicity, collisions were allowed for simulations using the approach of [turpin2014] to avoid imposing the separation condition required for that approach to possess collision-free guarantees; this was conservative in the sense that the relative performance of the proposed methods relative to [turpin2014] was only degraded by this assumption. Also, for simplicity simple 1-degree (constant speed) polynomials were used for trajectory generation.
With respect to the metric, plotted in Fig. 10, the proposed altitudes approach gave the best results for all densities. At low densities, the proposed time delay approach gave nearly the same performance as the altitudes approach as a consequence of small time delays which vanish as the density goes to zero. At higher agent densities, the time delay approach result began to increase as the physical extent of the agents became more influential. Both proposed approaches performed better at all densities than the approach of [turpin2014].
6.4 Computation Time
Achieving low computation time is an important practical consideration for successful deployment of large robot teams. The simulations were implemented in MATLAB running on a desktop with an AMD Ryzen 7 2700X eight-core processor running at 3.7GHz. The results are given in Figures 11 and 12, where reasonable computation times for large teams are observed. Explicitly optimizing the code for performance or parallelization could decrease the computation times even further. The overall computation is split into three major segments: the generation of initial trajectories which occurs when finding the cost matrix for input into the goal assignment, the Hungarian algorithm which actually does the goal assignment, and the combined collision detection and resolution steps. This encompasses nearly all of the computations, with the exception of the base polynomial generation and some post-processing steps which together take negligible time to execute.
The goal assignment computation time grew as as expected from a standard computational complexity analysis [munkres1957]. The trajectory generation and collision resolution steps grew only as since the average number of pairwise trajectories and collisions grew with the number of pairs of agents. At ever higher agent numbers it is inevitable that the goal assignment will began to dominate.
7 Experimental results
A series of experiments on physical hardware was performed to validate the performance and safety of the proposed approach.
7.1 System description
A proprietary branch of the Crazyswarm system was used, which encompasses both hardware and software [Preiss2017]
. State estimation was accomplished by taking position measurements of infrared (IR) markers on each quadrotor with an external Vicon camera system. The point cloud of these individual position measurements were then resolved into body coordinate frame (state) estimates using the object tracker portion of the Crazyswarm software. These state estimates were then used by the Crazyswarm software to generate feedback control signals, which were then broadcast over wireless radios to the flying vehicles and electrically converted to motor voltages, completing the feedback loop. The controller used was the standard “Mellinger” controller implemented by the Crazyswarm package, which is a modified version of the nonlinear reference-tracking controller proposed by[mellinger2011] which takes advantage of differential flatness of the quadrotor. See the documentation at https://github.com/TSummersLab/crazyswarm for further details of the software and hardware setup.
7.2 Trajectory tracking errors
One immediate practical issue was reference trajectory tracking in the presence of noise; although the planned trajectories could be followed perfectly in the absence of disturbances, the presence of disturbances precludes this possibility. Natural sources of disturbances included ambient air currents from air conditioner vents, downwash from other agents, ground aerodynamic effects, other unmodeled dynamics, and sensor (camera) noise. By simple enlargement of the collision volumes and ensuring bounds on the position error of all agents, collision avoidance remained guaranteed. Feedback control within the Crazyswarm package ensured the position deviation of each agent from the desired position remained small at all times. In particular, it was found from the experiments that during all trajectory traversals that the radial position error was bounded by 8 cm and the vertical position error was bounded by 7cm; the plot in 17 demonstrates satisfaction of these bounds. Additionally, it was found that the effects of downwash were accounted for by extending the bottom of the collision cylinder by an additional 20cm. Thus, by choosing a collision cylinder with dimensions enlarged by these amounts relative to the physical dimensions of the quadrotor i.e. diameter of cm and height of cm, the vehicle was guaranteed to always be strictly contained within the collision volume, maintaining collision avoidance guarantees. This can be observed from from Figure 16; the trajectory generation tightly respected the collision constraints, as the minimum clearance approached zero without becoming negative. Likewise, during the physical experiment the agents did not experience any collisions, as evidenced by the strictly positive clearance. This was true for all experiments.
7.3 Experiment description and findings
One experiment (“X20”) is now presented with agents moving in a 2m by 3m room from start locations randomly selected from a grid with 0.5m spacing to goals arranged in an “X” configuration roughly 2.8m across the widest section; see Figures 14 and 15. In this experiment the time delay collision resolution method was used. This was for the practical reason that the height of the room limited the number of usable altitudes; in outdoor environments the height of the flyable space would be much greater. The kinematic constraints imposed during trajectory generation were the same as for the simulation results i.e. those listed in Table 1.
The results in Figures 14, 15, 16, 17 demonstrate that the proposed method reliably generated trajectories that could be successfully tracked by a physical quadrotor team and executed in a reasonable time frame with guaranteed absence of collisions.
Videos demonstrating the experiment described in this paper as well as several others are available at https://youtu.be/OapaAQAGWDE. The code which implements the algorithms described in this work and which supports both the virtual simulations and physical experiments can be found in https://github.com/TSummersLab/cannon-tags.
This work demonstrated tractable centralized methods for solving the goal assignment and inter-agent-collision-free trajectory planning problem for multiple robots. The assignment of agents to goals achieved a low total time-in-motion, and the resulting polynomial-in-time trajectories took full advantage of (possibly heterogeneous) speed capabilities. The results of numerical simulations revealed promising decreases in the total time with only mild increases in the computation time over existing approaches, allowing faster task completion in practical terms. The proposed algorithm also allowed us to eliminate restrictions present in other methods such as enforcement of synchronized start and end times and minimum separation of start and goal locations.
Future work is envisioned where the proposed framework would be used as a high-level centralized planner, combined with other decentralized techniques for dealing with lower-level local obstacles and disturbances. The ability to use different altitudes i.e. all three spatial dimensions is crucial to the proper working of the proposed approach; operating spaces limited to a single 2D plane are not supported. Future work will investigate using curved (polynomial) paths to alleviate this issue while retaining tractability.
Future work also includes extension to agents with more complex dynamics and/or motion constraints, dealing with uncontrolled obstacles, combining time delays with altitudes, reassigning goals dynamically to further reduce would-be collisions, and a parallel implementation to decrease solve times. Investigation of the setting when there are more goals than agents and the setting of multiple stages is also warranted, both requiring dynamic goal assignment and replanning.
Regarding the hardware implementation, refinements to the localization and state estimation furnished by the camera system as well as using more sophisticated controllers which account for downwash and ground effects [Yeo2015, Shi2018] could further reduce the magnitude of the actual position errors and allow shrinkage of the collision volumes.