1 Introduction
Reliable collision avoidance is quickly becoming a mainstay requirement of any scalable mobile robotics system. As robots continue to be deployed around humans, assurances of safety become more critical, especially in high traffic areas such as factory floors and hospital corridors. We define a class of distributed collision avoidance methods, known as the reciprocally safe methods, which we prove are guaranteed to be collision free by construction. This class contains a number of wellknown, published algorithms, providing an alternative proof of collision avoidance. We then present a special case of this class that allows a group of robots to avoid colliding with one another, even when each robot has its own (potentially noisy) estimates of other robots’ states, as is common with noisy onboard sensors. The method additionally requires no explicit communication among the robots, nor does it require these estimates to be consistent with those of other robots. More specifically, we assume that each robot keeps an uncertainty set (e.g., unions and intersections of ellipsoids) that contain the possible future locations of other robots.
The resulting policy is distributed in the sense that each robot only requires an estimate of the relative positions of the other robots. In other words, robots do not need to communicate their positions to one another to coordinate their actions. Each robot uses its own position estimates for the other robots to find a safereachable set for itself, which is characterized by a generalized Voronoi cell. Our algorithm then computes a projection onto this safereachable set, which we show reduces to an efficiently solvable convex optimization problem. We then extend the projection method to find smooth polynomial trajectories, instead of just a single point, which lie entirely inside the agents’ safereachable set. Our method is amenable to fast convex optimization solvers. Using our method, a quadrotor maneuvering in a 3D space with 100 other quadrotos can compute its control action in approximately , including setup and solution time. Because the resulting method is reciprocally safe, we have the immediate implication that, if each robot uses this policy, mutual collision avoidance is guaranteed.
Summary.
This paper is organized as follows. The remainder of this section discusses related work. The following section defines the class of ‘reciprocally safe methods’, proves that reciprocal collision avoidance is guaranteed for any method in this class, and then discusses some basic, but useful, extensions to this proof. The following section presents a particular instance of a reciprocally safe method, called ‘CARP,’ short for Collision Avoidance by Reciprocal Projections. This method is specifically constructed to guarantee safety, even in the presence of noisy sensor data, while also being fast to execute onboard, as the resulting trajectory can be computed by solving a small convex optimization problem. The following section shows some benchmarks of the algorithm on synthetic data, a large team of simulated robots in both 2D and 3D, and, finally, on a team of quadrotors with noisy onboard data. We conclude with some thoughts and possible extensions. This paper extends the previous conference submission [ASS19] by adding a smooth polynomial trajectory generation method via the reciprocal projection framework as well as hardware demonstrations on a quadrotor platform. It also simplifies and extends the proof and the constructions given in the original paper.
1.1 Related work
The most closely related methods for fully distributed collision avoidance in the literature are the velocity obstacle (VO) methods, which can be used for a variety of collision avoidance strategies. These methods work by extrapolating the next position of an obstacle using its current position and velocity. One of the most common tools used for mutual collision avoidance is the Reciprocal Velocity Obstacles (RVO) [VdBLM08, VdBGLM11, VdBGS16]
method in which each agent solves a linear program to find its next step. The Buffered Voronoi Cell (BVC) method of
[ZWBS17] provides similar avoidance guarantees, but does not require the ego agent to know other agents’ velocities, which can be difficult to estimate accurately. The BVC algorithm opts instead for defining a given distance margin to compute safe paths. BVC methods have been coupled with other decentralized path planing tools [ŞHA19] in order to successfully navigate more cluttered environments, but require that the other agents’ positions are known exactly.Uncertainty.
While both VO and BVC methods scale very well to many (more than 100) agents, they also require perfect state information of other agents’ positions (BVC), or positions and velocities (RVO). In many practical cases, high accuracy state information, especially velocity, may not be accessible as agents are estimating the position of the same objects they are trying to avoid. Extensions to VO that account for uncertainty have been studied under bounded ([CHTM12]) and unbounded ([GSK17]) localization uncertainties by utilizing chance constraints. While these have been extended to decentralized methods ([ZA19]
), they assume constant velocity of the obstacles at plan time. Unbounded localization estimates, modeled as Gaussian Mixture Models, were combined with RVO in
[AM21a], and have been used effectively, but these methods only estimate the velocity at plan time and do not consider how the velocity of the other agents may change over the horizon. Combined Voronoi partitioning and estimation methods have been studied for multiagent path planning tasks [BCH14], but still require communication to build an estimate via consensus. The VRVO method of [AM21b] is the most similar to our method in that it augments the safe reachable set by adding RVO style constraints to the BVC as well as deflating the safereachable regions based on higher order dynamics. In contrast to these, our method does not require any communication or velocity state information, nor does it require the true position of the other agents. Instead, the algorithm uses only an estimate of the current position of nearby agents and their reachable set within some time horizon.Comparison.
Our algorithm takes a desired trajectory or goal point (which can come from any source, akin to [VdBLM08, ZWBS17]), and returns a safe next step for an agent to take while accounting for both the uncertainty and the physical extent of other agents. The focus of this work is on fast, onboard refinement rather than total path planning. More specifically, while the algorithm presented could be used to reach a far away goal point, it is likely more useful as a smalltomedium scale planner for reaching waypoints in a larger, centrally or locally planned trajectory.
Higher order planners.
Similarly, single agent path planners such as A* [HNR68] or rapidlyexploring random trees (RRTs) [LKJ01] can be applied to the multiagent case, but solution times grow rapidly due to the exploding size of the joint state space. For example, graphsearch methods can be partially decoupled [WC11] to better scale for larger multiagent systems, but can explore the entire joint statespace in the worst case. Fast Marching Tree (FMT) methods [JSCP15] are similar to RRTs in that they dynamically build a graph via sampling. But, while FMT methods have better performance in higher dimensional systems, they still require the paths to be centrally calculated. A* can also be used in dense environments for decentralized multiagent planning when combined with barrier functions [MJWP16], but require the true position of all other agents. While all of these methods require global knowledge and large searches over a discrete set, they can be used as waypoint generators that feed into our method—for use in, e.g., cluttered environments.
Nonconvex methods.
Optimization methods that use sequential convex programming (SCP) [SHL13, ASD12, MCH14] have also been studied for multiagent path planning; however, these algorithms are still centralized and may exhibit slow convergence, making them unreliable for online planning. For some systems, these methods can be partially decoupled [CCH15], reducing computation time at the cost of potentially returning infeasible paths. Our method, in comparison, is fully decentralized and produces an efficient convex program for each agent. The solution of this program is a safe waypoint for the agent, which, unlike SCP methods, requires no further refinement.
2 Reciprocally safe methods
In this section we define a class of methods, called the reciprocally safe methods, and show that any method in this class is guaranteed to be collisionfree, assuming the agents start out in a collisionfree configuration. We will then show that this proof contains a number of results in the literature as special cases, and give some simple extensions, such as ensuring a minimum separation.
2.1 Method description
We start with agents in an unbounded Euclidean space. For each agent , let denote its position, where is the dimension of the space, at time . At each time step , each agent will have an estimate of possible future positions of every other agent , given by some set , with , and . (Note that this set need not be bounded, nor closed for the proof.) We will assume that is consistent; i.e., that agent ’s position at time is always within the uncertainty set of agent :
for every agent with and every given time .
Reciprocally safe set.
A set for agent , at time , is reciprocally safe if, for every other agent with , we have that
(1) 
One way of interpreting is as a set of positions which are known to be safe to move to, given the position estimates of every other agent. As before, we do not require this set be bounded or even closed.
We will then say that the agents implement a reciprocally safe method if there exists reciprocally safe sets such that
if is nonempty, and , otherwise, for each and . Written out, we say that agents implement a reciprocally safe method whenever they move to a reciprocally safe position whenever such a position is available, and do not move otherwise. We will now show that any reciprocally safe method is collisionfree, assuming the agents start from a collisionfree configuration.
Proof.
The proof is almost by definition. Consider any two distinct agents with at some time . We will show that and always have positive distance from each other, assuming that and are some positive distance apart. First, if is nonempty, then, by definition, we have
but since the set has empty intersection with the uncertainty set, by assumption,
then,
and, finally, since the uncertainty set contains the true position of agent at time ,
so and must have positive separation.
On the other hand, if is empty, and is nonempty, the proof follows similarly by replacing with . Finally, if both and are empty, then
so and have positive separation because and did, by assumption.
Because this is the case for any and every , any reciprocally safe method is collision free as any two agents always have positive separation, assuming they have positive separation at .
2.2 Basic extensions
We outline some immediate extensions to the proof above.
Positive margin.
In the previous proof, while there is a guarantee that the agents will be positively separated, the separation could be arbitrarily small. On the other hand, we can imagine that the agents are all required to have some amount of margin that is bounded from below. We will denote the required margin by some set , which we view as a “safe” region around the agent. Usually will be a ball in dimensions:
where is the desired minimal distance from all other agents, but we may generally take any set we wish.
To guarantee this, we can strengthen condition (1) to
whenever the set is nonempty. Here, we define
as the set sum, or Minkowski sum, of and . Another way of stating this is, if the set is nonempty, it must only contain points which have a margin of at least from the future estimated positions. The resulting proof is similar to the base method and gives the stronger guarantee that
for any two agents . In the special case that is an ball, this would imply that agents and are at least distance apart.
Stopping set.
The proof above requires the condition that if the set is empty. This is, of course, not always possible for realistic agents such as drones, which cannot stop immediately. In this case, we can relax the condition to the following condition:
where is agent ’s stopping set (similar to the braking set used in [AM21b]) which is assumed to be compact. We then strengthen the initial condition that and are positively separated for , to
and the reciprocal safety condition that must satisfy when it is nonempty, to
for each with . Positive separation between and is then always guaranteed in this case by a similar proof, since disjoint compact sets have positive separation. Note that this only guarantees onestep separation, from time to time . We can then guarantee positive separation at all times by requiring the additional condition that, if and are both empty, we have
i.e., agent only needs to ‘stop once.’
2.3 Example methods
There are several simple methods which satisfy the reciprocal safety property, and are therefore guaranteed to be collision free. We describe some basic examples in this section, which include some methods in the literature that are known to be collision free by other proof techniques, showing that this class is general enough to include a number of known results.
Trivial method.
Perhaps the simplest of all possible reciprocally safe methods is the trivial method, which, for all agents , defines as
and sets the position estimates to be all of for any distinct agents with ,
In other words, the agents’ position estimates of each other are ‘maximally bad,’ i.e., they include all of , and there is no safe position to move to, as for each agent and time . It is follows that the sets satisfy the reciprocal safety conditions (1), and that the agents must always have . Because of this, the agents never move from their starting locations, which is easily seen to be collision free, when starting from a collision free configuration.
While this example is not useful in practice, it is a good initial exercise to check the conditions necessary for a method to be reciprocally safe.
Buffered Voronoi cell.
Another reciprocally safe method is the buffered Voronoi cell method of [ZWBS17]. In this method, the future position estimate that agent has of agent at time , is given by
where is the (known) position of agent at time , is the position of agent , and denotes some margin. In other words, the possible future positions of agent are the set of points which are further from agent than from by at least . (We take a simpler approach here than that of [ZWBS17] for the sake of presentation, but the proof is nearly identical.) In this case, the set of ‘safe’ locations for agent is defined similarly:
The reciprocal safety condition can be readily verified, since for distinct agents and , and any point in the uncertainty region satisfies
by definition. So, cannot be in the reciprocally safe set , since means that also satisfies,
a contradiction.
Any method that agent uses to choose its next location within the set is guaranteed to be collision free by the above proof. Additionally, because the set is a convex set as it is the intersection of a number of convex sets, many convex optimization problems can easily include the constraint that the agent’s future position must lie in this set. Because convex problems are almost always efficiently solvable, even with onboard computational constraints, finding feasible points that best satisfy some requirement can often be done in real time.
Method refinement.
Very generally, we have the following additional result: if a method is reciprocally safe, any refinement of such a method is also always safe. That is, given a reciprocally safe method, where the agents have estimates and reciprocally safe sets at each time , we say a second method is a refinement of the first if it has estimates that satisfy , and reciprocally safe sets that satisfy . This implies that if the original method is reciprocally safe, i.e., satisfies (1), then a refinement is also reciprocally safe as it also satisfies (1), and is therefore collision free. More intuitively, this is can be restated as the fact that having better estimates, or a more restricted action space, can never make an algorithm unsafe. This construction then immediately includes, for example, the safety results of [AM21b], where the position estimates are the buffered Voronoi cells given in the previous example, intersected with an additional reciprocal velocity obstacle (RVO) cone.
3 General method
In this section, we present a reciprocally safe method that generalizes the buffered Voronoi cells of [ZWBS17] to include measurement uncertainty and higher order dynamics. The resulting method relies on the solution of a small convex optimization problem at each time step that is unlikely to have a closed form, but can still be solved with onboard systems in under a millisecond with modern solvers.
To do this, we first introduce the idea of generalized Voronoi cells—a natural way of extending Voronoi cells from collections of points to collections of sets. In many cases, the resulting generalized Voronoi cells cannot be defined in terms of a finite number of closed form inequalities, but, because these sets are always convex, we can write new expressions that depend on a larger number of variables that do have closed forms.
3.1 Generalized Voronoi cells
Given a point and a collection of sets , we will define the generalized Voronoi cell of with respect to the family as
(2) 
Here, we have defined to be the distancetoset function:
for . (If is empty, we set the distance as , for convenience.) We can view the points in as the set of points which are closer to than to any point in any one of the sets . Note that we can write
We will make use of this fact later in what follows.
Comparison to Voronoi cells.
The usual definition of the Voronoi cell is, given a family of points , the cell generated for the th point is defined as the set of all points closer to than to any other point ; i.e.,
This is the special case of (2) where we take the generalized Voronoi cell of with respect to , and every set in the family is a singleton; i.e., for each .
Convexity.
Like the usual definition of a Voronoi cell, the generalized Voronoi cell of with respect to is convex, even when the sets are not. To see this, note that we can write as
which is the intersection of a family of hyperplanes. (This follows by squaring both sides of the inequality and cancelling the
term.) Because the intersection of convex sets is convex, and hyperplanes are convex sets, then is always convex.3.2 Projective method
We will now discuss a simple version of the method and its applications to singleintegrator dynamics. Later in this section, we will also show how to extend this to more general dynamics and more general sets.
Projection.
A common objective for an agent to optimize is its distance to some desired goal. We will call this goal point , for agent . At every time step , the optimization problem for agent is then:
minimize  
subject to 
The optimization variable here is , while the problem data are the goal point and the reciprocally safe set . If a solution to the problem exists, the agent takes a step towards , otherwise, if there are no feasible points, then the agent stops in place. (We will see extensions to the more general case where the agent has some stopping distance later in this section.)
Such a method is often called a “greedy” method, as the agent attempts to get as close as possible as it can to the goal position, while remaining safe. We refer to this specific way of picking the next possible point as a ‘projective method’ since a solution is often called the projection of onto the set . We will show how to construct reciprocally safe sets for all agents , assuming that each agent has some (potentially noisy) estimate of the future locations of all other agents.
Safe estimates.
To simplify notation, we will write the algorithm for a single agent at a given time step . Because of this, we write for , which is the agent’s current position at time , and for , which are the estimates agent has of agent at the next time step, .
Using this notation, we can view the set (i.e., the set of points which are closer to than they are to any ) as a set of positions which agent is guaranteed to reach before any agent . More specifically:
(3) 
for each . We also note that this set is an overlyconservative set as there are many sets that also satisfy (3) that strictly contain .
A natural choice for a reciprocally safe set for agent , is then
since we know that any choice of is guaranteed to be safe, by construction. This implies that, if all agents choose points within (and simply stop if the set is empty) then the method is guaranteed to be collision free. The resulting optimization problem is:
minimize  (4)  
subject to 
with variable . Note that, since is a generalized Voronoi cell, then is convex. We will use this fact to give an efficient method for optimizing a goal function, when the sets are the intersections and unions of wellknown convex sets.
3.3 Projecting onto generalized Voronoi cells
We will show how to efficiently solve (4) by first reducing it to a problem over several constraints, each of which are simpler than the original. We then show how these constraints can be reduced to a number of inequalities which are easily compiled down to wellknown conic constraints, when the uncertainty sets are unions and intersections of ellipsoids and polygons. Similarly to the previous subsection, we will only consider agent ’s position at time , denoted simply as and the uncertain estimates of the future positions of the other agents as for with .
A basic reduction.
We first note that, given a problem of the form of (4), we can write the equivalent problem:
minimize  
subject to 
In other words, we have ‘split’ the single constraint to constraints given by for each . This follows from the fact that
which is readily verified from (2). From this, it suffices to show how to write the constraint for a single set . We will write for the set in question, with the understanding that stands for any ‘anonymous’ set.
Convex sets.
In general, the set is defined by
(5) 
where is a convex function. For example, in the case that is an ellipse defined by where and is positive definite, we have that
On the other hand, if is a polyhedron, it is defined by a number of affine inequalities; i.e.,
where and . There are a number of other possible functions, such as indicator sets among many others, but we focus on these two cases as the most common types of sets.
Constraint rewriting.
Given any set defined by a function , as in (5), the corresponding constraint is:
From (2), this is true, if, and only if, also satisfies
We will rewrite this as
(6) 
by expanding the squared norm on both sides, cancelling like terms, and using the definition of . In general, it is unlikely that there is a closed form solution for the right hand side of the inequality, even in the special cases where is an ellipse or a polyhedral set. To get around this, we will use a duality trick, introduced originally in [ASS19], to rewrite the right hand side as an unconstrained infimum. This new infimum has a simple analytical solution in the important cases where is an affine function (when is a polyhedron) and when is a convex quadratic (when is an ellipsoid). There are likely more applications of this method to more complicated functions , but we focus on these two important cases. We encourage readers to apply this method to other functions which may be useful in practice.
Weak duality.
One simple approach to finding a reasonable replacement for (6) is to find an approximation of the right hand side that is concave and reasonably tight. One standard approach is by Lagrange duality [BV04], where the ‘hard constraint’ is relaxed to a linear penalty term with some weights , i.e., inequality (6) is replaced with
(7) 
This new infimum is unconstrained and sometimes, but not always, admits closed form solutions when the original does not. (In many cases, the closed form solutions, when they exist, are wellknown.) For convenience, we will define the dual function as
and note that, for any and we have that
(8) 
which is known as weak duality [BV04]. Replacing inequality (6) with inequality (7) is often called a restriction: if is feasible for some in (7) then is also feasible for (6). In other words, the new constraint is at least as tight as the original. An important fact of the dual function is that it is concave in its arguments, because is an infimum over a family of functions that are affine in and . This implies that (7) is a convex inequality constraint.
Strong duality.
Due to strong duality, the new constraint is, in fact, equivalent to the original. That is, for every , there exists some such that inequality (8) holds at equality:
Because of this, if is feasible for (6) then there exists some such that is also feasible for (7), and vice versa. In other words, replacing inequality (6) with (7) and solving this new problem (with as an additional variable) gives two equivalent problems in that a feasible for the first corresponds to a feasible pair , with the same objective value, for the second.
Dual functions for known sets.
Given that both constraints are equivalent, the last remaining point is to write down the dual functions for some known sets. We will make use of the fact that the minimizer of a convex quadratic is
(9) 
for any positive definite matrix
and vector
. This follows from an application of the first order optimality conditions.Polyhedral uncertainty.
In the case where is a polyhedron, we have that
for some and . In this case, the dual function is
where we have replaced with for convenience. Using (9) and the fact that , we get:
as required.
Ellipsoidal uncertainty.
The case where is an ellipsoid with parameters and is rather similar. In this case, the function is a quadratic and the dual function is
Replacing, again, with for convenience. Collecting the quadratic, linear, and constant terms and applying (9), we have:
This function is concave, and its corresponding constraint is easily representable as a semidefinite constraint. On the other hand, most embedded solvers, often due to space and time restrictions, do not support semidefinite constraints. We can turn this into a constraint that is representable as a small family of secondorder cone constraints, which are often more efficient in practice and can be handled by embedded solvers such as ECOS [DCB13].
Since is positive definite, it has an eigendecomposition given by
where is a diagonal matrix and
is an orthogonal matrix such that
. Using this, we can write:This implies that
Because the matrix is diagonal, then:
(10) 
where is the th column of . This expression is a sum of quadraticoverlinear terms, which are easily representable as secondorder cone constraints [LVBL98] and are supported in most embedded solvers.
Unions and intersections of sets.
If is the union of a number of ellipsoids or polyhedra, then, as before, we can simply split into its individual components and add each as a constraint. On the other hand, can also be the intersection of ellipsoids and polyhedra; i.e., is the intersection of the polyhedra specified by and (as the intersection of polyhedra results in another polyhedron) and the ellipsoids given by for .
The derivation here is again nearly identical to the previous. We will write as the Lagrange multiplier for the polyhedral constraint, while we write for for each of the ellipsoidal constraints. The dual function is then:
where the function is the ‘matrix fractional’ function:
Surprisingly, since the are positive semidefinite, the corresponding equality constraint can also be written as a number of second order cone constraints, though this reduction is slightly more complicated and we do not present it here. See, e.g., [LVBL98] for more information.
3.4 Safe quadrotor trajectory planning
For higher order dynamical systems such as quadrotors, it is more desirable to plan entire safe trajectories rather that just finding a single safe point. We can plan smooth polynomial trajectories for each agent , such that the entire trajectory is inside each agent’s reciprocally safe set, . After the polynomial trajectory is found, the required control inputs can be found via differential flatness [MK11]. In order to generate these trajectories and verify that they are safe, we require two extensions on the original CARP formulation: (1) a way to find a polynomial that is entirely inside the , and (2) a method of expanding the uncertainty estimate to include the stopping set of other quadrotors.
Bézier curves.
Instead of the standard polynomial formulation, we use a Bézier curve to represent the polynomial trajectory. A th order 3dimensional Bézier curve, , is defined by a set of “control” points , for . We define the th order Bézier curve as a linear combination of Bernstein polynomials:
(11) 
This curve has the property that it always lies in the convex hull of the control points, i.e., for every . To find a polynomial of this form that is entirely inside the safe region, we can add additional constraints to (4) to constrain each control point to be inside the set . Finally, since the derivatives of a Bézier curve are linear combinations of the control points, we can account for the initial and desired final dynamic state (consisting of the position, velocity, and acceleration) by adding additional linear constraints to problem (4).
Stopping margins.
To include the stopping margins or each quadrotor, we simply expand the estimate , similar to [AM21b], by a sphere with radius , where and are the maximum velocity and acceleration of the quadrotor, respectively. This provides safe, yet conservative bound, for the stopping distance other agents will have during the planning horizon. In practice, if a velocity measurement is available then a more accurate inflation can be found. Since the original ellipsoidal estimate is only contains a point estimate of the other agents, we can expand the ellipsoid to account any number of arbitrary margins. Given a set of arbitrary ellipsoids , there exists an analytical method [BABD06, LZW16] that finds the smallest (in the sense of major axes length) external bounding ellipsoid for the Minkowski sum of all the ellipsoids . Thus we can, in a procedural and consistent manner, combine different margins.
Persistently safe receding horizon controller.
We extend problem (4) to account for additional constraints placed on the th order Bézier polynomial. To do this, we formulate the following optimization problem,
minimize  (12)  
subject to  
The optimization variables are and for , while is the current position, and and are the current and desired final velocities, respectively. For compactness, we write , and to be defined as in (10). As written, this finds a second long trajectory, but the section time for the trajectory can be changed by appropriately scaling the variable in (11). Initial and final accelerations can also be added to (12). Since this method is a refinement of (4) (as any solution to (12) is feasible for (4) with ) then this method is guaranteed to be reciprocally safe. In the case where the optimization is infeasible, the agent can fall back to the previously calculated safe trajectory. Figure 2 shows an example instance of a polynomial trajectory being generated inside the safereachable area.
4 Results
4.1 Projection Implementation
To get an accurate estimate of the speed of the projection algorithm, the optimization problem outlined in (4) was implemented^{1}^{1}1https://github.com/angeris/CARP.jl in the Julia language [BEKS17] using the JuMP mathematical programming language [DHL17] and solved using ECOS [DCB13]. We generated 285 instances of the problem, each with 100 randomly generated ellipsoids in . Timing and performance results for generating and solving the corresponding convex program can be found in table 1. Figure 3 shows how the performance scales as the number of other agents increases. All times reported are on a 2.9GHz 2015 dualcore MacBook Pro.
Time  Total (GC %) 

