Swarm Formation Control

Multi-agent consensus, formation keeping, and collision avoidance using velocity obstacles.

Two runs of one scenario. Same goals, same controller, same initial positions, same step size. The only difference is whether the avoidance layer is in the loop. The table says the minimum separation goes from 0.0000 to 1.1391; the picture says what the swarm had to do to get there.

The horizon that was declared and never applied

The avoidance layer scores every candidate velocity with the penalty rule of van den Berg, Lin and Manocha (2008):

penalty(v') = penalty_weight / ttc(v') + norm(v_pref - v')

where ttc(v') is the time to collision with the nearest neighbour if the agent took v'. The class had a horizon field from the beginning. It was documented, it was validated in __post_init__, and it never touched the penalty. Every predicted contact fed the first term no matter how far in the future it was.

Nothing failed. The head-on pair passed cleanly, the eight agent swap resolved, and the tests were green, which is why the field sat unused long enough to be worth writing about. What was actually broken was the meaning of penalty_weight. Without truncation, a crowded scene has no collision free candidate: keep going long enough in any direction and you meet somebody, so every candidate carries a non-zero weight / ttc term and the entire penalty landscape lifts with the weight. Once the weight is large enough, the candidate with the largest time to collision wins outright, and that candidate is a full stop.

With truncation, a candidate whose nearest predicted contact lies beyond the horizon scores exactly zero risk. The weight then multiplies nothing and the deviation term decides, which is what makes an unobstructed agent hold its preferred velocity exactly.

uv run python examples/publication_figures.py sweeps the penalty weight over the eight agent swap with the horizon applied and with it ignored. Ignored is reproduced by setting horizon=inf, which is exactly what an unused field amounts to.

Penalty weight	Horizon ignored, goal residual	Horizon 5.0 applied, goal residual
0.25	7.5563	7.5280
0.5	0.0000	0.0000
1.0	0.0000	0.0000
2.0	0.0000	0.0000
4.0	0.0000	0.0000
6.0	12.0000	0.0000
8.0	12.0000	0.0000
12.0	12.0000	0.0000

A residual of 12.0000 is the distance from an agent's start to its goal, so those three rows are runs in which nothing moved at all: the fraction of steps on which the preferred velocity survived the layer is 0.0000 for each of them, and every agent finished where it started. Truncation does not make the layer safer, it makes the weight mean something. Both columns fail at 0.25 for the opposite reason, with the risk term too small to pay for a detour, so agents hold their preferred heading into the crowd and stall there without touching, at a minimum separation of 1.1881. The lower edge of the usable range is not the horizon's doing.

The honest half of this. The eight agent swap and the head-on pair are both exactly symmetric, and in an exactly symmetric encounter every agent faces a mirror-image penalty landscape with equal reason to pass on either side. What resolves it is that argmin returns the first minimiser, so the side chosen falls out of the ordering of the candidate grid. That is an implementation artefact, not a mechanism. A symmetry breaking perturbation was the obvious fix and turned out not to be needed once the weight behaved, so there is none in the code, and the design notes record the tie break as a limitation rather than a feature. A deployment facing symmetric traffic should add an explicit passing convention.

The control laws, and where they live

Agents hold only local information: each knows its own position and the positions of the neighbours it can talk to, with no central coordinator. Three layers sit on top of that, and the third one exists because the first two treat agents as points and will happily route them through each other.

Agreement. The communication topology is a weighted graph whose Laplacian L = D - A has zero row sums by construction, so the all-ones vector spans its null space. The continuous protocol is x_dot = -L x and the discrete protocol iterates the Perron matrix P = I - eps L, from Olfati-Saber and Murray (2004) and Olfati-Saber, Fax and Murray (2007). On a connected undirected graph the disagreement decays exponentially at the Fiedler value, the second smallest Laplacian eigenvalue. That is the reason this repository leans on consensus: the topology alone predicts a rate, and a prediction can be falsified by a measurement.

Shape. Agreement collapses the swarm to a point, so holding a configuration means regulating displacements instead: u = -L (p - d). Substituting e = p - d recovers the consensus protocol exactly, so the same rate applies without restatement. Rendezvous is the degenerate case with every offset zero, and the implementation produces bit-identical commands to the plain consensus controller there, which a test asserts.

Safety. Two discs with relative position dp, combined radius R and relative velocity u overlap when norm(dp - u t) <= R, a quadratic in t whose first non-negative root is the time to collision. Solving it analytically rather than constructing the collision cone keeps the test dimension agnostic and lets it vectorise across every candidate velocity and every neighbour at once. The reciprocal formulation tests the reflected velocity 2 v' - v_i - v_j instead of v' - v_j, so each agent takes half the responsibility for the manoeuvre rather than reacting to a prediction its neighbour is simultaneously invalidating.

Where each piece lives, with dependencies running downward only. model imports nothing from the package and examples/ is wiring with no logic of its own.

Module	Responsibility
src/swarm_control/model/graph.py	Communication topology: adjacency, Laplacian, components, spanning tree, Fiedler value
src/swarm_control/model/state.py	Agent and swarm state, agent radii and speed bounds, velocity saturation
src/swarm_control/model/formation.py	Desired relative configuration, its canonical centred form, and feasibility against agent radii
src/swarm_control/algorithm/protocols.py	Controller, AvoidanceLayer, and ReferenceTrajectory protocols that make the stages swappable
src/swarm_control/algorithm/consensus.py	Continuous and discrete protocols, Perron matrix, exact matrix exponential solution, rate prediction
src/swarm_control/algorithm/formation.py	Displacement based formation law and its translation invariant error
src/swarm_control/algorithm/leader_follower.py	Reference trajectories, goal seeking, leader-follower law, closed form steady state lag
src/swarm_control/algorithm/velocity_obstacle.py	Time to collision, truncated VO and RVO membership, candidate sampling in the plane and in space, penalty selection
src/swarm_control/pipeline/simulator.py	Fixed step loop applying controller then avoidance, with no knowledge of either
src/swarm_control/pipeline/trace.py	Structured run record, including preferred and executed velocities, with archive round trip
src/swarm_control/analysis/metrics.py	Consensus error, formation error, tracking error, separation and clearance, empirical decay rate
src/swarm_control/analysis/figures.py	Trajectory, comparison, convergence, separation, and sweep figures on the Agg canvas, without pyplot global state

The alternatives considered and set aside, in particular artificial potential fields, rigidity based formation control, and ORCA, are recorded with their reasons in docs/design-notes.md, together with what this package still does not do.

Installation

Requires Python 3.12 or later. CI runs the whole suite on 3.12 and 3.13, on Linux and on Windows, so the version floor in pyproject.toml is a tested claim rather than a declared one.

git clone https://github.com/Eelis03/swarm-formation-control.git
cd swarm-formation-control
uv sync

Using pip instead of uv:

python -m venv .venv
.venv/bin/activate      # Windows: .venv\Scripts\activate
pip install -e ".[dev]"

The package ships a py.typed marker, so the annotations it is checked against are visible to a type checker in any project that installs it.

Running a scenario

Build a topology, pick a controller, and run it. The Fiedler value of the graph predicts the rate before anything is simulated.

import numpy as np

from swarm_control import (
    CommunicationGraph,
    ConsensusController,
    Simulator,
    SwarmConfig,
    SwarmState,
    consensus_error,
    convergence_report,
)

graph = CommunicationGraph.path(6)
start = SwarmState.at_rest(np.random.default_rng(7).uniform(-5.0, 5.0, size=(6, 2)))

trace = Simulator(
    controller=ConsensusController(graph),
    config=SwarmConfig.uniform(6),
    dt=0.005,
).run(start, steps=12000)

print(graph.algebraic_connectivity())  # 0.267949, the Fiedler value
print(convergence_report(graph, trace))  # predicted 0.267949, empirical 0.268129
print(consensus_error(trace)[-1])  # 3.429e-07

Adding the avoidance layer changes only the avoidance argument, which is how the safety contribution is isolated in the results below.

from swarm_control import GoalController, VelocityObstacleAvoidance, minimum_separation

goals = np.array([[5.0, 0.0], [-5.0, 0.0]])
config = SwarmConfig.uniform(2, radius=0.5, max_speed=1.0)
head_on = SwarmState.at_rest(np.array([[-5.0, 0.0], [5.0, 0.0]]))
controller = GoalController(goals, gain=2.0, speed=1.0)

guarded = Simulator(
    controller=controller,
    config=config,
    dt=0.02,
    avoidance=VelocityObstacleAvoidance(config, mode="rvo", margin=0.1),
)
print(minimum_separation(guarded.run(head_on, steps=900)))  # 1.1002, safety radius is 1.0

unguarded = Simulator(controller=controller, config=config, dt=0.02)
print(minimum_separation(unguarded.run(head_on, steps=900)))  # 0.0000, the agents pass through

Runnable examples live in examples/:

uv run python examples/consensus_convergence.py
uv run python examples/formation_keeping.py
uv run python examples/collision_avoidance.py
uv run python examples/leader_follower.py
uv run python examples/publication_figures.py

Each accepts --steps to shorten the run, --outdir to redirect figures, and --no-figures to skip figure generation.

Results

Every number below is the output of the command shown above it, on Python 3.12 with numpy 2.5.1, scipy 1.18.0 and matplotlib 3.11.1. Initial conditions are drawn from a fixed seed inside each script, so the values reproduce exactly.

Consensus against the Fiedler value prediction

uv run python examples/consensus_convergence.py, path graph on 6 agents, explicit Euler at dt = 0.005 for 12000 steps.

Quantity	Value
Fiedler value, lambda_2	0.267949
Predicted convergence rate	0.267949
Empirical convergence rate	0.268129
Relative error, measured against predicted	0.067050 %
Final consensus error	3.429e-07
Distance from agreement value to initial average	2.303e-15

The measured rate is a least squares fit to the log disagreement over the middle of the run; the prediction comes from the graph spectrum alone. The two lines being parallel over five decades is the claim, and the vertical offset between them is the opening transient, during which the faster eigenmodes are still decaying. The small positive bias in the rate is expected: explicit Euler contracts the slow mode by 1 - dt * lambda_2 per step, an effective rate of -log(1 - dt lambda_2) / dt, which exceeds lambda_2 by about dt * lambda_2 / 2, or 0.07 % here.

The discrete protocol on the same graph, with eps = 0.45 against a maximum stable step of 0.5, reaches the average to 4.767e-15 after 3000 iterations, with an agent-to-agent spread of 5.551e-16.

A disconnected graph does not reach consensus

Same protocol and same initial condition, with the topology split into two components of three agents.

Quantity	Value
Connected components	2
Fiedler value, lambda_2	3.925e-17
Distance to the per-component averages	2.366e-13
Separation between the two component averages	2.445472
Final consensus error	2.995079

Each component agrees internally to 2.4e-13 while the two limits stay 2.45 apart, so the swarm reaches two values and not one. The zero Fiedler value predicts this in advance.

Formation keeping

uv run python examples/formation_keeping.py, hexagon of radius 2.0, ring topology on 6 agents, dt = 0.005 for 6000 steps, agent radius 0.3.

Quantity	Without avoidance	With RVO avoidance
Initial formation error	13.935675	13.935675
Final formation error	5.055e-12	9.337e-12
Minimum separation over the run	0.348851	0.703149
Minimum clearance over the run	-0.251149	+0.103149
Collision free	no	yes

The formation is reached either way, but the unguarded run passes agents through each other on the way in: their contact distance is 0.600000 and they close to 0.348851. The predicted rate for this topology is 1.000000 and the measured rate is 1.002588, a relative error of 0.258794 %.

With a rendezvous formation the controller reduces to plain consensus. The maximum difference between the two commands is 0.000e+00, and the swarm collapses to a single point with a final consensus error of 3.159e-12.

Collision avoidance against its own control condition

uv run python examples/collision_avoidance.py. Agent radius 0.5, so the safety radius is 1.000. Controller, initial conditions, and step size are identical across the three rows of each block; only the avoidance layer differs.

Head-on encounter, two agents on the same line with swapped goals:

Avoidance layer	Minimum separation	Clearance	Collision free	Goal residual
none	0.0000	-1.0000	no	0.0000
velocity obstacle	1.1003	+0.1003	yes	0.0000
reciprocal velocity obstacle	1.1002	+0.1002	yes	0.0000

Antipodal swap, 8 agents crossing a circle of radius 6.0, the scenario drawn at the top of this page:

Avoidance layer	Minimum separation	Clearance	Collision free	Goal residual
none	0.0000	-1.0000	no	0.0000
velocity obstacle	0.0021	-0.9979	no	2.9019
reciprocal velocity obstacle	1.1391	+0.1391	yes	0.0000

The same swap in space, 6 agents crossing the centre of an octahedron of radius 4.0, with no argument of the layer changed and only the dimension of the state different:

Avoidance layer	Minimum separation	Clearance	Collision free	Goal residual
none	0.0000	-1.0000	no	0.0000
velocity obstacle	0.3172	-0.6828	no	0.0000
reciprocal velocity obstacle	1.1021	+0.1021	yes	0.0000

The three blocks make different points. In the pairwise encounter both formulations work: the unguarded agents pass exactly through each other at a separation of 0.0000, and enabling either layer raises that to 1.1002 with both agents still reaching their goals. Since nothing else changed between the rows, the avoidance layer is what produced the separation.

In the crowd the plain velocity obstacle fails outright. At 0.0021 it is no safer than running without avoidance, and its goal residual of 2.9019 shows the agents did not even arrive. This is the oscillation the reciprocal formulation exists to remove: each agent chooses against a prediction that its neighbours are simultaneously invalidating, so the group churns near the centre of the circle instead of resolving. The reciprocal formulation holds 1.1391 and reaches every goal, so the safety margin is not bought with deadlock.

The spatial block did not run at all until recently. The layer was planar only, because the candidate grid was built in polar coordinates, and it raised on any other dimension. The collision test itself was never planar, so the restriction lived entirely in the sampler: it is now a Fibonacci lattice over the sphere in three dimensions and the same polar grid in two. The cost is the candidate count, which is quadratic rather than linear in the angular resolution, so the default 16 headings become 256 directions and an agent scores 1282 candidates per step instead of 82. Four dimensions and above still raise, and the error says which dimensions are covered. The reasoning, and what the closure left behind, are in docs/design-notes.md.

Leader-follower tracking

uv run python examples/leader_follower.py, ring topology on 6 agents, leaders 0 and 3, hexagon formation, dt = 0.005 for 6000 steps. Only the two leaders receive the reference.

Reference	Tracking error, initial to final	Formation error, initial to final
Stationary at [8.0, 5.0]	11.242516 to 3.253237e-12	10.066059 to 4.575562e-12
Constant velocity, speed 0.447214	4.241016 to 0.365148	10.066059 to 0.516398

The stationary case converges to machine precision, which shows the reference propagating from the two leaders to the four followers through the graph alone. The moving case settles at a constant offset instead, because the followers have only proportional action against a ramp. That offset is not a fitted number: the steady state condition v_ref = -gain * L_ff f on the grounded Laplacian predicts a tracking error of 0.365148 and a formation error of 0.516398, matching the measurements to six figures, with a largest follower lag of 0.447214.

The figures, and what CI does not do with them

The three images above are snapshots, committed to docs/figures so that a reader sees them without running anything. One command regenerates all three and prints the measurements each one plots:

uv run python examples/publication_figures.py

They total 127 KB against a 250 KB budget, which the script reports on every run, at 110 dpi and a figure size chosen per plot rather than left at the matplotlib default. Nothing in CI compares them byte for byte. Matplotlib output is not byte reproducible across platforms, versions, or font configurations, so a hash comparison on a two platform matrix would fail on the rendering stack rather than on anything about this repository. What CI checks instead is that the code producing them still runs, through the integration tier that executes every script in examples/.

What is checked

uv run pytest
uv run ruff check .
uv run ruff format --check .
uv run mypy
uv run pytest --cov=src/swarm_control --cov-report=term-missing

156 tests run in well under a minute at 91 % line coverage. CI enforces a floor of 89 %, the measured value rounded down and reduced by two, so a platform difference in which branches execute cannot fail the build on its own while a real drop in tested code still does.

The suite has three tiers. The property tier asserts, among others, that every Laplacian has zero row sums and annihilates the all-ones vector; that the multiplicity of the zero eigenvalue equals the number of connected components; that a connected undirected graph reaches the average of the initial states while a disconnected one reaches per-component averages that stay apart; that the measured decay rate matches the Fiedler value within a relative tolerance of 2 %, a bound stated in tests/conftest.py together with the discretisation bias it accommodates; that the minimum separation in a head-on encounter stays above the safety radius with avoidance and falls below it without, in the plane and in space, which is what attributes the separation to that layer; that the spherical candidate lattice has no preferred axis, asserted through its resultant; and that the formation error converges to zero for every feasible formation. The integrator is separately checked against the exact matrix exponential solution and confirmed to be first order in the step size.

The regression tier replays a recorded run that exercises the graph, the formation law, and the RVO layer together, comparing positions, preferred velocities, and executed velocities against tests/data at an absolute tolerance of 1e-9. Because the avoidance layer selects one entry from a discrete candidate set, a change in the ordering or the penalty rule appears as a discontinuity rather than a drift. The archive is also asserted against the properties it is meant to demonstrate, so a wrong recording cannot pass by matching itself. It can be regenerated with uv run python tests/test_regression.py.

The integration tier runs every script in examples/ under a reduced iteration count, with and without figure generation, and fails if the list of scripts and the contents of the directory drift apart.

References

Algorithms

• Olfati-Saber, R. and Murray, R. M. "Consensus Problems in Networks of Agents with Switching Topology and Time-Delays." IEEE Transactions on Automatic Control, 49(9):1520 to 1533, 2004. DOI: 10.1109/TAC.2004.834113. Source of the continuous protocol and of the result that the disagreement decays at the Fiedler value on a connected undirected graph.
• Olfati-Saber, R., Fax, J. A. and Murray, R. M. "Consensus and Cooperation in Networked Multi-Agent Systems." Proceedings of the IEEE, 95(1):215 to 233, 2007. DOI: 10.1109/JPROC.2006.887293. Source of the Perron matrix formulation of the discrete protocol and of the step size bound that keeps it row stochastic.
• Ren, W. and Beard, R. W. "Consensus Seeking in Multiagent Systems Under Dynamically Changing Interaction Topologies." IEEE Transactions on Automatic Control, 50(5):655 to 661, 2005. DOI: 10.1109/TAC.2005.846556. Source of the directed spanning tree condition, implemented as the rank test in CommunicationGraph.has_spanning_tree.
• Ren, W., Beard, R. W. and Atkins, E. M. "Information Consensus in Multivehicle Cooperative Control." IEEE Control Systems Magazine, 27(2):71 to 82, 2007. DOI: 10.1109/MCS.2007.338264. Source of the displacement based formation law and its reduction to consensus in the shifted coordinate.
• Fiedler, M. "Algebraic Connectivity of Graphs." Czechoslovak Mathematical Journal, 23(2):298 to 305, 1973. DOI: 10.21136/CMJ.1973.101168. Origin of the algebraic connectivity and of the monotonicity property asserted in the graph tests.
• Fiorini, P. and Shiller, Z. "Motion Planning in Dynamic Environments Using Velocity Obstacles." The International Journal of Robotics Research, 17(7):760 to 772, 1998. DOI: 10.1177/027836499801700706. Source of the velocity obstacle and of the collision cone this implementation evaluates analytically.
• van den Berg, J., Lin, M. C. and Manocha, D. "Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation." IEEE International Conference on Robotics and Automation (ICRA), pages 1928 to 1935, 2008. DOI: 10.1109/ROBOT.2008.4543489. Source of the reciprocal formulation and of the candidate selection penalty weight / ttc(v') + norm(v_pref - v') whose horizon term is the subject of the first section above.
• van den Berg, J., Guy, S. J., Lin, M. C. and Manocha, D. "Reciprocal n-Body Collision Avoidance." Robotics Research, Springer Tracts in Advanced Robotics, volume 70, pages 3 to 19, 2011. DOI: 10.1007/978-3-642-19457-3_1. The ORCA successor, considered and not implemented; the reasoning is in docs/design-notes.md.
• Gonzalez, A. "Measurement of Areas on a Sphere Using Fibonacci and Latitude-Longitude Lattices." Mathematical Geosciences, 42(1):49 to 64, 2010. DOI: 10.1007/s11004-009-9257-x. Source of the Fibonacci lattice used to sample candidate headings in three dimensions, where the polar grid of the planar case has no direct analogue.

Dependencies

• numpy (BSD-3-Clause). Array representation of states, adjacency and Laplacian matrices, and every vectorised control computation, including the batched time to collision evaluation across candidate velocities.
• scipy (BSD-3-Clause). scipy.linalg.expm supplies the exact matrix exponential solution of the continuous protocol, which serves as the integration-error-free reference the simulator is validated against.
• matplotlib (matplotlib license, a BSD-style permissive license). Trajectory, comparison, convergence, separation and sweep figures, used through the Agg canvas so no interactive backend is required.
• pytest and pytest-cov (both MIT), ruff (MIT) and mypy (MIT). Development only: test runner, coverage measurement, linter, and static type checker.

License

Released under the MIT license. See LICENSE.