PV-OSIMr: A Lowest Order Complexity Algorithm for Computing the Delassus Matrix

作者: Ajay Suresha Sathya, Wilm Decré, Jan Swevers · IEEE Robotics and Automation Letters (RA-L) 2024

许可协议

• 开放获取
• 论文: CC BY-SA 4.0

以下内容均依据论文全文，按其 CC BY-SA 4.0 许可协议使用；其中的图已注明作者。

一句话总结

重新安排 PV-OSIM 的计算，使被传播的约束始终不超过六维，从而以 O(n + m²) 计算 Delassus 矩阵，作者证明这在渐近意义上是最优的。

为何重要

具有大量接触的机器人，例如带脚和多指手的人形机器人，需要以高频率计算该矩阵以用于仿真和控制，因此运算更少意味着求解器更快。由于构造 m × m 矩阵和访问全部 n 个关节是不可避免的，作者认为 O(n + m²) 是最优阶。该算法通过 CasADi 代码生成实现，并且据论文所述，还以 C++ 在 Pinocchio 库中实现。

问题

Delassus 矩阵，即逆操作空间惯性矩阵，将约束力与约束加速度联系起来，是约束与接触动力学仿真以及操作空间控制所必需的，在 MPC 和 RL 中它往往是代价最高的一步。朴素计算的代价为 O(n³ + m²n)。已知最好的算法 EFPA 达到 O(n + md + m²)，而作者早先提出的 PV-OSIM 为 O(n + m²d)，当许多约束必须经过许多关节传播时，其扩展性较差。

提出的方法

PV-OSIMr 修改了 PV-OSIM 的反向遍历：它不再传播随约束数量增长的约束空间矩阵，而是在相邻的分支连杆（集合 N）之间传播固定的 6×6 扩展力传播子和空间逆惯性。在 N 上进行的有限前向遍历计算逆惯性，有限反向遍历计算每个末端执行器的约束空间传播子，Delassus 矩阵的每个块都在常数时间内组装。由此得到 O(n + m²) 的复杂度。

关键见解

• 当一个连杆承载超过六个约束维度时，先传播 6D 空间量、之后再恢复约束空间项，比直接传播约束空间项代价更低。
• 代价高昂的量只需在受约束连杆的最近公共祖先处计算，因此额外的遍历只在缩减后的连杆集合上进行。
• 更低的渐近复杂度并不保证在小型机器人上运算更少：由于 EFPA 采用三次遍历结构并需要逐连杆的坐标系变换，它在几乎所有测试机构上都比 PV-OSIM 和 PV-OSIMr 代价更高。
• LTL-OSIM 尽管复杂度更高，但在单只 Shadow 手上胜过 EFPA，因为这只手的分支结构产生了它可以利用的稀疏性。

主要结果

• 复杂度：PV-OSIMr 为 O(n + m²)，对比 PV-OSIM O(n + m²d)、EFPA O(n + md + m²)、KJR O(n + m²d)、LTL-OSIM O(nd² + m²d + md²) 和朴素方法 O(n³ + m²n)。
• Atlas 站立、每只脚有四个 3D 点接触（每只脚 12D）：PV-OSIM 比 PV-OSIMr 多需要 19.9% 的运算；每只脚使用一个 6D 焊接约束时，两种算法完全相同。
• 带 Shadow 手的 Atlas（每个指尖一个 3D 约束，每只手 15D）：第 IV-D 节报告运算次数比次高效的算法 PV-OSIM 最多少约 34%，而结论部分称在 Atlas 示例上最多提升 24%。
• 在每个分支末端带 6D 焊接约束的分支测试机构上，PV-OSIMr 在单个分支时与 PV-OSIM 持平，从两个分支起扩展更平缓；对于每侧十个分支的长机构，EFPA 最终胜过 PV-OSIM。
• 有一种情况下 PV-OSIM 所需运算约少 1.5%：双手双脚都带 6D 焊接约束的 Atlas，其约束在到达浮动基座之前只经过三个关节。
• 所有基准测试结果均为 CasADi 实现中的运算次数，而非实际运行时间。

局限

• 当超过六个约束维度只经过少数（三个或更少）单 DoF 关节时，PV-OSIM 可能代价更低，因为其秩 1 更新优于 PV-OSIMr 的两次秩 6 计算。
• 当约束只在浮动基座处汇合，或机构是单条链时，PV-OSIMr 相比 PV-OSIM 没有收益。
• 结果以运算次数报告，而非实测运行时间。
• 该算法针对运动学树提出；C++ 版 Pinocchio 实现原定在论文被接收后才发布。

对领域的影响

该论文获得 RA-L 2025 最佳论文奖，被引用 4 次。作者在 GitHub 上以源代码形式发布了其 CasADi 实现，并表示已在 Pinocchio 库中实现了 C++ 版本，将在论文被接收后发布。

摘要

我们提出 PV-OSIMr，这是一种为运动学树计算 Delassus 矩阵（也称逆操作空间惯性矩阵）的高效算法，其计算复杂度为文献中已知的最低阶。PV-OSIMr 是利用力传播子和运动传播子的可组合性，对 Popov-Vereshchagin（PV）求解器的计算进行优化而推导出来的。其计算复杂度为 O(n + m²)，相比之下，原始 PV-OSIM 算法为 O(n + m²d)，扩展力传播子算法（EFPA）为 O(n+md+m²)，其中 n 为关节数，m 为约束数，d 为运动学树的深度。由于计算 Delassus 矩阵需要构造一个 m × m 大小的矩阵，并且必须至少考虑全部 n 个关节一次，PV-OSIMr 的渐近计算复杂度是最优的。我们进一步对算法进行了基准测试，发现它在实践中往往比 PV-OSIM 和 EFPA 更高效。

链接

• arXiv 2310.03676: https://arxiv.org/abs/2310.03676
• DOI 10.1109/LRA.2024.3469829: https://doi.org/10.1109/LRA.2024.3469829
• PDF: https://arxiv.org/pdf/2310.03676v1
• 代码: https://github.com/AjSat/spatial_V2