Test-Time Trajectory Optimization for Autonomous Driving

Abstract: End-to-end planners for autonomous driving typically generate a set of candidate trajectories, score each one, and return the highest-scoring candidate. However, the scorer is applied only after the proposals are generated and cannot influence the set of trajectories: a weak set of candidates limits planning performance regardless of the scorer's quality. We instead treat the scorer as a learned trajectory-level reward function and search for trajectories that maximize it. Our method, TOAD, runs the Cross-Entropy Method at test time, warm-started from the planner's proposals. It requires no retraining and is plug-and-play for existing planners. Across six base planners, TOAD improves results on NAVSIM-v1 (94.7 PDMS), NAVSIM-v2 (56.3 EPDMS), and the closed-loop HUGSIM benchmark. The code will be made publicly available via the project page: https://valeoai.github.io/TOAD/.

• The paper introduces TOAD, a framework that uses test-time optimization via CEM to refine candidate trajectories into higher-reward, dynamically feasible paths.
• It demonstrates that decoupling trajectory generation from scoring overcomes limitations of fixed proposal sets, enhancing safety and driving performance.
• Experimental results on NAVSIM and HUGSIM benchmarks confirm substantial improvements with minimal computational overhead across diverse planning methods.

Test-Time Trajectory Optimization for Autonomous Driving: An Analysis of TOAD

Introduction and Motivation

End-to-end (E2E) planners for autonomous vehicles typically employ a two-stage process: the generation of candidate trajectories followed by selection based on a learned scorer that imitates an expert driving oracle. However, the prevalent use of fixed proposal sets throughout the literature inherently constrains the expressivity and efficacy of the final selected trajectory. Despite recent increases in candidate set sizes, the structure is fundamentally one-shot and does not leverage the scorer during trajectory generation, resulting in inevitable suboptimality when proposals are misaligned with the underlying reward function.

The paper "Test-Time Trajectory Optimization for Autonomous Driving" (2606.07170) introduces TOAD: a framework that reframes the learned scorer as a trajectory-level reward amenable to direct test-time optimization via the Cross-Entropy Method (CEM). This approach is designed as a plug-in for existing planners, thereby decoupling trajectory generation and scoring, and exposing the generalization capacity requirements for scorers under distributional shift. Distinctively, TOAD emphasizes the critical importance of scorer generalizability, demonstrating that search-based optimization is only productive if the scorer can reliably assess out-of-distribution proposals—a criterion unmet by most fixed-vocabulary-trained scorers.

Figure 1: Overview of TOAD, with a frozen base planner providing initial proposals and CEM-based optimization iteratively searching the trajectory space guided by a learned reward.

Methodology

TOAD’s test-time search paradigm is instantiated as follows. A frozen base planner, which may employ any candidate generation approach (fixed vocabulary or generative), outputs a set of candidate trajectories and a selected anchor $\tau_{\text{base}}$. TOAD then applies CEM in the trajectory control space, initializing the search around the anchor and the proposal distribution to maintain reliability of the reward model. At each iteration:

1. Candidate control sequences are sampled from a Gaussian parameterized by current mean and variance.
2. Each sequence is mapped into the trajectory space via a kinematic bicycle model.
3. The trajectory is scored by the frozen, learned reward (scorer), as well as regularized for comfort (control limits/jerk) and anchor deviation.
4. Elites are selected, and the search distribution is refit.

By working in control space rather than pose space, TOAD ensures dynamic feasibility and smoothness of candidates.

A critical insight from the ablation and reward structure is that most fixed-vocabulary scorers (e.g., GTRS, SparseDriveV2) degrade substantially on out-of-set trajectories, inducing "reward hacking" or poor optimization. Only decoupled, generative scorers such as DrivoR's, trained on freely decoded trajectories, provide consistent guidance during search.

Experimental Results

NAVSIM Benchmarks

