Efficient Reciprocal Collision Avoidance between Heterogeneous Agents Using CTMAT

We present a novel algorithm for reciprocal collision avoidance between heterogeneous agents of different shapes and sizes. We present a novel CTMAT representation based on medial axis transform to compute a tight fitting bounding shape for each agent. Each CTMAT is represented using tuples, which are composed of circular arcs and line segments. Based on the reciprocal velocity obstacle formulation, we reduce the problem to solving a low-dimensional linear programming between each pair of tuples belonging to adjacent agents. We precompute the Minkowski Sums of tuples to accelerate the runtime performance. Finally, we provide an efficient method to update the orientation of each agent in a local manner. We have implemented the algorithm and highlight its performance on benchmarks corresponding to road traffic scenarios and different vehicles. The overall runtime performance is comparable to prior multi-agent collision avoidance algorithms that use circular or elliptical agents. Our approach is less conservative and results in fewer false collisions.


page 1

page 7

page 8


AutoRVO: Local Navigation with Dynamic Constraints in Dense Heterogeneous Traffic

We present a novel algorithm for computing collision-free navigation for...

V-RVO: Decentralized Multi-Agent Collision Avoidance using Voronoi Diagrams and Reciprocal Velocity Obstacles

We present a decentralized collision avoidance method for dense environm...

RoadTrack: Tracking Road Agents in Dense and Heterogeneous Environments

We present an algorithm to track traffic agents in dense videos. Our app...

Heter-Sim: Heterogeneous multi-agent systems simulation by interactive data-driven optimization

Interactive multi-agent simulation algorithms are used to compute the tr...

Collision Detection for Agents in Multi-Agent Pathfinding

Recent work on the multi-agent pathfinding problem (MAPF) has begun to s...

Responsibility-associated Multi-agent Collision Avoidance with Social Preferences

This paper introduces a novel social preference-aware decentralized safe...

Online Synthesis for Runtime Enforcement of Safety in Multi-Agent Systems

A shield is attached to a system to guarantee safety by correcting the s...

1. Introduction

Computing collision-free trajectories for each agent is a fundamental problem in multi-agent navigation. The main goal is to ensure that each agent must take action to avoid collisions with other moving agents or obstacles and make progress towards its goal position. This problem has been well studied in AI and robotics Fraichard and Asama (2004); Bera et al. (2017); Fox et al. (1997); Godoy et al. (2016), VR Bruneau et al. (2015); Narang et al. (2016), computer games and crowd simulation  Karamouzas et al. (2017); Stüvel et al. (2017); He et al. (2016); Best et al. (2014), traffic simulation, emergent behaviors Reynolds (1987), etc.

Figure 1. Traffic scenario. (a) Image of street traffic with different vehicles of varying sizes. The contours of vehicles are shown in green. (b) Our CTMAT representation for each agent with red medial axis. (c) Comparison of different kinds of representations of vehicle agents that lie in the red rectangle in (a). Our CTMAT representation is less conservative as compared to circles and ellipses and has similar runtime performance.

In this paper, we address the problem of computing collision-free paths for a large number of heterogeneous agents at interactive rates. Such agents are characterized by varying shapes and sizes (see Fig. 1) and can be in close proximity. In order to achieve real-time performance, most practical algorithms  Helbing and Molnar (1995); Reynolds (1987); Van Den Berg et al. (2011); Van den Berg et al. (2008) use a disc representation for each agent. However, one obvious disadvantage of using discs is that it results in a conservative approximation for each agent, especially when the shape is not round or has a large aspect ratio. This can result in a large number of false positives and the resulting multi-agent algorithms may not work well in dense scenarios. Other multi-agent algorithms use an ellipse Best et al. (2016); Narang et al. (2017) or a capsule shape Stüvel et al. (2017) to represent each agent. However, these representations can become very conservative for some shapes and may also result in high number of false positives. The recent interest in autonomous driving simulators and navigation has motivated the development of a new set of multi-agent navigation algorithms for heterogeneous agents, whose shapes may correspond to cars, bi-cycles, buses, pedestrians, etc., that share the same road Best et al. (2017); Pendleton et al. (2017); Katrakazas et al. (2015); Saifuzzaman and Zheng (2014); Ziegler et al. (2014); Sun et al. (2014); Urmson et al. (2009); Bareiss and van den Berg (2015). Using a simple shape approximation like a circle or an ellipse for all agents can be very conservative for dense traffic situations, as shown in Fig. 1. Instead, we need efficient and accurate multi-agent navigation algorithms that can model agents with varying shapes and sizes.

Main Results: We present an algorithm for efficient collision-free navigation between heterogeneous agents by using a novel medial-axis agent representation (CTMAT). Based on CTMAT, we also present a reciprocal collision avoidance scheme (MATRVO). In order to represent the heterogeneous agents for local navigation, we compute a compact representation of each agent based on the Medial Axis Transform (MAT)  Bium ([n. d.])

