In this work, we present several tools for efficient sequential hierarchical least-squares programming (S-HLSP) for lexicographical optimization tailored to robot control and planning. As its main step, S-HLSP relies on approximations of the original non-linear hierarchical least-squares programming (NL-HLSP) to a hierarchical least-squares programming (HLSP) by the hierarchical Newton's method or the hierarchical Gauss-Newton algorithm. We present a threshold adaptation strategy for appropriate switches between the two. This ensures optimality of infeasible constraints, promotes numerical stability when solving the HLSP's and enhances optimality of lower priority levels by avoiding regularized local minima. We introduce the solver $\mathcal{N}$ADM$_2$, an alternating direction method of multipliers for HLSP based on nullspace projections of active constraints. The required basis of nullspace of the active constraints is provided by a computationally efficient turnback algorithm for system dynamics discretized by the Euler method. It is based on an upper bound on the bandwidth of linearly independent column subsets within the linearized constraint matrices. Importantly, an expensive initial rank-revealing matrix factorization is unnecessary. We show how the high sparsity of the basis in the fully-actuated case can be preserved in the under-actuated case. $\mathcal{N}$ADM$_2$ consistently shows faster computations times than competing off-the-shelf solvers on NL-HLSP composed of test-functions and whole-body trajectory optimization for fully-actuated and under-actuated robotic systems. We demonstrate how the inherently lower accuracy solutions of the alternating direction method of multipliers can be used to warm-start the non-linear solver for efficient computation of high accuracy solutions to non-linear hierarchical least-squares programs.

翻译：本文提出了若干面向机器人控制与规划中字典序优化的高效序贯分层最小二乘规划（S-HLSP）工具。作为其核心步骤，S-HLSP通过分层牛顿法或分层高斯-牛顿算法将原始非线性分层最小二乘规划（NL-HLSP）近似为分层最小二乘规划（HLSP）。我们提出了一种自适应阈值切换策略，用于在两种算法间实现合理切换。该策略能确保不可行约束的最优性，提升HLSP求解时的数值稳定性，并通过避免正则化局部最优解来增强低优先级层的最优性。我们介绍了求解器$\mathcal{N}$ADM$_2$——一种基于主动约束零空间投影的分层最小二乘交替方向乘子法。其中，主动约束零空间所需基由基于欧拉法离散系统动力学的高效回溯算法提供，该算法利用线性化约束矩阵中线性无关列子集的带宽上界。关键在于，该方法无需进行代价高昂的初始秩揭示矩阵分解。我们展示了全驱动情形下高度稀疏的基如何能在欠驱动情形下保持。在由测试函数组成的NL-HLSP以及全驱动与欠驱动机器人系统的全身轨迹优化中，$\mathcal{N}$ADM$_2$的计算速度始终优于同类现成求解器。我们展示了如何利用交替方向乘子法固有精度较低的求解结果来热启动非线性求解器，从而高效计算非线性分层最小二乘规划的高精度解。