📌 一句话概括
TOAD 将端到端规划器中已训练好的轨迹评分器视为奖励函数，在测试时通过交叉熵方法（CEM） 搜索连续轨迹空间来最大化该奖励，在不重训练、不修改架构的前提下，一致提升任意基规划器的驾驶质量。
📄 论文信息
- 论文标题：Test-Time Trajectory Optimization for Autonomous Driving
- arXiv：2606.07170 (2026 年 6 月)
- 作者团队：Yihong Xu, Éloi Zablocki, Yuan Yin, Elias Ramzi, Ellington Kirby, Alexandre Boulch, Matthieu Cord — valeo.ai, Sorbonne Université
- 项目页：https://valeoai.github.io/TOAD/
- 关键词：autonomous driving, end-to-end planning, test-time optimization, CEM
🤔 要解决什么问题？
Score-and-Select 范式的根本矛盾
当前端到端规划器遵循**“生成-评分-选择”**范式：产生候选轨迹
$$\mathcal{T}=\{\tau^{(i)}\}_{i=1}^{N}$$，评分器
$$S(\tau;o,g)$$对每个候选打分，选最高分者输出。这隐含着致命假设——候选集中至少有一条接近最优。当候选集质量不理想时，再好的评分器也只能"矮子里拔将军"。
| 方法 | 候选生成方式 | 评分器能否影响候选集？ | 核心痛点 |
|---|---|---|---|
| Hydra-MDP | 预计算 8K+ 固定词表 + MLP 评分 | ❌ 不能 | 词表离散，评分器无法调整轨迹 |
| GTRS / ZTRS | Transformer 解码 + 固定词表评分 | ❌ 不能 | 评分器仅排序，不参与搜索 |
| iPad | 隐式查询解码 + MLP 评分 | ❌ 不能 | 评分器无法修复次优候选 |
| DrivoR | 扩散模型解码 + 自由评分器 | ❌ 不能 | 评分器强但仍受限于候选多样性 |
| RAP | 自回归预测 + 评分 | ❌ 不能 | 自回归误差累积，候选集退化 |
核心矛盾
评分器经过训练能准确评估轨迹质量，但其能力仅在"选中输出"时被使用一次——评分器蕴含的丰富驾驶知识被极大浪费。它本可以"指导轨迹生成"，现实中却只被用于"选择已有轨迹"。
TOAD 的出发点直指上述矛盾：既然评分器本质上是一个学习到的轨迹级奖励函数，为什么不在测试时用它来搜索整个连续轨迹空间？ 评分器不再是被动的选择器，而是主动的优化导向器。
💡 核心贡献
- 重新定义评分器的角色：将端到端规划器中的轨迹评分器从"一次性排序工具"转变为可优化的奖励函数，开辟了测试时搜索的新范式。
- 提出 TOAD 测试时优化框架：基于 CEM 的采样优化器，在控制空间中搜索，由基规划器的输出 Warm-start 并约束在信任区域内，即插即用，无需重训练。
- 揭示关键发现：测试时搜索成功的前提是评分器在训练分布之外仍保持准确——固定词表（Fixed-Vocabulary）评分器在搜索时反而会降低性能，暴露了标准评估无法发现的隐藏弱点。
- 全面领先的实验结果：在 6 种基规划器上一致提升，NAVSIM-v2 上 DrivoR + TOAD 达到 56.3 EPDMS，逼近使用特权信息的 PDM-Closed（56.6）。
🧠 方法详解
1. 从评分器到奖励函数
TOAD 的核心洞察简洁而有力：评分器
$$S(\tau;o,g)$$在训练阶段被优化以模仿驾驶质量 Oracle（安全性、进展性、舒适性等），其输出本质上是一个学习到的轨迹级奖励函数。
轨迹表示：轨迹
$$\tau = (p_1, \dots, p_{n_p})$$是一系列未来自车位姿，
$$p_t = (x_t, y_t, \theta_t) \in \mathbb{R}^3$$包含横向位置、纵向位置和朝向角。
问题重新形式化：TOAD 将规划从离散选择问题扩展为连续优化问题：
$$\tau^* = \arg\max_{\tau \in \mathcal{T}_{\text{feasible}}} S(\tau; o, g)$$其中
$$\mathcal{T}_{\text{feasible}}$$是满足动力学约束的轨迹集合，搜索空间从有限离散集扩展到无限连续集——这是从"挑选"到"创造"的本质跃迁。
图1：TOAD 核心思想。左侧传统范式在离散候选中选择；右侧 TOAD 将评分器作为奖励函数在连续控制空间中进行 CEM 搜索。
2. 控制空间优化与自行车模型
直接在轨迹位姿空间采样会产生动力学不可行的锯齿状轨迹。TOAD 通过运动学自行车模型（Bicycle Model, BM） 将搜索空间变换到控制空间：
$$u = (u_t)_{t=1}^{n_p},\quad u_t = (a_t, \omega_t) \in \mathbb{R}^2$$其中
$$a_t$$为纵向加速度，
$$\omega_t$$为偏航角速度。BM 的递推关系为：
$$ \begin{aligned} v_{t+1} &= v_t + a_t \cdot \Delta t \\ \theta_{t+1} &= \theta_t + \omega_t \cdot \Delta t \\ x_{t+1} &= x_t + v_t \cos(\theta_t) \cdot \Delta t \\ y_{t+1} &= y_t + v_t \sin(\theta_t) \cdot \Delta t \end{aligned} $$BM 可逆：
$$\tau = \operatorname{BM}(u, v_0)$$且
$$u = \operatorname{BM}^{-1}(\tau, v_0)$$，允许 TOAD 在控制空间搜索后无缝恢复物理轨迹。使用控制空间的优势包括：物理可行性保证（任何
$$u$$经 BM 展开都满足运动学约束）、维度压缩（
$$\mathbb{R}^{2n_p}$$vs
$$\mathbb{R}^{3n_p}$$）、以及平滑性先验（加速度/角速度更适合高斯采样）。
3. 信任区域与正则化
为防止搜索漂移到评分器不可靠的分布外区域，TOAD 设计了锚点正则化与舒适性正则化：
$$J(u) = S(\operatorname{BM}(u, v_0); o, g) - \lambda_a C_{\text{anchor}}(u) - \lambda_c C_{\text{comf}}(u)$$锚点正则化
$$C_{\text{anchor}}(u) = \|u - u_{\text{base}}\|^2$$将搜索约束在基规划器控制序列
$$u_{\text{base}}$$的邻域内，
$$\lambda_a$$控制搜索的"自由度"。
舒适性正则化：
$$C_{\text{comf}}(u) = \sum_{t} \big( \alpha_1 |a_t| + \alpha_2 |\dot{a}_t| + \alpha_3 |\omega_t| + \alpha_4 |\dot{\omega}_t| \big)$$惩罚纵向加速度、加加速度（jerk）、偏航角速度和角加速度，确保轨迹乘坐舒适。
图2：TOAD 完整流程。基规划器输出经 BM 逆映射作为 CEM 初始化和信任区域锚点，CEM 在控制空间迭代搜索后择优输出。
4. CEM 迭代搜索算法
TOAD 的核心优化引擎是交叉熵方法（CEM）：通过迭代拟合精英样本的分布来逼近全局最优。
| 步骤 | 操作 | 公式 |
|---|---|---|
| 初始化 | 均值设为基规划器输出，标准差由候选集离散度决定 | $$\mu_0 = u_{\text{base}},\ \sigma_0 = \max(\beta \cdot \operatorname{std}(\mathcal{U}), \epsilon)$$ |
| 采样 | 从高斯分布采样 $$M$$ 条控制序列 | $$u^{(i)} \sim \mathcal{N}(\mu_k, \operatorname{diag}(\sigma_k^2))$$ |
| 展开评估 | BM 展开后用评分器和正则化计算目标 | $$J_i = S(\operatorname{BM}(u^{(i)})) - \lambda_a C_{\text{anchor}}(u^{(i)}) - \lambda_c C_{\text{comf}}(u^{(i)})$$ |
| 精英选择 | 保留得分最高的 $$E = M/8$$ 条 | $$\mathcal{E}_k = \{u^{(i)} \mid J_i \in \text{top-}E\}$$ |
| 分布更新 | 用精英集重拟合高斯分布 | $$\mu_{k+1} = \operatorname{mean}(\mathcal{E}_k),\ \sigma_{k+1} = \operatorname{std}(\mathcal{E}_k)$$ |
| 迭代 | 重复共 $$K$$ 次 | $$k = 1, \dots, K$$ |
| 最终输出 | 收敛均值展开为轨迹，与基线择优 | $$\tau^* = \arg\max_{\tau \in \{\tau_{\text{mean}}, \tau_{\text{base}}\}} S(\tau) - \lambda_c C_{\text{comf}}(\cdot)$$ |
超参数：采样
$$M=64$$，精英
$$E=8$$（12.5% 精英率）。迭代
$$K$$因规划器类型而异：on-the-fly 类（DrivoR、iPad、RAP）取
$$K=5$$；固定词表类（Hydra-MDP、GTRS、ZTRS）取
$$K=100$$。Warm-start 标准差
$$\beta=0.5$$，最小标准差
$$\epsilon=10^{-4}$$。
图3：TOAD 优化过程的动态示意。CEM 分布的均值和标准差随迭代更新，搜索逐渐聚焦于高奖励区域。
🔬 实验与结果
实验设置
- 基准：NAVSIM-v1（开环，147 场景，PDMS）、NAVSIM-v2（伪闭环，402 高难场景，EPDMS）、HUGSIM（闭环 3D Gaussian Splatting 仿真）
- 基规划器（6 种）：iPad、Hydra-MDP、GTRS、ZTRS、RAP、DrivoR
- 条件：TOAD 应用于各规划器时不修改任何参数
NAVSIM-v2 主要结果
| 方法 | EPDMS (↑) | 对比增益 |
|---|---|---|
| PDM-Closed（特权信息） | 56.6 | — |
| iPad | 34.7 | — |
| + TOAD | 49.8 | +43.6% |
| RAP-DINO | 39.6 | — |
| + TOAD | 49.0 | +23.9% |
| Hydra-MDP | 40.9 | — |
| + TOAD | 49.7 | +21.6% |
| GTRS | 45.4 | — |
| + TOAD | 51.7 | +13.8% |
| ZTRS | 48.1 | — |
| + TOAD | 49.2 | +2.3% |
| DrivoR | 54.6 | — |
| + TOAD | 56.3 | +3.1% |
NAVSIM-v1 主要结果
| 方法 | PDMS (↑) | 对比增益 |
|---|---|---|
| PDM-Closed（特权信息） | 89.1 | — |
| Human driver | 94.8 | — |
| ZTRS | 86.9 | — |
| + TOAD | 89.0 | +2.4% |
| GTRS | 90.4 | — |
| + TOAD | 90.9 | +0.6% |
| Hydra-MDP | 90.9 | — |
| + TOAD | 91.4 | +0.6% |
| iPad | 91.7 | — |
| + TOAD | 93.4 | +1.9% |
| RAP-DINO | 93.8 | — |
| + TOAD | 93.9 | +0.1% |
| DrivoR | 94.6 | — |
| + TOAD | 94.7 | +0.1% |
TOAD 在 6 × 2 = 12 个对比中全部正向提升，零退化。增益幅度与规划器基础能力成反比——弱规划器收益最大。
收益来源分析
图4：成功案例。基规划器（DrivoR）产生碰撞轨迹（EPDMS: 0.0），TOAD 搜索找到安全避让轨迹（EPDMS: 0.75），展示评分器引导搜索纠正碰撞的能力。
图5：失败案例。基规划器保持道路行驶（EPDMS: 0.79），TOAD 被评分器的误导性高分引至路外（EPDMS: 0.0）——当奖励信号有误导时，最大化它反而有害。
核心消融实验
| 配置 | iPad EPDMS | Hydra-MDP EPDMS |
|---|---|---|
| 基线 | 34.7 | 40.9 |
| + 平滑（仅 BM 后处理） | 35.0 (+0.8%) | 38.8 (-5.1%) |
| + 用 DrivoR 评分器重排序 | 45.6 (+31.5%) | 37.3 (-8.8%) |
| + 用 GTRS 评分器搜索 | 23.9 (-31.1%) | 38.1 (-6.9%) |
| + 用 SparseDriveV2 评分器搜索 | 34.6 (-0.4%) | 29.6 (-27.7%) |
| + TOAD（用 DrivoR 评分器） | 49.8 (+43.6%) | 49.7 (+21.6%) |
关键结论：
- 搜索 > 重排序：同一评分器用于 CEM 搜索远超仅重排序，证实搜索本身带来额外收益
- 评分器泛化决定成败：只有 DrivoR 的解耦评分器有效，固定词表评分器反而降低性能
- 弱规划器收益更大：iPad +43.6% vs DrivoR +3.1%，TOAD 缩小了规划器与评分器最优之间的 gap
CEM 迭代深度消融
| K (CEM 迭代数) | iPad EPDMS | Hydra-MDP EPDMS |
|---|---|---|
| K=0 (基线) | 34.7 | 40.9 |
| K=1 | 45.2 | 42.8 |
| K=5 | 49.8 | 44.2 |
| K=25 | 50.1 | 47.5 |
| K=100 | 50.1 | 49.7 |
| K=200 | 49.9 | 49.5 |
iPad（on-the-fly）在 K=5 时已收敛；Hydra-MDP（固定词表）需要 K=100 充分探索。
评分器泛化能力消融
图6：不同评分器用于 TOAD 的对比。DrivoR 评分器一致提升；固定词表评分器多数情况降低性能。
图7：CEM 迭代数 K 的影响。On-the-fly 类 5 次收敛；固定词表类需 100 次以上。
图8：采样数 M 与精英数 E 的消融。M=64, E=8 已接近饱和。
计算开销
| 组件 | 延迟 | 说明 |
|---|---|---|
| DrivoR 基线 | 30.9 ms | 单次推理 |
| DrivoR + TOAD（K=5, M=64） | 32.8 ms | 仅增加 1.9 ms |
| iPad + TOAD（K=5, M=64） | +2.7 ms | 同样极低 |
| 单次评分器前向 | ~0.03 ms | 瓶颈在采样质量不在计算 |
闭环 HUGSIM 结果
| 方法 | 碰撞率 (↓) | 驾驶分数 (↑) |
|---|---|---|
| DrivoR | 11.4% | 0.71 |
| + TOAD | 8.2% | 0.78 |
| 相对变化 | -28.1% | +9.9% |
📊 相关方法对比
| 方法 | 搜索空间 | 需重训练？ | 评分器角色 | 修正弱候选 | 附加延迟 |
|---|---|---|---|---|---|
| Score-and-Select | 离散候选集 | 否 | 排序选择 | ❌ 不能 | 极低 |
| Planning w/ Diffusion | 扩散隐空间 | 可能需要 | 条件指导 | ⚠️ 有限 | 中 |
| MCTS-based | 离散动作树 | 否（规则） | 无 | ⚠️ 有限 | 高 |
| Learning Refinement | 连续轨迹 | 是 | 训练信号 | ✅ 但需训练 | 低 |
| TOAD (Ours) | 连续控制空间 | 否 | 奖励函数 + 搜索导向 | ✅ 无需重训练 | ~2ms |
TOAD 是唯一不重训练、不修改架构、即插即用的测试时优化方法，附加延迟仅约 2 ms。
💡 个人思考与关键洞察
从实验结果看 TOAD 的行为模式
TOAD 的实验数据揭示了一个有趣的单调性：基规划器越弱，TOAD 增益越大。iPad（EPDMS 34.7 → 49.8，+43.6%）的增益是 DrivoR（54.6 → 56.3，+3.1%）的 14 倍。这一模式很容易被解释为"弱规划器的提升空间更大"，但更深层的原因是：弱规划器的候选集与评分器最优轨迹之间的 gap 更大，而 TOAD 的连续搜索恰恰填补了这一 gap。从工程角度看，这意味着 TOAD 提供了自动的规划器能力补偿——它不会锦上添花太多，但能雪中送炭。
另一值得注意的现象是：即使 DrivoR 本身已是 SOTA，TOAD 仍将其从 54.6 推进到 56.3，逼近 PDM-Closed（56.6）这个使用特权信息的上限。这说明评分器对最优轨迹的估计能力可能远高于规划器候选集的上限——即评分器"知道什么是最好的，但规划器做不到"——而 TOAD 的搜索弥补了这一差距。
1. 评分器是未被充分利用的奖励函数。当前端到端规划器将评分器仅用于一次性排序，忽视了它作为可优化奖励函数的潜力。TOAD 证明了"用评分器引导搜索"这一简单思路的强大效果——即使评分器本身是为"排序"任务训练的，其输出依然可以作为连续空间中的优化目标。这提示社区：训练评分器时，不应仅优化其排序能力，还应考虑它在连续空间中的平滑性和可优化性。一个理想的评分器不仅在候选集上能排序，还应在轨迹空间的局部邻域中具有有意义的梯度信息。
2. 固定词表评分器的隐藏弱点暴露了评估方法的系统性盲区。固定词表评分器在标准评估中表现良好（因为测试候选也来自同一词表），但在搜索场景下一败涂地——离开词表后输出急剧退化。这意味着当前社区对评分器的评估存在分布内过拟合的问题：标准评估只测试排序精度，从未测试在未知轨迹上的泛化能力。这类似于在训练集上测试分类器——高分不代表真实性能。未来有必要将"分布外评分一致性"作为评分器的标准评估维度。
3. 测试时计算的有效分配具有工程价值。TOAD 仅增加约 2 ms 延迟即可获得显著性能提升（弱规划器增益可达 43.6%）。考虑到车载计算平台通常在规划周期内有余量，TOAD 的极低开销使其在工程上非常实用——为自动驾驶的测试时计算分配提供了新思路：与其无限扩大候选集规模，不如在更少但更高质量的候选上做连续空间搜索。在部署中，这可能比训练更大的模型更具性价比。
4. CEM 选择的背后：无梯度优化的合理性。作者选择 CEM 而非梯度下降的动机明确：评分器（尤其是 Transformer 架构的）不是严格可微的，CEM 对优化景观中的局部陷阱也具有更强的鲁棒性。但 CEM 本质上是零阶优化，效率在大规模场景中受限。一个值得探索的方向是将 TOAD 与可微模拟器结合——若 BM 和评分器均可微，梯度下降一步可能逼近 CEM 数十步的效果。论文中 K=5 时 iPad 已收敛到 49.8，但 K=1 时也达到了 45.2（CEM 单步就完成了大部分提升），这暗示评分器的优化景观可能是相对平滑的。
5. 与 LLM 推理时搜索的范式共鸣。TOAD 与 LLM 领域的推理时搜索（如 o1、DeepSeek-R1 的 test-time compute scaling）异曲同工——在冻结的模型上花更多推理计算来提升输出质量。LLM 中表现为 chain-of-thought、self-consistency、tree search；自动驾驶中表现为 TOAD 的 CEM 轨迹搜索。这种跨领域共鸣暗示着测试时计算扩展可能是基础模型的通用能力：无论是生成文本还是驾驶轨迹，在生成器上加一层搜索器都可能带来一致提升。TOAD 可视为自动驾驶领域的"推理时搜索"先驱。
6. 局限与未来方向。TOAD 受限于评分器质量：当评分器给出误导性奖励时（如图 5），最大化它反而有害。这不只是 TOAD 的局限，而是所有"基于学习奖励的优化"方法的根本困境。未来方向包括：(a) 训练分布外鲁棒的评分器——在评分器训练中引入对抗/多样性轨迹增强，使其在连续空间中有更好的单调性；(b) 结合世界模型的多步前瞻——不仅评估即时安全性，还预测轨迹的长尾后果（如是否会导致未来不可恢复的危险状态）；(c) 分层搜索——先在语义层（换道/跟车/绕行）做顶层搜索，再用 TOAD 对选定语义做连续精化，将搜索扩展到更长规划时域。
📚 参考资料
- arXiv: 2606.07170
- 项目页：https://valeoai.github.io/TOAD/
- 代码：https://github.com/valeoai/TOAD