This paper presents an algorithm for finding a kinematically feasible path for a nonholonomic system in the presence of obstacles. We first consider the path planning problem without obstacles by transforming it into a nonlinear least squares problem in an augmented space which is then iteratively solved. Obstacle avoidance is included as inequality constraints. Exterior penalty functions are used to convert the inequality constraints Into equality constraints. Then the same nonlinear least squares approach is applied. We demonstrate the efficacy of the approach by solving some challenging problems, including a tractor-trailer and a tractor with a steerable trailer backing in a loading dock. These examples demonstrate the performance of the algorithm in the presence of obstacles and steering and jackknife angle constraints.
IEEE Transaction on Robotics and Automation, 13(3), June, 1997, pp.443-451.