SanD-Planner: Sample-Efficient Diffusion Planner in B-Spline Space for Robust Local Navigation

01 Motivation

在杂乱、动态的环境中生成可靠的局部规划一直是移动机器人落地的关键瓶颈。核心难点在于：获取跨多样场景的大规模专家演示成本高，且如何在有限数据下提升学习效率仍是开放问题。近期的模仿学习（IL）方法（如 NavDP）依赖聚合大规模真实/仿真数据集来提升泛化，但这种“数据扩张”路线带来巨大的算力和采集开销。Generating reliable local plans in cluttered, dynamic environments has long been a key bottleneck for deploying mobile robots. The core difficulty is twofold: collecting large-scale expert demonstrations across diverse scenes is expensive, and how to raise learning efficiency under limited data remains an open problem. Recent imitation learning (IL) methods such as NavDP rely on aggregating large-scale real/simulated datasets to improve generalization, but this "data scaling" route incurs enormous compute and collection cost.

“In this paper, we explore a complementary direction by asking whether local navigation can achieve high performance in a ‘data desert’ of limited expert demonstrations.”

作者认为，与其继续扩大演示规模，不如通过更强的结构化归纳偏置（inductive bias）来提升样本效率——这正是 SanD-Planner 用 B 样条（B-spline）参数化轨迹的出发点：B 样条天然具有 C² 连续性与局部支撑（local support）特性，能让噪声/遮挡带来的远端预测误差被局限在远端控制点，不会传播到近端执行段。The authors argue that rather than further scaling up demonstrations, sample efficiency should be improved through a stronger structural inductive bias — which is precisely the motivation for SanD-Planner's B-spline parameterization of trajectories: B-splines are inherently C² continuous and have local support, so far-field prediction errors caused by noise or occlusion stay confined to the distal control points and do not propagate into the near-term executed segment.

02 Method

在每个规划步 t，模型接收 4 帧历史深度图像、相对目标点 g_t 与上一步的运动上下文（motion context）v_t^prev，经域专用编码器（domain-specific encoders）与两层 Transformer 融合成统一潜在上下文 C_t；扩散策略在该上下文条件下，通过迭代去噪生成一组固定数量（8 个）的 B 样条控制点，再由几何 critic 模块从候选中选出最优、可行的轨迹执行，并将其初始速度反馈回下一步以维持时序一致性。At each planning step t, the model takes 4 historical depth frames, the relative goal point g_t, and the previous motion context v_t^prev; domain-specific encoders and a two-layer Transformer fuse them into a unified latent context C_t. Conditioned on that context, the diffusion policy generates a fixed number (8) of B-spline control points by iterative denoising; a geometric critic module then selects the best feasible trajectory among the candidates for execution and feeds its initial velocity back to the next step to maintain temporal consistency.

B 样条轨迹参数化（Trajectory Parameterization via B-Spline Control Points）Trajectory Parameterization via B-Spline Control Points

不同于按规划时域线性增长输出维度的 waypoint 方法，SanD-Planner 只预测固定的 8 个 B 样条控制点，把几何先验作为归纳偏置嵌入表示中，在不增加学习复杂度的前提下扩展有效规划时域，缓解“近视”（myopic）行为。B 样条构造上保证 C² 连续，因此平滑性是表示本身自带的属性而非需要学习的特征；同时其局部支撑特性使远端感知噪声/遮挡只影响远端控制点，不会破坏近端执行段的稳定性。Unlike waypoint methods whose output dimension grows linearly with the planning horizon, SanD-Planner predicts only a fixed set of 8 B-spline control points, embedding a geometric prior into the representation as an inductive bias; this extends the effective planning horizon without adding learning complexity and mitigates "myopic" behavior. B-splines are C² continuous by construction, so smoothness is an intrinsic property of the representation rather than a feature that must be learned; meanwhile their local support makes far-field perception noise and occlusion affect only the distal control points, without undermining the stability of the near-term executed segment.

扩散策略 + 安全感知候选选择（Diffusion Policy + Safety-Aware Critic）Diffusion Policy + Safety-Aware Candidate Selection

轨迹生成与安全评估被显式解耦：一个可解释的在线 critic 模块集成了基于 ESDF（Euclidean Signed Distance Field）的安全检查器，配合显式的 clearance（净空）与 time-to-completion（完成时间）指标，对扩散策略生成的候选轨迹做折扣安全代价（discounted safety cost）与路径效率（path efficiency）打分，从而免去为可行性评估单独学习价值函数（value function）的训练负担，让策略只需专注于拟合专家轨迹分布。此外，previous-velocity token v_t^prev 被用于跨步的时序一致性调节。Trajectory generation and safety assessment are explicitly decoupled: an interpretable online critic module integrates an ESDF-based (Euclidean Signed Distance Field) safety checker together with explicit clearance and time-to-completion metrics, scoring the candidate trajectories produced by the diffusion policy on discounted safety cost and path efficiency. This removes the training burden of separately learning a value function for feasibility assessment, letting the policy focus solely on fitting the expert trajectory distribution. In addition, the previous-velocity token v_t^prev is used to regulate temporal consistency across steps.

03 Experiments

