CONSORT: Uncertainty-Constrained Multi-Robot Planning for Cooperative Localization in GPS-Denied Environments

Hexagonal lattice with an obstacle in the middle. The primary agent's path passes above the obstacle to the goal; the support agent's path dips below it toward a landmark, takes a reading, and relays position data to the primary near the goal. — The escort problem. The primary must reach the goal with its localization uncertainty under a terminal bound, and the support is routed to hold it there. The support agent localizes with a landmark and relays position data, enabling the primary agent to take a more direct path.

Abstract

Centralized multi-agent path planning with cooperative localization in GPS-denied environments is generally computationally intractable when formulated in full belief space due to tightly coupled uncertainty dynamics. Prior work typically approximates belief dynamics or restricts inter-agent coupling. This work examines a multi-agent planning problem in which a primary agent must reach a goal in minimum time subject to a chance-constrained collision-avoidance requirement and a terminal bound on localization uncertainty. We obtain a tractable surrogate of the underlying belief-space problem by optimizing over the mean-state trajectories of all agents and propagating covariance for terminal constraint evaluation only. We propose CONSORT (Cooperative Navigation with Support-Optimized Routing and Trajectories), a discrete-to-continuous pipeline combining hexagonal-grid joint search with spline-based trajectory refinement. Across 50 randomized scenarios, the planners separate as the terminal uncertainty bound tightens, with CONSORT solving 80% of scenarios where the strongest baseline solves 54%, at comparable path length. In marine simulations, with the plans executed under feedback control on an independently developed factor-graph estimate, the trajectories remain collision-free and their terminal uncertainty falls within 6% of the planner’s prediction.

Method

Optimize the mean trajectories of the whole team, and propagate covariance along them only to check the constraints.

Stage 1

Joint search on a hexagonal lattice

Each graph node pairs a hexagonal cell with one of six headings, and three moves leave it: straight, or a ±60° turn into the cell faced by the new heading, the lattice’s counterpart of the turn-radius bound. An A*-style search advances every agent one edge per expansion, propagates each covariance along its edge, and fuses them at communication checkpoints. Pareto pruning over path length and covariance keeps the joint space manageable, and the search is anytime with respect to the terminal bound.

Stage 2

Continuous B-spline refinement

The seed becomes clamped cubic B-splines with its cell centers as control points. Each segment is enclosed in its MINVO hull, which makes obstacle clearance linear in the control points. A log-barrier merit function, minimized with Adam, shortens the primary’s path while keeping every constraint satisfied, and the same objective pulls an infeasible seed toward feasibility. Refinement cuts the lattice’s zigzag corners, shortening the seed by up to 7%.

Stage

Primary agent
Support agent
Lattice seed
Pruned move
Control point
Start
Goal
Landmark
Obstacle

States expanded: 1748
Pruned moves: 2593
Frontier g: 1200 m

The joint A* advances both agents one lattice edge per expansion. Each path is a state it popped, replayed in order of the primary’s path length g rather than pop order, so the frontier sweeps outward. Red crosses are moves pruned for crossing the island. It returns a 1200 m seed whose primary ends at σ = 1.92 m, inside the 2.0 m bound.

Problem formulation and belief model

The joint belief of a team grows exponentially with its size, and inter-agent measurements couple the agents’ beliefs. CONSORT therefore takes as decision variables only the heading profile of every agent and the primary’s path length S₁. All agents move at one constant speed, so arc length is a shared clock and minimum time means minimum length:

\begin{aligned} \min_{\{\theta_i(\cdot)\},\,S_1}\quad & S_1 \\ \text{s.t.}\quad & \det\big(\Sigma_1(S_1)\big)^{1/4} \le \bar u \\ & a_j^{\top} x_i(s) \ge b_j + r_a + z_\delta \sqrt{a_j^{\top}\big(\Sigma_i(s) + \Sigma_o\big)\,a_j} \\ & |\kappa_i(s)| \le 1/r_{\min} \\ & x_i(0) = x_i^0,\quad \Sigma_i(0) = \Sigma^0,\quad x_1(S_1) = G \end{aligned}

Constraints

Terminal uncertainty: The primary must arrive with det(Σ)^1/4 ≤ ū, the standard deviation of an isotropic covariance of the same determinant.
Chance-constrained clearance: Every agent clears every obstacle with probability at least 1 − δ at every instant. The margin covers the vehicle radius and the position uncertainty of both agent and obstacle.
Turn radius: Each trajectory keeps its curvature within 1/r_min.
Boundary conditions: Agents start from known positions with a common prior covariance, and the primary must end at the goal G.

Belief model

Covariance is not a decision variable. It is a deterministic function of the mean trajectories, built from three parts:

Dead reckoning: Between measurements, position variance grows linearly with distance, faster along the direction of travel than across it.
Landmark fixes: Range-bearing measurements whose weight decays smoothly with distance through a logistic taper, with no hard cutoff.
Covariance intersection: At fixed arc-length intervals, agents exchange estimates and fuse them by covariance intersection, which stays consistent under unknown cross-correlation.

The constraint ladder

One scenario, four terminal bounds. Every tightening is paid for in primary path length.

Terminal bound

Primary agent
Support agent
Start
Goal
Landmark
Obstacle
Landmark reading
Communication
Beyond range, reduced weight
Communication range

