Collision-probability constrained PRM for a manipulator with base pose uncertainty

We address the motion planning problem for a manipulator system with base pose uncertainty, e.g., when the manipulator is mounted on a mobile base. Using a particle based representation for the uncertainty, we extend the PRM (probabilistic roadmap) approach to deal with this base uncertainty. Because of the uncertainty, a path for the manipulator is associated with a probability of being collision-free, which fundamentally changes the nature of the PRM´s query phase. We plan for a shortest path such that the probability of the manipulator being collision-free is higher than a user defined threshold, were the manipulator to follow the path. The path query problem becomes a collision probability constrained shortest path problem (CP-CSPP), and is shown as NP-hard w.r.t. the number of the particles. We then present a lazy query algorithm, called Lazy-CPC-PRM (collision probability constrained LazyPRM), based on a k-shortest path algorithm in conjunction with a labeling algorithm. Lazy-CPC-PRM exploits a key insight that if a portion of a path considered by the algorithm is invalid (the probability of it being collision-free is less than a threshold) or is dominated by another sub-path, then all the longer paths containing this portion can not be the solution path. This leads to significant efficiency gains in practice. Although, worst case complexity is exponential in the number of particles, we empirically show the effectiveness of our query algorithm with 30 particles for a simulated 3-dof manipulator mounted on a mobile base.