. Our formulation is based on a simplified discretization of the medial axis that captures the shape of agents. By linearly interpolating every two adjacent medial circles of MAT, we can get a set of tuples, which constitute CTMAT. Each tuple, composed of two circular arcs and two line segments (see Fig. 

3), is used to efficiently compute the Minkowski Sum, which is used for velocity obstacle computation for reciprocal collision avoidance. Our CTMAT representation can handle both convex and concave agents for reciprocal collision avoidance. We use precomputed tables of Minkowski Sums and precomputed width table of these agents to further accelerate the algorithm in handling a large environment that contains thousands of agents. We also update the orientation of each agent to generate collision-free trajectories. For the runtime performance, without precomputation, the average query time to perform the collision avoidance test between two CTMATs with one tuple is about microseconds on a single CPU core. In practice, our MATRVO algorithm with one tuple for each agent is X slower than ORCA collision avoidance algorithms for circular agents, while it is X faster than ERVO algorithm with elliptical agents.

The rest of the paper is organized as follows. We give a brief overview of prior work in collision avoidance and multi-agent navigation algorithms in Section 2. In Section 3, we provide an overview of MAT and how we construct our CTMAT representation for a given agent shape. In Section 4, we present our collision-free navigation algorithm (MATRVO) for heterogeneous agents. We present an acceleration scheme that uses precomputed Minkowski sums in Section 5 and present our approach to computing the orientation for local navigation in Section 6. We describe the implementation and highlight the results in Section 7.

Figure 2. Pipeline of our algorithm. For given contour of agent, we compute CTMAT representation. Then, we use MATRVO to compute velocities of agents. To speed up the algorithm, we can precompute Minkowski Sums and width table. We can update the orientation to match new velocities.

2. Related Work

The problem of collision avoidance and motion planning for robots or agents has been extensively studied. Many prior approaches Fraichard and Asama (2004); Borenstein and Koren (1991); Kanehiro et al. (2008); Simmons (1996)

assume that the obstacles are static or slow moving as compared to the robot. Prior algorithms for collision avoidance and collision-free navigation in dynamic environment can be classified into two categories, including centralized methods and decentralized methods. The former  

LaValle and Hutchinson (1998); Sanchez and Latombe (2002); Švestka and Overmars (1998) regard all the agents as part of a single global system and decide the actions for each agent in the unified configuration space. These methods can provide global guarantees, but their complexity increases significantly with the number of agents in the scene. In practice, they are limited to scenarios with a few agents. Decentralized methods  Fox et al. (1997); Hsu et al. (2002); Sanchez and Latombe (2002); Petti and Fraichard (2005); Karamouzas et al. (2017); Godoy et al. (2016) compute the motions and trajectories for the agents independently. They usually make use of local environmental information to compute a local trajectory according to agents’ positions and current motions. Some of these earlier methods did not account for reactive behaviors of other agents.

Among decentralized approaches, velocity obstacle (VO) Fiorini and Shiller (1998) is a widely used algorithm for collision avoidance for a robot navigating among dynamic obstacles. It has been extended to model reciprocal behaviors between agents Van den Berg et al. (2008); Van Den Berg et al. (2011) and can provide sufficient conditions for collision avoidance. Furthermore, they can also account for kinematic and dynamic constraints of agents Bareiss and Van den Berg (2013); Alonso-Mora et al. (2012); Lalish and Morgansen (2012). All these methods assume that each agent is represented as a disc. Some other local navigation methods for disc-based agents include cellular decomposition Schadschneider (2001), rule-based methods  Reynolds (1987) and force-based methods  Helbing and Molnar (1995); Karamouzas et al. (2009); Pelechano et al. (2007).

In order to better approximate human and robot shapes, efficient reciprocal velocity obstacle methods have been proposed for elliptical agents Best et al. (2016). The resulting algorithm extends ORCA and takes advantage of the precomputed table of Minkowski Sums of ellipses to compute velocities for all the agents in real time. Based on this ellipse representation, Narang et al. Narang et al. (2017) can accurately model human motions in dense situations, including shoulder turning and side-stepping. A capsule-shaped approximation of agents has been used for character animation Stüvel et al. (2017) to generate torso-twisting and side-stepping characters of crowd model. All these methods are less conservative than disc-based agent representations and can also model orientation changes. However, they are limited to homogeneous environments, where each agent has the same shape. It could be quite expensive to extend these methods to heterogeneous environments where the agents have different sizes and shapes.

Figure 3. (a) MAT of a 2D shape: Red curve is the medial axis and black circles are several sampling medial circles of the shape. (b) Tuple: Linear interpolation of two neighboring medial circles. (c) Simplified combination of tuples by our algorithm. Blue dotted curve is the contour of the original shape. (d) CTMAT representation of the original shape.
Figure 4. The process of generating CTMAT for a given agent: (a) Transfer the curved contour of agent to a polygon. (b) Sample on the boundary of polygon and compute the CDT of the point samples. (c) Compute circumcircles for J-triangles and T-triangles. (d) Select valid circles as medial circles and compute the corresponding tuples. (e) Modify tuples to cover the agent. (f) The CTMAT.

3. CTMAT: Approximation of Agents

Our approach is designed for heterogeneous environments, where each agent could have a different shape that is convex or non-convex. Our algorithm only assumes that we are given the 2D boundary or contour of each agent. One of our goals is to compute a representation that is tight–fitting and useful for efficient reciprocal collision avoidance for a large number of agents. We present a new representation that exploits the properties of the medial axis transform of the object.

3.1. Medial Axis Transform

