Direct trajectory optimization and costate estimation of finite-horizon and infinite-horizon optimal control problems using a Radau pseudospectral method

A method is presented for direct trajectory optimization and costate estimation of finite-horizon and infinite-horizon optimal control problems using global collocation at Legendre-Gauss-Radau (LGR) points. A key feature of the method is that it provides an accurate way to map the KKT multipliers of the nonlinear programming problem to the costates of the optimal control problem. More precisely, it is shown that the dual multipliers for the discrete scheme correspond to a pseudospectral approximation of the adjoint equation using polynomials one degree smaller than that used for the state equation. The relationship between the coefficients of the pseudospectral scheme for the state equation and for the adjoint equation is established. Also, it is shown that the inverse of the pseudospectral LGR differentiation matrix is precisely the matrix associated with an implicit LGR integration scheme. Hence, the method presented in this paper can be thought of as either a global implicit integration method or a pseudospectral method. Numerical results show that the use of LGR collocation as described in this paper leads to the ability to determine accurate primal and dual solutions for both finite and infinite-horizon optimal control problems.