Minimum  13.20 (00.00%) 
Median  17.12 (00.00%) 
Mean  17.55 (08.80%) 
Maximum  36.77 (10.59%) 
4.2 Trajectory Simulations
The projection algorithm was implemented in both 2D and 3D with a varying number of agents. In this set up, each agent knows their own position exactly and maintains a noisy estimate of other agents’ positions, with uncertainties represented as ellipsoids. This estimate is updated by a setmembership based filter [BR71, LZW16, SS19]
, a variant of the Kalman filter. We expand the uncertainty ellipsoid by a given margin to account for the robot’s physical size. If this margin is also ellipsoidal, then a small ellipsoid which contains the Minkowski sum of the uncertainty ellipsoid and the margin can be found in closed form
[LZW16, SS19]. This new bounding ellipsoid is used in the projection algorithm to account for a user defined margin, along with the uncertainty ellipsoid containing the noisy sensor information.Figure 4 shows the minimum interagent distances for each agent in the simulation scenario mentioned above. The collision threshold was set to , twice the radius of the agents. Although our method results in longer paths, it remains collision free, while RVO’s paths result in collision.
Figure 5 shows six instances of a 3D simulation with 10 agents using the polynomial method. The agents start at the sides of a cube and are constrained to a maximum speed of and a maximum measurement error set to .The agents, displayed as quadrotors, each have a bounding box of and an additional ellipsoidal margin with an axis length of in the and dimensions, and in the dimension. This margin effectively gives a buffer of in the plane and a large buffer of in . We assume nonspherical margins in this simulation since, in the case of quadrotor flight, large margins in the direction can prevent unwanted effects due to downwash ([PHAS17]). The minimum interagent distance during the simulation, as measured from the centers of the agents, was .
4.3 Hardware Demonstrations
We implemented^{2}^{2}2https://github.com/StanfordMSL/carp_ros our collision avoidance the method on a set of 2 PX4 based quadrotor platforms (figure 1) to demonstate our method on a real robotic system^{3}^{3}3A video of the simulations and experiments can be found at https://youtu.be/ozbMovG4ow.. Each quad (see figure 6) is outfitted with a x86 based UP board and is tracked via a motion capture system. Each agent is sent its own position and maintains a noisy ellipsoidal estimate other agents’ positions, which are filtered by a setmembership filter [BR71, LZW16, SS19] and updated via noisecorrupted measurements. Planning and closed loop trajectory control, as well as estimation and filtering, are all done onboard. The planner is updated at , while the filter is run at . Lowlevel control is done with a feedforward PID velocity controller. We implemented both the direct goal projection method as well as the polynomial trajectory generation method. The quadrotors measure by by and are given an additional margin in the plane and a margin in the axis. Figures 7 and 8 show the distance between agents over the course of the flight, as well as the final trajectory and the intermediate trajectories for the projection method (figure 7) and the polynomial method (figure 8).
5 Closing Remarks
In this work, we presented a scalable system that can work with simple or complex, distributed or centralized highlevel planners to provide safe trajectories for a team of agents. Under the assumptions stated, we showed that collision avoidance is guaranteed provided each agent follows this method. However, we observe practical collision avoidance behavior even if only the ego agent follows this method. Computational performance results and simulations provide evidence that this algorithm can potentially be used in safetycritical applications for mobile robots with simple dynamics. Our open source library can also be used directly as a ROS package.
Acknowledgments. This work was supported in part by the FordStanford Alliance program, and by DARPA YFA award D18AP00064. Guillermo Angeris is supported by the National Science Foundation Graduate Research Fellowship under Grant No. DGE1656518.
References
 [AM21a] Senthil Hariharan Arul and Dinesh Manocha. Swarmcco: Probabilistic reactive collision avoidance for quadrotor swarms under uncertainty. IEEE Robotics and Automation Letters, 6(2):2437–2444, 2021.
 [AM21b] Senthil Hariharan Arul and Dinesh Manocha. Vrvo: Decentralized multiagent collision avoidance using voronoi diagrams and reciprocal velocity obstacles. arXiv preprint arXiv:2102.13281, 2021.
 [ASD12] F. Augugliaro, A. P. Schoellig, and R. D’Andrea. Generation of collisionfree trajectories for a quadrocopter fleet: A sequential convex programming approach. In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1917–1922, Oct 2012.
 [ASS19] Guillermo Angeris, Kunal Shah, and Mac Schwager. Fast Reciprocal Collision Avoidance Under Measurement Uncertainty. In 2019 International Symposium of Robotics Research (ISRR), Hanoi, Vietnam, 2019.
 [BABD06] Y. BecisAubry, M. Boutayeb, and M. Darouach. A stable recursive state estimation filter for models with nonlinear dynamics subject to bounded disturbances. In Proceedings of the 45th IEEE Conference on Decision and Control, pages 1321–1326, Dec 2006.
 [BCH14] S. Bandyopadhyay, S. Chung, and F. Y. Hadaegh. Probabilistic swarm guidance using optimal transport. In 2014 IEEE Conference on Control Applications (CCA), pages 498–505, Oct 2014.
 [BEKS17] Jeff Bezanson, Alan Edelman, Stefan Karpinski, and Viral B. Shah. Julia: A fresh approach to numerical computing. SIAM review, 59(1):65–98, 2017.
 [BR71] D. Bertsekas and I. Rhodes. Recursive state estimation for a setmembership description of uncertainty. IEEE Transactions on Automatic Control, 16(2):117–128, Apr 1971.
 [BV04] Stephen Boyd and Lieven Vandenberghe. Convex optimization. Cambridge university press, 2004.
 [CCH15] Yufan Chen, Mark Cutler, and Jonathan P How. Decoupled multiagent path planning via incremental sequential convex programming. In 2015 IEEE International Conference on Robotics and Automation (ICRA), pages 5954–5961, 2015.
 [CHTM12] D. Claes, D. Hennes, K. Tuyls, and W. Meeussen. Collision avoidance under bounded localization uncertainty. In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1192–1198, Oct 2012.
 [CR16] Jiahao Chen and Jarrett Revels. Robust benchmarking in noisy environments. arXiv preprint arXiv:1608.04295, 2016.
 [DCB13] Alexander Domahidi, Eric Chu, and Stephen Boyd. Ecos: An socp solver for embedded systems. In 2013 European Control Conference (ECC), pages 3071–3076. IEEE, 2013.

