Safety Envelope for Orthogonal Collocation Methods in Embedded Optimal Control

Orthogonal collocation methods are direct approaches for solving optimal control problems (OCP). A high solution accuracy is achieved with few optimization variables, making it more favorable for embedded and real-time NMPC applications. However, collocation approaches lack a guarantee about the safety of the resulting trajectory as inequality constraints are only set on a finite number of collocation points. In this paper we propose a method to efficiently create a convex safety envelope containing the trajectory such that the solution fully satisfies the OCP constraints. We make use of Bernstein approximations of a polynomial's extrema and span the solution over an orthogonal basis using Legendre polynomials. The tightness of the safety envelope estimation, high accuracy in solving the underlying differential equations, fast rate of convergence and little conservatism are properties of the presented approach making it a suitable method for safe real-time NMPC deployment. We show that our method has comparable computational performance to pseudospectral approaches and can accurately approximate the original OCP up to 9 times more quickly than standard multiple-shooting method in autonomous driving applications, without adding complexity to the formulation.