Nonholonomic path planning with inequality constraints

This paper presents an algorithm for finding a kinematically feasible path which satisfies a given set of nonholonomic constraints while enforcing both equality and inequality constraints on the configuration vector. The path planning problem is transformed from a finite time nonlinear control problem into a static root finding problem which is iteratively solved. By using an exterior penalty function method, the constrained root finding problem is converted to an unconstrained problem. Convergence of the algorithm depends upon a certain gradient operator being full rank. It has recently been shown that, in the absence of any configuration inequality constraints, the full rank condition is generic. In this paper, we show the full rank condition for a special inequality constraint case. An experiment in which the algorithm is applied to an actual double tractor-trailer vehicle is presented