cuRobo V2 入门教程（一）：基于深度融合距离场的动态感知运动生成方法，适用于高自由度机器人

B-样条轨迹优化，可确保平滑性并遵守扭矩限制；
原生GPU TSDF/ESDF感知管道，可生成覆盖整个工作空间的密集带符号距离场；与仅在稀疏分配的块内提供距离的现有方法不同，该方法在操作尺度上比最先进方法快达10倍，内存消耗减少8倍，且碰撞召回率高达99%；
可扩展的原生 GPU 全身计算，即拓扑感知运动学、可微逆动力学和 Map-Reduce 自碰撞检测，该方案在实现最高 61 倍加速的同时，还能扩展至高自由度类人机器人（此前基于 GPU 的实现方案在此类场景下均告失败）。

在基准测试中，cuRoboV2在3公斤载荷下成功率达99.7%（基准方法仅为72–77%），在48自由度类人机器人上实现99.6%的无碰撞反向运动学（此前方法完全失败），且重定位约束满足率达89.5%（相比PyRoki的61%）；这些无碰撞运动生成的行走策略，其跟踪误差比 PyRoki 低 21%，跨种子方差比 mink 低 12 倍。为提升可发现性而进行的自底向上代码库重构，使大型语言模型（LLM）编码助手能够编写高达73%的新模块，其中包括手动优化的CUDA内核，这证明了结构良好的机器人代码能够释放人类与LLM之间的高效协作潜力。这些进展共同构建了一个统一的、具备动力学意识的运动生成栈，其适用范围从单臂机械手扩展至完整的类人机器人。

一、引言

有效的机器人自主性需要能够生成运动轨迹，以便安全、高效且灵活地导航并与环境交互。历史上，针对这一问题的研究主要分为两个子领域：(1) 全局运动规划，即从静态起始状态计算至目标的全局路径 [42, 66, 68]；(2) 反应式运动生成，即对移动目标进行高频跟踪 [2, 11, 16, 59]。尽管这些任务的范围不同，但它们都面临着相同的根本要求：既要满足机器人硬件的限制，又要避免碰撞（包括与场景的碰撞以及机器人自身的碰撞）。然而，在这两个领域中，当前的方法都难以同时满足这些要求，导致功能上存在关键缺口。是什么阻碍了现有方法实现统一、高性能的自主性？我们识别出三个根本障碍：

可行性差距 虽然无碰撞运动规划可以快速完成[59, 66]，但这种速度往往是以忽略动力学为代价的，从而导致物理上无法执行的运动。大多数规划器输出的分段线性关节路径假设扭矩无限大，忽略了机器人的质量和动量。因此，“时间最优”的规划方案往往会超出扭矩限制，特别是在负重较大的情况下，这需要进行激进的后处理，从而使原始规划的安全保证失效。相反，动态轨迹优化方法虽然遵守这些限制，但在处理网格或深度基碰撞避让的非凸约束时难以扩展。
感知与反应能力的权衡 当前的反应式方法迫使人们在安全性与高保真感知之间做出选择。解析控制器（例如 RMP[9, 61]、MPC[2, 70]）虽能提供安全保障，但处理原始深度数据时往往速度过慢，因此只能处理简化的几何基元。相反，基于学习的方法[14, 16, 43]虽能快速处理视觉观测数据，却无法提供安全部署所需的严格碰撞保证。此外，这些方法缺乏泛化能力，因此在面对新环境时需要进行大量重新训练。
可扩展性瓶颈：适用于单臂平台的方法，在应用于双臂机械手或类人机器人等高自由度系统时，往往难以扩展。我们发现，在这些高维空间中，最先进的规划器[66]收敛缓慢甚至无法收敛，特别是在杂乱场景中的无碰撞反向运动学（IK）问题上——这正是现实世界中双臂操作和类人机器人运动重定位的重要要求。

研究成果

为克服这些障碍，我们提出了 cuRoboV2，这是一个集成的运动生成框架，包含三项核心算法创新：

B-样条优化建模解决了可行性缺口问题。通过优化 B-样条控制点，我们的方法能够自动确保运动平滑性，使求解器既能满足全局规划的扭矩限制，又能处理反应性控制中的非静态边界条件，从而为运动生成提供了一种统一的表示形式。
原生GPU感知管道解决了感知与响应速度之间的权衡问题。通过采用基于体素的投影策略（该策略可消除原子级竞争），将多种输入（深度、网格、长方体）融合为具有毫米级分辨率的块稀疏TSDF。与 nvblox [50] 等现有 GPU 库不同，后者仅计算稀疏分配块内的距离，而忽略工作空间其余部分的距离信息；我们通过并行带状算法 (PBA+) 按需生成覆盖整个工作空间的密集 ESDF，该算法采用基于聚合的初始化阶段，从而实现完整的 CUDA 图捕获。这使得在工作空间的任意点都能进行 𝑂 (1) 级距离查询，在实际应用规模下，其速度比 nvblox 快达 10 倍，内存消耗减少 8 倍，且碰撞召回率高达 99%，从而能够以毫秒级速率进行更新，满足响应式控制的需求。
可扩展运动学、动力学与自碰撞算法突破了可扩展性瓶颈。我们设计了一系列原生GPU构建模块，包括基于稀疏雅可比矩阵计算的拓扑感知运动学、可微逆动力学以及Map-Reduce自碰撞算法。这些模块能够扩展至人形机器人领域——这是此前GPU实现方案无法企及的，并可实现高达40倍的加速。这种吞吐量提升了优化迭代速度，使求解器能够在遵守扭矩限制的同时，快速收敛于高自由度双臂机器人及人形机器人的解。

