Collocation Methods for Second Order Systems

Collocation methods for numerical optimal control commonly assume that the system dynamics is expressed as a first order ODE of the form (x)over dot = f(x, u, t), where x is the state and u the control vector. However, in many systems in robotics, the dynamics adopts the second order form (q)over dot = g(q, (q)over dot, u, t), where q is the configuration. To preserve the first order form, the usual procedure is to introduce the velocity variable v = (q)over dot and define the state as x = (q, v), where q and v are treated as independent in the collocation method. As a consequence, the resulting trajectories do not fulfill the mandatory relationship v(t) = (q)over dot (t) for all times, and even violate (q)over dot = g(q, (q)over dot, u, t) at the collocation points. This prevents the possibility of reaching a correct solution for the problem, and makes the trajectories less compliant with the system dynamics. In this paper we propose a formulation for the trapezoidal and Hermite-Simpson collocation methods that is able to deal with second order dynamics and grants the mutual consistency of the trajectories for q and v while ensuring (q)over dot = g(q, (q)over dot, u, t) at the collocation points. As a result, we obtain trajectories with a much smaller dynamical error in similar computation times, so the robot will behave closer to what is predicted by the solution. We illustrate these points by way of examples, using well-established benchmark problems from the literature.