On NAVSIM-v1 and NAVSIM-v2, TOAD improves performance for six representative planners, ranging from fixed-vocabulary (Hydra-MDP, GTRS, ZTRS) to on-the-fly generators (iPad, RAP, DrivoR), with the following highlights:

• NAVSIM-v2: TOAD yields +43.6% EPDMS for iPad, +21.6% for Hydra-MDP, and attains a new state of the art with DrivoR at 56.3 EPDMS—just 0.3 points below privileged PDM-Closed and outperforming all learned approaches.
• NAVSIM-v1: Gains are more modest due to higher baseline saturation, but still observable, especially for weaker planners.

The distribution of improvements is inversely proportional to base planner strength; weaker planners benefit more as TOAD bridges the gap between the best in-set proposal and the scorer's optimal achievable trajectory.

Closed-Loop Evaluation

On HUGSIM, a photorealistic closed-loop benchmark, DrivoR+TOAD achieves consistent improvement in safety metrics (no-collision, drivable-area, time-to-collision) and comfort, at times trading off route completion for more cautious, robust behavior.

Figure 2: Success case—TOAD refines the trajectory to avoid a frontal collision, exploiting solutions not accessible to the proposal-only base planner.

Analysis of Gains

Ablation demonstrates that:

• The performance gains are exclusively attributed to search discovering new trajectories, not from smoothing or naive re-ranking.
• The use of a generalizing scorer is essential; otherwise, test-time optimization is counterproductive or even harmful.

Component-level studies reveal that:

• Control-space optimization is superior to direct trajectory-space sampling, due to regularization and feasibility constraints.
• Initializing the search near the base planner promotes effective exploitation, but TOAD can generalize to single-trajectory planners as well.

Figure 3: Component ablation indicates that the scorer term is the primary driver of gains, with additional improvements from anchor and comfort regularization.

Computational Considerations

The computational overhead introduced by TOAD is minimal—ranging from 1.9ms to 20.4ms per sample depending on the base planner and number of CEM iterations, compared to baseline inference times of 100–780ms. When the scorer shares a backbone with the planner (as in DrivoR), TOAD's marginal cost is nearly negligible.

Progression of CEM Search

Empirical visualization of CEM iterations shows the refinement and convergence of sampled trajectories towards solutions with higher reward and improved safety/progress metrics.

Figure 4: CEM iteration visualization—elite mean trajectories progressively realign towards feasible, higher-reward solutions, diverging from the suboptimal proposal anchor.

Implications and Future Directions

This work elucidates a fundamental limitation of the established "score-and-select" E2E paradigm—namely, the inherent expressivity bottleneck due to fixed candidate sets. By leveraging the scorer as a test-time reward and optimizing in the trajectory/control space, TOAD unlocks performance gains orthogonal to scorer capacity improvements or candidate set expansion.

The results call into question the ongoing trend of enlarging fixed vocabularies, advocating instead for scorer architectures and training procedures that explicitly target off-distribution generalization. The demonstrated state-of-the-art results and negligible test-time overhead suggest that search-based optimization is immediately practical, especially if coupled with robust reward models.

Open directions include:

• The development of scorers with even broader generalization via self-supervised or world-model-based rewards, removing the dependence on annotated trajectory datasets.
• Investigating the complete removal of the proposal generator in favor of "search from scratch" with sufficiently expressive reward models.
• Exploring adaptive and context-sensitive regularization that may balance route completion and safety according to scenario-specific risk assessments.

Conclusion

TOAD establishes that the bottleneck of E2E autonomous driving planning is not the reward-modeling capacity of the scorer but the restricted expressivity of fixed proposal sets. The deployment of test-time search, underpinned by a generalizing reward function and principled trust-region optimization, reliably closes this gap across diverse planners and complex benchmarks with negligible added inference cost. The methodology fundamentally reframes the role and design of planners in the E2E driving stack, posing significant implications for future research in both model selection and real-world deployment strategies.