这些基于GPU的原生创新技术相结合，实现了统一的、考虑动力学因素的运动生成，其应用范围从单臂机械手到完整的类人机器人，涵盖规划、反应式控制和重定位等各个方面。本文剩余部分组织结构如下：第2节综述了相关工作，第3节提出了运动优化问题。随后，我们介绍了三个核心模块：B样条轨迹优化（第4节）、TSDF/ESDF感知管道（第5节）以及适用于高自由度机器人的可扩展运动学、动力学和自碰撞检测（第6节）。第7节在基准测试和真实世界操作场景中对这些成果进行评估，第8节报告了基于从头开始重构代码库所实现的LLM辅助开发。第9节为结论。

二、相关工作

2.1 全局规划

运动规划算法主要分为搜索型、采样型和基于优化的方法。搜索型规划器[21, 39, 42]将状态空间离散化，但在高维空间中表现欠佳。采样型规划器[19, 32, 38, 44]具有概率完备性，但难以处理成本项，且需要进行可能违反动力学约束的后处理。轨迹优化[60, 65]可同时优化平滑度、避障及自定义成本，但其将轨迹参数化为关节位置路点，并依赖加速度正则化来保证平滑度。B样条提供了一种替代方案，被用于轨迹平滑[53]以及移动[10]和空中[36]机器人的路径规划。现有的机械手B样条公式仅限于多面体障碍物[30, 67]等凸约束，未能解决基于深度图像的碰撞表示、三角网格或扭矩限制所产生的非凸约束。cuRoboV2提出了一种适用于高自由度机械手的轨迹优化B样条公式，该方法可在不牺牲收敛性的前提下，容纳扭矩限制等额外约束。

2.2 反应式运动生成

实际应用要求系统能够根据视觉观察对动态环境做出反应。基于模型的方法（如势场[34]、黎曼运动策略[9, 61]、几何织物[72]和运动性能控制[2, 70]）虽能提供安全性保障，但容易陷入局部最优解，且需要使用简化的几何基元。基于学习的方法[12, 16, 17, 24, 73]将经典规划器提炼为神经策略或暖启动优化器[6, 27, 29, 75]；强化学习（RL）还训练出了多臂抓取策略[37]。然而，这些方法缺乏分布外泛化能力，需要针对每个具身进行重新训练，且无法保证无碰撞执行。

2.3 硬件加速

FPGA/ASIC [44, 46, 69] 和 SIMD [68] 可加速基于采样的规划。GPU已被应用于多体动力学（MPC）[2, 26]和优化问题[28, 66]。cuRobo [66]提出了基于GPU并行化的轨迹优化和考虑碰撞的反向运动学（IK）算法，而PyRoki [35]则提供了跨平台的运动学优化方案。这些方法主要针对单臂机器人；若扩展至高自由度（DoF）机器人，收敛性会下降且计算时间会增加。

[1] 如[8]所示，可以在cuRobo中实现自定义成本项，但这相当复杂，且需要对该框架有深入的实际了解。

[2] cuRobo实现了MPPI算法[2]，虽然具备响应性，但无法像L-BFGS那样生成高质量的运动轨迹。

[3] PyRoki 宣称具备无碰撞规划功能，但如我们在第 7 节中所示，其甚至在全局反向运动学（Global-IK）问题上也无法收敛，且由于碰撞检测实现效率低下，运行速度极其缓慢。

2.4 视觉表示

基于几何的方法（如 KinectFusion [47]）将场景表示为 TSDF，但缺乏用于路径规划的碰撞查询功能。nvblox [50] 为基于梯度的路径规划提供了 GPU 加速的 ESDF，但仅在分配的块中计算 ESDF，而非密集场，且在毫米级分辨率下会产生较高的内存开销。基于 CPU 的方法 [20, 52] 则缺乏实时处理能力。神经网络表示法[13, 33, 43]能够实现快速碰撞检测，而学习型编码器[12, 16, 17, 24, 58, 73]在分布外场景中往往表现不佳。cuRoboV2提出了一种原生GPU感知管道，支持毫米级分辨率的碰撞查询，以满足机械臂操作需求。

2.5 人机动作重定位

将人类动作转移到类人机器人上对于运动策略训练至关重要。端到端学习方法[18, 22, 23, 31]直接利用重定位后的人类动作数据训练强化学习策略，在训练过程中过滤掉不可行的动作，但依赖策略来隐式处理自碰撞和关节限制问题。基于优化的重定位框架（如 GMR [1]）使用 mink [76] 或 PyRoki [35] 等求解器解决每帧的逆运动学问题，但这些求解器要么完全缺乏自碰撞规避功能（mink），要么在高自由度机器人上无法收敛（PyRoki）。因此，重定位后的运动轨迹常包含自穿透和关节限值违规现象，这些问题会传播到策略训练中，导致系统不稳定及跨样本方差增大。cuRoboV2 通过为高自由度机器人提供无碰撞且满足关节限值的逆运动学（IK）来填补这一空白，生成可行的参考运动轨迹，从而在不改变学习流程的情况下直接提升策略质量。

三、问题建模

