Quanta Magazine

Reciprocal Velocity Obstacles (RVO) for collision avoidance in C++

Reciprocal Velocity Obstacles (RVO) is a concept used in robotics to prevent multiple agents from colliding with each other while moving towards their goals. The primary advantage of RVO is that it allows for decentralized planning, where each agent makes decisions based only on the state of its neighbors without the need for a central controller.

The source code for this article in C++ you can find on my GitHub.

The Basic Idea

Imagine two agents, A and B, moving in a 2D plane (I provide simulation for 2D and 3D). Each agent wants to reach its destination without colliding with the other. The “Velocity Obstacle” (VO) for A with respect to B is the set of all relative velocities of A with respect to B that would result in a collision at some point in the future.

RVO extends this concept by defining the set of velocities that, if chosen by both agents, will ensure neither agent has to drastically alter its course to avoid the other.

Definitions

1. Robot A and Robot B: Two agents in a 2D or 3D space that need to
   avoid each other while moving towards their respective goals.
2. Velocity Obstacle (VO): The set of all velocities of Robot A that would
   result in a collision with Robot B, assuming Robot B keeps its current
   velocity.
3. Reciprocal Velocity Obstacle (RVO): Assumes both Robot A and
   Robot B make an equal effort to avoid each other. It’s the set of relative
   velocities that will lead to a collision if both robots continue on their path.

Mathematical Formulation

1. Relative Velocity: Let vA​ and vB​ be the velocities of agents A and B, respectively. The relative velocity of A with respect to B is given by:

2. Velocity Obstacle (VO): The VO for A with respect to B, is the set of all relative velocities that will lead to a collision between A and B within a specified time horizon τ. Mathematically, it’s defined as:

3. Reciprocal Velocity Obstacle (RVO): The RVO is defined as the Minkowski sum of the VO and the velocity of Robot B, shifted by half of the relative velocity

Solution Approach in provided C++ programs

Given two robots A and B in a space (2D or 3D), each robot has:

1. A current position P.
2. A goal position G.
3. A defined speed s.
4. A defined radius r.

The objective is to move each robot towards its goal while avoiding collisions with other robots.

Relative Position: For any two robots A and B, the relative position is given by:

Distance Between Robots: The Euclidean distance DAB between robot A and B is:

Avoidance Mechanism: If the distance DAB is less than the sum of the radii of the two robots and their speeds:

Then, an avoidance vector A is computed using the cross product:

Direction Towards Goal: If the robots are not in danger of colliding, they move towards their goals. The direction vector D for robot A is:

Update Position: The robot’s position is updated based on either the avoidance vector (if a collision is imminent) or the direction vector (if the path is clear):

or

Simulations

I have provided a simple simulation in C++ that illustrates the concept of our discussion.

Collision avoidance in 2D (by author)

Collision avoidance in 3D (by author)

Conclusion

RVO provides a robust framework for multi-agent collision avoidance. By considering the relative velocities and making a shared effort to avoid collisions, agents can navigate complex environments efficiently and safely. The decentralized nature of RVO makes it scalable and suitable for real-time applications.

Thank you for reading.