Efficient Lexicographic Optimization for Prioritized Robot Control and Planning

📄 arXiv: 2403.09160v1 📥 PDF

作者: Kai Pfeiffer, Abderrahmane Kheddar

分类: cs.RO

发布日期: 2024-03-14

💡 一句话要点

提出高效的字典优化方法以解决机器人控制与规划问题

🎯 匹配领域: 支柱一：机器人控制 (Robot Control)

关键词: 机器人控制 规划算法 最小二乘编程 数值稳定性 高效求解器

📋 核心要点

  1. 现有的非线性分层最小二乘编程方法在求解机器人控制与规划问题时面临数值稳定性和计算效率的挑战。
  2. 论文提出的顺序分层最小二乘编程(S-HLSP)方法，通过近似和阈值适应策略，提升了求解的稳定性和效率。
  3. 实验结果表明，$ ext{NADM}_2$求解器在全驱动和欠驱动机器人系统的轨迹优化中，计算时间显著优于现有求解器。

📝 摘要（中文）

本文提出了几种高效的顺序分层最小二乘编程(S-HLSP)工具，旨在实现针对机器人控制与规划的字典优化。S-HLSP的主要步骤依赖于将原始非线性分层最小二乘编程(NL-HLSP)近似为分层最小二乘编程(HLSP)，采用分层牛顿法或分层高斯-牛顿算法。我们提出了一种阈值适应策略，以便在这两者之间进行适当切换，从而确保不满足约束条件的最优性，促进HLSP求解时的数值稳定性，并通过避免正则化局部极小值来增强低优先级水平的最优性。我们引入了求解器$ ext{NADM}_2$，它基于活动约束的零空间投影，采用交替方向乘子法求解HLSP。该方法通过一个计算高效的回退算法提供活动约束的零空间基，避免了昂贵的初始秩揭示矩阵分解。我们展示了在全驱动和欠驱动机器人系统中，$ ext{NADM}_2$在NL-HLSP上的计算速度优于现有的竞争求解器。

🔬 方法详解

问题定义：本文旨在解决机器人控制与规划中的非线性分层最小二乘编程(NL-HLSP)的求解效率和数值稳定性问题。现有方法在处理不满足约束的情况下，往往会导致局部极小值和计算不稳定。

核心思路：提出顺序分层最小二乘编程(S-HLSP)，通过将NL-HLSP近似为HLSP，并结合阈值适应策略，确保求解过程中的最优性和稳定性。

技术框架：整体架构包括S-HLSP的求解过程，采用分层牛顿法或高斯-牛顿算法进行近似，并通过$ ext{NADM}_2$求解器实现活动约束的零空间投影。

关键创新：引入了$ ext{NADM}_2$求解器，避免了昂贵的初始秩揭示矩阵分解，并通过回退算法高效提供活动约束的零空间基，这一设计显著提升了计算效率。

关键设计：在求解过程中，采用了阈值适应策略以便在分层牛顿法和高斯-牛顿算法之间切换，确保了求解的数值稳定性和高效性。

📊 实验亮点

实验结果显示，$ ext{NADM}_2$求解器在处理NL-HLSP时，计算时间比现有的竞争求解器快，尤其在全驱动和欠驱动机器人系统的轨迹优化中，表现出显著的性能提升，具体提升幅度未明确给出。

🎯 应用场景

该研究的潜在应用领域包括机器人路径规划、动态控制和自主导航等。通过提升机器人系统的控制效率和稳定性，能够在复杂环境中实现更高效的任务执行，具有重要的实际价值和未来影响。

📄 摘要（原文）

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.