我们把运动生成形式化为一个轨迹优化问题，其目标是找到一组动作序列，使机器人从起始状态移动到目标姿态（或者更具体地说，移动到一个或多个连杆的目标姿态）。所得轨迹应在满足一系列硬约束（如避免自身碰撞和与环境的碰撞，以及遵守机器人的运动学和动力学限制）的同时，使目标函数（通常与运动平滑度和能耗相关）最小化。设 𝜃𝑡 ∈ R𝑑 为时刻 𝑡 时的关节角度，且设 Θ𝑡 = [𝜃𝑡 , §𝜃𝑡 , •𝜃𝑡 , ®𝜃𝑡 ] 表示机器人的状态（角度、速度、加速度和加加速度），其中 𝑑 表示关节数。给定初始状态 Θ0 及一个或多个连杆目标姿态 𝑋𝑚 𝑔 ∈ SE(3)，𝑚 = 1, . . . , 𝑀，我们寻求一条轨迹 𝑈，使其满足以下优化问题：1

其中，式(1)中的损失函数旨在促进平滑且高效的运动；式(2)用于评估特定时刻的B样条函数；式(3)对关节角度、速度等施加限制；式(4)和式(5)通过正向运动学确保路径既不与场景发生碰撞，也不与机器人自身发生碰撞；式(6)确保最终状态接近目标姿态，其中𝜂𝑚为阈值；式(7)通过逆动力学计算扭矩𝜏𝑡 ∈ R𝑑；式(8)确保遵守扭矩限制𝜏−和𝜏+；式(9)确保机器人完全停止。式(2)至(8)中的约束条件适用于所有时间步长𝑡和连杆𝑚。

该公式与先前的轨迹优化方法[60, 66]一致，并涵盖了若干核心机器人学问题。该优化问题的变体主要用于获取无碰撞轨迹，最初是从局部轨迹优化[60, 64]开始的，该方法使用一个粗略的初始轨迹作为种子，并在局部范围内收敛到无碰撞轨迹。Sundaralingam等人[66]通过并行运行多个轨迹优化种子，并利用基于无碰撞图的规划器作为种子，将该方法推广为全局无碰撞运动规划。其特例包括逆运动学，即在无时间约束条件下对单一状态进行优化。在短暂的执行时段后重新求解该优化问题，可产生如模型预测控制（MPC）等反应性控制策略。

图 1：轨迹优化循环。每次迭代都会评估 B-样条路点，并行计算运动学和逆动力学，在独立的 CUDA 线程上并行评估成本（场景碰撞、自碰撞、配置空间边界），汇总这些成本，通过所有前向运算反向传播梯度，并利用 L-BFGS 算法更新 B-样条控制点。

图1展示了轨迹优化循环。每次迭代包括六个步骤：

B样条插值根据控制点生成轨迹路点；
正向运动学计算连杆姿态和雅可比矩阵，并利用递归牛顿-欧拉算法（RNEA）额外计算关节扭矩；
成本评估并行运行，分别计算世界碰撞、自碰撞和状态成本；
成本聚合将各轨迹的成本进行汇总；
反向计算通过所有正向运算计算梯度；
优化器更新控制点。

四、B-样条基作为优化空间

每个关节轨迹 $U \in \mathbb { R } ^ { d \times K }$ 被表示为在控制点 $u _ { k } \in \mathbb { R } , k = 0, . . . , K-1$ 上的均匀三次B-样条，其中 𝑘 对应于每个 𝑑 个关节；这些控制点在式(2)中充当优化变量。三次及以上阶的B样条可生成C²连续的轨迹，且其局部支撑性确保了仅改变单个结点只会影响少数相邻段，如图2所示。因此，这种选择构成了既光滑又紧凑的基。这种隐式平滑性使我们能够满足式(1)中的加速度和速度正则化项，且所需的决策变量远少于典型的每时间步位置参数化方案，同时仍能保持问题对基于梯度的求解器具有良好的条件数。

图 2：三次 B 样条的局部支撑特性。扰动结点 𝑢3 仅影响 4 个相邻的曲线段（橙色区域）。插值点（圆点）在时间上均匀分布；在优化过程中，该区域内的每个点都会为𝑢3提供一个梯度（通过GPU波段级缩减进行累积）。（为简化起见，图中显示𝑁interp = 2。）

对于特定关节，给定控制点 $u _ { k } , k \, = \, 0 , . . . , K \, - \, 1$ ，则在由插值参数 𝛼 ∈ [0, 1] 指定的点处，对该样条在一段区间（相邻结点之间）内进行求值。我们以均匀的时间间隔进行采样，确保每个区间包含相同数量的点，从而保证速度、加速度和抖动是在一致的时间步长下计算的。对于三次样条的情况，状态向量 $\boldsymbol { \Theta } _ { t } = [ \theta _ { t } , \dot { \theta } _ { t } , \ddot { \theta } _ { t } , \ddot { \theta } _ { t } ] ^ { \top }$ ，其控制点为 $U _ { k } = [ u _ { k - 1 } , u _ { k } , u _ { k + 1 } , u _ { k + 2 } ] ^ { \top }$ ，由下式给出：

$\begin{array} { r } { \Theta _ { t } = \underbrace { \mathrm { T P } ( \alpha ) \mathrm { C } } _ { \mathrm { B } ( \alpha ) } U _ { k } , } \end{array}$

其中 $\mathrm { T } = \mathrm { d i a g } \bigl ( 1 , d t _ { u } ^ { - 1 } , d t _ { u } ^ { - 2 } , d t _ { u } ^ { - 3 } \bigr )$ 表示时间缩放，且 $d t _ { u } \in \mathbb { R }$ 是一个固定的时间间隔，

$\mathrm { P } ( \alpha ) = \left[ \! \! { \begin{array} { c c c c } { \alpha ^ { 3 } } & { \alpha ^ { 2 } } & { \alpha } & { 1 } \\ { 3 \alpha ^ { 2 } } & { 2 \alpha } & { 1 } & { 0 } \\ { 6 \alpha } & { 2 } & { 0 } & { 0 } \\ { 6 } & { 0 } & { 0 } & { 0 } \end{array} } \! \! \right]$