The medial axis, proposed by Bium ([n. d.]), is an intrinsic shape representation that naturally captures the symmetry and interior properties of an object. In 2D, the medial axis of a shape, which is bounded by planar curve , is the locus of the centers of circles that are tangent to curve at two or more points, as shown in Fig.  3 shows. The circles are called medial circles. If the distance to the boundary is regarded as the radius of a medial axis point, we obtain the Medial Axis Transform (MAT), denoted as , or 3, where is the center of the medial circle and is the radius. The contour of an object can be reconstructed from MAT and the accuracy of the reconstructed shape is related to the number of sampled medial points.

3.2. CTMAT Representation

The exact representation of a medial axis is a continuous shape, but it is hard to compute. In most cases, we just compute a discretized representation in terms of a set of medial circles and their neighboring relationship, which is defined by the relative locations of the centers on the medial axis. When reconstructing a shape from the stored discrete information, we perform linear interpolation Li et al. (2015) between any pair of neighboring medial circles. Finally, the outermost contour is the reconstructed shape approximation of the agent. Fig. 3 shows a linear interpolation of two neighboring sampling medial circles. We call its contour a tuple, which is a basic unit of our representation. The tuple consists of two line segments , and two circular arcs , of the green area. After interpolation, the contour of the set of tuples of the original object corresponds to our CTMAT representation. The CTMAT representation of an agent is denoted as , which is also composed of line segments and circular arcs. To improve the efficiency of our algorithm, we make a tradeoff between the number of tuples and the conservative nature of our representation (Fig. 3) to approximate the original shape. The final CTMAT is shown in Fig. 3. In order to illustrate the structure of CTMAT, we the draw detailed combination of tuples with medial axis inside in the figure along with the contour to represent CTMAT.

3.3. CTMAT Computation

Our approach to computing the CTMAT consists of two parts - computing the reconstructed shape of the simplified medial axis and modifying the tuples to enclose the agent. Fig. 4 shows our pipeline. To ensure that the contour of agent can be easily represented, we first approximate the closed curve with a polygon, which is similar to the original shape and overestimates it. Many methods have been proposed Amenta et al. (2001); Lee (1982); Culver et al. (2004) to compute the MAT. Among them, Voronoi-based approaches  Brandt and Algazi (1992); Attali and Montanvert (1997); Attali and Lachaud (2001); Dey and Zhao (2004) are widely used in practice. In 2D, the Voronoi vertices approximate the medial axis and converge to the exact medial axis if the sample density of the points on the contour approaches infinity. Therefore, we uniformly sample on the polygonal contour. All the vertices of the polygon are added to the samples. The sampling density depends on user-specified accuracy. Given a set of sampling points of agent’s boundary, one easy method to compute Voronoi vertices is to compute Delaunay Triangulation (DT) Watson (1981). For convex shapes, we just compute DT. For non-convex shapes, we use Constraint Delaunay Triangulation (CDT) Chew (1989) to guarantee that all the sampling line segments, which is constituted by two adjacent sampling points, belong to the edges of triangulation. Moreover, we need to delete the outer triangles. Fig. 4 shows the result of this step for a non-convex shape. The complexity of computing DT or CDT is , where is the number of sample points. There are three kinds of triangles Prasad (1997) in the resulting triangulation. Those triangles with two external edges located in the terminal of a branch or a protrusion of the shape are called T-triangles. Those triangles with one external edge are S-triangles. Finally, the triangle with no external edges determines a junction of branches of the polygon is the J-triangle. The circumcircle of each triangle can be used to approximate the medial circle. The neighboring relationship is decided by their related triangles. If two triangles share the same edge, they are neighbors of each other. However, using a CTMAT representation with too many medial circles can slow down the runtime collision avoidance scheme. It turns out that performing the interpolation based on the T-triangle and J-triangle would cover the S-triangles, and we only compute the medial circles based on T-triangles and J-triangles, as shown in Fig. 4. If there is a set of consecutive S-triangles between two circles, we think these two circles are adjacent. To further simplify the representation, we use the following conditions to select the final valid medial circles and compute corresponding tuples for CTMAT.


where , and , are the centers and the radii of two neighboring medial circles respectively, and represents the distance between the two centers. If , we delete one of the two medial circles. If the medial circles of two related triangles are of different types, we delete the circle corresponding to the T-triangle. Otherwise, we remove the smaller one. After deleting a circle, the neighboring relationships of the removed circle are added to the remaining one. , standing for the threshold of filter, is a user-defined parameter and we use in our experiment. Fig. 4 shows the result after filtering.

After computing the simplified combination of tuples, we modify that representation so that it totally covers the agent. We use an optimization algorithm for this modification step with the goal of generating as small tuples as possible to contain the boundary of the agent and maintaining the original tuples’ shape (Fig. 4) as much as possible. Assume the variables of the current medial circle are represented as the center and the radius . The optimization algorithm is applied to each medial circle and can be expressed as:


where and are the original center and radius of , stands for the distance from the initial position and stands for the difference from the initial radius, is the set of tuples related to . is the set of sampling line segments, which locate inside before the modification. is composed of two kinds of sampling line segments which are not inside any tuple, one is that just one terminal belongs to , another is that the nearest distance to current reconstructed shape is the distance to . The distance between a line segment and a tuple is defined as the bigger Hausdorff distance of two terminals to the tuple. The constraints are used to ensure that each sampling line segment is located inside at least one tuple. is the objective function that consists of a mixture of continuous and combinatorial terms: it cannot be optimized directly by gradient-based methods. Instead, we use a greedy strategy to locally optimize the configuration of each medial circle. We consider as the center and test positions from small radius in a search web, which is constructed by a set of concentric circles and a set of rays from their center. For each crossed node of the web, we compute the minimal . Moreover, we compute the minimal value of in a defined range. Finally, we obtain the our required tuples and CTMAT, as shown in Fig. 4 and Fig. 4.

