A Perturbation Refinement Method for Nonholonomic Motion Planning

The kinematic path planning problem of a nonholonomic system can be posed as a point-to-point control problem of a nonlinear system. This system has the property that it is not locally stabilizable but is globally controllable. In this paper, we present a new algorithm for constructing a feasible path between specified initial and final configurations. Polyhedral constraints in both configuration and non-configuration spaces (possibly non-convex) can also be incorporated in this framework. The algorithm is based on the assumption that the system is usually controllable about a non-trivial trajectory, which is true for many cases. An initial trajectory is iteratively warped until the desired final configuration is reached. Examples of a front-wheel driven car, and tractors with one, two, and three trailers are included to illustrate the efficacy of the proposed algorithm.