[DHL17]
Iain Dunning, Joey Huchette, and Miles Lubin.
JuMP: A modeling language for mathematical optimization.
SIAM Review, 59(2):295–320, 2017.  [GSK17] B. Gopalakrishnan, A. K. Singh, M. Kaushik, K. M. Krishna, and D. Manocha. Prvo: Probabilistic reciprocal velocity obstacle for multi robot navigation under uncertainty. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 1089–1096, Sep. 2017.

[HNR68]
P. E. Hart, N. J. Nilsson, and B. Raphael.
A formal basis for the heuristic determination of minimum cost paths.
IEEE Transactions on Systems Science and Cybernetics, 4(2):100–107, July 1968.  [JSCP15] Lucas Janson, Edward Schmerling, Ashley Clark, and Marco Pavone. Fast marching tree: A fast marching samplingbased method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 34(7):883–921, 2015.
 [LKJ01] Steven M LaValle and James J Kuffner Jr. Randomized kinodynamic planning. The international journal of robotics research, 20(5):378–400, 2001.
 [LVBL98] Miguel S. Lobo, Lieven Vandenberghe, Stephen Boyd, and Hervé Lebret. Applications of secondorder cone programming. Linear algebra and its applications, 284(13):193–228, 1998.
 [LZW16] Yushuang Liu, Yan Zhao, and Falin Wu. Ellipsoidal stateboundingbased setmembership estimation for linear system with unknownbutbounded disturbances. IET Control Theory & Applications, 10(4):431–442, feb 2016.
 [MCH14] Daniel Morgan, SoonJo Chung, and Fred Y Hadaegh. Model predictive control of swarms of spacecraft using sequential convex programming. Journal of Guidance, Control, and Dynamics, 37(6):1725–1740, 2014.
 [MJWP16] X. Ma, Z. Jiao, Z. Wang, and D. Panagou. Decentralized prioritized motion planning for multiple autonomous uavs in 3d polygonal obstacle environments. In International Conference on Unmanned Aircraft Systems (ICUAS), pages 292–300, 2016.
 [MK11] Daniel Mellinger and Vijay Kumar. Minimum snap trajectory generation and control for quadrotors. In 2011 IEEE international conference on robotics and automation, pages 2520–2525. IEEE, 2011.
 [PHAS17] J. A. Preiss, W. Hönig, N. Ayanian, and G. S. Sukhatme. Downwashaware trajectory planning for large quadrotor teams. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 250–257, Sep. 2017.
 [ŞHA19] Baskın Şenbaşlar, Wolfgang Hönig, and Nora Ayanian. Robust trajectory execution for multirobot teams using distributed realtime replanning. In Distributed Autonomous Robotic Systems, pages 167–181. Springer, 2019.

