Multiobjective Path Planning: Localization Constraints and Collision Probability

We present a novel path planning algorithm that, starting from a probabilistic roadmap, efficiently constructs a product graph used to search for a near optimal solution of a multiobjective optimization problem. The goal is to find paths that minimize a primary cost, such as the path length from start to goal, subject to a bound on a secondary cost such as the state estimation error covariance. 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 product graph whose number of vertices and edges provides a good tradeoff between optimality and computational complexity. We further show how our approach can be extended to handle constraints on the probability of collision avoidance specified at every vertex along the path. Numerical examples show 1) how the computed paths change as a function of the specified bound on the secondary costs, and 2) the tradeoff between accuracy and computational efficiency of the proposed approach compared with methods where the product graph is built by quantizing the secondary cost uniformly.