Sequential Hierarchical Least-Squares Programming for Prioritized Non-Linear Optimal Control

TL;DR

This work develops a sequential hierarchical least-squares programming (S-HLSP) framework that solves prioritized non-linear optimization problems by alternating HLSP subproblems under a trust-region and a hierarchical step-filter. It advances a sparse reduced-Hessian interior-point solver (s- NIPM-HLSP) that preserves banded structure through a recycling turnback nullspace basis, enabling scalable resolution for long-horizon discrete optimal control and inverse kinematics. A nullspace-trust-region adaptation (NSTRA) and a comprehensive hierarchical filter ensure global convergence to a local KKT point while improving convergence across all priority levels. Demonstrations on test functions, inverse kinematics (HRP-2), and a discrete OCP for a robot (Solo12) show substantial computational gains and accurate, feasible solutions, highlighting the method’s practicality for real-time control and planning tasks.

Abstract

We present a sequential hierarchical least-squares programming solver with trust-region and hierarchical step-filter with application to prioritized discrete non-linear optimal control. It is based on a hierarchical step-filter which resolves each priority level of a non-linear hierarchical least-squares programming via a globally convergent sequential quadratic programming step-filter. Leveraging a condition on the trust-region or the filter initialization, our hierarchical step-filter maintains this global convergence property. The hierarchical least-squares programming sub-problems are solved via a sparse reduced Hessian based interior point method. It leverages an efficient implementation of the turnback algorithm for the computation of nullspace bases for banded matrices. We propose a nullspace trust region adaptation method embedded within the sub-problem solver towards a comprehensive hierarchical step-filter. We demonstrate the computational efficiency of the hierarchical solver on typical test functions like the Rosenbrock and Himmelblau's functions, inverse kinematics problems and prioritized discrete non-linear optimal control.

Figures (12)

• Figure 1: A symbolic overview of the sequential hierarchical least-squares programming (S-HLSP) with trust region and hierarchical step-filter based on the SQP step-filter fletcher2002b to solve non-linear hierarchical least-squares programs \ref{['eq:NL-HLSP']} with $p$ levels.
• Figure 2: Non-linear test functions, Newton's method, s-$\mathcal{N}$IPM-HLSP: Primal $x$ over S-HLSP iteration with (top) and without (bottom) nullspace trust-region adaptation. The black vertical lines indicate the current hierarchy level being resolved by the HSF.
• Figure 3: Non-linear test functions, Newton's method, s-$\mathcal{N}$IPM-HLSP: error $\Vert f \Vert_2$ over S-HLSP iteration.
• Figure 4: Non-linear test functions, Newton's method: computation times, number of inner iterations, KKT residuals and overall number of non-zeros handled throughout the whole hierarchy over S-HLSP outer iteration for the different HLSP sub-solvers.
• Figure 5: Non-linear test functions, Newton's method: number of non-zeros on each priority levels for the different HLSP solvers.

Theorems & Definitions (3)

• Remark 1
• Theorem 3.1
• Theorem 3.2