【学习笔记】基于扩散模型的运动规划学习与适应

《Motion Planning Diffusion: Learning and Adapting Robot Motion Planning with Diffusion Models》

一、 研究背景与动机

1. 核心问题：基于优化的运动规划方法（如 CHOMP, TrajOpt）非常依赖初始轨迹。一个不好的初始轨迹（例如，直接从起点到终点的直线）很容易让优化陷入局部最小值，尤其是在环境复杂、存在狭窄通道的情况下，导致无法找到无碰撞路径。
2. 传统解决方案的局限 ：
   • 通常做法是先运行一个基于采样的规划器（如 RRT-Connect）得到一个无碰撞的初始路径，然后再用优化器进行平滑。但这在复杂、高维度的场景中可能很慢。
   • 另一种思路是利用过去解决过的规划问题，学习一个轨迹分布的"先验知识"。这个先验知识可以为新问题提供一个好的初始解，从而提升规划效率。
3. 理想的先验知识应具备的特性 ：
   • 接近无碰撞：初始化本身就应该是一个可行的（或接近可行的）路径。
   • 能够表示复杂轨迹：能应对高维度的配置空间。
   • 能够编码多模态：对于同一个规划任务，可能存在多种不同的、同样有效的解决方案（例如，可以从障碍物的左边或右边绕过去）。一个好的先验应该能捕捉到这种多模态性。

二、 主要贡献与核心思想

本文提出了 Motion Planning Diffusion (MPD) 算法，旨在解决上述问题。其核心贡献可以概括为：

1. 创新的方法 ：MPD 是一种结合了学习 和适应 的方法。它利用扩散模型 从先前的运动规划解决方案中学习轨迹分布的先验知识，并在面对新任务（如新障碍物）时，通过成本引导（cost-guided） 的方式从后验分布中采样，从而生成既符合先验知识又满足当前任务目标（如无碰撞）的轨迹。
2. 高效的轨迹表示 ：论文采用 B-样条曲线 来参数化轨迹，而不是传统的密集路径点。这种表示方式具有以下优势：
   • 固有平滑性：B-样条曲线及其导数（速度、加速度）是光滑的，无需额外添加平滑项。
   • 低维参数化：轨迹由少量控制点决定，比密集路径点维度更低，计算效率更高。
   • 局部支持性：修改一个控制点只会影响轨迹的局部区域，便于进行局部优化。
3. 规划即推理框架 ：MPD 将运动规划问题重新表述为后验分布采样 问题。后验分布 p(轨迹 | 任务目标) ∝ 似然(任务目标 | 轨迹) * 先验(轨迹)。其中：
   • 先验(轨迹) 是使用扩散模型学习到的轨迹分布。
   • 似然(任务目标 | 轨迹) 对应于任务特定的成本函数（如碰撞成本、关节限位成本、到达目标姿态的成本等）。
4. 成本引导采样 ：在扩散模型的反向去噪过程中，MPD 使用任务成本函数的梯度来引导采样，使生成轨迹的每一步都偏向于低成本（高似然）的区域。这个过程将采样和优化无缝地结合在一起。

三、 方法详解 (核心算法)

MPD 的整个流程分为学习阶段 和规划阶段。

1. 学习阶段：学习轨迹先验

   • 数据生成：对于给定的机器人环境和任务，使用采样规划器（如 RRT-Connect）生成大量无碰撞的轨迹作为训练数据。每条轨迹都对应一个上下文（如起始关节位置、目标末端执行器位姿）。
   • 轨迹编码 ：将每条轨迹拟合为一条 B-样条曲线，得到其控制点 w。扩散模型学习的是控制点 w 的分布。
   • 扩散模型训练 ：
     • 训练一个条件扩散模型 p_θ(w | c)，其中 c 是上下文（起始位置和目标位姿）。
     • 扩散模型由两部分组成：一个前向扩散过程 ，逐渐向数据（B-样条控制点）中添加高斯噪声，直至变成纯噪声；以及一个反向去噪过程 ，训练一个神经网络 ϵ_θ 来预测加入的噪声。
     • 网络架构采用 1D U-Net ，并使用 FiLM (Feature-wise Linear Modulation) 模块来融入上下文 c 和当前去噪时间步 i。

2. 规划阶段：成本引导的后验采样

   • 输入：一个新的任务，包括起始位姿、目标位姿，以及可能存在的新的、在训练时未出现的障碍物。
   • 成本函数 ：构建可微分的成本函数，包括：
     • 碰撞成本：基于符号距离函数 (SDF)，计算机器人模型（用球体表示）与环境的穿透深度。
     • 关节限位成本：惩罚超出关节位置、速度、加速度限制。
     • 任务成本：衡量最终末端执行器位姿与目标位姿的误差。
     • 速度/加速度成本（可选）：用于使轨迹更平滑。
   • 成本引导的 DDIM 采样 ：
     • 采样从纯噪声 w_N 开始。
     • 在每一步去噪 i，模型 ϵ_θ 会预测出一个"先验"的均值 μ_i。
     • 关键步骤 ：如果当前步需要进行成本引导，MPD 会计算当前轨迹的成本梯度 g = -∇_w Σ λ_j C_j(τ)。然后，根据公式 μ_i' = μ_i + Σ_i * g，将先验均值向低成本方向移动。这里 Σ_i 是后验协方差，用于控制引导强度。
     • 经过多步引导和去噪后，最终得到一组（一个批次）B-样条控制点 w_0，进而生成完整的、平滑的、无碰撞的轨迹。
     • 使用 DDIM (Denoising Diffusion Implicit Models) 技术来加速采样过程，将采样步数从训练的 100 步减少到 15 步。

