ORCA under noise: collision avoidance when you can't trust your sensors

For my bachelor thesis I took a well-known collision avoidance algorithm, fed it wrong position data on purpose, and measured collisions. This post walks through what broke, what I did about it, and what it cost.

The setting

Picture a few dozen robots, drones or underwater vehicles sharing a space. Each one has its own goal and moves on its own. Nobody plans centrally. Central multi-robot planning can be PSPACE-hard and gets expensive as the group grows, so each participant has to decide by itself, every tick, how to move without hitting anyone.

Optimal Reciprocal Collision Avoidance (ORCA) by van den Berg et al. is one way to do this. It needs no communication: every participant observes the others’ positions and velocities and computes a safe velocity for itself. If everyone follows its rules and the assumptions hold, nobody collides.

The catch: “observes”

ORCA assumes accurate positions and velocities. Real systems don’t provide those reliably. GPS can be off by a few metres, localization based on UWB ranging has uncertainty, and sensor data arrives late.

The reciprocity is where this hurts. Two participants are supposed to share the work of avoiding each other. With noisy measurements, they may disagree about where the other one is. Both do their half, but their corrections may not add up to a safe manoeuvre. That is the problem I wanted to test.

How ORCA works

Take two participants $A$ and $B$ with positions $\mathbf{p}_A, \mathbf{p}_B$ and radii $r_A, r_B$. The velocity obstacle is the set of velocities of $A$ (relative to $B$) that lead to a collision within a time window $\tau$:

where $D(\mathbf{p}, r)$ is the open disk of radius $r$ around $\mathbf{p}$. Geometrically that’s a truncated cone in velocity space.

If the current relative velocity lies inside that cone, let $\mathbf{u}$ be the smallest change that pushes it out, and $\mathbf{n}$ the outward normal at that point. The reciprocal trick is that $A$ and $B$ each take half of the correction. That gives $A$ a half-plane of permitted velocities:

$B$ computes the mirrored half-plane from the same observations, so neither needs to talk to the other. With $n$ participants, $A$ intersects all its half-planes with its speed limit and picks the permitted velocity closest to where it actually wants to go:

That is a small linear program, and it’s fast enough for thousands of agents.

Measuring it

I wrote a simulation in Go where each participant runs as its own process and receives the world state each tick, with configurable position noise sampled from normal distributions. The ORCA implementation itself is in Rust. The setup was a 1000 × 1000 m square, participants with a 5 m radius, 2 to 64 participants, and noise settings from 0 to 10 m. In total I ran more than 16,000 simulations.

I counted ticks in which a participant overlapped another participant, using their physical radii. A prolonged overlap counts more than once; these are not counts of separate crashes. The results below use the average count per participant.

At the tested noise settings up to 2 m, plain ORCA produced no collisions. At 5 and 10 m, collisions became more frequent, especially with more participants. At the 10 m setting with 64 participants, the average was more than six overlapping ticks per participant.

What I changed

Safe zones. Each participant gets a standard deviation $\varphi$ describing how uncertain its position estimate is, for itself and for every other participant. During the velocity obstacle computation the radius is inflated by two standard deviations:

So each participant now avoids where the others probably are, not where they were measured. (In the thesis I called $2\varphi$ a 95% confidence range. That is true per axis. For a 2D Gaussian, a disk of radius $2\varphi$ covers about 86%, and 95% would need about $2.45\varphi$, assuming independent axes with the same standard deviation.)

More foresight. A larger $\tau$ moves the truncation of the cone towards the origin, so participants start dodging earlier. This helps them avoid dense situations where no safe velocity is available.

The implementation also needed changes for deadlocks, even without noise.

A rotation preference. In my original implementation, two participants approaching head-on could both slow to a stop: their corrections were symmetric. I tilted the correction vector slightly so both “prefer a side” and curve around each other instead of freezing.

Participant obstacles. Participants that have reached their goal stand still, so they no longer take half the responsibility. If two of them stand close together, a third one may not fit between them but also can’t get around them using the original velocity selection, and it deadlocks. I treated nearby stationary participants as a single static line-segment obstacle. The moving participant then takes full responsibility for avoiding it. An isolated stationary participant is treated as a point obstacle.

Figure 4.2 from my thesis: joining B and C into an obstacle gives A a way around them. It still cannot escape if stationary obstacles surround it.

Results and costs

With these changes, the average collision count dropped by more than 80%. At the 5 and 10 m settings where I obtained usable results, participants averaged less than one overlapping tick each. There was a limit, though: with 64 participants and high noise, the inflated radii caused deadlocks and I got no usable results.

Figure 6.3 from my thesis, with 32 participants. The horizontal axis uses the thesis’s noise settings, labelled “Average Inaccuracy”. The collision count is the average number of overlapping ticks per participant.

It isn’t free. Inflated radii mean more half-planes with an empty intersection, and every time that happens the algorithm has to relax the constraints step by step until a velocity fits. In dense scenarios with high noise, the time until all participants reached their goals increased by up to 50% compared with the same improved algorithm at noise settings of 2 m or less. That is a comparison between noise levels, not a 50% overhead over plain ORCA.

Testing shared state

As a last experiment I let all participants see the same noisy picture of the world, which is a stand-in for communication. The positions are still wrong, but consistently wrong for everyone.

With shared state enabled in the improved algorithm, I observed zero collisions in the tested scenarios. Working from the same estimates removes the disagreement between participants when they construct their half-planes. It does not make the estimates accurate, or prove that collisions are impossible under arbitrary noise.

At low noise, shared state was even faster than separate observations. At high noise, the larger safe zones required more steps to reach the goals, and total runtime more than doubled compared with low-noise runs. This was a simulation of shared information; I did not measure the cost of a real communication network.

What I took away

Giving each participant more room reduced collisions, but left a tradeoff: larger margins made it harder to find a velocity that let everyone move. The shared-state experiment gave me a reason to investigate communication alongside those margins. In these simulations, agreeing on the observations helped even when the observations were noisy.

That question is where my master thesis picks up. Instead of every participant guessing on its own, I’m looking at Gaussian Belief Propagation, where participants exchange beliefs about each other and agree on a consistent picture, even over unreliable links. More on that once it’s done.

The animation on the home page is a simplified relative of this: sampling-based reciprocal velocity obstacles in plain TypeScript. A WASM build of the real Rust implementation is on my list.

Reference: J. van den Berg, S. J. Guy, M. Lin, D. Manocha, “Reciprocal n-Body Collision Avoidance”, Robotics Research, Springer, 2011.