在 InternNav Benchmark（提供逼真室内场景，最小化 sim-to-real gap）上评估，包含 ClutteredEnv（杂乱几何障碍，2020 episodes）与 InternScenes（照片级室内场景，4040 episodes）两个子基准，指标为 Success Rate (SR) 与 Success weighted by Path Length (SPL)。对比基线为 iPlanner、ViPlanner 与 NavDP（三者结果取自基准论文 [5]）。Evaluation is carried out on the InternNav Benchmark, which provides photorealistic indoor scenes that minimize the sim-to-real gap. It comprises two sub-benchmarks: ClutteredEnv (cluttered geometric obstacles, 2020 episodes) and InternScenes (photorealistic indoor scenes, 4040 episodes). The metrics are Success Rate (SR) and Success weighted by Path Length (SPL). The baselines are iPlanner, ViPlanner, and NavDP, whose results are taken from the benchmark paper [5].

在 InternScenes 上，SanD-Planner 大幅超越 NavDP（"+6.3% SR and 3.0% SPL"），仅用 NavDP 约 0.25%（500 vs 200k 条轨迹）的训练数据，单张 RTX 4080 训练约 5 小时（NavDP 需 32×A100 训练 24 小时）。在 ClutteredEnv 上 SanD-Planner SR 最高（90.1%），但 SPL（84.0%）略低于 NavDP（87.7%）——论文指出这部分归因于 B 样条参数化偏向更安全但更保守的绕行策略，并认为该差距源于数据规模有限而非方法本身的局限。On InternScenes, SanD-Planner substantially outperforms NavDP ("+6.3% SR and 3.0% SPL") while using only about 0.25% of NavDP's training data (500 vs 200k trajectories), trained in about 5 hours on a single RTX 4080 (NavDP requires 24 hours on 32×A100). On ClutteredEnv, SanD-Planner attains the highest SR (90.1%), but its SPL (84.0%) is slightly below NavDP (87.7%) — the paper attributes this partly to the B-spline parameterization favoring safer but more conservative detours, and argues the gap stems from the limited data scale rather than from a limitation of the method itself.

Ablations

轨迹表示消融（Table III）：在保持流水线其余部分不变、统一预测 8 个锚点的前提下对比三种表示。B 样条在 ClutterEnv 上取得 93.0% SR / 83.8% SPL，显著优于 waypoints（75.5% / 65.8%）和 cubic spline（75.0% / 70.6%）；在 InternScenes 上同样最优（83.5% / 72.1% vs. 75.5%/67.8% 与 72.0%/65.8%）。论文将其归因于 B 样条的局部支撑特性能隔离远场不确定性，而插值三次样条是全局耦合的，遮挡区域的预测误差会向后传播、破坏近场执行段的稳定性。Trajectory representation ablation (Table III): Three representations are compared with the rest of the pipeline unchanged and 8 anchor points predicted in all cases. On ClutterEnv the B-spline reaches 93.0% SR / 83.8% SPL, clearly ahead of waypoints (75.5% / 65.8%) and cubic spline (75.0% / 70.6%); it is likewise the best on InternScenes (83.5% / 72.1% vs. 75.5%/67.8% and 72.0%/65.8%). The paper attributes this to the local support of B-splines isolating far-field uncertainty, whereas an interpolating cubic spline is globally coupled, so prediction errors in occluded regions propagate backwards and undermine the stability of the near-field executed segment.

速度 token 消融（Table IV）：去掉 previous-velocity token v_t^prev 后，ClutteredEnv 平均 SR 从 93.0% 降至 86.0%，SPL 从 0.84 降至 0.80。论文分析：缺少该时序条件时，在几何对称障碍物（如立柱）附近容易产生双模态的绕行候选，critic 逐步独立选择会导致绕行模式在连续重规划间反复切换，产生振荡转向甚至碰撞；加入速度 token 后候选更集中、朝向更稳定。Velocity token ablation (Table IV): Removing the previous-velocity token v_t^prev drops the average SR on ClutteredEnv from 93.0% to 86.0% and the SPL from 0.84 to 0.80. The paper's analysis: without this temporal conditioning, bimodal detour candidates easily arise near geometrically symmetric obstacles such as pillars, and the critic's independent step-by-step selection makes the detour mode flip repeatedly between consecutive replans, producing oscillatory steering and even collisions; adding the velocity token makes the candidates more concentrated and the heading more stable.

Real-world Experiments

真实实验部署在 Unitree Go2 四足机器人上，搭载带 IR 滤光片的 Intel RealSense D435 深度相机，算法在有线连接的笔记本上以约 10Hz 运行，采用 warm-start 策略（从上一步解的第 6/10 步开始部分去噪）实现高频重规划，每步并行采样 K=16 条候选轨迹。作者在静态/动态行人、狭窄迷宫、杂乱办公室、室外光照变化场景以及爬楼梯任务中验证了零样本迁移能力，未做任何微调或域适应。The real-world experiments are deployed on a Unitree Go2 quadruped robot carrying an Intel RealSense D435 depth camera with an IR filter. The algorithm runs at roughly 10Hz on a tethered laptop and uses a warm-start strategy (partial denoising starting from step 6/10 of the previous solution) for high-frequency replanning, sampling K=16 candidate trajectories in parallel at each step. The authors validate zero-shot transfer with static/dynamic pedestrians, narrow mazes, a cluttered office, outdoor scenes with changing illumination, and stair-climbing tasks, without any fine-tuning or domain adaptation.

04 Limitations