四、 实验评估

论文设计了一系列实验来回答几个关键问题，并验证 MPD 的有效性。

• 实验设置 ：
  • 从简单的2D点机器人到复杂的7自由度（7-dof）Panda 机械臂，在多种模拟环境和真实环境（仓库拾取放置任务）中进行测试。
  • 评估了训练环境 和新增障碍物（未见过的环境） 下的表现。
  • 对比基线包括：
    • GPprior+Cost：使用高斯过程先验（直线初始化）+ 成本优化，类似 CHOMP。
    • CVAE：使用条件变分自编码器学习轨迹分布。
    • Prior+Cost：先从一个生成模型（扩散模型或 CVAE）采样轨迹，然后进行成本优化。
    • RRT-Connect + Cost：先运行采样规划器，再优化。
• 主要实验结果 ：
  1. 多模态学习能力 (Q1)：MPD 成功学习了多模态轨迹分布。例如，在4连杆平面臂任务中，对于同一个末端执行器目标位姿，MPD 生成了多个不同的、无碰撞的关节配置，展示了其多模态表达能力。
  2. 与基线对比 (Q2) ：
     • 在训练环境下，MPD 和 Diffusion Prior 的成功率都较高，且多样性优于 CVAE。
     • 在新增障碍物的情况下，MPD 相比 Prior+Cost 方法（先采样后优化）展现出显著优势。例如，在仓库环境中，MPD 的有效轨迹比例（Fraction valid）远高于 Dprior+Cost。这表明在去噪过程中进行引导比先采样再优化更有效，因为一旦轨迹碰撞部分在优化前就已经生成，就难以摆脱。
     • 相比无信息的 GP 先验，MPD 的成功率大幅提升，证明了学习先验的价值。
  3. 成本引导的必要性 (Q3) ：通过一个2D点机器人的简单示例直观展示了 MPD 的必要性。Dprior+Cost（先采样后优化）由于采样出的轨迹已经深陷障碍物，优化无法将其推出；而 MPD 通过去噪过程中的逐步引导，成功生成了绕过障碍物的无碰撞轨迹。
  4. B-样条参数化的优势 (Q4) ：
     • 平滑性：与稀疏路径点表示相比，B-样条生成的轨迹在速度、加速度上明显更平滑（通过KDE图展示）。
     • 计算效率：由于控制点维度更低，MPD 的去噪网络处理速度更快。生成轨迹后，只需一次插值即可得到密集路径点。
  5. 人类演示学习与适应 (Q5) ：通过动力示教（kinesthetic teaching），让人类演示拾取放置任务的轨迹，并训练扩散模型。在测试时，即使桌面上新增加了人类演示时未出现过的物体，MPD 仍然能通过成本引导，生成避开这些新障碍物，同时又与人类演示风格相近的无碰撞轨迹。这证明了 MPD 可以从真实人类数据中学习，并适应新的场景。
  6. 计算效率：MPD 的总规划时间约 0.56 秒（包括扩散和梯度计算），与并行运行一次 RRT-Connect 相比，不仅更快，而且通过增加采样批次，成功率更高。

五、 局限性与未来工作

• 轨迹时长固定 ：当前假设轨迹时长 T 是固定的，但实际中优化时长可能带来更好的性能。未来工作可以考虑使用另一个 B-样条来参数化相位-时间关系。
• 计算速度：虽然已使用 GPU 并行化，但成本函数的计算由于 Python GIL 限制仍是顺序的，速度瓶颈。使用 JAX 等 JIT 编译框架可能进一步提升效率。
• 环境泛化：目前 MPD 针对一个特定环境进行专精（specialization），通过成本引导适应环境微小变化。若要泛化到结构完全不同的新环境，需要大量数据覆盖各种环境配置，这仍是挑战。

六、 结论

本文提出的 Motion Planning Diffusion (MPD) 是一个创新且强大的机器人运动规划框架。它巧妙地将扩散模型的强大生成能力与基于梯度的优化相结合，通过 B-样条实现了高效、平滑的轨迹表示。实验证明，MPD 不仅能从先验数据中学习多模态的轨迹分布，还能在规划时通过成本引导，灵活、高效地适应新障碍物等任务变化，生成高质量的无碰撞轨迹。该方法在模拟和真实世界的多项任务中均表现出优越性，为"学习与规划"相结合的研究提供了新的思路。