Figure 5. Computing the Minkowski Sum of Two tuples. (a) The tuples of two agents and . (b) Offsetting by two circles of . (c) Positioning new tuples of (b) in right place and computing their tangent lines. (d) Getting the Minkowski Sum .
Figure 6. MATRVO algorithm to compute collision-free velocity. (a) Two agents with one tuple are moving towards their goal positions. (b) The velocity obstacle for induced by takes the shape of a truncated cone, which is formed by the Minkowski Sum of and , scaled by and its two tangent lines from the origin. (c) The half-plane constraint is the set of permitted velocities for agent with respect to . (d) After adding all the constraints, we compute a feasible velocity in the intersection region of all the half-planes.

For agent , , where stands for the subset of 2D space.


Let represent the polygonal contour of , represent the line segment on and denote a tuple. For , , , . Then, . Then, and . Because , we get . ∎

Our approach can be used to represent heterogeneous agents. Fig. 1 is a scene of the top view of traffic scenario with different kinds of vehicles. CTMATs for all the vehicles are shown in Fig. 1. The comparison among disc, ellipse and CTMAT in representing the part of the traffic scene inside a red rectangle is in Fig. 1, which illustrates that CTMAT provides a tighter approximation.

4. Velocity Obstacles for Heterogeneous Agents

In general, the physical workspace of robots or agents is in 3D, presented by . We project the geometric representation of agents to a lower dimension , and generate CTMAT as the underlying and conservative approximation of each agent. In order to compute collision-free velocities, we treat each tuple, which is convex, as a separate computational unit to compute the velocity of each agent.

4.1. Tuple Definition

In the following, to make description clearer, we will assume the CTMAT of our agent has just one tuple. The cases that agent is represented by a few of tuples will be discussed later. The shape of tuple in of each agent is decided by its two medial circles - with bigger radius and with smaller radius. If two medial circles have the same radius, and

are randomly assigned to them. Let position vector

and radius represent and and represent . In order to benefit later computation, we also store four tangent points of tuple, shown in Fig. 3, which can be represented by . The case that the computed CTMAT of agent degenerates to one circle is easier to deal with Van Den Berg et al. (2011). We just consider common situations. So the information of the tuple of agent can be represented as . The preferred velocity and current velocity are denoted by and repectively. Let denote the preferred orientation and denote current orientation for the agent. Then the state space of agent is given by . If the agent has more than one tuple, can be changed to a set of tuples.

4.2. Local Collision Avoidance

Our algorithm is based on Velocity Obstacle (VO) Fiorini and Shiller (1998) and its follow-up ORCA  Van Den Berg et al. (2011). For two agents and , the velocity obstacle of induced by is represented by , which consists of all the relative velocities of with respect to that would cause a collision with

at some moment before time

. Conversely, assuming and are current velocity of and respectively, the condition can guarantee that agent and are collision-free for at least time. and are symmetric in the origin. Formally,


where is the Minkowski Sum between and .

The tuple of and and its parameters are shown in Fig. 5. The first step of computing is to offset by and of respectively and get two new expanded tuples and , shown in Fig. 5. The offsetting operation is composed of two substeps - one is enlarging the two circular arcs and the other is shifting two tangent lines of in terms of vector and . After getting new tuples, we translate them to correct places according to the position of . moves according to the vector , and translates by the vector . The result is in Fig.5. We only need compute tangent lines of two new tuples and then the boundary of , defined by , can be extracted. is still composed of line segments and circular arcs, which brings convenience to later computation of nearest point and forward face.

4.3. Neighboring Obstacle Constraints

It comes to compute the velocity obstacle for agent. Also taking agent and its neighboring agent as an example, we know their current velocities and their Minkowski Sum . The next step is to find lines from the origin and tangent to . It is easy to compute these lines because of the geometric properties of the components of . Then we can get the tangents of scaled by the inverse of and the forward face, shown in Fig. 6. The forward face is also composed of line segments or circular arcs or combination of them, so the nearest point of the boundary of velocity obstacle for the relative velocity can be computed easily.

Then, we compute valid velocities for agent by making use of the velocity obstacle. The process is extended from ORCA  Van Den Berg et al. (2011) and we denote the permitted velocities for for reciprocal collision avoidance with respected to as . As Fig. 6 shows, is the vector from to the nearest point. Agent should change its velocity by under the assumption that will do the same. And the collision-free velocity for with the neighbor is defined by the half-plane, passing through the point and vertical to . Suppose agent and its neighbor has tuples and tuples respectively, the planes caused by them is . If we have found all neighboring agents of by searching in kD-tree, we can compute all the half-planes, the constraints for , and get the final intersected valid area for permitted velocities of .


To guarantee our agent always has a tendency towards its goals, we choose a velocity inside that has the smallest deviation from its preferred velocity . That is


