A Legendre-Gauss Pseudospectral Collocation Method for Trajectory Optimization in Second Order Systems

Pseudospectral collocation methods have proven to be powerful tools to solve optimal control problems. While these methods generally assume the dynamics is given in the first order form $\dot{x} = f (x, u, t)$, where x is the state and u is the control vector, robotic systems are typically governed by second order ODEs of the form $\ddot{q} = g(q, \dot{q}, u, t)$, where q is the configuration. To convert the second order ODE into a first order one, the usual approach is to introduce a velocity variable v and impose its coincidence with the time derivative of q. Lobatto methods grant this constraint by construction, as their polynomials describing the trajectory for v are the time derivatives of those for q, but the same cannot be said for the Gauss and Radau methods. This is problematic for such methods, as then they cannot guarantee that $\ddot{q} = g(q, \dot{q}, u, t)$ at the collocation points. On their negative side, Lobatto methods cannot be used to solve initial value problems, as given the values of u at the collocation points they generate an overconstrained system of equations for the states. In this paper, we propose a Legendre-Gauss collocation method that retains the advantages of the usual Lobatto, Gauss, and Radau methods, while avoiding their shortcomings. The collocation scheme we propose is applicable to solve initial value problems, preserves the consistency between the polynomials for v and q, and ensures that $\ddot{q} = g(q, \dot{q}, u, t)$ at the collocation points.