计算三次单项式及其在𝛼处的导数（每一行都是对上一行的求导），其中三次B样条系数由以下公式给出：

$\mathrm { C } = \dfrac { 1 } { 6 } \left[ \begin{array} { c c c c } { - 1 } & { 3 } & { - 3 } & { 1 } \\ { 3 } & { - 6 } & { 3 } & { 0 } \\ { - 3 } & { 0 } & { 3 } & { 0 } \\ { 1 } & { 4 } & { 1 } & { 0 } \end{array} \right]$

4.1 关于样条控制点的梯度

求解该优化问题需要计算 $\nabla _ { U } \mathcal { L }$ ，其中 $\mathcal { L }$ 是式(1)中的损失函数。根据链式法则，对于局部控制点向量 $U _ { k }$ ，其每样本的梯度贡献由下式给出：

$\dfrac { \partial \mathcal { L } } { \partial U _ { k } } = \mathrm { B } ( \alpha ) ^ { \top } g ,$

其中 $\begin{array} { r } { g \mathrm { ~ } = { \frac { \partial { \mathcal { L } } } { \partial \Theta _ { t } } } } \end{array}$ 是状态的上游梯度。反向传播即为式(10)中正向传播的转置，其中时间缩放由 B(𝛼) 内的 T 隐式处理。由于控制点 𝑢𝑘 会影响多个区段（图 2），其总梯度是所有相关 𝑁interp 个插值点的贡献之和，

$\nabla _ { u _ { k } } \mathcal { L } = \sum _ { i = 1 } ^ { N _ { \mathrm { i n t e r p } } } \frac { \partial \mathcal { L } } { \partial u _ { k , i } } ,$

在我们的实现中，我们将 𝑁interp 设为 4。由于该求和运算对运行时影响最大，我们将其作为高度并行的缩减操作在 GPU 上实现。我们的内核以交错模式将线程分配给各个节点（图 2）：每个线程累加一个局部和，然后通过波段级缩减生成最终的梯度。这种设计将计算与缩减融合为单次迭代，从而避免了对全局内存的中间写入操作。

4.2 边界条件

样条参数化的一个关键优势在于它能隐式满足边界状态，从而消除了约束耦合及其通常随之产生的振荡修正。静态目标仅要求我们将最后一个结点重复四次，这便自动满足了式(9)。当初始状态 Θ0 非静态时，我们引入三个虚拟结点，并配有相应的幽灵/虚拟控制点 $u _ { - 3 } , u _ { - 2 } , u _ { - 1 }$ ，这些点由式(10)在 𝛼 = 0 且 $\ddot { { \boldsymbol { \theta } } _ { 0 } } = { \boldsymbol { 0 } }$ 时计算得出。通过将这些结点锚定于初始状态，我们消除了相互冲突的边界约束，使优化器能够专注于剩余的目标函数，

$\begin{array} { l } { { \displaystyle { u _ { - 3 } = - \frac { 1 } { 6 } \ddot { \theta } _ { 0 } d t _ { u } ^ { 2 } + \theta _ { 0 } } , } } \\ { { \displaystyle { u _ { - 2 } = \frac { 1 } { 3 } \ddot { \theta } _ { 0 } d t _ { u } ^ { 2 } + \theta _ { 0 } + \dot { \theta } _ { 0 } d t _ { u } } , } } \\ { { \displaystyle { u _ { - 1 } = \frac { 1 1 } { 6 } \ddot { \theta } _ { 0 } d t _ { u } ^ { 2 } + \theta _ { 0 } + 2 \dot { \theta } _ { 0 } d t _ { u } } . } } \end{array}$

五、用于场景碰撞规避的ESDF

碰撞查询是运动生成中的关键计算瓶颈。近期方法通过将机器人近似为球体[59, 66, 68]，将其转化为带符号距离到点的查询，从而降低了这一计算成本。然而，这些方法面临两大挑战：(1) 将深度观测数据与已知的几何基元融合为一个持久的世界模型；(2) 高效生成任务特异的距离场。我们采用如图 3 所示的两阶段表示法来解决这些难题：(1) 一种块稀疏 TSDF，它通过无锁集成内核将深度和几何基元融合为持久世界模型（第 5.1 节）；以及 (2) 一种按需生成的稠密 ESDF，其分辨率适配任务需求，并具备用于水密几何体的内部符号恢复功能（第 5.4 节）。

5.1 基于块稀疏的TSDF存储

对于许多常见任务，TSDF需要在米级环境中表示毫米级分辨率的几何结构。我们采用块稀疏表示法（图 4），将体积划分为 83 体素的块，并仅分配靠近观测表面的块。哈希表将块坐标映射到池索引 [49]，并使用比较并交换（CAS）机制处理并发插入操作。当体素权重衰减至阈值以下时，相关块将被移回空闲列表以供重复使用，从而限制了峰值内存占用，且不受场景持续时间的影响。每个体素维护两个独立的有符号距离通道，以 float16 格式存储：用于深度观测的累积加权平均值（depth_sum, depth_wt，每体素 4 字节），以及用于解析原语的 geom_sdf（每体素 2 字节）。在查询时，有效带符号距离为 min(depth, geom)，因此深度融合表面与已知几何体共同参与碰撞规避。对于一个包含 10 万个活动块的典型室内场景，总 GPU 内存约为 350 MB，大约是等效稠密网格的 1/11。