We use linear programming to compute and Eq. 3, and the runtime is where is the number of constraints. If there is a feasible solution for the agent, the agent’s motion is guaranteed to be collision-free. When the agents are densely distributed in the scenario, MATRVO may be empty and no feasible solution can be found. In that situation, we minimally penetrate the constraints and use another linear programming  Van Den Berg et al. (2011); Best et al. (2016). If MATRVO algorithm is able to compute a feasible velocity, the resulting motion for agent is collision-free.


For agent in the scenario, its neighboring agents can be represented as . All the tuples of construct constraints with all the tuples of . If we compute a feasible velocity by linear programming, then tuple , tuple , . Then, agent , . Then, will not collide with any other neighboring agent at any moment before time . ∎

4.4. Runtime Analysis

Let represent the number of tuples of and represent the th neighboring agent of . The runtime of computing can be denoted as follows.


where denotes the number of neighboring agents of , is the runtime of computing when has just one neighbor and both of them have one tuple.

Figure 7. Swept Tuple. (a) An original tuple. (b) Rotate the tuple by an angle with as the pivot. is the middle point on circular arc of the original tuple and the corresponding point after rotating is represented as .
Figure 8. Computing the width for agent: (a) An agent with two tuples. (b) Convex hull of the agent. is the width of the orientation of the arrow.
Figure 9. A sequence of frames in the simulation of traffic scenario by MATRVO. For each scenario we compute the new velocity of each agent using MATRVO and compute collision-free trajectories between two frames.
Figure 10. A sequence of frames in the simulation of antipodal circle scenario by MATRVO.

5. Precomputation of Minkowski Sums

One important factor influencing the efficiency is the computation of the Minkowski Sum. To further accelerate our algorithm, we make use of a precomputational table of Minkowski Sums Best et al. (2016).

Given a tuple, we define the orientation angle , which is the angle between the x-axis and its main axis decided by two centers of circular arcs. To make our discrete table cover all the orientations of tuples, we first compute a new representation of the tuple after rotating it for an interval angle . Like Fig. 7 shows, we select as the rotation center because the approximation error for the tuple is smaller than using when their radii are different. The blue contour in Fig. 7 is composed of new circular arcs and line segments. The special one is the circular arc , which does not come from the original medial circles but from a new circle with as the center and as the radius. However, it does not effect our computation of the Minkowski Sum since it is also a circular arc. The structure has no essential difference with the tuple. Let denote the set of angles and represent the set of precomputed contours by rotating a tuple from to for each two ordered angles in . After getting the set for each kind of tuple, we could construct tables of Minkowski Sum for tuples. When there comes a tuple with orientation , we can easily get the corresponding element in by searching . In this way, for a pair of tuples, we can efficiently find the corresponding Minkowski Sum in the precomputed tables. In our experiment, we set to .

After computing the Minkowski sum using the table, we use our method to compute the forward face and nearest point. Our precomputed method provides improvement in runtime performance, when each agent is represented using one tuple.

6. Orientation Update

Our representation of agents provides more degree of freedom in terms of selecting a feasible motion to avoid collisions. When the space in the environment is too narrow for the agent to move ahead, the agent can rotate its body to find a relatively small width under the direction of its velocity to pass through the space. In order to perform these computations efficiently, we use a precomputed width table to search for the rotated angle at runtime.

In order to design a solution for general scenarios, we assume the agent has two tuples, as shown in Fig. 8. The first step is to add more tangent lines to obtain the convex hulls of the agent, which consists of circular arcs and line segments. The width of the convex hull corresponds to the width of agent. The arrow in Fig. 8 shows the orientation of the agent and the distance between two parallel tangent lines of the convex hull in the direction of the arrow is the width of current agent. Three medial circles in this example are , and with centers , and , respectively. Assuming two parallel tangent lines are tangent to medial circle and , we can compute the value of by , where and are the radii of and , respectively, is the distance between two circles’ centers and angle is the acute angle between the vertical direction of orientation and the line passing through and . This equation corresponds to the case when two parallel tangent lines are tangent to the same medial circle. If the orientation arrow rotates by , the change of width can be represented as a piecewise continuous function with clear ranges. We precompute the function in a table so that our algorithm can search for the width and find the minimal rotated angle for the agent to pass through the clearance efficiently. We update the orientation after every time step to compute the new velocity for the agent and use the approach in Narang et al. (2017) to guarantee that the rotating action is collision-free. More details are given in the report XXX (2017).

7. Results

In this section, we highlight the performance of our algorithm on different benchmarks and compare its performance with prior multi-agent navigation algorithms. We implemented the algorithm and conducted experiments in C++ on a Windows 10 laptop with Intel i7-6700 CPU and 8GB RAM. Our algorithm can be easily parallelized on multiple cores. All the results in this paper were generated on a single CPU core.

Fig. 9 shows a sequence of agent positions (corresponding to different vehicles) of the simulation result of a traffic scenario by MATRVO. We have computed a representation and position for each vehicle based on the discrete time instances in a given video. For each column, the top image corresponds to the scene of moving vehicles and the bottom one is the corresponding simulated traffic scene. Our algorithm can tightly represent different kinds of agents (vehicles) and closely match the actual traffic. Fig. 10 shows a sequence of frames of simulating the agent positions in the antipodal scenario that has been used in prior benchmarks Van Den Berg et al. (2011); Best et al. (2016). This scenario requires every agent on a circle to reach the antipodal position as the final goal. The type of agent used in this benchmark is shown in the dotted rectangle. We also use another four scenarios to test the performance of our algorithm. The result in Fig. 13 illustrates that our algorithm could be applied to large scenarios with hundreds of or thousands of agents and used for interactive navigation. In Fig. 13, two agents should pass through the narrow hallway and reach the opposite positions. In this example, no disc-based agent or ellipse-based agent could pass through it due to the narrow space. Our CTMAT representation works well in such scenarios because of a tighter and more flexible representation. Fig. 13 and Fig. 13 show the performance of multi-agent navigation among static obstacles.