[SHL13]
John Schulman, Jonathan Ho, Alex X Lee, Ibrahim Awwal, Henry Bradlow, and
Pieter Abbeel.
Finding locally optimal, collisionfree trajectories with sequential
convex optimization.
In Robotics: science and systems
, volume 9, pages 1–10. Citeseer, 2013.
 [SS19] Kunal Shah and Mac Schwager. Multiagent cooperative pursuitevasion strategies under uncertainty. In Distributed Autonomous Robotic Systems, pages 451–468. Springer, 2019.
 [VdBGLM11] Jur Van den Berg, Stephen Guy, Ming Lin, and Dinesh Manocha. Reciprocal nbody collision avoidance. Springer Tracts in Advanced Robotics, 70:3–19, 04 2011.
 [VdBGS16] Jur Van den Berg, Stephen Guy, Jamie Snape, Ming Lin, and Manocha. Rvo2 library: Reciprocal collision avoidance for realtime multiagent simulation, 2016.
 [VdBLM08] Jur Van den Berg, Ming Lin, and Dinesh Manocha. Reciprocal velocity obstacles for realtime multiagent navigation. In 2008 IEEE International Conference on Robotics and Automation, pages 1928–1935. IEEE, 2008.
 [WC11] G. Wagner and H. Choset. M*: A complete multirobot path planning algorithm with performance bounds. In 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 3260–3267, Sep. 2011.
 [ZA19] H. Zhu and J. AlonsoMora. Chanceconstrained collision avoidance for mavs in dynamic environments. IEEE Robotics and Automation Letters, 4(2):776–783, 2019.
 [ZWBS17] Dingjiang Zhou, Zijian Wang, Saptarshi Bandyopadhyay, and Mac Schwager. Fast, online collision avoidance for dynamic vehicles using buffered voronoi cells. IEEE Robotics and Automation Letters, 2(2):1047–1054, 2017.