图3：TSDF-ESDF处理流程。深度图像通过基于体素的投影（第5.2节）融合为块稀疏TSDF，并采用解析方法标记已知几何信息。每次更新后，TSDF权重会衰减（考虑时间与视锥体因子），权重低于阈值的块会被标记为“墓碑”并回收利用。按需生成稠密ESDF分为三个阶段：(1) 从零交叉点初始化表面点，(2) 通过PBA+算法传播距离，(3) 恢复截断带之外几何层体素的符号。

图 4：块稀疏 TSDF 存储。仅分配靠近观测表面的块。哈希表通过 CAS 将块坐标映射到池索引。每个体素存储两个独立的 float16 通道（深度和几何），在查询时返回其最小值。回收的块通过空闲列表栈进行管理。

图 5：Voxel-Project 深度整合。（a）逐像素光线检测当前深度帧所触及的块。（b）过滤重复键，并通过 CAS 为存活的块分配内存，从而得到 𝐾 个池索引。（c）第 4 阶段反向映射：每个体素将其自身投影到图像中，读取投影像素处的深度，并直接写入带符号的距离，从而消除所有原子级竞争。

5.2 深度与原始积分

深度积分采用“体素投影”策略（图 5）。其核心思想是围绕体素组织 GPU 工作：每个体素被分配一个独立的线程，将自身投影到深度图像中，并通过一次无竞争的存储操作写入其带符号的距离值。该管道分为四个阶段：

块检测。对于每个像素，截断带内的光线采样用于确定哪些体素块被当前深度帧覆盖，从而启动 𝑁 = 像素数 × 采样数个线程。
去重。来自无有效深度像素（例如超出范围或缺失读数）的键值将被丢弃，其余键值经过去重处理，最终仅保留本帧中可见的 𝐾 个唯一块键。
块分配。每个唯一键值通过比较并交换（CAS）操作插入哈希表，并在可用时复用空闲列表中的回收块。输出结果是可见块的池索引组成的紧凑数组。
基于体素的集成。针对所有可见块，按每个体素启动一个线程（𝐾 × 512 个线程）。每个线程计算其体素在世界坐标系中的中心位置，使用相机内参将其投影到图像平面上，读取投影像素处的深度值，并直接写入带符号的距离和权重。由于每个体素仅由一个线程独占，因此无需进行原子操作。

围绕体素组织工作可实现合并的内存写入（相邻线程写入相邻体素），并避免在积分内核中进行哈希表查找，因为池索引已在第3阶段解析完毕。其代价是第4阶段具有数据依赖的启动维度（𝐾×512），需要进行设备到主机的同步以读取𝐾。由于多个图像像素可能映射到同一个相邻体素，我们将积分权重乘以体素在像素中的投影面积，𝑐 = (𝑓𝑥 𝑣/𝑧) (𝑓𝑦 𝑣/𝑧)，其中 𝑓𝑥、𝑓𝑦 分别为焦距，𝑣 为体素大小，𝑧 为相机视场深度。每次观测的权重为 𝑤 = max(𝑐, 1)，以确保权重至少为1。

长方体和网格直接渲染到几何通道中。对于每个基元，我们计算其在体素坐标系中的轴对齐边界框，并选择其解析SDF落在截断带内的候选块。经过去重和哈希表分配后，我们将每个体素更新为与该基元之间的最小有符号距离。

5.3 基于视锥体的衰减与块回收

为了追踪动态场景，我们在每次积分步骤后应用两级乘法权重衰减。对每个已分配的块应用时间衰减因子 𝛼𝑡（例如 0.99），从而逐渐淡化旧的观测值。此外，针对当前摄像机视锥体内的块，还会应用视锥体衰减因子 𝛼𝑓（例如 0.5），从而能够快速适应视场中移动或消失的物体。每帧的综合权重更新为

截锥体成员资格是在块级别上通过保守的包围球测试确定的（每个块一个线程），精度可达±半块。衰减操作本身是对区块数据张量进行的一次融合乘法运算，无需任何原子操作。衰减完成后，通过 PyTorch 归约计算每个区块的权重之和；总权重低于阈值的区块将通过“墓碑化”（tombstoning）其哈希槽并将其区块索引推入空闲列表以供重复使用，从而被回收。

5.4 带符号恢复的按需ESDF

为了使机器人能够在不发生碰撞的情况下与环境交互并进行导航，它需要能够查询空间中任意一点到最近表面的带符号距离。这一需求促使我们需要在工作空间内构建一个高密度的ESDF，但由于内存限制，这往往导致ESDF只能以较低的分辨率存储。此外，由于机器人被近似为碰撞球体，将 ESDF 精细化到远超最小球体半径的程度将导致收益递减：三线性插值误差本身已仅占球体半径的极小部分，因此碰撞检测仍能保持可靠性。这促使我们以比 TSDF 更粗糙的、针对特定任务的分辨率生成 ESDF。对于使用精细球体（1–5 cm）的操纵任务，5 mm 分辨率的 ESDF 已足够；而对于在更大工作空间内使用较粗球体（例如 50 cm）的移动底盘规划任务，则更粗糙的 ESDF 更为合适。这种解耦还带来了性能上的益处：更精细的 TSDF 可避免积分过程中的原子级竞争，而更粗糙的 ESDF 则能减少内存和计算开销。在给定工作空间边界和分辨率的前提下，我们分三个阶段生成一个密集的ESDF，从而支持 𝑂 (1) 次三线性距离查询。

5.4.1 点位初始化

