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 = 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 = g ( q , ˙ q , u , t ) , where q is the configuration. To preserve the first order form, the usual procedure is to introduce the velocity variable v = ˙ q 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 ( t ) for all times, and even violate ¨ q = g ( q , ˙ q , 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 = g ( q , ˙ q , 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.