Technical Summary: Test-Time Trajectory Optimization for Autonomous Driving (TOAD)

1. Problem Statement

End-to-end (E2E) autonomous driving planners typically follow a "generate-and-select" paradigm: they produce a finite set of candidate trajectories (either from a pre-computed vocabulary or generated on-the-fly) and use a learned scorer to rank them, returning the highest-scoring option.

The authors identify a critical bottleneck in this pipeline: the scorer is applied only after generation and cannot influence the set of candidates. If the initial set of proposals is suboptimal (e.g., all candidates are unsafe or inefficient), the planner is forced to return the "best of a bad set," regardless of the scorer's quality. Current methods treat the scorer merely as a ranking mechanism for a fixed set, missing the opportunity to use it as an objective function to actively search for better trajectories.

Furthermore, the paper highlights a hidden weakness in many existing scorers: those trained to evaluate fixed vocabularies often fail to generalize to trajectories outside their training distribution. When used as a reward function for search, these scorers can lead to "reward hacking" or performance degradation because they are inaccurate on novel, freely decoded trajectories.

2. Methodology: TOAD

The authors propose TOAD (Test-time Optimization for Autonomous Driving), a plug-and-play procedure that reframes the frozen E2E scorer as a learned reward function and performs a search over the trajectory space at inference time.

Core Algorithm: Cross-Entropy Method (CEM)

TOAD wraps a frozen base planner in an inference-time loop based on the Cross-Entropy Method (CEM):

1. Warm-Start & Trust Region: The search is initialized using the base planner's selected trajectory ($\tau_{base}$) and its candidate set. This confines the search to a "trust region" around the planner's proposals, ensuring the search stays within the distribution where the scorer is reliable.
2. Control Space Search: Instead of sampling directly in pose space (which yields dynamically infeasible trajectories), TOAD samples in control space. Trajectories are mapped to sequences of longitudinal acceleration ($a_t$) and yaw rate ($\omega_t$) using an inverse kinematic bicycle model. This ensures all sampled trajectories are smooth and kinematically feasible.
3. Objective Function: The CEM maximizes a composite objective $J(u)$:
   $$J(u) = S(\text{BM}(u, v_0); o, g) - \lambda_a C_{anchor}(u) - \lambda_c C_{comf}(u)$$
   • $S$ (Scorer): The learned reward from the frozen planner.
   • $C_{anchor}$: A regularizer penalizing deviation from the base planner's anchor trajectory to maintain the trust region.
   • $C_{comf}$: A closed-form comfort regularizer (penalizing acceleration, jerk, etc.) to provide a signal the learned scorer may estimate coarsely.
4. Iterative Refinement: In each iteration, $M$ control sequences are sampled from a Gaussian distribution, rolled out into trajectories, scored, and the top-$E$ "elite" sequences are used to refit the Gaussian parameters ($\mu, \sigma$).
5. Final Selection: After $K$ iterations, the trajectory induced by the converged mean is compared against the original base proposal. The one with the higher final score (including comfort penalties) is selected.

Key Design Choice: The Scorer

A critical finding is that not all scorers work for TOAD.

• Fixed-Vocabulary Scorers: Scorers trained only to rank pre-computed trajectories (e.g., Hydra-MDP, GTRS) fail when used as rewards for search. They degrade sharply on trajectories outside their specific vocabulary.
• Disentangled Scorers: TOAD requires a scorer trained to evaluate freely decoded trajectories (e.g., DrivoR's scorer). This scorer remains accurate off the proposal distribution, making it a valid reward function for the CEM loop.

3. Key Contributions

1. Reframing the Scorer: The authors propose viewing the E2E trajectory scorer not just as a ranking tool for a fixed set, but as a learned reward function capable of guiding a search for optimal trajectories.
2. TOAD Framework: A test-time search algorithm that plugs into any existing base planner without retraining. It uses CEM to optimize trajectories in control space, warm-started from the planner's proposals.
3. Identification of Scorer Limitations: The work exposes that standard ranking evaluation does not reveal a scorer's inability to generalize. TOAD demonstrates that successful test-time optimization requires a scorer that generalizes beyond its training proposals, a property lacking in fixed-vocabulary approaches.
4. Plug-and-Play Performance: TOAD improves performance across six diverse base planners (both vocabulary-based and on-the-fly generators) on multiple benchmarks with negligible computational overhead and no model updates.

4. Experimental Results

The method was evaluated on NAVSIM-v1, NAVSIM-v2, and the closed-loop HUGSIM simulator.

• NAVSIM-v2 (New State of the Art):

  • TOAD applied to DrivoR achieved 56.3 EPDMS, a new state-of-the-art result that nearly matches the privileged PDM-Closed baseline (56.6) which uses ground-truth perception.
  • Significant gains were observed across all planners. For example, iPad improved by +43.6% (from 34.7 to 49.8 EPDMS), while DrivoR improved by +3.1%.
  • The search effectively compressed the performance gap between weak and strong planners, with weaker planners gaining the most.

• NAVSIM-v1:

  • Consistent improvements were seen, with DrivoR + TOAD reaching 94.7 PDMS, just 0.1 points below the human driver baseline (94.8).

• HUGSIM (Closed-Loop):

  • TOAD improved safety metrics (No-Collision, Drivable Area, Time-to-Collision) and comfort on photorealistic scenes.
  • While Route Completion (RC) sometimes decreased (indicating more cautious driving), the overall Driving Score (HDS) improved, particularly on nuScenes and KITTI360.

• Ablation Studies:

  • Search vs. Re-ranking: Simply re-ranking the base planner's proposals with the scorer yielded inconsistent results (sometimes hurting performance). The gains came specifically from the search discovering new trajectories outside the original set.
  • Scorer Necessity: Using non-generalizing scorers (e.g., GTRS) as rewards caused performance to drop below the baseline, confirming the need for a disentangled, generalizing scorer.
  • Control Space: Optimization in control space outperformed direct trajectory space sampling.

• Computational Cost:

  • The overhead is minimal. The search adds only 1.9 ms to 20.4 ms per sample (depending on the number of iterations $K$).
  • When the scorer is part of the base planner (sharing the perception backbone), the cost is near-free. When external, the cost is dominated by the redundant perception backbone (~100 ms), but the search itself remains lightweight.

5. Significance and Claims

The paper claims that the primary bottleneck in current E2E planning is often the candidate set, not the scorer. By treating the scorer as an optimizable reward function, TOAD unlocks performance that was previously inaccessible to the planner.

The authors emphasize that TOAD is a modest, plug-and-play improvement that requires no retraining. Its success hinges on the availability of a generalizing scorer. The work suggests a clear direction for future planner design: building scorers that are disentangled from the trajectory generator and trained to evaluate freely decoded trajectories. This would allow strong test-time search to come "for free," significantly boosting safety and comfort without architectural changes to the generation network.

The paper concludes that while TOAD currently relies on a base planner for initialization, a sufficiently accurate and generalizing scorer could eventually allow for search from scratch, potentially removing the need for a base generator entirely. However, this remains a future direction dependent on scaling scorer capacity and data.