Collocation methods for second and higher order systems

TL;DR

This paper identifies a fundamental flaw in applying collocation methods to second and higher order systems in robotics and proposes improved methods that significantly reduce discretization errors without increasing computational cost.

Contribution

It develops new versions of trapezoidal and Hermite-Simpson collocation methods that address inconsistencies in discretizing higher order ODEs in optimal control problems.

Findings

New methods reduce dynamic transcription error by an order of magnitude

Improved collocation methods do not significantly increase computational cost

Address a critical flaw in existing collocation approaches for higher order systems

Abstract

It is often unnoticed that the predominant way to use collocation methods is fundamentally flawed when applied to optimal control in robotics. Such methods assume that the system dynamics is given by a first order ODE, whereas robots are often governed by a second or higher order ODE involving configuration variables and their time derivatives. To apply a collocation method, therefore, the usual practice is to resort to the well known procedure of casting an M th order ODE into M first order ones. This manipulation, which in the continuous domain is perfectly valid, leads to inconsistencies when the problem is discretized. Since the configuration variables and their time derivatives are approximated with polynomials of the same degree, their differential dependencies cannot be fulfilled, and the actual dynamics is not satisfied, not even at the collocation points. This paper draws attention to this problem, and develops improved versions of the trapezoidal and Hermite-Simpson collocation methods that do not present these inconsistencies. In many cases, the new methods reduce the dynamic transcription error in one order of magnitude, or even more, without noticeably increasing the cost of computing the solutions.