Trajectory Optimization for Cooperative Navigation Applications

Abstract

Translate Article

External navigational aiding sources such as satellite navigation systems or other static beacons with local coverage are usually required to correct drifts and errors arising from standalone inertial and vision-based navigation. Cooperative navigation enables external aiding among mobile vehicles that share location fixes and reduce each others' localization uncertainty. Each vehicle in a cooperative team may have its own independent objective aside from the joint objective of reducing the team's localization uncertainty. We consider a formulation where areas in the environment with varying levels of navigational aiding are represented by a spatiotemporally varying scalar field, which we refer to as the benefit field. Static (immobile) navigation aids such as beacons can be represented by a time-invariant benefit field. Mobile aids such as one or more cooperative navigational aiding vehicle, can be represented by time-varying benefit fields. The novelty of this work is in the application of optimal control techniques to cooperative navigation. We study optimal trajectory generation to maximize exposure to navigational aiding, thereby reducing localization uncertainty. To this end, we study solutions of the boundary value problem resulting from variational necessary conditions, as well as direct trajectory optimization using the Legendre pseudospectral method. Through numerical simulations, we demonstrate the effectiveness of the proposed methods in reducing the localization uncertainty of the agent.