Nonholonomic Motion Planning as Efficient as Piano Mover's

Abstract: We present an algorithm for non-holonomic motion planning (or 'parking a car') that is as computationally efficient as a simple approach to solving the famous Piano-mover's problem, where the non-holonomic constraints are ignored. The core of the approach is a graph-discretization of the problem. The graph-discretization is provably accurate in modeling the non-holonomic constraints, and yet is nearly as small as the straightforward regular grid discretization of the Piano-mover's problem into a 3D volume of 2D position plus angular orientation. Where the Piano mover's graph has one vertex and edges to six neighbors each, we have three vertices with a total of ten edges, increasing the graph size by less than a factor of two, and this factor does not depend on spatial or angular resolution. The local edge connections are organized so that they represent globally consistent turn and straight segments. The graph can be used with Dijkstra's algorithm, A*, value iteration or any other graph algorithm. Furthermore, the graph has a structure that lends itself to processing with deterministic massive parallelism. The turn and straight curves divide the configuration space into many parallel groups. We use this to develop a customized 'kernel-style' graph processing method. It results in an N-turn planner that requires no heuristics or load balancing and is as efficient as a simple solution to the Piano mover's problem even in sequential form. In parallel form it is many times faster than the sequential processing of the graph, and can run many times a second on a consumer grade GPU while exploring a configuration space pose grid with very high spatial and angular resolution. We prove approximation quality and computational complexity and demonstrate that it is a flexible, practical, reliable, and efficient component for a production solution.