IEEE Transactions on Robotics

Motion Planning Diffusion: Learning and Adapting Robot Motion Planning With Diffusion Models

作者：João Carvalho, An Thai Le, Piotr Kicki, Dorothea Koert, Jan Peters · 机构：Intelligent Autonomous Systems Lab, Computer Science Department, Technical University of Darmstadt, Darmstadt, Germany; Poznan University of Technology, Poznan, Poland; IDEAS, Warsaw, Poland; Centre for Cognitive Science, Technical University of Darmstadt, Darmstadt, Germany; German Research Center for AI (DFKI), Research Department: SAIROL, Darmstadt, Germany; Hessian.AI, Darmstadt, Germany

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：4881-4901

路径规划运动规划控制优化操作

精读笔记

Problem Setting

《Motion Planning Diffusion: Learning and Adapting Robot Motion Planning With Diffusion Models》（IEEE Transactions on Robotics / 2025）处理的是 optimization-based motion planning 的初始化/先验问题：给定一类重复出现的规划任务，能否从过去解中学习一个可复用的轨迹分布，并在新障碍或新目标下快速适配。

真正困难点不是单次规划，而是“全局可行性”和“局部优化性”的矛盾。CHOMP/TrajOpt/GPMP 这类方法局部性强，straight-line initialization 在 narrow passage 或复杂障碍下容易卡死；RRT-Connect 之类 sampling planner 能找通路，但慢、路径不平滑，后续还要 smoothing。学习 prior 的路线看似自然，但 motion planning 的 prior 必须同时满足三个条件：覆盖多种 homotopy mode、生成样本接近可行空间、还能在测试时被新约束改变。

这篇论文实际瞄准的是中间地带：不是做一个完整替代 classical planner 的神经规划器，而是把历史 planner / human demonstration 的解压缩成 motion memory，再用 differentiable cost 在测试时修正。关键矛盾是：prior 太弱则退化成普通 trajectory optimization；prior 太强则新增障碍下无法偏离训练分布。MPD 的设计基本都围绕这个 prior-cost tradeoff 展开。

Motivation

已有路线的缺口比较清楚。kNN / memory-based solution reuse 不可扩展，且在高维 context 上距离度量很脆；ProMP / GP prior 适合 smooth unimodal behavior，但遇到多个绕障模式会平均掉；GMM 在高维轨迹参数空间里训练和模式数选择都麻烦；VAE/GAN 虽能生成，但 VAE 容易 mode averaging，GAN 训练不稳定且不天然适合 test-time cost guidance；EBM 表达力强但采样困难。

作者的核心观察是：motion planning prior 不应该只是提供一次 warm start，而应该在整个生成过程中持续约束轨迹，使轨迹既靠近历史可行分布，又逐步响应当前 cost。换句话说，问题不是“先生成，再优化”，而是“边生成，边规划”。这正好契合 diffusion 的 classifier guidance / score guidance 机制：prior score 来自模型，任务 likelihood gradient 来自规划 cost。

另一个动机是 waypoint 表示和 motion planning 的 smoothness 要求并不对齐。dense waypoint diffusion 把平滑性留给额外 cost 或后处理，导致优化与生成目标相互拉扯。B-spline 的引入不是简单工程替换，而是把“轨迹应当平滑”编码进生成空间本身，减少模型需要学习的自由度。

Core Idea

MPD 的核心思想是把 motion planning 变成 guided posterior sampling：扩散模型学习条件轨迹先验 p(τ|c)，测试时由 collision、goal pose、joint limits、velocity/acceleration 等 cost 定义 likelihood p(O|τ)，最终不是求一个 MAP trajectory，而是在 reverse diffusion 中采样 p(τ|O,c)。这使得轨迹的多模态性可以保留下来，同时每一步 denoising 都有机会被当前环境的 cost gradient 修正。

这与“学习一个 planner”或“学习一个 initializer”有本质差别。initializer 路线把学习和优化串联：先从 prior 出一个样本，然后 cost optimizer 接管；一旦样本大段进入障碍，局部优化可能很难拉出来，且容易破坏 prior 中的风格/模式。MPD 则把 prior 和 cost 交织在 reverse process 内，信息流从一次性 warm start 变成逐步 posterior refinement。

B-spline 参数化提供了另一个 inductive bias：模型不直接学习高频 waypoint 序列，而学习低维控制点。这样 diffusion 主要建模轨迹拓扑和粗形状，平滑性由基函数保证，局部控制点移动只影响局部轨迹段。这个 representation alignment 很重要：motion planning 关心连续曲线及其导数，而不是离散点本身。

Method

1. Conditional diffusion prior：它解决的是多模态 motion memory 的表达问题。输入 context 可以是 start/goal joint configuration，也可以是 start joint + desired end-effector pose。模型输出 B-spline 内部控制点的 denoising noise。必要性在于相同 context 下可能存在多个绕障模式或多个 IK-equivalent goal configuration，确定性回归会平均失败。