我们识别出表面点位，即稀疏 TSDF 中零交叉点附近的体素，这些点位将作为距离变换的初始点（图 6a）。这种从稀疏到稠密的转换是 ESDF 生成的计算瓶颈，因为对于每个表面体素，都必须读取稀疏 TSDF 并写入稠密 ESDF。针对这种转换，存在两种策略：散射与聚合（图 6a）。散射策略遍历已分配的 TSDF 块（工作量为 𝑂 (𝐵 · 83)，其中 𝐵 是已分配块的数量），将每个表面体素（满足 |sdf| < 0.9 𝑣tsdf）投影到 ESDF 坐标系中，并写入该点。当 ESDF 比 TSDF 更粗糙时，多个 TSDF 体素会映射到同一个 ESDF 单元，需要原子写入来解决写入冲突。此外，工作维度取决于 𝐵，而 𝐵 随帧变化，这使得无法捕获 CUDA 图。聚合操作反向进行映射：每个 ESDF 单元通过 𝑂 (1) 次哈希查找查询稀疏 TSDF，以确定其是否包含表面。我们对每个单元采样七个位置（中心及六个轴对齐的面中心，每个偏移量为 1/2 𝑣esdf），若任一采样点满足 |sdf| < 0.9 𝑣tsdf，则将该单元标记为站点。由于每个单元仅由一个线程写入，因此无需原子操作，且内存写入可完全合并。工作维度固定为 |Gesdf| = 𝐷×𝐻×𝑊，与场景内容无关，这使得该内核兼容 CUDA 图捕获，并使整个 ESDF 管道（种子 → PBA+ → 符号恢复）能够作为单个捕获图运行。其权衡在于覆盖范围：当 𝑣esdf ≪ 𝑣tsdf 时，位于七个采样点之间的薄表面可能会被遗漏；在实际应用中，对于典型的分辨率比（2–4×），7 点模板足以覆盖单元体体积。

5.4.2 距离传播

我们利用并行带状算法（PBA+）[3] 计算精确的最近点分配，该算法利用欧几里得平方距离的可分离性，将三维沃罗诺伊图分解为三个独立的轴对齐遍历（图 6b）。第一阶段通过双向泛洪算法沿每条Z列解决一维最近邻点问题：正向扫描传播最近观察到的点，而反向扫描则保留正向或反向候选点中距离更近的那一个。随后，通过第二阶段和第三阶段（分别沿 Y 轴和 X 轴进行扫描）将所得的局部沃罗诺伊图扩展为 2D 和 3D 图。每个阶段均应用 Maurer 的抛物线交点测试 [41]：给定沿扫描轴排序的候选点 𝐴、𝐵、𝐶，当距离抛物面 𝑑²(𝐴, ·) 与 𝑑²(𝐶, ·) 的交点位于点 𝐵 的位置之前时，点 𝐵 即被判定为受支配点并被剔除。通过每列单次线性遍历构建此压缩堆栈，随后沿堆栈遍历为每个体素分配最近的点，可在每个轴上实现 𝑂 (𝑁 ) 工作量、𝑂 (1) 误差的距离变换。第 3 阶段在轴转置数据上复用相同的扫描和着色逻辑，因此仅需三个独特的核函数。整个传播过程可生成精确的欧几里得沃罗诺伊图，这与JFA [62]等近似迭代方法不同，后者需要 dlog2 𝑁 e + 3 次遍历，且在对抗性配置下可能遗漏站点。由于内核调用次数固定（五次）且与场景内容无关，该管道完全兼容CUDA图捕获。传播完成后，根据存储的点坐标计算每个体素处的欧几里得距离。

带符号恢复PBA+算法返回无符号距离；其难点在于恢复截断带之外体素的符号（图6c）。对于基于深度的TSDF，我们假设截断带外的符号为正（外部），因为深度观测仅能看到前表面。对于水密几何体（解析基元），表面点位于零交叉处，此时 TSDF 存在歧义；我们通过采样相邻体素来解决此问题，该体素沿点到查询点的方向偏移至查询点。由于该相邻点位于同一 Sign Recovery PBA+ 生成无符号距离；挑战在于恢复 TSDF 截断带之外体素的符号（图 6c）。对于基于深度的 TSDF，我们假设截断带外的值为正（外部），因为深度观测仅能看到前表面。对于水密几何体（解析基元），表面点位于零交叉处，此时 TSDF 存在歧义；我们通过采样相邻体素来解决此问题，该体素沿点到查询点的方向从该点偏移至查询点。由于该相邻点位于同一

图 6：ESDF 生成管道。(a) 点位初始化：scatter 操作为每个 TSDF 体素启动一个线程并写入 ESDF（需要原子操作）；gather 操作为每个 ESDF 体素启动一个线程，并通过 7 点模板探查 TSDF（无需原子操作，且符合 CUDA 图安全规范）。(b) PBA+ 通过可分离扫描传播位置所有权：第一阶段通过双向泛洪扫描行，第二阶段沿列构建 Maurer 堆栈以生成精确的沃罗诺伊图；在 3D 场景中，第三阶段对转置数据重用相同的内核以解析剩余轴。 (c) 对于超出截断带的体素，相邻的 TSDF 采样点用于确定其内部或外部符号。

5.4.3 符号修复

PBA+ 计算出的距离值均为无符号的；其难点在于恢复截断带（TSDF）之外体素的符号（图 6c）。对于基于深度的 TSDF，我们假设截断带外的符号为正（外部），因为深度观测仅能看到前表面。对于水密几何体（解析基元），表面点位于零交叉处，此时 TSDF 存在歧义；我们通过采样相邻体素来解决此问题，该体素沿点到查询点的方向偏移至查询点。由于该相邻点位于与查询点同侧的表面上，其 TSDF 符号明确无误：负值表示内部，正值表示外部。关键在于，相邻查找仅采样静态（几何）SDF 通道，因为只有水密基元在远离表面时具有可靠的符号；深度融合表面是开放的，其在截断带之外的符号未定义。若邻近查找未命中（例如位于分配块之外），则回退至查询体素本身的组合SDF；无观测数据的体素默认取正值（外部）。获得 ESDF 后，我们将碰撞约束计算为时间步之间近似的扫掠体积检查，沿轨迹以带符号的距离进行步进。该成本采用 CHOMP 速度度量 [60]，通过速度对梯度进行缩放以加速脱离碰撞。

