Infrastructure Planning for Multi-Vehicle Routing with Cooperative Localization

Onboard sensing is crucial for autonomous navigation of Unmanned Vehicles (UVs) in challenging environments where relying solely on Global Navigation Satellite Systems (GNSS) can be disadvantageous. Navigation solutions featuring onboard sensing can be can be applied for navigating with respect to both known or unknown environments. The scope of this paper pertains to the problem of multi-vehicle cooperative navigation in a known environment, where known features are identified for localization. The location of features is crucial for localization accuracy. We propose a precise feature-injection solution based on the observability property, which dictates localization accuracy, to exercise control over vehicle trajectories. Specifically, for a given nonlinear multi-vehicle system, we are able to identify the optimal locations for feature placement while simultaneously minimizing the required number of features. The efficacy of the proposed approach is validated via various simulations.