2. Cost-guided reverse diffusion：它解决的是测试时约束变化问题。每个 denoising step 中，先由 diffusion 得到 prior mean，再用可微 planning cost 的梯度对 mean 做有限幅度更新。机制上相当于在局部 Gaussian denoising posterior 中加入 likelihood 的一阶近似。论文实际还做了若干工程修正：去掉随时间衰减的 covariance scaling、允许多步梯度、限制偏离 prior 的幅度、调低 prior strength。这些修正说明原始 classifier guidance 直接用于 motion planning 并不够稳定。

3. B-spline trajectory representation：它解决的是 smoothness 和维度问题。扩散模型学习控制点而非 dense waypoints；轨迹、速度、加速度由固定基函数解析得到。核心变化是把 smoothness 从 cost regularization 迁移到表示层，降低 denoising 网络输入维度，并让碰撞梯度通过 B-spline basis 回传到控制点。

4. Differentiable planning costs：cost 包括环境碰撞、自碰撞、joint limits、task pose error、velocity/acceleration。这里的关键不是 cost 本身新，而是这些 cost 被用作 posterior sampling 的 likelihood gradient。SDF + sphere robot model 使碰撞梯度可算，这也是方法可工作的基础前提。

5. Batch posterior sampling：MPD 不是保证单条轨迹一定成功，而是并行采样一批 posterior particles，再从中选可行/低 cost 的轨迹。这个机制很关键：多模态与成功率很大程度来自 batch sampling + filtering，而不是单样本确定规划能力。

Key Insight / Why It Works

最核心的有效性来源不是“diffusion 很强”这个泛泛说法，而是三个因素叠加。

第一，历史规划解提供了强 data prior，本质上是把 expensive sampling-based search amortize 到模型参数中。RRT-Connect 预先探索过 configuration-free space，diffusion 学到的是这些解的压缩分布。因此 MPD 的能力边界很可能接近训练数据覆盖的 homotopy classes。它不是在测试时重新做全局搜索，而是在 learned trajectory manifold 附近做 guided deformation。所谓 planning/reasoning 更像 memory reuse + local adaptation。

第二，guided denoising 比 prior-then-optimize 更适合避障。先采样再优化时，轨迹已经落到某个完整曲线，如果大段穿过新障碍，局部 cost gradient 可能陷入差的 basin；而 MPD 在高到低噪声的过程中逐步塑形，collision gradient 可以更早改变轨迹形态，避免最终样本进入不可修复状态。这是论文最有价值的机制 insight。

第三，B-spline 是非常关键的 representation alignment。waypoint diffusion 把连续轨迹问题离散化，smoothness 必须靠额外 regularizer；B-spline 让生成空间天然排除大量不平滑解，同时低维控制点降低学习难度和采样成本。这里的收益不是单纯 engineering，而是正确的 inductive bias。

但也要直说：部分增益可能主要来自 scaling / data / batch compute。训练数据由 classical planner 生成，模型继承了 planner 的覆盖；测试时一次采样 100 条轨迹并过滤，和单条 RRT 或单条 optimizer 比并不完全对等。扩散相对 CVAE 的优势在多模态和高维下合理，但文中没有完全隔离 architecture、training budget、latent size、sample count 对结果的影响，增益归因不完全干净。

此外，cost guidance 的理论推导依赖局部 Gaussian posterior 和 likelihood 一阶近似，但实践中作者显式打破了一些原始形式，例如去掉 covariance / sqrt schedule 衰减、调 prior strength、多步梯度。这说明方法成立更多是“diffusion prior + constrained gradient optimization 的实用融合”，而不是严格 posterior sampling 的精确实现。

Relation To Prior Work

这篇处在几条技术谱系的交叉点：learning priors for trajectory optimization、planning-as-inference、classifier-guided diffusion、movement primitive representation。

和 CHOMP/TrajOpt/GPMP 的差别在于 prior 的表达能力。GPMP 的 GP prior 主要编码 smoothness/dynamics，是 unimodal 的；MPD 的 diffusion prior 编码历史解分布，尤其是多种绕障模式。它不是替代 cost optimization，而是给 optimization 一个 learned multimodal support。

和 memory of motion / kNN warm start 的差别在于压缩和连续泛化。MPD 不保存 growing database，也不直接插值最近邻轨迹，而是用条件生成模型近似轨迹分布。但如果从能力本质看，它仍然是 motion memory，只是 memory 被参数化为 diffusion score。

和 ProMP / GP human demonstration prior 的差别在 multimodality 和 posterior sampling。ProMP/GP 的 closed-form 或 MAP 优化很优雅，但只适合单峰或近单峰行为；MPD 可以保留多个 demonstration mode，并用 cost guidance 在测试时适配障碍。

