Structure-Exploiting Newton-Type Method for Optimal Control of Switched Systems

TL;DR

This paper introduces a specialized Newton-type method leveraging problem structure for efficient optimal control of switched systems, significantly improving computational speed and robustness over standard solvers.

Contribution

It develops a structure-exploiting Riccati recursion algorithm for Newton steps in optimal control, enabling linear time complexity and improved convergence for switched systems.

Findings

Method is up to 100 times faster than standard NLP solvers.

Successfully applied to whole-body control of quadrupedal robots.

Ensures convergence near local minima with indefinite Hessians.

Abstract

This study proposes an efficient Newton-type method for the optimal control of switched systems under a given mode sequence. A mesh-refinement-based approach is utilized to discretize continuous-time optimal control problems (OCPs) and formulate a nonlinear program (NLP), which guarantees the local convergence of a Newton-type method. A dedicated structure-exploiting algorithm (Riccati recursion) is proposed to perform a Newton-type method for the NLP efficiently because its sparsity structure is different from a standard OCP. The proposed method computes each Newton step with linear time-complexity for the total number of discretization grids as the standard Riccati recursion algorithm. Additionally, the computation is always successful if the solution is sufficiently close to a local minimum. Conversely, general quadratic programming (QP) solvers cannot accomplish this because the Hessian matrix is inherently indefinite. Moreover, a modification on the reduced Hessian matrix is proposed using the nature of the Riccati recursion algorithm as the dynamic programming for a QP subproblem to enhance the convergence. A numerical comparison is conducted with off-the-shelf NLP solvers, which demonstrates that the proposed method is up to two orders of magnitude faster. Whole-body optimal control of quadrupedal gaits is also demonstrated and shows that the proposed method can achieve the whole-body model predictive control (MPC) of robotic systems with rigid contacts.