Multi-objective path planning in GPS denied environments under localization constraints

The main contribution of this paper is a novel planning algorithm that, starting from a probabilistic roadmap, efficiently constructs an expanded graph used to search for the optimal solution of a multi-objective problem. The primary cost is the shortest path from start to goal and the secondary cost is related to the state estimation error covariance. This needs to be optimized as we assume the navigation to be in a GPS denied environment. The proposed algorithm is efficient as it relies on a scalar metric, related to the largest eigenvalue of the error covariance, and adaptively quantizes the secondary cost, yielding a graph whose number of vertices and edges provides a good tradeoff between optimality and computational complexity. Numerical examples show the advantage of the proposed approach compared to methods where the expanded graph is built by quantizing the secondary cost uniformly.