六、扩展至高自由度机器人

将运动优化扩展至类人机器人等高自由度机器人时，会面临简单机械手所不具备的挑战。特别是，分支运动学树会产生多个末端执行器，其雅可比矩阵必须并行计算（图7）；而自碰撞对的数量会随连杆数量的增加呈二次增长，导致简单的成对查询受限于内存容量。此外，为确保执行器扭矩限制，优化循环中需采用可微逆动力学；而仿生关节通过单个执行器将多个连杆机械耦合在一起。我们通过五项关键创新应对这些挑战：(1) 面向正向运动学的自适应内核调度，其可从融合的单内核执行扩展至复杂机器人的双内核拆分（第6.1.1节）； (2) 预计算拓扑缓存，通过利用运动学稀疏性实现并行梯度反向传播（第 6.1.2 节）；(3) 两阶段过滤方案，用于计算分支树和模仿关节的稀疏雅可比矩阵（第 6.1.3 节）； (4) 用于自碰撞检测的融合Map-Reduce内核，可将受内存限制的成对查询转换为受计算资源限制的归约操作（第6.2节）；以及 (5) 可微逆动力学引擎，可在优化过程中直接强制执行扭矩限制（第6.3节）。

图7：对并行计算构成挑战的运动学结构。(a) 单臂串联链，即传统情况。(b) 仿生关节，其中驱动𝜃1会机械性地约束𝜃2和𝜃3。(c) 具有分支运动学链的人形机器人。我们引入预计算拓扑缓存，用于𝑂 (1) 祖先查找和两阶段雅可比矩阵滤波，以高效并行化梯度和雅可比矩阵计算。

6.1. 运动学改进

6.1.1. 正向运动学

单次正向运动学调用必须生成坐标系变换、工具姿态、碰撞球位置、质心以及雅可比矩阵。cuRobo [66] 引入了一种融合的单内核方法，可在单次调用中计算坐标系变换和碰撞球位置，但该方法仅在四个线程上进行并行化，且不支持质心和雅可比矩阵的计算。我们基于机器人复杂度对此进行了扩展，采用了自适应内核调度（图 8）。对于碰撞球数量较少（≤ 100）的简单机器人，融合内核现在可在相同的四线程设计中计算所有五项输出。对于具有大量碰撞球（> 100）的复杂机器人（如双臂机器人和类人机器人），计算被拆分为两个内核（图 8）：第一个内核根据关节角度计算坐标系变换并将其写入全局内存；第二个内核读取这些变换，并在多个线程上并行计算碰撞球位置、工具姿态、质心和雅可比矩阵。与所有阶段都在四个线程上串行化的融合内核不同，这种拆分允许第二个内核根据球体和连杆的数量来扩展线程数，从而大大减少了计算时间。

图 8：自适应前向运动学执行。对于简单机器人，单个融合内核负责计算坐标系变换、球体位置和雅可比矩阵。对于复杂机器人，该过程被拆分为两个内核：第一个内核计算坐标系变换，第二个内核计算球体位置和雅可比矩阵。

6.1.2. 并行梯度反向传播

上文所述的自适应内核调度虽然加速了正向运动学计算，但将梯度反向传播至关节仍是一个瓶颈：每个连杆的梯度必须通过所有祖先关节向后传递。我们通过在 GPU 波段内将连杆分配给不同线程，在连杆层面上实现了这一过程的并行化（算法 1）。例如，人形机器人右手的梯度仅影响右臂和躯干关节。为利用这种运动学稀疏性，我们使用预计算的拓扑缓存，该缓存可在 𝑂 (1) 时间内为 𝑂 中的任意连杆提供有序的祖先连杆列表，从而使每个线程仅需遍历相关的运动学链。这种数据驱动的设计还通过将每个连杆的梯度映射到正确的驱动关节，隐式地处理了模仿关节。

6.1.3. 稀疏雅可比矩阵的计算

对于Levenberg-Marquardt等求解器，我们通过将每一列进行并行化来计算完整的运动学雅可比矩阵（算法2）。每个线程负责单个关节 𝑗，并采用两阶段过滤过程。首先，预计算的 affects[ 𝑗, 𝑒] 缓存提供 𝑂(1) 复杂度的粗略检查，用于剔除无法影响目标连杆 𝑒 的整个子树。对于剩余的关节（包括控制多个连杆的模拟关节），第二阶段的精细过滤器会检查每个机械耦合连杆是否是𝑒的直接祖先。这确保了每个雅可比矩阵列仅累积来自运动链中相关连杆的贡献。

我们的稀疏雅可比矩阵（算法 2）支持用于逆运动学（IK）的莱文伯格-马夸特（LM）求解器，这种匹配非常自然，因为逆运动学本质上是一个非线性最小二乘问题，其目标是将姿态误差的平方 ‖r‖² 最小化，其中残差 r = T𝑔𝑜𝑎𝑙 FwdKin(q)。LM 通过 J>J 近似海森矩阵，并采用信任域阻尼以实现全局鲁棒性（算法 3），在该最小二乘结构下，其收敛速度快于 L-BFGS 等一阶方法。我们改编了 Newton [48] 的基于 Warp [40] 的 LM 实现方案，使其采用我们的雅可比矩阵，从而支持具有多个工具坐标系的高自由度机器人。

