Trajectory planning based on optimal control and exact derivatives

Abstract

To solve the trajectory planning problem, a path constrained optimal control model is established and is solved by gradient-based numerical method. Since it is very computationally expensive for the adjoint equation method to calculate the gradients of path constraints, an exact derivative method is used to calculate the gradients of objective and path constraints accurately and efficiently. The method is achieved by regarding the objective and path constraints as explicit functions of the parametrized controls. Then the gradient can be calculated in reverse mode of automatic differentiation which requiring Jacobian information of the integrator for solving state equation. The Jacobian matrix of the new states with respect to current states and controls is analytically derived for the 4th-order Runge-Kutta method and is calculated and stored when integrating the state equation. From a study case, an OCP with inequality path constraints was discretized to nonlinear programming problem by control vector parameterization (CVP) and was solved by sequential quadratic (SQP) method. The simulation case for a differential drive robot demonstrates the efficiency of the proposed method.