On Free Velocity Cones of Narrow Passages of High-DoF Kinematic Chains

Narrow passages in free space pose great challenges to many sampling-based motion planners. In problems with high-dimensional configuration spaces (C-spaces), narrow passages in free space (C-free) can be hard to find and navigate and, sometimes, even counterintuitive. In this article, we present an algorithm to construct cones of available instantaneous velocities (“free velocity cones” or, briefly, “free cones”) on the boundary of C-free to facilitate finding and navigating narrow passages. This is accomplished by first developing free cones and associated measures of local C-space narrowness for a single rigid link. These results are then extended to kinematic chains (open and closed) of arbitrary degrees of freedom. It turns out that the degeneracy of the free cones dictates the existence of C-space narrow passages, and the locations and orientations of the links in the workspace relate intimately to the corresponding connected component of C-free. This observation leads us to a modified probabilistic roadmap (M-PRM) algorithm and a modified rapidly exploring random vine (M-RRV) algorithm of combining the enumeration of topological components and random sampling of the free cones. Experimental results from applying our algorithms to several challenging examples show that our new algorithms are more efficient than some variants of the PRM algorithms, and several variants of the rapidly exploring random tree (RRT) algorithms, such as RRT-CONNECT and RRV.