对于无碰撞反向运动学（IK），我们采用两阶段策略：首先，LM在无碰撞约束条件下收敛至目标姿态；随后，L-BFGS在自碰撞和环境约束下对解进行精化。由于LM在目标附近提供了良好的初始解，因此L-BFGS只需进行微调即可解决碰撞问题，而无需从头开始搜索。

6.2. 基于Map-Reduce的自碰撞检测

cuRobo [66] 通过将球体加载到共享内存中，在单个线程块内计算自碰撞成本。然而，对于高自由度机器人，对数会呈二次增长，从 7 自由度 Franka 机器人的 818 对，到 48 自由度类人机器人的 16.2 万对，这迫使每个线程必须遍历大量对，从而导致计算串行化。我们通过两阶段 Map-Reduce 来解决这个问题：(1) 将对划分为多个块，每个块将球体加载到共享内存中，并进行归约以找出其穿透性最强的对；(2) 最终内核对各块的最大值进行归约，以确定全局最大值，如图 9 所示。

6.3. 基于可微逆动力学的扭矩限制

违反执行器限制的轨迹无法执行。虽然大多数规划器将扭矩约束视为事后检查，但我们通过基于递归牛顿-欧拉算法（RNEA）[15]的可微逆动力学引擎直接强制执行这些约束。RNEA 通过在运动学树上进行三次迭代，计算产生给定运动 (𝑞, §𝑞, •𝑞) 所需的关节扭矩 𝜏： (1) 从基点到末端的正向遍历，用于传播连杆速度和加速度；(2) 力计算阶段，根据各连杆的惯性与运动状态计算其合扭矩；(3) 从末端到基点的逆向遍历，将这些力累加为关节扭矩。𝑂(𝑛) 的复杂度使 RNEA 成为逆动力学分析的标准选择，但其顺序树遍历方式给 GPU 并行化带来了挑战。

图 9：两阶段自碰撞检测。对于复杂的机器人，碰撞对会被划分到不同的 GPU 块中（映射）。第一个内核用于寻找局部极值，第二个内核则通过优化这些局部极值来寻找全局极值。

现有 GPU 实现的局限性目前存在两种 GPU 加速的 RNEA 实现：Newton [48] 和 GRiD [57]。Newton 支持运行时惯性参数的更改（例如载荷），但由于其模块化架构，每次调用会启动六个内核，并在反向传播过程中重新计算前向量，因此在我们的轨迹优化批处理规模下会成为瓶颈。GRiD 生成针对特定机器人的 CUDA 代码，其中包含完全展开的循环和硬编码索引，在小批量处理时能实现高吞吐量。然而，它需要针对每台机器人重新编译，不支持运行时惯性参数的更改，且其对共享内存的密集使用（对于 6×6 空间变换和惯性矩，每个连杆需 36 个浮点数）在类人机器人等高自由度机器人上会超出 SM 的限制。我们在表 2 中总结了这些不足之处。

我们将RNEA实现为通用机器人GPU内核，这些内核通过连杆数𝑛、自由度𝑑和批处理大小𝐵进行参数化。运动学树拓扑、空间惯性矩𝐼𝑘以及关节类型均在运行时提供，因此单个编译后的二进制文件即可支持任何由URDF描述的机器人。有三项设计选择决定了性能表现：紧凑的空间表示可减轻共享内存压力；基于VJP的后向计算可避免𝑂(𝑛²)雅可比矩阵的显式计算；以及采用波段级同步的树级并行计算。

紧凑的空间表示。我们不直接计算完整的 6×6 空间变换和惯性矩（72 个浮点数/链接），而是将变换存储为因式分解的 Featherstone 形式，即 3×3 旋转矩阵 𝑅 加上平移矩阵 𝑝（12 个浮点数），并通过恒等式 𝑋 · [𝜔; 𝑣] = [𝑅>𝜔; 𝑅> (𝑣 + 𝜔 × 𝑝)] 进行实时计算。空间惯性矩采用紧凑的 12 个浮点数表示（质量、质心、旋转惯性张量），其乘积通过解析计算而非 6×6 矩阵乘法获得。

基于VJP的反向传播。GRiD 按照 Carpentier [4] 的方法计算完整的 𝑛 × 𝑛 雅可比矩阵 𝜕𝜏/𝜕𝑞 和 𝜕𝜏/𝜕 §𝑞，这会产生 𝑂 (𝑛2) 的计算开销。对于基于梯度的优化，我们改用直接计算向量-雅可比矩阵乘积（VJP）：给定 𝜕L/𝜕𝜏，我们通过一次伴随 RNEA 迭代，在 𝑂 (𝑛) 时间内获得 𝜕L/𝜕𝑞、𝜕L/𝜕 §𝑞 以及 𝜕L/𝜕 •𝑞。当提供外力 𝑓ext 时，反向传播还会计算 𝜕L/𝜕𝑓ext = − ¯𝑓，从而实现通过接触力或交互力进行梯度流。

内存布局。正向内核缓存了未填充的压缩速度和加速度（12个浮点数），以及为对齐而填充的力（8个浮点数），每条连杆共计20个浮点数。变换值由𝑞重新计算而非直接使用缓存，以此牺牲每个链接约48条算术指令，换取12个浮点数的全局内存带宽。后向内核每条线程块仅需将机器人惯性参数（质量、质心、旋转惯性）加载一次到块共享内存中，从而消除了不同批次间的冗余全局加载。