Near-optimal motion planning for nonholonomic systems with state/input constraints via quasi-Newton method

An optimal motion planning scheme using quasi-Newton method is proposed for nonholonomic systems. A cost functional is used to incorporate the final state errors, control energy, and constraints on states and controls. The motion planning is to determine control inputs to minimize the cost functional and is formulated as a nonlinear optimal control problem. By using the control parametrization, one can transform an infinite-dimensional optimal control problem to a finite-dimensional one and use quasi-Newton method to solve for a feasible trajectory which satisfies nonholonomic constraints and state/input constraints. The proposed scheme was applied to a free-floating robot for numerical simulation. A three-link planar floating robot was designed and built to verify the proposed scheme. The robot consists of two one-link arms connected to a main base via revolute joints. From the experimental results, the proposed optimal motion planning scheme controls the robot from initial state to final state along the planned path within ±0.1 rad final position error