Figure 11. Performance comparison of ORCA, ERVO with precomputation, MATRVO and MATRVO with precomputation in the antipodal circle scenario.
Figure 12. Performance comparison of MATRVO without precomputation, when underlying agents have different numbers of tuples in the CTMAT representation.
Figure 13. Different Benchmarks: (a) agents from two vertical directions walk ahead; (b) Two agents approach and rotate in the narrow hallway; (c) agents from two vertical directions walk ahead in 4-square scene. (d) agents walk through a narrow door; CTMAT and MATRVO are able to perform collision-free navigation in these scenarios.

In order to evalute the efficiency of our algorithm, we compare the performance of ORCA Van Den Berg et al. (2011), ERVO Best et al. (2016) with precomputation, MATRVO and MATRVO with precomputation in different scenarios, like the antipodal circle. We highlight the average frame update time as a function of the number of agents in Fig. 11. Agents use one tuple for CTMAT in this comparison. We observe that MATRVO with precomputation is at most X slower than ORCA. The performance of agents with different numbers of tuples without precomputation is shown in Fig. 12.

During the process of computing collision-free trajectories, we need check for collisions between the agents. Representations that are too conservative may result in a high number of false positives and may not be able to compute collision-free trajectories in dense situations. Currently, we use a brute-force method to perform collision checks between the exact polygonal representations of the agents and use that data as the ground truth and compute the number of false positives for ORCA, ERVO, and MATRVO for agents in the anti-podal benchmark (see Table 1).

Test Agent ORCA Van Den Berg et al. (2011) ERVO Best et al. (2016) MATRVO
54.5% 37.9% 8.5%
60.4% 31.4% 9.8%
47.3% 46.2% 9.2%
Table 1. Comparison of ratios of false positives of ORCA, ERVO, and MATRVO in antipodal circle scenario.

8. Conclusion, Limitations and Future Work

We present a novel algorithm for reciprocal collision avoidance between heterogeneous agents. For an arbitrary-shaped agent, we represent it with CTMAT and use MATRVO to compute collision-free trajectories for multiple agents. Taking advantage of the geometrical properties of MAT, our representation is less conservative and more flexible than current disc or ellipse-based approximation. Moreover, we can handle both convex or non-convex agents. Due to the simplicity of the formulation, MATRVO is very fast and can be used for interactive multi-agent navigation of thousands of agents on a single CPU core. We demonstrate the performance of our algorithm in simulating different scenarios and highlight the benefits over prior multi-agent navigation schemes.

Our approach has some limitations. First, the new velocity and agent’s orientation are not computed simultaneously. Second, like other VO-based methods, our algorithm also assumes perfect sensing and does not take into account uncertainty. In real traffic, different kinds of vehicles have different dynamics and we need to take them into account. As part of future work, we would like to overcome these limitations and evaluate the performance of MATRVO in more complex scenarios.

9. Acknowledgements

