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

Tony Zhang; Ryan Bartholomew; Kalliyan Velasco; Joshua Mangelson; Brady Moon

cs.ROOct 30, 2026

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

Authors: Tony Zhang, Ryan Bartholomew, Kalliyan Velasco, Joshua Mangelson, Brady Moon

Organizations: Authors are with the FROST Lab, Ira A. Fulton College of Engineering, Brigham Young University, Provo, UT 84602, USA. · T. Zhang is with the Kahlert School of Computing, University of Utah, Salt Lake City, UT 84112, USA, and contributed to this work as a visiting undergraduate researcher in the FROST Lab.

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.

Figures & tables

Fig. 1: 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. Support agent localizes with landmark and relays position data , enabling the primary agent to take a more direct path. Both trajectories are searched on the heading-aware hexagonal lattice and refined as B-splines.

Fig. 4: Wall-clock time per planner over successful runs at five constraint levels (log scale). Boxes span the interquartile range, whiskers extend to

1.5\times

IQR, and runs beyond the whiskers are omitted. Levels with fewer than five solved scenarios are drawn as individual runs.

	Constraint level (% of $U_{\mathrm{ref}}$ )
Method	100	70	50	30
Baseline comparison, 50 screened scenarios
CONSORT (ours)	1.14 $\pm$ 0.09	1.22 $\pm$ 0.23	1.34 $\pm$ 0.26	1.41 $\pm$ 0.21
Formation	1.22 $\pm$ 0.12	1.34 $\pm$ 0.25	1.33 $\pm$ 0.16	1.48 $\pm$ 0.45
Sequential	1.18 $\pm$ 0.14	1.41 $\pm$ 0.92	1.36 $\pm$ 0.50	2.76 $\pm$ 1.51
CL-GBT	1.47 $\pm$ 0.33	1.78 $\pm$ 0.43	2.01 $\pm$ 0.41	2.07 $\pm$ 0.26 †

TABLE I: Mean

\pm

SD of primary path length over each method’s own solved scenarios, normalized by

L_{\mathrm{ref}}

. Entries are not comparable across methods without the success rates in Figs. 3 and 5 , since a method that solves only its easiest scenarios reports a short path. † marks fewer than five solved scenarios. “–” means none solved.

Factor	Variables	$\sigma$
Odometry (between, per 5 m node)	$\mathbf{T}_{k-1},\mathbf{T}_{k}$	1 ∘ /axis; 0.22, 0.02, 0.01 m
Depth (unary)	$\mathbf{T}_{k}$	0.5 m
Compass (unary)	$\mathbf{T}_{k}$	0.86 ∘ /axis
Landmark az./el./range	$\mathbf{T}_{k},\ell$	1.05 ∘ , 1.05 ∘ , 2.09 m; $r<100$ m
Inter-agent relative pose	$\mathbf{T}^{s}_{k},\mathbf{T}^{p}_{k}$	0.86 ∘ /axis; $0.5/\sqrt{w}$ m
Initial pose prior	$\mathbf{T}_{0}$	10 ∘ /axis; 1.15, 0.76, 0.5 m

TABLE II: Factor noise of the simulated estimator, derived from the planner’s model: odometry from the

q_{\parallel}

, the compass from

q_{\perp}

, landmark noise rescaled to one sighting per 5 m node, communication noise

\sigma_{c}/\sqrt{w}

for link weight

w

, and the start prior from

\Sigma^{0}

. The depth factor keeps the stack’s 0.5 m against the 0.01 m injected.

Multi-Robot Motion Planning

Multi-Robot Motion Planning coordinates collision-free paths and trajectories for robot teams sharing space, tasks, and changing environments.

See all 150 papers

Optimal Path Planning with Localization constraints
Oct 29, 2026Chinmay Mundane, Manav Mishra, PB SujitUAV Navigation Constrained Motion Planning
IISER Bhopal, India
Belief-Aware Multi-Agent Path Finding under Map Uncertainty
Sep 30, 2026Viraj Parimi, Shao-Hung Chan, Han Zhang +2Belief-Space Planning Gaussian Belief Propagation
Computer Science and Artificial Intelligence Laboratory, Massachusetts Institute of Technology, Cambridge, MA 01239. · Symbotic Inc., Wilmington, MA 01887, USA
Multi-Agent Next-Best-View Optimization for Risk-Averse Planning
Jun 2, 2026Amirhossein Mollaei Khass, Vivek Pandey, Guangyi Liu +3Next-Best-View Planning Multi-Robot Exploration
Department of Mechanical Engineering and Mechanics, Lehigh University · Amazon Robotics

Oct 29, 2026J/K move · Enter open · S save

Optimal Path Planning with Localization constraints

Chinmay Mundane, Manav Mishra, PB Sujit

IISER Bhopal, India

UAV Navigation Constrained Motion Planning

Open paper AskPDF arXiv

This paper studies UAV path planning in areas where GPS signals are not present. Because these drones can easily get lost when they don't have GPS, they must rely on dead reckoning and occasionally pass near cellular towers, which act as landmarks to reset and fix their position. The goal is to find the shortest path from a start to a goal location while keeping the estimation uncertainty within a given bound. To solve this problem, we propose a localization-constrained A* planner that treats uncertainty as a hard constraint during search. The robot state and covariance are propagated using an Extended Kalman Filter (EKF), and only paths that satisfy the covariance limit are considered. We also show that the proposed method does not affect heuristic used in A* and remains admissible and consistent, ensuring optimality among all feasible paths. Extensive Monte Carlo simulations and PyBullet quadrotor test show that the proposed method finds shorter and more reliable paths compared to RRT* and PRM, while always satisfying the uncertainty constraint.

Abstract

Multi-Robot Motion Planning

Explore similar work

Optimal Path Planning with Localization constraints

Belief-Aware Multi-Agent Path Finding under Map Uncertainty

Multi-Agent Next-Best-View Optimization for Risk-Averse Planning