Bound ū: 13.75 m
Primary length: 1741 m
Terminal σ: 9.73 m

(a) The support climbs into the north pocket for the weak landmark there and rejoins.

The four plans of Fig. 2, replayed from the planner’s runs: a 1700 m corridor with 14 obstacles and three landmarks. Green links are the planner’s exchanges, scheduled every 100 m of arc and faded by its weight. The weight halves at the 300 m communication range but has no hard cutoff, so an exchange beyond the range still counts, at reduced weight, and is drawn dashed. Orange lines are landmark readings. The faint ring, the primary’s communication range, appears as the support nears its edge.

Benchmark results

50 randomized scenarios with one primary and one support, as the terminal bound tightens from 100% to 30% of the single-agent reference.

80% vs. 54%

of scenarios solved at half the reference bound, against the strongest baseline

1.14–1.41×

mean primary path length relative to the single-agent reference, from the loosest bound to the tightest

18 s

median wall-clock time per run, against 14 s for Formation and Sequential

Success rate versus constraint level for CONSORT and four baselines. — Fig. 3. Fraction of the 50 scenarios solved (left) and mean primary path length over the solved scenarios, normalized by the single-agent reference length (right), as the bound tightens.

Mean primary path length over the solved scenarios, relative to the single-agent reference, versus constraint level. — Fig. 3. Fraction of the 50 scenarios solved (left) and mean primary path length over the solved scenarios, normalized by the single-agent reference length (right), as the bound tightens.

Mean and standard deviation of primary path length, normalized by the single-agent reference length, at four constraint levels.
Method	Constraint level (% of U_ref)
Method	100	70	50	30
Baseline comparison, 50 screened scenarios
CONSORT (ours)	1.14 ± 0.09	1.22 ± 0.23	1.34 ± 0.26	1.41 ± 0.21
Formation	1.22 ± 0.12	1.34 ± 0.25	1.33 ± 0.16	1.48 ± 0.45
Sequential	1.18 ± 0.14	1.41 ± 0.92	1.36 ± 0.50	2.76 ± 1.51
CL-GBT	1.47 ± 0.33	1.78 ± 0.43	2.01 ± 0.41	2.07 ± 0.26^†
Greedy	1.24 ± 0.10	1.21 ± 0.06	1.13 ± 0.01^†	1.13 ± 0.01^†
Ablation, 30 unscreened scenarios
CONSORT (full)	1.12 ± 0.14	1.17 ± 0.23	1.22 ± 0.22	1.29 ± 0.22
Discrete only	1.20 ± 0.15	1.23 ± 0.23	1.25 ± 0.22	1.29 ± 0.22
Straight seed + cont.	0.99 ± 0.04	1.00 ± 0.00^†	–	–

Table I. Mean ± SD of primary path length over each method’s own solved scenarios, normalized by L_ref. Entries are not comparable across methods without the success rates, since a method that solves only its easiest scenarios reports a short path. † marks fewer than five solved scenarios; “–” means none solved.

Baselines

All four share CONSORT’s covariance model, obstacle test, and refinement. They differ in how they route the team.

Greedy

The primary takes its shortest route; the support takes whichever next step most reduces the primary’s uncertainty.

Formation

The primary plans alone while the support holds a fixed position beside it.

Sequential

Agents plan one at a time: the primary, then the support against it, then the primary once more.

CL-GBT

A sampling-based tree over both agents’ joint state, after Theurkauf et al., instead of a lattice search.

Validation in HoloOcean

One plan, executed 30 times with a vehicle, controller, and estimator that share none of the planner’s equations.

Two torpedo-shaped autonomous underwater vehicles over the seabed in the HoloOcean simulator. — Fig. 7. The two vehicles in HoloOcean, each following its planned spline at a fixed depth over the seabed.

Planned and mean executed trajectories of both agents with three-sigma cross-track envelopes around a wall. — Fig. 8. Closed-loop Monte Carlo of the behind-wall plan, with each agent’s mean executed trajectory and ±3σ cross-track envelope. The primary passes north of the wall while the support diverts south to the landmark and rejoins before the goal.

30 / 30

runs reached the goal within 2% of the planned length, and none penetrated the wall

1.70 m vs. 1.80 m

terminal σ measured from the true error, against the planner’s prediction

28 / 30

runs with the estimator’s terminal marginal under the 1.8 m bound

Every planner in the benchmark shares one covariance model, so the benchmark cannot tell whether that model is calibrated. Here torpedo-shaped vehicles follow the plan under HoloOcean 2.0’s depth and heading autopilot, steering on an independently developed GTSAM factor-graph estimate, with acoustic exchanges every 100 m that succeed only within the planner’s communication range. The true terminal error came within 6% of the prediction, on the conservative side, though it also carried a 2.6 m along-track bias that no covariance check captures.

BibTeX

@misc{zhang2026consort,
  title         = {{CONSORT}: Uncertainty-Constrained Multi-Robot Planning for Cooperative Localization in {GPS}-Denied Environments},
  author        = {Tony Zhang and Ryan Bartholomew and Kalliyan Velasco and Joshua Mangelson and Brady Moon},
  year          = {2026},
  eprint        = {2610.41235},
  archivePrefix = {arXiv},
  primaryClass  = {cs.RO},
  url           = {https://arxiv.org/abs/2610.41235}
}