Configuration Space Distance Fields for Manipulation Planning

作者: Yiming Li, Xuemin Chi, Amirreza Razmjoo, Sylvain Calinon · RSS 2024

许可协议

开放获取
论文: CC BY 4.0

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

一句话总结

直接在关节角上定义距离场，使全身 IK 变为一步梯度投影，并让规划器在 7-DoF Franka 上避开 SDF 所导致的局部极小值。

为何重要

已经以距离函数为输入的规划器和控制器（QP、iLQR、geometric fabrics）可以用 CDF 替代 SDF，直接在关节空间中工作。在平面机器人和 Franka 上，同样的规划器使用 CDF 时比使用 SDF 时成功率更高、跟踪误差更低。

问题

有符号距离场通常定义在 3D 任务空间中，因此操作控制先在那里测量距离，再迭代求解逆运动学以得到关节运动。由于正运动学是非线性的，从关节空间看时，该场会失去单位梯度性质，基于梯度的求解器可能在梯度消失处或奇异点处停滞。

提出的方法

构型空间距离场（CDF）以弧度给出使机器人任一部分与某一点接触所需的最小关节运动；它满足程函方程（eikonal equation），因此沿梯度投影即可到达最近的接触构型。接触构型在离线阶段针对工作空间中的每个网格点，利用机器人 SDF 模型通过批量 L-BFGS 求得；逐点的场在线通过取最小值运算进行融合；一个 5 层 MLP（神经 CDF）则提供紧凑、无需网格且可微的版本。

关键见解

定义在关节角上的距离场保持单位范数梯度，因此针对接触目标的全身 IK 简化为一次投影 q′ = q − f_c ∇f_c，无需 Gauss-Newton 迭代。
场值只取决于接触连杆之前的关节；当接触构型样本较少时，按连杆取最小值可提高精度。
逐点的场可以预先计算，并在线通过取最小值（即构型空间中的并集）进行组合，因此该表示不依赖于特定场景。
场的梯度会沿着在关节空间中绕过障碍物的弯曲路径，这有助于反应式规划器避开任务空间 SDF 中出现的局部极小值。

主要结果

Franka 上从 10,000 个随机初始构型出发的全身 IK（表 II）：采用 2 步投影的 CDF 在 10.6 ms 内找到 8,773 个有效解；SDF + L-BFGS 在 971 ms 内找到 3,652 个，Neural-JSDF + L-BFGS 在 272 ms 内找到 264 个。正文称这相当于每秒超过 700,000 个有效解，是 SDF 方法的 180 倍。
神经 CDF 精度：经过 3 次投影迭代后，MAE 为 1.39 ± 0.51 cm，投影成功率为 91.1 ± 8.12%（阈值 3 cm）；在 RTX 3090 上，批大小为 1 时 GPU 推理耗时 0.49 ms，批大小为 10^5 时为 4.61 ms。
Franka 上 100 次运行的运动规划：CDF 搭配 IPOPT、QRQP、OSQP、qpOASES 和 geometric fabrics 的成功率为 88–93%，而 SDF 为 38–74%；iLQR 为 48% 对 38%。在 2-DoF 平面机器人上，CDF 达到 76–95%，而 SDF 为 40–68%。
仿真中的守门员任务（100 次投掷）：使用 CDF 时机械臂拦截到球的比例为 82%，使用 SDF 时为 35%。
双臂大箱子接触规划：CDF + 过滤的规划时间为 7.65 s，平均关节距离为 1.37 rad，而 SDF + Gauss-Newton 优化为 54.80 s 和 2.85 rad。
使用神经 CDF 的 QP 控制器运行频率高于 200 Hz；神经 CDF 在一块 RTX 3090 上训练约需 2 小时。

局限

该场是无符号的，在接触构型样本较少时，对接近接触的构型不够准确；作者建议加入符号信息。
它根据单个机器人的几何和运动学构建，每种机器人型号都必须重新计算；扩展到完整人形机器人等长运动链仍是开放问题。
离线计算较慢（寻找接触构型需要 1–10 s），网格版本内存效率低，神经版本则牺牲了精度；作者指出网络训练尚未充分优化。
距离以角度（关节欧氏）距离度量；构型空间流形上的测地距离留待未来工作。
真实机器人规划实验仅以定性方式展示（图 8），没有成功率，守门员任务也只在仿真中进行。

对领域的影响

入围 RSS 2024 杰出论文奖决选，被引用 48 次。官方项目页面链接了代码：github.com/yimingli1998/cdf。

摘要

有符号距离场是机器人领域常用的一种隐式形状表示，它以一种易于与控制、优化和学习技术相结合的形式，提供关于物体和障碍物的几何信息。SDF 最常用于表示任务空间中的距离，这对应于我们在 3D 世界中感知到的熟悉的距离概念。然而，在数学上 SDF 也可以用于其他空间，包括机器人构型空间。对于机械臂而言，构型空间通常对应于机器人每个关节的关节角。在机器人规划中，人们习惯于表达构型空间的哪些部分与障碍物无碰撞，但较少把这一信息看作构型空间中的距离场。在本文中，我们展示了在机器人构型空间中考虑 SDF 以用于优化的潜力，我们称之为构型空间距离场。与在任务空间中使用 SDF 类似，CDF 提供高效的关节角距离查询，并可直接获取导数。大多数方法将整体计算拆分为两部分，先在任务空间中计算一部分，再在构型空间中计算一部分。与之不同，CDF 使控制、优化和学习问题能够以统一的方式利用这种隐式结构。具体而言，我们提出了一种计算和融合 CDF 的高效算法，可推广到任意场景。我们还提出了一种使用多层感知机的相应神经 CDF 表示，以获得紧凑且连续的表示，同时提高计算效率。我们通过平面避障示例，以及 7 轴 Franka 机器人上的逆运动学和操作规划任务，展示了 CDF 的有效性。

链接

arXiv 2406.01137: https://arxiv.org/abs/2406.01137
DOI 10.48550/arXiv.2406.01137: https://doi.org/10.48550/arXiv.2406.01137
PDF: https://arxiv.org/pdf/2406.01137v1
代码: https://github.com/yimingli1998/cdf
项目主页: https://sites.google.com/view/cdfmp/home