A gradient-based approach to collision-free quasi-optimal trajectory planning of nonholonomic systems

This paper discusses the optimal trajectory planning problem of nonholonomic systems in the presence of obstacles. The problem can be formulated as an optimal control problem by incorporating final state errors and obstacle avoidance conditions in the cost function. To solve the highly nonlinear optimal control problem, we introduce the concept of the order of priority into the trajectory generation procedure. We place the first priority on the convergence of the state to a desired state. The second priority is given to obstacle avoidance and the third priority is given to trajectory optimization. Then, a gradient-based iterative algorithm which synthesizes the gradient function in a hierarchical manner considering the order of priority is proposed. The simulation results of the 3-link planar free-joint manipulator are given to show the effectiveness of the proposed algorithm.