和 Diffuser / diffusion policy 的关系更近：都是 trajectory diffusion + guidance。但 MPD 面向 open-loop point-to-point robot motion planning，不是 offline RL 的 trajectory planning，也不是短时闭环 visuomotor action generation。其新增信息主要是：把环境 collision cost 作为 test-time differentiable likelihood，用 B-spline control points 替代 waypoint token，并把目标放在优化规划的初始化/先验问题上。

和早期 MPD / EDMP / diffusion trajectory prior 工作相比，实质创新主要在 B-spline 表示和更系统的 cost-guided planning framing；guided diffusion 本身不是新思想，planning-as-inference 也不是新思想，真正有价值的是把这些组件组合到 motion planning 的正确坐标系里，并验证在机械臂场景中比 prior-then-optimize 更稳。

Dataset / Evaluation

评价覆盖了从低维 2D 到 7-DOF Panda 的多类任务，并包含训练环境与新增障碍环境。这个设置能较好验证论文的核心 claim：在固定环境结构下，learned trajectory prior 可以被测试时 cost guidance 调整，以避开训练时不存在的局部障碍。真实世界部分包括 warehouse 场景和 human kinesthetic demonstration，说明方法不是纯 toy setup。

但 evaluation 的泛化边界比较明确：大多数任务是单环境或少数固定环境，新增障碍是局部 perturbation，不是跨环境结构泛化。作者也基本承认不做大规模 environment conditioning。因而实验支持的是“specialized environment 中的 amortized planning + adaptation”，不支持“通用神经 motion planner”。

与 RRT-Connect 的比较有参考价值但不能过度解读。MPD 预先用 RRT 生成大量数据，测试时再并行采样；RRT 在线运行单条路径。MPD 快是因为把 search cost 离线摊销了，不是凭空获得全局搜索能力。这个结论仍然有工程意义，尤其适合重复任务环境，但不是 classical planner 在开放问题上的直接替代。

B-spline ablation 比较有说服力：它验证了平滑性来自表示偏置，而不是扩散模型自动学会了 smooth trajectory。CVAE 对比则支持 diffusion 在多模态高维轨迹建模上的优势，但文中未充分说明不同生成模型在容量、训练轮次、调参程度上的完全公平性。

Limitation

最大限制是 data support。MPD 只能在 learned prior 覆盖或邻近区域内做有效变形；如果新增障碍封死训练中常见通道，要求进入训练数据几乎没有覆盖的 homotopy class，cost guidance 很可能失败。这里没有 probabilistic completeness，也没有全局搜索保证。

第二，方法强依赖固定或近固定环境结构。论文强调 warehouse 这类 shelves/static layout + movable objects 的场景是合理的，但这也限定了适用范围。若环境几何大幅变化，除非把环境编码进 context 并采集大量跨环境轨迹，否则 prior 会失效。所谓泛化不是开放世界泛化，而是局部结构稳定下的 adaptation。

第三，posterior sampling 的“贝叶斯”解释有一定近似性。实践中为了让 cost guidance 有效，作者去掉 schedule/covariance 的衰减、手动设置 prior strength、限制 step deviation。这些都是必要 trick，但也意味着采样分布并不严格等于原始公式下的 posterior。理论成立性更多是启发式。

第四，固定 trajectory duration 是明显上限。速度/加速度约束在 phase-time 映射下与 T 耦合，当前方法不优化时间，因此动态可行性只是软约束加过滤，不是真正的 time-optimal 或 kinodynamic planning。

第五，真实部署假设偏强：需要可靠 SDF、障碍 pose、机器人 sphere approximation、可微 kinematics Jacobian 和 GPU batch compute。碰撞几何误差、感知延迟、未建模物体都会直接影响 cost gradient。文中真实实验展示了可行性，但没有充分分析这些误差下的鲁棒性。

第六，增益归因不完全清晰。MPD 的成功同时来自 offline planner 数据、diffusion 多模态、B-spline 表示、batch sampling、DDIM 加速、test-time gradient steps。哪些是不可替代核心，哪些只是 scaling / engineering，文中部分 ablation 支持但没有完全拆干净。

Takeaway

  • 1. 对 motion planning 来说，diffusion 最有价值的用法不是直接输出一条轨迹，而是作为可被 test-time cost steering 的多模态 prior。
  • 这个范式比 learned initializer 更自然。
  • 2. 表示选择比模型花样更重要。
  • B-spline 把 smoothness、低维性、局部可控性对齐到规划问题本身，是这篇最值得迁移的 insight。

一句话总结

MPD 是把 sampling-based planner / demonstration 产生的运动记忆压缩成 B-spline diffusion prior，并用测试时可微 cost 在 denoising 过程中做 posterior-style 修正的一类 amortized motion planning 方法，其实质贡献在于 prior-cost 融合机制和轨迹表示对齐，而不是通用规划推理。