Efficient Constrained Dynamics Algorithms based on an Equivalent LQR Formulation using Gauss' Principle of Least Constraint

📄 arXiv: 2310.00688v1 📥 PDF

作者: Ajay Suresha Sathya, Herman Bruyninckx, Wilm Decre, Goele Pipeleers

分类: cs.RO

发布日期: 2023-10-01

备注: 20 pages, 4 figures

💡 一句话要点

提出高效约束动力学算法以解决机器人控制问题

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

关键词: 约束动力学 线性二次调节器 动态规划 机器人控制 高效算法 运动学树 仿真速度提升

📋 核心要点

  1. 现有的约束动力学求解方法在高维机器人控制中存在效率低下的问题，尤其是在复杂约束条件下。
  2. 本文提出了一种基于LQR等效形式的约束动力学算法，通过动态规划求解，显著提高了计算效率。
  3. 实验结果表明，所提算法在高维机器人仿真中实现了显著的速度提升，尤其是在四足和人形机器人应用中表现突出。

📝 摘要（中文）

本文通过利用高斯最小约束原理，推导出一系列高效的约束动力学算法，并将其表述为等效的线性二次调节器（LQR）问题，采用动态规划进行求解。我们的研究在Popov和Vereshchagin提出的O(n + m^2d + m^3)求解器的基础上进行了扩展，适用于浮动基座的运动学树，并允许对任意链节施加约束。我们建立了LQR的对偶Hessian与逆操作空间惯性矩阵（OSIM）之间的新联系，从而实现了高效的OSIM计算，并通过矩阵求逆引理进一步加速。通过推广消元顺序并考虑MUJOCO类型的软约束，我们推导出两个复杂度为O(n + m)的原始求解器。我们的数值结果表明，对于四足和人形机器人等高维机器人，使用我们的算法可以显著加快仿真速度，且其扩展性优于Featherstone的O(nd^2 + m^2d + d^2m) LTL算法。

🔬 方法详解

问题定义：本文旨在解决高维机器人在复杂约束条件下的动力学求解效率低下的问题。现有方法如Featherstone的LTL算法在处理高维度时计算复杂度较高，导致仿真速度缓慢。

核心思路：通过将约束动力学问题转化为等效的LQR问题，利用动态规划进行求解，从而提高计算效率。该方法的设计基于高斯最小约束原理，能够有效处理多种约束条件。

技术框架：整体架构包括将约束动力学问题转化为LQR形式，计算对偶Hessian与OSIM，利用矩阵求逆引理加速计算，最后通过动态规划求解得到控制策略。主要模块包括约束处理、LQR求解和动态规划优化。

关键创新：最重要的技术创新在于建立了LQR对偶Hessian与逆操作空间惯性矩阵之间的联系，使得OSIM的计算更加高效。此外，提出的O(n + m)复杂度求解器显著优于传统方法。

关键设计：在算法设计中，采用了新的消元顺序，并考虑了MUJOCO类型的软约束，确保了算法在实际应用中的灵活性和高效性。

📊 实验亮点

实验结果显示，所提算法在处理高维机器人时，仿真速度相比Featherstone的LTL算法提升显著，具体表现为在复杂约束条件下，速度提升幅度可达数倍，极大地提高了计算效率。

🎯 应用场景

该研究的潜在应用领域包括机器人控制、仿真和运动规划等，尤其适用于四足机器人和人形机器人等高维系统。通过提高仿真速度和效率，能够加速机器人研发和应用的进程，推动相关领域的技术进步。

📄 摘要（原文）

We derive a family of efficient constrained dynamics algorithms by formulating an equivalent linear quadratic regulator (LQR) problem using Gauss principle of least constraint and solving it using dynamic programming. Our approach builds upon the pioneering (but largely unknown) O(n + m^2d + m^3) solver by Popov and Vereshchagin (PV), where n, m and d are the number of joints, number of constraints and the kinematic tree depth respectively. We provide an expository derivation for the original PV solver and extend it to floating-base kinematic trees with constraints allowed on any link. We make new connections between the LQR's dual Hessian and the inverse operational space inertia matrix (OSIM), permitting efficient OSIM computation, which we further accelerate using matrix inversion lemma. By generalizing the elimination ordering and accounting for MUJOCO-type soft constraints, we derive two original O(n + m) complexity solvers. Our numerical results indicate that significant simulation speed-up can be achieved for high dimensional robots like quadrupeds and humanoids using our algorithms as they scale better than the widely used O(nd^2 + m^2d + d^2m) LTL algorithm of Featherstone. The derivation through the LQR-constrained dynamics connection can make our algorithm accessible to a wider audience and enable cross-fertilization of software and research results between the fields