V-RVO: Decentralized Multi-Agent Collision Avoidance using Voronoi Diagrams and Reciprocal Velocity Obstacles
VV-RVO: Decentralized Multi-Agent Collision Avoidance using VoronoiDiagrams and Reciprocal Velocity Obstacles
Senthil Hariharan Arul and Dinesh Manocha Abstract — We present a decentralized collision avoidancemethod for dense environments that is based on bufferedVoronoi cells (BVC) and reciprocal velocity obstacles (RVO).Our approach is designed for scenarios with large number ofclose proximity agents and provides passive-friendly collisionavoidance guarantees. The Voronoi cells are superimposed withRVO cones to compute a suitable direction for each agentand we use that direction for computing a local collision-freepath. Our approach can satisfy double-integrator dynamicsconstraints and we use the properties of the BVC to formulate asimple, decentralized deadlock resolution strategy. We demon-strate the benefits of V-RVO in complex scenarios with tens ofagents in close proximity. In practice, V-RVO’s performanceis comparable to prior velocity-obstacle methods and thecollision avoidance behavior is significantly less conservativethan ORCA.
I. I
NTRODUCTION
Recent advancements in multi-agent robotics support itsuse in last-mile delivery, warehouse inventory management,and urban surveillance. Many of these applications use alarge number of robots (e.g., a few hundred) in a decentral-ized manner [1]. A key challenge in these scenarios is multi-agent navigation, which includes the computation of safe,collision-free paths between agents in a shared environment.Prior multi-agent methods compute collision-free trajecto-ries using centralized or decentralized methods. Centralizedalgorithms such as [2], [3], [4] can generate collision-freetrajectories simultaneously for all agents by considering themas one composite system. These techniques can provide rig-orous guarantees in terms of (probabilistic) completeness ordeadlock avoidance. In the worst case, their computation timecan increase exponentially with the number of agents [5],[6]. In practice, these methods are used for a few agents.In contrast, in a decentralized algorithm, each agent makesan independent decision to avoid an impending collision [7],[8], [9], [10]. As a result, these algorithms are scalable andcan handle a large number of agents. However, guaranteeingcollision avoidance for decentralized methods is non-trivial,especially for agents with higher-order dynamics constraints.
A. Prior Work
There is extensive work on developing decentralizedcollision-avoidance methods for multi-agent simulation, *This work was not supported by any organization Senthil Hariharan Arul is with the Department of Electrical and Com-puter Engineering, University of Maryland, College Park, MD 20740, USA [email protected] Dinesh Manocha is with the Department of Computer Science, Univer-sity of Maryland, College Park, MD 20740, USA [email protected] where each agent computes collision-free paths indepen-dently using local sensing of state. Our approach is basedon the concept of velocity obstacles. Velocity Obstacle(VO) [11] computes a set of velocities that could result in acollision between agents or with dynamic obstacles. RVO [7]extends the VO concept by assuming that the circular agentsshare equal responsibility for collision avoidance by com-puting velocity cones for each pair of nearby agents. In theORCA algorithm [8], the RVO constraints are linearized toreduce the feasible velocities to a convex set (i.e. ORCAconstraints), and linear programming is used to quickly finda feasible solution. ORCA provides a sufficient conditionfor collision-free navigation for single integrator dynamics.The simplicity of the ORCA formulation makes it possible toextend that algorithm to elliptical agents [12], double integra-tors [13], linear agents [14], [15], non-holonomic agents [16],combining with statistical inference techniques [17], etc.However, the linear constraints in the velocity space tend tobe overly conservative, especially when many agents are inclose proximity or in dense scenarios with a large number ofagents. As a result, there may be no feasible solution to theORCA constraints and the resulting algorithms are unable tocompute collision-free trajectories. As we take into accountadditional constraints corresponding to high-order dynamicsor non-holonomic agents, the size of the feasible solutionset decreases further and makes the approach even moreconservative. Other methods have been proposed to acceler-ate the performance in crowded or challenging environmentsusing interpolating bridges in the narrow passages [18].In this paper, we present a new algorithm, V-RVO, thatis less conservative than ORCA and has similar runtimeperformance.Buffered Voronoi cell (BVC) [9] is an efficient decentral-ized method, which can compute collision-free trajectoriesfor single integrator agents. In BVC, collision avoidance isperformed by reasoning in the position space using only theagent’s position information. Robots deviate from the goaldirection only when the agents are in close proximity. Thisis in contrast to velocity obstacle methods, which identifythat the current velocity would result in future collision andperform an avoidance maneuver. BVC’s guarantees need nottranslate to high-order dynamics because the agent’s boundedacceleration could regard a computed path as infeasible.An MPC planner that uses BVC and the braking distancefor navigation in noisy scenarios has been proposed [19].This is similar to our method where we use BVC andbraking distance for passive guarantees . A decentralizedmulti-agent rapidly-exploring random tree (DMA-RRT) is a r X i v : . [ c s . R O ] F e b resented in [20], where an RRT path planner and a tokenpassing method are used for replanning.Other techniques for collision avoidance between multipleagents are based on a time-to-collision model [10], [21].Another guaranteed technique is based on inevitable collisionstates (ICS) [22] and computes a set of agent states thathave no collision-free trajectories for an infinite time horizon.ICS provides a theoretical guarantee in terms of collisionavoidance, but this method can be very conservative andcould regard the entire workspace as forbidden, even in thepresence of a few agents [23]. [24] considers a relaxed safetycondition known as passive motion safety, where agentscannot collide if they have a non-zero velocity. B. Main Contributions
We present a novel multi-agent collision avoidance algo-rithm (V-RVO) that combines the benefits of velocity-spacemethods (e.g., velocity obstacles) and position-based meth-ods (e.g., BVC). We use the velocity obstacle constraintsfor each agent to avoid a collision by reasoning over atime-horizon.At the same time, we exploit the characteristicsof BVC to guarantee collision avoidance for higher-orderdynamics.In our method, each agent computes a buffered Voronoicell (BVC) based on the neighbor’s position information.We superimpose the RVO cones constraints on the BVC tocompute an appropriate goal point on the BVC boundary.Next, we compute a shrunken
BVC by considering theagent’s (and the moving obstacles’) stopping distance. Werefer to this retracted Voronoi Cell as the braking-awarebuffered
Voronoi cell (baBVC). We compute the brakingdistance and control input by using the kinematic equationsfor each agent. Our V-RVO method computes a safe pathfor each agent inside the baBVC. We prove that baBVChas the property that any agent on the edges of baBVCcan incorporate a braking maneuver to halt before reachingthe BVC edges. We leverage the property that an agent’sBVC is disjoint from its neighbors to provide the collisionavoidance guarantees for agents with single and double inte-grator dynamics. Furthermore, we propose a simple deadlockresolution strategy. As compared to prior methods, V-RVOhas the following benefits: • V-RVO retains many benefits of ORCA, like handlinghigh-order dynamics and anticipatory collision avoid-ance. The use of baBVC makes it possible to com-pute collision-free trajectories in dense scenarios, whereORCA is either overly conservative or fails (see Figs. 7). • Unlike ORCA and BVC, V-RVO can provide safetyguarantees for second order agents (Figure 6).We have implemented our method and compared its per-formance with ORCA on agents with single- and second-order dynamics for dense scenarios. The average runningtime for V-RVO is a few milliseconds per agent and is about X slower than ORCA. In practice, V-RVO can computecollision-free trajectories for many challenging benchmarkswith − agents, where ORCA tends to fail. TABLE I: Symbols and notation used in the paper Notation Definition A i Refers to the i th agent p i A i [ p i,x , p i,y ] g i A i v i A i [ v i,x , v i,y ] u i Control Input of the A i [ a i,x , a i,y ] R i Radius of A i ’s enclosing circle N i Neighbors of A i ¯ N i Neighbors sharing a Voronoi edge with A i V i , ¯ V i Voronoi and Buffered Voronoi cell for A i ∂ ¯ V i Boundary of the set ¯ V i (cid:15) p , (cid:15) v Small positive constants
II. B
ACKGROUND AND P ROBLEM F ORMULATION
In this section, we give an overview of various conceptsused in our approach. The symbols and notations used in thepaper are summarized in Table 1.
A. Problem Formulation
We consider an environment with N agents moving ina shared workspace W ⊂ R . Each agent A i , where i ∈{ , , ...N } , has its bounding geometric shape approximatedas a circle of radius R i . Two agents A i and A j are collision-free if (cid:107) p i − p j (cid:107) ≥ R i + R j . That is, the agents are collision-free if the inter-agent dis-tance between A i and A j is greater than the sum of theirradii. Our goal is to compute a collision-free path for eachagent towards its goal position in a decentralized manner.Furthermore, our goal is to provide passive-friendly collisionavoidance guarantees for agents with higher-order dynamics. B. Assumptions
We assume each agent has the exact state information,including positions and velocities, of the neighboring agents.The environment may consist of other static and dynamic ob-stacles. The exact position of the dynamic obstacle is known,though no assumptions are made about its trajectories. Wealso assume that the maximum velocity and acceleration ofsuch agents are known a priori.
Fig. 1: The figure illustrates a navigation scenario with four agents movingto their antipodal positions. The three RVO cones, corresponding to threeneighbors of A i , are denoted by the blue conical regions and are super-imposed onto the gray BVC region. The p goal (green point) on the BVCboundary is in the direction of the goal ( g i ). Since this point lies in theRVO cone, we compute the closest point on the BVC boundary outside theRVO cones. Point p rvo is denoted in blue and the agent is directed towardsthis point to avoid a collision. . Passive Safety GuaranteesPassive safety is a relaxed safety condition where the agentcannot collide while it is in motion [24]. A collision canoccur only in situations where the agent is at rest and amoving obstacle collides with the agent. In passive-friendlysafety , in addition to passive safety , the agent providessufficient time for the moving obstacle to stop or replan toavoid collisions. However, a collision with a moving obstacleis possible if they plan on colliding. Passive-friendly safetyalso helps to operate safely in the presence of heterogeneousagents that do not use V-RVO. D. Buffered Voronoi Cell (BVC)
We use the notion of computing Buffered Voronoi Cell(BVC), proposed by [9]. BVC is a contracted Voronoi regionsuch that an agent whose center point is on the edge ofa BVC has its bounding circle inside the Voronoi cell.Given N agents on a 2-D plane, the buffered Voronoi cellcorresponding to an agent A i is given as ¯ V i = (cid:26) p ∈ R | (cid:18) p − p i + p j (cid:19) T p ij + R i (cid:107) p ij (cid:107) ≤ , ∀ j (cid:54) = i (cid:27) . (1) Here, p ij = p i − p j . The method plans a collision-free pathfor the agent A i that is constrained to lie within ¯ V i . E. Reciprocal Velocity Obstacle
Reciprocal Velocity Obstacle (RVO) computes a set ofrelative velocities between two agents that can result in afuture collision. Let us consider two agents A i and A j in ashared workspace W . The RVO can be geometrically definedas RV O ij ( v j , v i ) = { v |∃ t ∈ [0 , τ ] :: t v ∈ D ( p ji , R ij ) } . (2)Here, D ( P , r ) = { q | (cid:107) q − p (cid:107) ≤ r } represents a disk ofradius r and center p . The variable p ji represents the relativeposition given by p j − p i and R ij represents the combinedradius given by R i + R j . If A i chooses a velocity outside theRVO induced by A j , the agent is guaranteed to be collision-free, provided the trajectories of both agents are governedby single-integrator dynamics.III. V-RVO: O UR H YBRID N AVIGATION A LGORITHM
In this section, we describe our multi-agent navigationmethod. This includes computing the BVC for each agent,followed by RVO superposition and baBVC computation.Algorithm 1 highlights our proposed method.
A. RVO Superposition
In our method, we combine RVO constraints with BVCto compute a collision avoiding direction. Let us consider anagent A i . At each time step, the agent constructs its BVC( ¯ V i ), and for every A j ∈ N i we construct an RVO conew.r.t. each nearby agent. Because the RVO constraints (Equa-tion II-E) are defined in the relative velocity space, each coneis transformed into the velocity space of A i . The origin ofthe velocity space is the center of A i . Thus, the RVO conesare superimposed onto the constructed BVC with p i as theorigin for the velocity space. The union of the ∂ ¯ V i sections outside the RVO conesprovides safe directions of computing the velocity, as theyare outside the RVO cones and within the BVC. We denotethis union of ∂ ¯ V i sections by ∂ ¯ V CF,i and expressed as: ∂ ¯ V CF,i = ∂ ¯ V i − (cid:91) j ∈N i ∂ ¯ V i ∩ RV O ij ( v j , v i ) . (3)A point in the set ∂ ¯ V CF,i with the least angular deviationfrom the goal direction is computed and is denoted by p rvo,i : p rvo,i = argmin p ∈ ∂ ¯ V CF,i arccos (cid:18) p . ( p i − g i ) (cid:107) p (cid:107) . (cid:107) p i − g i (cid:107) (cid:19) . (4) The agent A i moves towards the p rvo,i to avoid collisionswith other agents. In order to provide passive-friendly guar-antees, the agent’s trajectory is planned such that it can brakeand stop at the point p rvo,i . Figure 1 illustrates a simplescenario with four agents. B. Braking-aware BVC
As mentioned in Section II-D, the BVC is generated bybuffering the Voronoi cell edges by the agent’s radius. Sincean agent with single integrator dynamics can instantaneouslychange its velocity, the agent is guaranteed to be collision-free inside the BVC. For agents with double integratordynamics the BVC is further buffered based on the minimumbraking distance of the agent and the moving obstacle.We use the kinematic equations of the agent to computethe buffering distance, as shown in Figure 2. Let us considera time horizon t h . The agent A i moves to a position p rvo,i as computed using RVO constraints. Since the agent has tohalt at a point on the edge of the BVC to remain withinits Voronoi cell, we plan a path from p i to p rvo,i using thekinematic constraints. Consider a third position p int,i withinthe BVC such that the agent at p int,i with a velocity v int,i can brake and stop at p rvo,i in time t b . We compute thevelocity v int,i by solving the problem separately in X and Y coordinates. s int,x = v x,i t h + 12 t h (cid:0) v int,x,i − v x,i (cid:1) t h , (5) s stop,x = v int,x a max . (6)Here, s int,x is the distance travelled while converting the ve-locity from v x,i to v int,x , and s stop,x is the distance travelledwhen the agent decelerates to a stop. Using Equations (5)and (6) along with the relationship s x = s int,x + s stop (also there abs sum should be sx), we arrive at the followingquadratic equation. v int,x,i + ( a max t h ) .v int,x,i + ( u x,i a max t h − a max s ) . (7)The velocity v int,i is computed from the quadratic equationby choosing the velocity with the least angular deviationfrom the goal direction. The position p int,i can be computedusing v int,i .We assume low-velocity moving obstacles are present inthe environment. The BVC is buffered by a distance d obs ig. 2: We show an agent (red circle), Voronoi cell (gray region) and theBVC (dark gray) computed with the agent’s radius. p int and v int are theposition and velocity at an intermediate point computed from Equation 7. s int and s stop are the displacement vectors. to provide enough stopping distance for the low-velocitymoving obstacle, which is computed as: d obs = s O − v max,obs a max . (8)Here, S O denotes the shortest distance between the ob-stacle’s center and ∂ ¯ V i . We use this buffered BVC as theinput from computing p rvo,i in Section III-A. This helpsprovide passive-friendly collision avoidance guarantees. Thisresults in a contracted BVC region, which we refer to asbraking-aware buffered Voronoi cell (baBVC). We assume S O ≥ v max,obs a max for this computation to maintain d obs ≥ . C. Multi-Agent Navigation
In a multi-agent scenario, all agents compute the positions p rvo,i and p int,i as described in Sections III-A and III-B.For agents with single integrator dynamics, the positions p rvo,i = p int,i as the velocity can be instantaneouslychanged to zero. The control input (velocity) applied to theagent is given as u i = (cid:107) v max (cid:107) p rvo,i − p i (cid:107) p rvo,i − p i (cid:107) . (9)If (cid:107) p rvo,i − p i (cid:107) ≤ , we apply u i = p rvo,i − p i .For scenarios with double integrator agents, the controlinput ( u i ) is acceleration. A constant acceleration for thetime horizon t h is computed as follows: u i = v int,i − v i t h . (10)For double integrator agents, we also consider accelerationbounds. The path between p int and p i may not be a straightline. Thus, we verify that next position p (cid:48) t,i planned forevery time step t in [0 ...t h ] (considering control input fromEquation 10) lies within ¯ V i . t h can be varied to account forthe acceleration bound. D. Collision-Free Guarantees
We prove that agents trajectories computed using V-RVOcan provide passive-safety guarantees.
Theorem 1:
Agents navigating in multi-agent scenariosusing Algorithm I are guaranteed to be collision-free.
Proof:
Let us consider N agents operating in a workspace W . Let V i be the Voronoi region corresponding to agent A i .BVC region ( ¯ V i ) is constructed by retracting the Voronoi Algorithm 1
V-RVO Navigation Algorithm
Input : p i , v i i ∈ N Output : u i i ∈ N for i ∈ N do
2: Compute BVC boundary ( ∂ ¯ V i )3: Compute RVO cones4: ∂ ¯ V CF,i = ∂ ¯ V i for j ∈ RVO Cones do ∂ ¯ V CF,i ← ∂ ¯ V CF,i ∩ BVC edge region not inside j th RVOcones { // compute collision-free ∂ ¯ V section } end for p rvo,i = argmin p ∈ ∂ ¯ V CF,i arccos (cid:18) p . ( p i − g i ) (cid:107) p (cid:107) . (cid:107) p i − g i (cid:107) (cid:19) { // positionin ∂ ¯ V CF,i with least angular deviation from goal direction }
9: Compute p slow,i , v slow,i from Equation (7)10: u i = v slow,i − v i t h { // compute control input } end for return u i edges by a distance equal to the agent’s radius. Thus, ¯ V i ⊂V i and BVC region between the agents is disjoint. That is, ¯ V i ∩ ¯ V j = ∅ ∀ i, j ∈ N, j (cid:54) = i .Consider the agents at their initial position p i ∀ i ∈ N (at t = t ). Let the corresponding region occupied by the agentgeometry be A ( p i ) . Assuming agents are collision-free at t = 0 , and from the definition of BVC (cid:107) p i − p j (cid:107) ≥ R i + R j ∀ j ∈ N/ { i } = ⇒ A ( p i ) ⊂ V i ⇐⇒ p i ∈ ¯ V i . (11)Provided A i chooses its next position p (cid:48) i ∈ ¯ V i , then A ( p (cid:48) i ) ⊂ V i and the agent continues to be collision freein the next time step.For a single integrator agent, the velocity ( v i ) can beinstantaneously modified. Hence, the path between p i and p (cid:48) i is a straight line ( p i p (cid:48) i ). The agent’s path is collision-freefrom the convexity of ¯ V i as p i , p (cid:48) i ∈ ¯ V i = ⇒ p i p (cid:48) i ⊂ ¯ V i = ⇒ A i ( ) ⊕ p i p (cid:48) i ⊂ V i . Here, is the origin and ⊕ denotes Minkowski sum.For double integrator agents, we compute a constantacceleration from Equation (10) applied for duration t h . Theacceleration moves the agent to an intermediate point p int,i such that the agent can decelerate to a stop at p rvo,i ∈ ∂ ¯ V i .Computing the agent’s future positions p (cid:48) t,i using (10) suchthat p (cid:48) t h ,i = p int,i and assuming a straight-line movementbetween time-steps. The agent’s path is collision-free as p (cid:48) t,i ∈ ¯ V i = ⇒ A ( p (cid:48) t,i ) ⊂ V i = ⇒A i ( ) ⊕ p (cid:48) t − ,i p (cid:48) t,i ⊂ V i ∀ t ∈ [1 ...t h ] . (12)The above conditions when satisfied provides passive collision avoidance guarantees. Since all agents in theworkspace W use V-RVO, the agents would brake to stop ata point on ∂ ¯ V i . Thus, agent do not collide with each other.In the presence of low-velocity moving obstacles, the BVCis initially buffered as mentioned in Equation III-B. Thisprovides a buffered distance for the moving-obstacle to brakewithout colliding. In this manner, we extend the passiveguarantees to the passive-friendly. ig. 3: (Left) image illustrates a navigation scenario with 4 agents (red disks) and their current velocities (denoted by arrows). (Center) image illustratesthe feasible velocity set for A i computed using linear ORCA constraints. In the (right) image the agent’s local trajectory is computed using our V-RVOalgorithm. The green conical region are the RVO constraints superimposed onto the BVC. The feasible regions and directions computed using V-RVO(shown in dark grey ) is larger than that compared to ORCA (center image).Fig. 4: A Dense scenario with 10 agents (circles). The agent A i isdeadlocked and unable to reach its goal location g i as its neighbors havereached their goal position. The neighbors in HOLD mode are shown in green color. A i chooses a neighbor and switches its location following the red path. The gray region corresponds to their respective BVCs. IV. D
EADLOCK
In this section, we present our distributed deadlock resolu-tion strategy based on the constructed BVC. An illustrativeexample is provided in Figure 4, and the pseudo-code issummarized in Algorithm 2.
A. Deadlock Identification
Deadlock occurs in situations where multiple agents blockeach other such that one or more agents are unable to reachtheir goal, and instead remain stationary (with zero velocity)to avoid a collision. In our method, each agent chooses apoint on the boundary of its baBVC and travels towards it.When an agent is in a deadlock, it is yet to reach its goalposition, and it is at a point on ∂ ¯ V i with a zero velocity. B. Deadlock Resolution
To resolve deadlocks, we define three operating modes forthe agents. Each agent A i can be in one of the three modes: DEADLOCK , HOLD , or
DEFAULT . In general, all agents arein the
DEFAULT mode, where they are either moving towardstheir goal, avoiding collisions, or have reached their goalposition. The agents in the
HOLD mode have their velocitiesset to zero and thus remain at the current position until themode is modified to
DEFAULT . When an agent is identifiedto be in a deadlock (as mentioned in the Section IV-A), theagent’s mode is modified to
DEADLOCK .Consider an agent A i whose mode corresponds to DEADLOCK . If the agent A i has neighboring agents witheither DEADLOCK or DEFAULT modes with zero velocity,then it initiates a switch to potentially solve the deadlock. • The agent initially identifies the neighboring agent ( A k )that is closest to the direction of the agent’s goallocation. Since A i and A k are neighboring agents, theyshare a common Voronoi edge. • The states of the neighbors ¯ N i and ¯ N k are set to HOLD .Since the A i and the agents in ¯ N i share Voronoi edges,setting the agents in ¯ N i ∪ ¯ N k to HOLD ensures that theunion of BVCs of A i and A k could be used for thesubsequent time steps. • We plan a path between the agents A i and A k acrosstheir common Voronoi edge such that the positions of A i and A k are interchanged. (Algorithm 2, line 5). • Once this position switch is completed, the agent stateis reset to
DEFAULT . Algorithm 2
Deadlock Resolution if (cid:107) p i − g i (cid:107) ≥ (cid:15) p and (cid:107) v i (cid:107) ≤ (cid:15) v then Mode i = DEADLOCK k = argmin j ∈N i arccos (cid:18) ( p i − g i ) . ( p i − p j ) (cid:107) p i − g i (cid:107) . (cid:107) p i − p j (cid:107) (cid:19) Mode j = HOLD ∀ j ∈ ¯ N i ∪ ¯ N k SwitchAgent ( i, k ) Mode j = DEFAULT ∀ j ∈ ¯ N i ∪ ¯ N k ∪ i ∪ k end if V. R
ESULT
In this section, we describe our implementation and high-light the performance of V-RVO in different scenarios. Wealso compare with ORCA.
A. Experimental Setup
We perform our evaluation on a . GHz Quad-Core IntelCore i processor with GB of memory. The agents andobstacles in our scenarios are circular agents of radius . m.The agents have maximum velocity and acceleration of m/s and m/s respectively. We use a time step of 0.1 secondsfor our simulation results. B. Simulation Results
We evaluate our method on simulation in a circular sce-nario, where each agent moves to its antipodal goal position.We vary the number of agents and increase the density in thescenario. Figures (5) and (6) illustrates the agent trajectoriesfor single and double integrator dynamics, respectively.In addition, we compared V-RVO and ORCA on a scenariowith 70 agents (Fig. 7). While V-RVO can handle such ig. 5: Collision-free trajectories for single integrator dynamics ( ˙ p = u ).The sub images represent scenarios with 4 agents (left), 10 agent (center),and 25 agents (right).Fig. 6: Collision-free trajectories for double integrator dynamics ( ¨ p = u ).The sub images represent scenarios with 4 agents (left), 10 agent (center),and 25 agents (right). ORCA cannot compute collision-free trajectories forthe 25-agent scenario. complex scenario, ORCA results in collisions. This is dueto the fact that ORCA constraints are not satisfied andit computes the closest velocity, which may not guaranteecollision-free navigation. Fig. 7: Trajectories for a scenario with agents. V-RVO (left) cangenerate collision-free trajectories for all the agents. ORCA (right) resultsin collisions, as shown with red agents. We also evaluated V-RVO in a scenarios with agentsapproaching with head on collisions. Figure 8 illustrates thecollision-free trajectories.
C. Deadlock Resolution
We evaluate the performance of our deadlock resolutionmethod on the formation benchmark. There are agentsand they are initially placed on the circle perimeter equallyspaced. The final positions of the agents are arranged in a × grid formation (see Fig. 10). Our deadlock resolutionmethod works well and the agents can reach the goal positionin Fig. 10 (right). D. Scalability
We compare the time required to compute the controlinputs for the agent in scenarios with a team size rangingfrom to agents. Each agent considers only its neighborswithin its sensing region for the computation. The average Fig. 8: We highlight the agent trajectories computed using V-RVO with 4pairs. The pink regions denote the BVCs associated with each agent duringeach time step. The left, center, and right images show the BVCs and thetrajectories in chronological order. t = 4 seconds t = 8 secondsORCAt = 4 seconds t = 8 secondsVRVO
Fig. 9: Image illustrates a three agent scenario where two red agents travelfrom right to left, while the blue agent travels from left to right. Thetrajectory of the agents are shown using colored disks where the agent’spositions for recent time steps are denoted by darker color shades. Whileusing ORCA, the blue agent is stays at the same position for several timesteps due to ORCA’s conservative nature. In contrast, while using VRVOthe blue agent deviates to avoid a collision. time per agent is in the range − ms and the overallperformance is almost linear in the number of agentsVI. C ONCLUSION AND F UTURE W ORK
In this paper, we presented a new decentralized methodbased on Voronoi diagrams and RVO that computes acollision-free path for agents operating in a shared workspacewith other agents, static and dynamic obstacles. Our V-RVOalgorithm provides passive collision avoidance guaranteesand we demonstrates its performance on agents with firstand second-order dynamics. We also presented a deadlockresolution strategy and demonstrates its benefits over priorvelocity-obstacle method.Our method has some limitations. Our approach can beconservative and not complete in terms of always findinga collision-free trajectory (if that exists). Furthermore, ourmethod does not guarantee deadlock resolution, similar toprior decentralized methods. Our Voronoi and baBVC im-plementations are not optimized and can be accelerated. Asa part of our future work, we plan to explore extending ourdeadlock resolutions and further improve the performance.We would also like to handle agents with non-holonomicconstraints or imprecise state information (e.g., due to sensorerrors). ig. 10: We highlight the performance of V-RVO algorithm without deadlockavoidance (left) and with deadlock avoidance (right). The adjacent goallocations in the grid are . m apart. The agents in green have reachedthere goal, while the agent in red are deadlocked, as shown with their whitetrajectories.Fig. 11: Average time per agent taken by V-RVO vs ORCA, as we varythe number of agents between − . V-RVO has additional overhead ofcomputing Voronoi regions and baBVC and is about X slower than ORCA. R EFERENCES[1] J. Li, A. Tinka, S. Kiesel, J. W. Durham, T. Kumar, and S. Koenig,“Lifelong multi-agent path finding in large-scale warehouses,” arXivpreprint arXiv:2005.07371 , 2020.[2] S. Tang and V. Kumar, “A complete algorithm for generating safetrajectories for multi-robot teams,” in
Robotics Research . Springer,2018, pp. 599–616.[3] W. H¨onig, J. A. Preiss, T. K. S. Kumar, G. S. Sukhatme, and N. Aya-nian, “Trajectory planning for quadrotor swarms,”
IEEE Transactionson Robotics , vol. 34, no. 4, pp. 856–869, 2018.[4] M. Hamer, L. Widmer, and R. D’andrea, “Fast generation of collision-free trajectories for robot swarms using gpu acceleration,”
IEEEAccess , vol. 7, pp. 6679–6690, 2019.[5] K. Solovey, O. Salzman, and D. Halperin, “Finding a needle in anexponential haystack: Discrete rrt for exploration of implicit roadmapsin multi-robot motion planning,”
The International Journal of RoboticsResearch , vol. 35, no. 5, pp. 501–513, 2016.[6] M. Goldenberg, A. Felner, R. Stern, G. Sharon, N. Sturtevant, R. C.Holte, and J. Schaeffer, “Enhanced partial expansion a,”
Journal ofArtificial Intelligence Research , vol. 50, pp. 141–187, 2014.[7] J. van den Berg, Ming Lin, and D. Manocha, “Reciprocal velocityobstacles for real-time multi-agent navigation,” in , 2008, pp. 1928–1935.[8] J. Van Den Berg, S. J. Guy, M. Lin, and D. Manocha, “Reciprocaln-body collision avoidance,” in
Robotics research . Springer, 2011,pp. 3–19.[9] D. Zhou, Z. Wang, S. Bandyopadhyay, and M. Schwager, “Fast, on-line collision avoidance for dynamic vehicles using buffered voronoicells,”
IEEE Robotics and Automation Letters , vol. 2, no. 2, pp. 1047–1054, 2017.[10] B. Davis, I. Karamouzas, and S. J. Guy, “Nh-ttc: A gradient-basedframework for generalized anticipatory collision avoidance,” arXivpreprint arXiv:1907.05945 , 2019.[11] P. Fiorini and Z. Shiller, “Motion planning in dynamic environmentsusing velocity obstacles,”
The International Journal of Robotics Re-search , vol. 17, no. 7, pp. 760–772, 1998.[12] A. Best, S. Narang, and D. Manocha, “Real-time reciprocal collisionavoidance with elliptical agents,” in . IEEE, 2016, pp. 298–305.[13] J. Van Den Berg, J. Snape, S. J. Guy, and D. Manocha, “Reciprocalcollision avoidance with acceleration-velocity obstacles,” in . IEEE,2011, pp. 3475–3482.[14] J. Van Den Berg, D. Wilkie, S. J. Guy, M. Niethammer, andD. Manocha, “Lqg-obstacles: Feedback control with collision avoid-ance for mobile robots with motion and sensing uncertainty,” in . IEEE,2012, pp. 346–353.[15] D. Bareiss and J. Van den Berg, “Reciprocal collision avoidancefor robots with linear dynamics using lqr-obstacles,” in . IEEE, 2013,pp. 3847–3853.[16] J. Alonso-Mora, A. Breitenmoser, M. Rufli, P. Beardsley,and R. Siegwart,
Optimal Reciprocal Collision Avoidance forMultiple Non-Holonomic Robots . Berlin, Heidelberg: SpringerBerlin Heidelberg, 2013, pp. 203–216. [Online]. Available:https://doi.org/10.1007/978-3-642-32723-0 15[17] S. Kim, S. J. Guy, W. Liu, D. Wilkie, R. W. Lau, M. C. Lin, andD. Manocha, “Brvo: Predicting pedestrian trajectories using velocity-space reasoning,”
The International Journal of Robotics Research ,vol. 34, no. 2, pp. 201–217, 2015.[18] L. He, J. Pan, and D. Manocha, “Efficient multi-agent global naviga-tion using interpolating bridges,” in . IEEE, 2017, pp. 4391–4398.[19] H. Zhu and J. Alonso-Mora, “B-uavc: Buffered uncertainty-awarevoronoi cells for probabilistic multi-robot collision avoidance,” in . IEEE, 2019, pp. 162–168.[20] V. R. Desaraju and J. P. How, “Decentralized path planning formulti-agent teams in complex environments using rapidly-exploringrandom trees,” in . IEEE, 2011, pp. 4956–4961.[21] Z. Forootaninia, I. Karamouzas, and R. Narain, “Uncertainty modelsfor ttc-based collision-avoidance,” in
Robotics: Science and Systems ,vol. 7, 2017.[22] T. Fraichard and H. Asama, “Inevitable collision states. a step to-wards safer robots?” in
Proceedings 2003 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS 2003) (Cat.No.03CH37453) , vol. 1, 2003, pp. 388–393 vol.1.[23] S. Bouraine, T. Fraichard, and H. Salhi, “Provably safe navigationfor mobile robots with limited field-of-views in unknown dynamicenvironments,” in2012 IEEE International Conference on Roboticsand Automation