DeepAI
Log In Sign Up

Reciprocal Multi-Robot Collision Avoidance with Asymmetric State Uncertainty

We present a general decentralized formulation for a large class of collision avoidance methods and show that all collision avoidance methods of this form are guaranteed to be collision free. This class includes several existing algorithms in the literature as special cases. We then present a particular instance of this collision avoidance method, CARP (Collision Avoidance by Reciprocal Projections), that is effective even when the estimates of other agents' positions and velocities are noisy. The method's main computational step involves the solution of a small convex optimization problem, which can be quickly solved in practice, even on embedded platforms, making it practical to use on computationally-constrained robots such as quadrotors. This method can be extended to find smooth polynomial trajectories for higher dynamic systems such at quadrotors. We demonstrate this algorithm's performance in simulations and on a team of physical quadrotors. Our method finds optimal projections in a median time of 17.12ms for 285 instances of 100 randomly generated obstacles, and produces safe polynomial trajectories at over 60hz on-board quadrotors. Our paper is accompanied by an open source Julia implementation and ROS package.

READ FULL TEXT VIEW PDF

page 2

page 20

05/30/2019

Fast Reciprocal Collision Avoidance Under Measurement Uncertainty

We present a fully distributed collision avoidance algorithm based on co...
01/11/2022

Decentralized Probabilistic Multi-Robot Collision Avoidance Using Buffered Uncertainty-Aware Voronoi Cells

In this paper, we present a decentralized and communication-free collisi...
02/17/2014

Conservative collision prediction and avoidance for stochastic trajectories in continuous time and space

Existing work in multi-agent collision prediction and avoidance typicall...
09/09/2019

DCAD: Decentralized Collision Avoidance with Dynamics Constraints for Agile Quadrotor Swarms

We present a novel, decentralized collision avoidance algorithm for navi...
10/14/2019

On the Effects of Collision Avoidance on Emergent Swarm Behavior

Swarms of autonomous agents, through their decentralized and robust natu...
07/12/2019

NH-TTC: A gradient-based framework for generalized anticipatory collision avoidance

We propose NH-TTC, a general method for fast, anticipatory collision avo...

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 well-known, 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 on-board 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 safe-reachable set for itself, which is characterized by a generalized Voronoi cell. Our algorithm then computes a projection onto this safe-reachable 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’ safe-reachable 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.

Figure 1: Two quadrotors executing a reciprocal collision avoidance maneuverer each agent (red, blue) maintains an ellipsoidal estimate of the other’s position. The agents move towards their goal point (square) projecting (triangle) it into their safe-reachable set. The solid curve shows the trajectory history and the dotted curve shows the path to the projected point.

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 on-board, 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 on-board 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 multi-agent path planning tasks [BCH14], but still require communication to build an estimate via consensus. The V-RVO 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 safe-reachable 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, on-board 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 small-to-medium 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 rapidly-exploring random trees (RRTs) [LKJ01] can be applied to the multi-agent case, but solution times grow rapidly due to the exploding size of the joint state space. For example, graph-search methods can be partially decoupled [WC11] to better scale for larger multi-agent systems, but can explore the entire joint state-space 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 multi-agent 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 multi-agent path planning; however, these algorithms are still centralized and may exhibit slow convergence, making them unreliable for on-line 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 collision-free, assuming the agents start out in a collision-free 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 collision-free, assuming the agents start from a collision-free 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 one-step 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 on-board 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 on-board 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 distance-to-set 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 single-integrator 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 overly-conservative 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 well-known 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 well-known 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 well-known.) 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 second-order 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 quadratic-over-linear terms, which are easily representable as second-order 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 3-dimensional 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 safe-reachable area.

Figure 2: Polynomial trajectory (blue, dotted) generated for an agent (blue circle) with non zero initial velocity (blue arrow) to avoid the ellipsoidal estimate of the obstacle (red ellipsoid) expanded by a margin (blue ellipsoid). The agent goal position (blue square) and the Bézier control points (black stars) are also shown.

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 implemented111https://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 dual-core MacBook Pro.

Figure 3: Graph showing total time for generating and solving optimization problem (4) as a function of the number of ellipsoids in the problem, when solved using the ECOS solver. Note the logarithmic scales on both axes.
Time Total (GC %)
Minimum 13.20 (00.00%)
Median 17.12 (00.00%)
Mean 17.55 (08.80%)
Maximum 36.77 (10.59%)
Table 1: Timings with garbage collection (GC) as a percentage of time spent building and solving problems with 100 randomly generated 3D ellipsoids. Statistics are based on 285 instances and were obtained from the BenchmarkTools.jl [CR16] package.

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 set-membership 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 inter-agent 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 4: Inter-agent distances RVO (top) and our method (bottom). The RVO simulation results in inter-agent distances below the collision threshold (black line).

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 non-spherical 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 inter-agent distance during the simulation, as measured from the centers of the agents, was .

Figure 5: Six time instances of a 3D simulation of 10 agents. Each agent adds an ellipsoidal margin (shown) elongated in the z-axis to account for downwash affects.

4.3 Hardware Demonstrations

Figure 6: Quadrotor equipped with a PX racer and UP Board companion computer.

We implemented222https://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 system333A video of the simulations and experiments can be found at https://youtu.be/oz-bMovG4ow.. 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 set-membership filter [BR71, LZW16, SS19] and updated via noise-corrupted measurements. Planning and closed loop trajectory control, as well as estimation and filtering, are all done on-board. The planner is updated at , while the filter is run at . Low-level control is done with a feed-forward 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).

Figure 7: Projection only method: Inter-agent distance (top) and trajectory locus (bottom) for two quadrotors. The minimum distance between agents is . The locus plot show the reprojected point (triangle) and goal point (square) over the course of the flight.
Figure 8: Bézier polynomial method: Inter-agent distance (top) and trajectory locus (bottom) for two quadrotors. The minimum distance between agents is . The locus plot show the trajectory (dotted) and control points (star) as well as the final goal point (square) over the course of the flight.

5 Closing Remarks

In this work, we presented a scalable system that can work with simple or complex, distributed or centralized high-level 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 safety-critical 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 Ford-Stanford Alliance program, and by DARPA YFA award D18AP00064. Guillermo Angeris is supported by the National Science Foundation Graduate Research Fellowship under Grant No. DGE-1656518.

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. V-rvo: Decentralized multi-agent 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 collision-free 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. Becis-Aubry, 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 set-membership 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 sampling-based 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 second-order cone programming. Linear algebra and its applications, 284(1-3):193–228, 1998.
  • [LZW16] Yushuang Liu, Yan Zhao, and Falin Wu. Ellipsoidal state-bounding-based set-membership estimation for linear system with unknown-but-bounded disturbances. IET Control Theory & Applications, 10(4):431–442, feb 2016.
  • [MCH14] Daniel Morgan, Soon-Jo 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. Downwash-aware 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 multi-robot teams using distributed real-time 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, collision-free trajectories with sequential convex optimization. In Robotics: science and systems

    , volume 9, pages 1–10. Citeseer, 2013.

  • [SS19] Kunal Shah and Mac Schwager. Multi-agent cooperative pursuit-evasion 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 n-body 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 real-time multi-agent simulation, 2016.
  • [VdBLM08] Jur Van den Berg, Ming Lin, and Dinesh Manocha. Reciprocal velocity obstacles for real-time multi-agent 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. Alonso-Mora. Chance-constrained 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, on-line collision avoidance for dynamic vehicles using buffered voronoi cells. IEEE Robotics and Automation Letters, 2(2):1047–1054, 2017.