MST-based method for 6DOF rigid body motion planning in narrow passages

We consider a problem of rigid body motion planning in a static 3D environment. In the past, methods based on random sampling like Probabilistic Roadmap and its variants proved to be able to efficiently solve many important instances of that problem. Performance of these methods degrades drastically in the presence of narrow passages. We propose a different approach to motion planning which combines elements of both cell decomposition methods and sampling based methods. We estimate signed distance to the boundary of free space at sampling points and use that information to guide farther exploration. Cell decomposition is used to generate deterministic sampling positions with non-uniform and dynamically adjusted densities. We report the results of experiments with implementation of our method.