Minimum Constraint Displacement Motion Planning

This paper formulates a new minimum constraint displacement (MCD) motion planning problem in which the goal is to minimize the amount by which constraints must be displaced in order to yield a feasible path. It presents a sampling-based planner that asymptotically approaches the globally optimal solution as more time is spent planning. Key developments are efficient data structures that allow the planner to select small subsets of obstacles and their displacements that are candidates for improving the current best solution, and local optimization methods to improve convergence rate. The resulting planner is demonstrated to successfully solve MCD problems with dozens of degrees of freedom and up to one hundred obstacles.