Path Planning for a Cooperative Navigation Aid Vehicle to Assist Multiple Agents Sequentially

TL;DR

This work addresses planning a single Cooperative Navigation Aid (CNA) to sequentially aid multiple underwater agents while minimizing the average navigation uncertainty under a mission time constraint. It develops a planar, constant-velocity model with scalar discrete-time Kalman filters, and derives a closed-form optimal time-to-aid for a single interception, coupled with a greedy, task-sequencing algorithm that uses a composite reward to balance uncertainty reduction, timing, and travel cost. The main contributions are the optimal time-to-aid expression and the greedy algorithm, validated by Monte Carlo simulations against exhaustive enumeration, showing improved cost with efficient computation. The approach offers a practical high-level scheduling method for CNAs in multi-agent navigation scenarios, with potential extensions to revisits, multi-agent servicing, and trajectories of unknown agents.

Abstract

This paper considers planning a path for a single underwater cooperative navigation aid (CNA) vehicle to sequentially aid a set of N agents to minimize average navigation uncertainty. Both the CNA and agents are modeled as constant-velocity vehicles. The agents travel along known nominal trajectories and the CNA plans a path to sequentially intercept them. Navigation aiding is modeled by a scalar discrete time Kalman filter. During path planning, the CNA considers surfacing to reduce its own navigation uncertainty. A greedy planning algorithm is proposed that uses a heuristic to schedule agents to the CNA that is based on the optimal time-to-aid, the overall navigation uncertainty reduction, and the transit time. The approach is compared to an optimal (exhaustive enumeration) algorithm through a Monte Carlo experiment with randomized agent trajectories and initial navigation uncertainty.

Figures (4)

• Figure 1: Geometry for computing the time to intercept agent $i$ by the CNA.
• Figure 2: Agent cost function and agent variance with time for the case of $(\nu_{0|0}^i,\nu^c_{Z|Z}) = (100,10)$ (top), $(\nu_{0|0}^i,\nu^c_{Z|Z}) = (1000,10)$ (middle), $(\nu_{0|0}^i,\nu^c_{Z|Z}) = (100,1000)$ (bottom) assuming that $T = 2000$. The optimal time-to-aid, as computed by \ref{['eq:zstar']}, is indicated by a square marker in each case.
• Figure 3: Top: Trajectories of agents (dashed line) along with their starting locations (circular markers) and end locations ("x" markers) with an overlay of an uncertainty circle indicating $\nu_{k|k}^i$ along the path. The optimized path of the CNA is shown as the solid black line. Middle and bottom: navigation variance as a function of time for the agents and CNA. Discontinuities in $\nu_{k|k}^i$ indicate an aiding measurement was received by the agent. The discontinuity in $\nu_{k|k}^c$ indicates the CNA completed a surfacing.
• Figure 4: Monte Carlo simulation results. The cost bounds from \ref{['eq:bounds']} are shown as dashed lines in the top panel.