This work was supported by Hong Kong RGC Grant (HKU 717813E). We also wish to thank Xinge Zhu for his valuable suggestions.


  • (1)
  • Alonso-Mora et al. (2012) Javier Alonso-Mora, Andreas Breitenmoser, Paul Beardsley, and Roland Siegwart. 2012. Reciprocal collision avoidance for multiple car-like robots. In Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 360–366.
  • Amenta et al. (2001) Nina Amenta, Sunghee Choi, and Ravi Krishna Kolluri. 2001. The power crust, unions of balls, and the medial axis transform. Computational Geometry 19, 2-3 (2001), 127–153.
  • Attali and Lachaud (2001) Dominique Attali and J-O Lachaud. 2001. Delaunay conforming iso-surface, skeleton extraction and noise removal. Computational Geometry 19, 2-3 (2001), 175–189.
  • Attali and Montanvert (1997) Dominique Attali and Annick Montanvert. 1997. Computing and simplifying 2D and 3D continuous skeletons. Computer vision and image understanding 67, 3 (1997), 261–273.
  • Bareiss and Van den Berg (2013) Daman Bareiss and Jur Van den Berg. 2013. Reciprocal collision avoidance for robots with linear dynamics using lqr-obstacles. In Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 3847–3853.
  • Bareiss and van den Berg (2015) Daman Bareiss and Jur van den Berg. 2015. Generalized reciprocal collision avoidance. The International Journal of Robotics Research 34, 12 (2015), 1501–1514.
  • Bera et al. (2017) Aniket Bera, Tanmay Randhavane, Rohan Prinja, and Dinesh Manocha. 2017. SocioSense: Robot Navigation Amongst Pedestrians with Social and Psychological Constraints. arXiv preprint arXiv:1706.01102 (2017).
  • Best et al. (2017) Andrew Best, Sahil Narang, Daniel Barber, and Dinesh Manocha. 2017. AutonoVi: Autonomous Vehicle Planning with Dynamic Maneuvers and Traffic Constraints. (2017).
  • Best et al. (2014) Andrew Best, Sahil Narang, Sean Curtis, and Dinesh Manocha. 2014. Densesense: Interactive crowd simulation using density-dependent filters. In Proceedings of the ACM SIGGRAPH/Eurographics Symposium on Computer Animation. Eurographics Association, 97–102.
  • Best et al. (2016) Andrew Best, Sahil Narang, and Dinesh Manocha. 2016. Real-time reciprocal collision avoidance with elliptical agents. In Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 298–305.
  • Bium ([n. d.]) Harry Bium. [n. d.]. A transformation for extracting new descriptions of shape. In Symposium on Modeis for the Perception of Speech and Visua1 Form.
  • Borenstein and Koren (1991) Johann Borenstein and Yoram Koren. 1991. The vector field histogram-fast obstacle avoidance for mobile robots. IEEE transactions on robotics and automation 7, 3 (1991), 278–288.
  • Brandt and Algazi (1992) Jonathan W Brandt and V Ralph Algazi. 1992. Continuous skeleton computation by Voronoi diagram. CVGIP: Image understanding 55, 3 (1992), 329–338.
  • Bruneau et al. (2015) Julien Bruneau, Anne-Helene Olivier, and Julien Pettre. 2015. Going through, going around: A study on individual avoidance of groups. IEEE transactions on visualization and computer graphics 21, 4 (2015), 520–528.
  • Chew (1989) L Paul Chew. 1989. Constrained delaunay triangulations. Algorithmica 4, 1-4 (1989), 97–108.
  • Culver et al. (2004) Tim Culver, John Keyser, and Dinesh Manocha. 2004. Exact computation of the medial axis of a polyhedron. Computer Aided Geometric Design 21, 1 (2004), 65–98.
  • Dey and Zhao (2004) Tamal K Dey and Wulue Zhao. 2004. Approximate medial axis as a voronoi subcomplex. Computer-Aided Design 36, 2 (2004), 195–202.
  • Fiorini and Shiller (1998) Paolo Fiorini and Zvi Shiller. 1998. Motion planning in dynamic environments using velocity obstacles. The International Journal of Robotics Research 17, 7 (1998), 760–772.
  • Fox et al. (1997) Dieter Fox, Wolfram Burgard, and Sebastian Thrun. 1997. The dynamic window approach to collision avoidance. IEEE Robotics & Automation Magazine 4, 1 (1997), 23–33.
  • Fraichard and Asama (2004) Thierry Fraichard and Hajime Asama. 2004. Inevitable collision states—A step towards safer robots? Advanced Robotics 18, 10 (2004), 1001–1024.
  • Godoy et al. (2016) Julio Erasmo Godoy, Ioannis Karamouzas, Stephen J Guy, and Maria L Gini. 2016. Implicit Coordination in Crowded Multi-Agent Navigation.. In AAAI. 2487–2493.
  • He et al. (2016) Liang He, Jia Pan, Sahil Narang, Wenping Wang, and Dinesh Manocha. 2016. Dynamic group behaviors for interactive crowd simulation. arXiv preprint arXiv:1602.03623 (2016).
  • Helbing and Molnar (1995) Dirk Helbing and Peter Molnar. 1995. Social force model for pedestrian dynamics. Physical review E 51, 5 (1995), 4282.
  • Hsu et al. (2002) David Hsu, Robert Kindel, Jean-Claude Latombe, and Stephen Rock. 2002. Randomized kinodynamic motion planning with moving obstacles. The International Journal of Robotics Research 21, 3 (2002), 233–255.
  • Kanehiro et al. (2008) Fumio Kanehiro, Florent Lamiraux, Oussama Kanoun, Eiichi Yoshida, and Jean-Paul Laumond. 2008. A local collision avoidance method for non-strictly convex polyhedra. Proceedings of robotics: science and systems IV (2008).
  • Karamouzas et al. (2009) Ioannis Karamouzas, Peter Heil, Pascal van Beek, and Mark H Overmars. 2009. A Predictive Collision Avoidance Model for Pedestrian Simulation. MIG 9 (2009), 41–52.
  • Karamouzas et al. (2017) Ioannis Karamouzas, Nick Sohre, Rahul Narain, and Stephen J Guy. 2017. Implicit crowds: Optimization integrator for robust crowd simulation. ACM Transactions on Graphics (TOG) 36, 4 (2017), 136.
  • Katrakazas et al. (2015) Christos Katrakazas, Mohammed Quddus, Wen-Hua Chen, and Lipika Deka. 2015. Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions. Transportation Research Part C: Emerging Technologies 60 (2015), 416–442.
  • Lalish and Morgansen (2012) Emmett Lalish and Kristi A Morgansen. 2012. Distributed reactive collision avoidance. Autonomous Robots 32, 3 (2012), 207–226.
  • LaValle and Hutchinson (1998) Steven M LaValle and Seth A Hutchinson. 1998. Optimal motion planning for multiple robots having independent goals. IEEE Transactions on Robotics and Automation 14, 6 (1998), 912–925.
  • Lee (1982) Der-Tsai Lee. 1982. Medial axis transformation of a planar shape. IEEE Transactions on pattern analysis and machine intelligence 4 (1982), 363–369.
  • Li et al. (2015) Pan Li, Bin Wang, Feng Sun, Xiaohu Guo, Caiming Zhang, and Wenping Wang. 2015. Q-mat: Computing medial axis transform by quadratic error minimization. ACM Transactions on Graphics (TOG) 35, 1 (2015), 8.
  • Narang et al. (2017) Sahil Narang, Andrew Best, and Dinesh Manocha. 2017. Interactive simulation of local interactions in dense crowds using elliptical agents. Journal of Statistical Mechanics: Theory and Experiment 2017, 3 (2017), 033403.
  • Narang et al. (2016) Sahil Narang, Andrew Best, Tanmay Randhavane, Ari Shapiro, and Dinesh Manocha. 2016. PedVR: Simulating gaze-based interactions between a real user and virtual crowds. In Proceedings of the 22nd ACM conference on virtual reality software and technology. ACM, 91–100.
  • Pelechano et al. (2007) Nuria Pelechano, Jan M Allbeck, and Norman I Badler. 2007. Controlling individual agents in high-density crowd simulation. In Proceedings of the 2007 ACM SIGGRAPH/Eurographics symposium on Computer animation. Eurographics Association, 99–108.
  • Pendleton et al. (2017) Scott Drew Pendleton, Hans Andersen, Xinxin Du, Xiaotong Shen, Malika Meghjani, You Hong Eng, Daniela Rus, and Marcelo H Ang. 2017. Perception, Planning, Control, and Coordination for Autonomous Vehicles. Machines 5, 1 (2017), 6.
  • Petti and Fraichard (2005) Stéphane Petti and Thierry Fraichard. 2005. Safe motion planning in dynamic environments. In Intelligent Robots and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International Conference on. IEEE, 2210–2215.
  • Prasad (1997) Lakshman Prasad. 1997. Morphological analysis of shapes. CNLS newsletter 139, 1 (1997), 1997–07.
  • Reynolds (1987) Craig W Reynolds. 1987. Flocks, herds and schools: A distributed behavioral model. ACM SIGGRAPH computer graphics 21, 4 (1987), 25–34.
  • Saifuzzaman and Zheng (2014) Mohammad Saifuzzaman and Zuduo Zheng. 2014. Incorporating human-factors in car-following models: a review of recent developments and research needs. Transportation research part C: emerging technologies 48 (2014), 379–403.
  • Sanchez and Latombe (2002) Gildardo Sanchez and J-C Latombe. 2002. Using a PRM planner to compare centralized and decoupled planning for multi-robot systems. In Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, Vol. 2. IEEE, 2112–2119.
  • Schadschneider (2001) Andreas Schadschneider. 2001. Cellular automaton approach to pedestrian dynamics-theory. arXiv preprint cond-mat/0112117 (2001).
  • Simmons (1996) Reid Simmons. 1996. The curvature-velocity method for local obstacle avoidance. In Robotics and Automation, 1996. Proceedings., 1996 IEEE International Conference on, Vol. 4. IEEE, 3375–3382.
  • Stüvel et al. (2017) Sybren A Stüvel, Nadia Magnenat-Thalmann, Daniel Thalmann, A Frank van der Stappen, and Arjan Egges. 2017. Torso crowds. IEEE transactions on visualization and computer graphics 23, 7 (2017), 1823–1837.
  • Sun et al. (2014) Hao Sun, Weiwen Deng, Sumin Zhang, Shanshan Wang, and Yutan Zhang. 2014. Trajectory planning for vehicle autonomous driving with uncertainties. In Informative and Cybernetics for Computational Social Systems (ICCSS), 2014 International Conference on. IEEE, 34–38.
  • Švestka and Overmars (1998) Petr Švestka and Mark H Overmars. 1998. Coordinated path planning for multiple robots. Robotics and autonomous systems 23, 3 (1998), 125–152.
  • Urmson et al. (2009) Chris Urmson, Chris Baker, John Dolan, Paul Rybski, Bryan Salesky, William Whittaker, Dave Ferguson, and Michael Darms. 2009. Autonomous driving in traffic: Boss and the urban challenge. AI magazine 30, 2 (2009), 17.
  • Van Den Berg et al. (2011) Jur Van Den Berg, Stephen Guy, Ming Lin, and Dinesh Manocha. 2011. Reciprocal n-body collision avoidance. Robotics research (2011), 3–19.
  • Van den Berg et al. (2008) Jur Van den Berg, Ming Lin, and Dinesh Manocha. 2008. Reciprocal velocity obstacles for real-time multi-agent navigation. In Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on. IEEE, 1928–1935.
  • Watson (1981) David F Watson. 1981. Computing the n-dimensional Delaunay tessellation with application to Voronoi polytopes. The computer journal 24, 2 (1981), 167–172.
  • XXX (2017) XXX. 2017. Efficient Reciprocal Collision Avoidance between Heterogeneous Agents Using CTMAT. Technical Report (2017).
  • Ziegler et al. (2014) Julius Ziegler, Philipp Bender, Markus Schreiber, Henning Lategahn, Tobias Strauss, Christoph Stiller, Thao Dang, Uwe Franke, Nils Appenrodt, Christoph G Keller, et al. 2014. Making Bertha drive—An autonomous journey on a historic route. IEEE Intelligent Transportation Systems Magazine 6, 2 (2014), 8–20.