Multi-Agent Navigation · Safe Control Density

Run

Run a minimal multi-agent crossing scenario with the three density constructions.

python examples/multi_agent/density_feedback/reactive.py --scenario crossing6
python examples/multi_agent/density_feedback/collision_cone.py --scenario crossing6
python examples/multi_agent/density_feedback/velocity_obstacle.py --scenario crossing6

System Dynamics

Each agent is a unicycle with state $x_i=[p_{x,i},p_{y,i},\theta_i]$ and controls $u_i=[v_i,\omega_i]$. The density construction is pairwise: every other agent contributes a smooth obstacle term.

$$ \dot p_{x,i}=v_i\cos\theta_i,\qquad \dot p_{y,i}=v_i\sin\theta_i,\qquad \dot\theta_i=\omega_i $$ $$ \rho_i(x_i)= \frac{\prod_{j\ne i}\Phi_{ij}(p_i,p_j,v_i,v_j)} {\left(\|p_i-p_i^\star\|^2+w_\theta\,\mathrm{wrap}(\theta_i-\theta_i^\star)^2\right)^\alpha} $$

Density Constructions

The baseline only sees distance. Collision-cone and velocity-obstacle density also use relative velocity, so agents react differently when another agent is crossing, approaching, or moving away.

Reactive bump

A neighbor is a circular bump obstacle. This is simple and fast, but it cannot tell whether the neighbor is actually moving toward a future collision.

$$\Phi_{ij}=\beta(\|p_i-p_j\|;r_{\mathrm{safe}},r_{\mathrm{sense}})$$

Collision cone

The cone is built from relative position and relative velocity. If the relative velocity points into the cone, the density term activates before the disks touch. We keep the usual spatial bump and multiply it by a second smooth bump on the collision-cone margin.

$$ \Phi_{ij}^{\mathrm{cc}} =\beta_{\mathrm{space}}(\|p_i-p_j\|) \,\beta_{\mathrm{vel}}(h_{\mathrm{cc}}) $$ $$ h_{\mathrm{cc}}=p^\top v+\|p\|\|v\|\cos\phi $$

Velocity obstacle

Velocity obstacles work directly in velocity space. Candidate velocities inside the cone would produce a future collision, so the density pushes the command toward a cone boundary. As with collision cones, the final obstacle term is a spatial bump times a smooth bump on the velocity-obstacle margin.

$$ \Phi_{ij}^{\mathrm{vo}} =\beta_{\mathrm{space}}(\|p_i-p_j\|) \,\beta_{\mathrm{vel}}(h_{\mathrm{vo}}) $$ $$ h_{\mathrm{vo}}=\max(\mathrm{cross}(\ell,v_{\mathrm{rel}}),\mathrm{cross}(v_{\mathrm{rel}},r)) $$

Interactive Result Viewer

Select a controller, density construction, and scenario. The feedback controller is fast and smooth because it maps a planar density command to unicycle inputs. The filter solves a one-step constrained unicycle problem at every timestep.

Controller Density Scenario

Crossing 2 Comparison Dashboards

The dashboards replay methods one after another. Completed tracks stay visible, while the active method shows moving triangles. The live plots show each agent's forward speed, yaw rate, and minimum pairwise clearance over time.

Density feedback methods

Density filter methods

Velocity obstacle: feedback vs filter

Collision cone: feedback vs filter

Controller Setups

The same density constructions can be used in two different control loops. Feedback is fast and direct; the filter is more faithful to the unicycle dynamics because it solves over the actual controls.

Density feedback

Density feedback first computes a planar single-integrator command for each agent. That command is then mapped to forward speed and yaw rate through a desired heading. This makes the method very fast and visually smooth, but the safety reasoning happens in planar velocity space before the unicycle mapping.

For collision cone and velocity obstacle density, the feedback controller uses relative velocity information from nearby agents. We keep only the most relevant neighbors so the command reacts to agents that can actually create a near-term collision.

Density filter

Density filter starts from a nominal unicycle command and solves a one-step constrained problem over $[v,\omega]$. It changes the nominal only when the predicted next state would violate the density condition. This is slower than feedback, but it uses the exact unicycle step in the safety correction.

The multi-agent filter uses synchronous velocity snapshots: every agent sees the same frozen neighbor velocities for that timestep, solves its local filter, and then all agents advance together. This keeps pairwise velocity-aware terms more symmetric in crossing scenarios.

Scenario Comparisons

Each section fixes the scenario. Within each density construction, the feedback result and filter result are shown side by side.

Crossing 2

Two-agent crossing

Two agents swap positions through a single interaction point. This is the cleanest case for seeing how relative velocity changes the avoidance direction.

Reactive bump

Feedback

Filter

Collision cone

Feedback

Filter

Velocity obstacle

Feedback

Filter

Crossing 4

Four-agent crossing

Four agents share the same central interaction region. This increases the number of simultaneous pairwise constraints while keeping the geometry easy to read.

Reactive bump

Feedback

Filter

Collision cone

Feedback

Filter

Velocity obstacle

Feedback

Filter

Crossing 6

Six-agent crossing

Six agents create dense pairwise interactions near the center. This case shows why nearest or most-dangerous neighbor selection matters for keeping velocity-aware filters tractable.

Reactive bump

Feedback

Filter

Collision cone

Feedback

Filter

Velocity obstacle

Feedback

Filter

Swap 8 opposite

Diagonal swap

Agents swap with diagonally opposite goals. The interaction is less synchronized than the crossing cases, but every agent still has to negotiate several moving neighbors.