Test-Time Trajectory Optimization for Autonomous Driving
TOAD applies test-time trajectory search using learned reward functions, boosting six planners; NAVSIM-v2 reaches 56.3 EPDMS.
Key Findings
Methodology
This paper introduces a framework where a trained trajectory reward function is used as an optimization target during inference. Using the Cross-Entropy Method (CEM), the approach starts from candidate trajectories generated by a frozen end-to-end planner. It samples control sequences in the control space, evaluates them with the learned reward, and iteratively refines the Gaussian distribution to maximize the reward. The reward function must generalize beyond the training proposals to remain reliable during search. This plug-and-play method requires no retraining and enhances multiple baseline planners, significantly improving metrics such as NAVSIM-v2 EPDMS from 34.7 to 49.8.
Key Results
- On NAVSIM-v1 and NAVSIM-v2, TOAD improves all six base planners, with gains up to 43.6%. Specifically, NAVSIM-v2 EPDMS increases from 34.7 to 49.8, approaching the PDM-Closed state-of-the-art at 56.6. For DrivoR, zero-shot performance reaches 56.3, setting a new benchmark. In the HUGSIM closed-loop simulation, safety metrics like TTC and NC are significantly improved, demonstrating enhanced robustness and safety.
- Comparison with simpler strategies (smoothing, re-scoring) shows that search discovers new trajectories outside the initial proposal set, outperforming re-ranking. The effectiveness depends on the reward function’s ability to generalize, with DrivoR’s disentangled scorer outperforming fixed-vocabulary scorers. Experiments across diverse planners confirm the method’s broad applicability.
- The approach is computationally efficient, adding less than 2ms per sample on NVIDIA A100 hardware, thanks to feature reuse. The primary source of gains is exploration in the control space, leading to safer and more comfortable trajectories, especially in challenging scenarios.
Significance
This work advances autonomous driving planning by transforming the trajectory scoring function into an optimization objective, enabling search-based improvements during inference. It addresses the core limitation of fixed candidate sets and static scoring, unlocking the potential for dynamic, adaptive trajectory refinement. The method enhances safety, comfort, and robustness, making it highly relevant for real-world deployment. Its plug-and-play nature facilitates integration into existing systems, promoting scalable improvements across diverse architectures. This paradigm shift from ranking to optimization could influence future research in model-based control and reinforcement learning, fostering more intelligent and adaptable autonomous systems.
Technical Contribution
The key innovation is the formulation of the learned trajectory reward as an optimization objective, combined with control-space sampling via CEM within a trust region anchored to the base planner’s proposals. This ensures the search remains within reliable regions, leveraging the generalization ability of the disentangled scorer. The approach allows for efficient, gradient-free optimization without retraining, and the control space parametrization guarantees smooth, feasible trajectories. It bridges the gap between reward learning and trajectory optimization, offering a versatile, plug-and-play solution for improving end-to-end planners.
Novelty
This is the first work to treat a learned trajectory reward function as an explicit optimization target during inference, rather than solely a ranking score. It emphasizes the importance of reward generalization, revealing the limitations of fixed-vocabulary scorers. The integration of CEM with control space sampling in a trust region around existing proposals is a novel approach that significantly enhances planning robustness and performance.
Limitations
- The success relies heavily on the reward function’s ability to generalize outside the training proposals; if the scorer degrades in unseen scenarios, the optimization may fail or produce unsafe trajectories.
- The method’s effectiveness in highly dynamic or complex environments remains to be validated, especially under severe perception uncertainties.
- While computationally efficient, real-time deployment in large-scale, dense traffic scenarios may require further optimization or hardware acceleration.
Future Work
Future directions include integrating environmental prediction models to guide the search, extending the framework to multi-objective optimization, and online adaptation of the reward function via reinforcement learning. Real-world testing on autonomous vehicles and scaling to more complex scenarios will be critical for industrial deployment. Additionally, combining this approach with learning-based perception modules could further improve robustness and safety.
AI Executive Summary
Autonomous driving systems have traditionally relied on generating a fixed set of candidate trajectories, which are then ranked by a learned scorer. However, this approach limits the potential of the system, as the candidate set constrains the solution space and the scorer only influences ranking post-generation. Recognizing this limitation, the authors propose TOAD, a novel inference-time optimization framework that transforms the learned trajectory reward into an explicit objective for search.
TOAD employs the Cross-Entropy Method (CEM) to iteratively sample and refine control sequences in the control space, starting from proposals generated by a frozen end-to-end planner. By constraining the search within a trust region around the initial proposals, the method ensures the trajectories remain feasible and close to the training distribution. The core insight is that a well-generalized reward function can guide the search towards trajectories that surpass the initial proposals in safety, comfort, and efficiency.
Experimental results demonstrate the effectiveness of TOAD across six different baseline planners on the NAVSIM-v1 and NAVSIM-v2 benchmarks. The method achieves up to 43.6% improvements in EPDMS metrics, with NAVSIM-v2 scores reaching 49.8, close to the state-of-the-art PDM-Closed at 56.6. In the more challenging HUGSIM closed-loop simulator, safety metrics such as time-to-collision and no-collision rates improve significantly, indicating safer driving behaviors.
The key advantages of TOAD include its plug-and-play nature, low computational overhead, and ability to discover new, better trajectories outside the initial proposal set. The approach highlights the importance of reward function generalization, revealing the limitations of fixed-vocabulary scorers. Overall, this work marks a significant step towards more adaptive, robust, and safe autonomous driving systems, with promising avenues for future research in multi-objective optimization and real-world deployment.
Deep Dive
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/.