Guiding End-to-End Driving Models with Endpoint-Constrained Trajectory Optimization

Brayden Zhang; Mahsa Golchoubian; Igor Gilitschenski; Boris Ivanovic; Kashyap Chitta

cs.ROSep 25, 2026

Guiding End-to-End Driving Models with Endpoint-Constrained Trajectory Optimization

Authors: Brayden Zhang, Mahsa Golchoubian, Igor Gilitschenski, Boris Ivanovic, Kashyap Chitta

Organizations: University of Toronto · Vector Institute · NVIDIA Research · ELLIS Institute Tübingen · KE:SAI

Abstract

End-to-end driving policies are commonly trained through open-loop behavior cloning, yet they must ultimately operate in closed-loop when deployed on a vehicle, creating a fundamental mismatch between training and execution. Beyond the commonly studied effects of covariate shift and causal confusion, we identify a complementary factor for this open-loop/closed-loop gap: waypoint-based supervision and displacement metrics do not ensure that the intermediate trajectory is physically coherent or easy for the controller to track. We observe that these inconsistencies concentrate primarily at intermediate waypoints, while the predicted endpoint remains comparatively reliable. Based on this observation, we introduce Endpoint-Constrained Optimization (ECO), a lightweight postprocessing layer that anchors the trajectory to the vehicle's executed history, preserves the policy's predicted endpoint, and reshapes the intermediate waypoints to improve feasibility. ECO requires no map, privileged simulator state, or additional training, and can be inserted between a broad range of waypoint-emitting policies and their controllers. Across two closed-loop simulators, it improves the aggregate closed-loop score of all six evaluated generative and regression-based driving policies, and the gains tend to increase with how often the base plans violate motion limits. On HUGSIM, ECO improves VaVAM from 18.1 to 31.0 HD-Score (+71%), achieving 1st place on the HUGSIM Closed-Loop Driving Challenge. Similarly, on AlpaSim, ECO increases the scene scores of VaVAM and DiffusionDrive by 123% and 22%, respectively. These results show that for a broad collection of end-to-end driving models, repairing the intermediate geometry of predicted trajectories without changing the policy's predicted endpoint can substantially improve closed-loop performance.

Figures & tables

Fig. 2 : Pipeline. The frozen end-to-end driving model receives camera inputs from the simulator and samples a trajectory for the next $N$ steps. ECO anchors $K=2$ past poses and the last predicted waypoint, then solves a trajectory optimization problem to refine the waypoints between them (Sec. IV ). The controller then tracks the refined plan.

Term	Definition
Smoothness $S(\mathbf{z})$	$\displaystyle\sum_{i=-K-1}^{N-3}\Big[\left\|\mathbf{p}_{i+2}-2\mathbf{p}_{i+1}+\mathbf{p}_{i}\right\|^{2}$ $\displaystyle\hskip 16.38895pt\hskip 8.19447pt+\operatorname{wrap}\!\left(\theta_{i+2}-2\theta_{i+1}+\theta_{i}\right)^{2}\Big]$
Turn penalty $C(\mathbf{z})$	$\displaystyle\sum_{i=-K}^{N-2}\mathbf{1}_{i}\,(\|c_{i}\|-c_{0})^{2},\hskip 8.19447ptc_{i}=(\mathbf{p}_{i}-\mathbf{p}_{i-1})\times(\mathbf{p}_{i+1}-\mathbf{p}_{i})$
Deviation $D(\mathbf{x},\mathbf{x}^{(0)})$	$\displaystyle\sum_{i=0}^{N-1}\left\|\mathbf{x}_{i}-\mathbf{x}_{i}^{(0)}\right\|^{2}$

TABLE I : Components of Eq. ( 1 ). Indices run over the extended trajectory

\mathbf{z}

(Sec. IV ).

Sent plan	ADE $\downarrow$	FDE $\downarrow$	$\Delta$ ADE / $\Delta$ FDE (m)	deficit [%] $\downarrow$	HD-Score $\uparrow$
	open loop (m)		paired $\Delta$ after ECO		closed loop
VaVAM	2.53	3.27	–	44	23.1
VaVAM + ECO	1.66	2.42	$-0.03{}$ / $+0.00$	0	39.6
UniAD	1.86	2.25	–	16	27.8
UniAD + ECO	2.02	2.61	$+0.03{}$ / $+0.00$	0	37.3

TABLE II : Small open-loop changes can produce large closed-loop gains. Results on the 34-scene nuScenes subset of the HUGSIM test set. ADE/FDE compare the plan to the ground truth, whereas paired

\Delta

measures the change from each raw plan to its ECO-processed version. Deficit is the fraction of waypoints violating a motion limit. ECO changes paired ADE/FDE by only centimeters while improving HD-Score by +16.5 for VaVAM and +9.5 for UniAD. The ADE/FDE columns are computed over the plans encountered in each closed-loop rollout and therefore reflect different visited-state distributions. Paired

\Delta

ADE/

\Delta

FDE instead compares each raw plan with its ECO-processed version at the same state, isolating the direct effect of post-processing.

Fig. 3 : Collisions and limit violations concentrate in the middle of a predicted plan. Measured with VaVAM, on the nuScenes subset of the HUGSIM test set. (a) Fraction of predicted waypoints whose ego footprint overlaps an agent box recorded in the rollout at the frame each waypoint is meant for (i.e., waypoints in collision). (b) Waypoints violating a motion limit for a single sampled plan, for the average of eight sampled plans, and for one plan after ECO.

	HUGSIM [ 11 ] Test Set (185 scenes), mean over 3 evaluation seeds
	HD-Score $\uparrow$
Policy	Waymo	nuScenes	KITTI-360	PandaSet	Overall	$R_{c}\uparrow$	NC $\uparrow$	TTC $\uparrow$	COM $\uparrow$
VaVAM [ 5 ]	25.8	23.1	4.0	23.1	18.1 $\pm$ 0.5	40.9	52	38	14
VaVAM + ECO	40.0	39.6	22.2	30.3	31.0 $\pm$ 0.6	45.2	54	46	97
$\Delta$ (ECO $-$ base)	+14.2	+16.5	+18.2	+7.2	+12.9 $\pm$ 0.8	+4.3	+2	+8	+83
LTF [ 9 ]	34.5	17.8	3.7	22.6	17.9 $\pm$ 1.1	31.6	44	39	100

TABLE III : Cross-policy and cross-simulator evaluation of ECO. Scores are percentages unless a unit is shown.

\pm

is the standard deviation over passes;

\Delta

rows combine the base and ECO deviations in quadrature. Top: HD-Score on the 185-scene HUGSIM test set by domain and overall, with route completion, no-collision, TTC, and comfort. Bottom: Evaluation in AlpaSim, an independent simulator with no comfort score; lin./nonlin. MPC: AlpaSim’s linear/nonlinear model-predictive controller. ECO consistently improves the aggregate scores of all base policies in both simulators.

	VaVAM [ 5 ]		UniAD [ 7 ]		DrivoR (w/ SimScale [ 42 ] ) [ 10 ]
	Train	Test	Train	Test	Train	Test
base policy	20.0	18.1	32.7 ⋆	26.8	35.7 ⋆	34.6
+ ECO	32.1	31.0	34.9	32.4	38.8	36.2

TABLE IV : HD-Score on both HUGSIM splits. Train (345 scenes) and test (185 scenes); ⋆ taken from [ 10 ] .

Fig. 4 : Results by HUGSIM difficulty tier. (a) Every scene of the 185-scene test set as one point: HD-Score without and with ECO. Points above the diagonal improve. (b) ECO helps most on easier scenes where the base policy is already competent.

Fig. 5 : ECO benefit tends to increase with base-plan deficit. Paired

\Delta

HD-Score with ECO against the fraction of base-plan waypoints that violate a motion limit, for VaVAM-B (357M parameters) at 2, 5, 10 (deployed), and 20 flow matching steps, VaVAM-S (204M parameters), VaVAM-L (1.4B parameters), and four other policies on the same nuScenes test split in HUGSIM.

Start boundary	Endpoint $\mathbf{x}_{N-1}$	HD-Score	$\Delta$ vs. ECO
$\mathbf{x}_{0}$ pinned	free	32.1	$-$ 7.5
history anchored	free	34.4	$-$ 5.2
$\mathbf{x}_{0}$ pinned	pinned	36.9	$-$ 2.7
history anchored	pinned	39.6	–
base policy		23.1	$-$ 16.5

TABLE V: Boundary conditions. VaVAM with the ECO objective on the nuScenes split of the HUGSIM test set. Plans are either anchored to the executed history

\mathbf{h}_{-K:0}

(first predicted waypoint

\mathbf{x}_{0}

free) or pinned at

\mathbf{x}_{0}

, with the endpoint

\mathbf{x}_{N-1}

either free or pinned.

Fig. 6 : Trajectory-shaping operators on a sampled plan. All operators are applied with the same boundary conditions as ECO: the executed history is anchored, the endpoint is pinned (square), and the first predicted waypoint is free. Rings denote motion-limit violations; HD-Scores are the averages on the nuScenes split (Sec. V-A ).

Explore similar work

OPTED: On-Policy Fine-Tuning for End-to-End Driving using a Render-Free Teacher
Sep 17, 2026Damiano Da Col, Maximilian Igl, Peter Karkus +5End-to-End Autonomous Driving RL Post-Training
KE:SAI · ETH Zürich · NVIDIA Research +2
Rectify, Don't Regret: On-Policy Closed-Loop Training for Multimodal Trajectory Prediction
Mar 24, 2026Harsh Yadav, Christian Bohn, Tobias MeisenTrajectory Prediction Multimodal Trajectory Prediction
University of Wuppertal
Odyssey: A Closed-Loop Benchmark for Long-Horizon Real-World Driving with Explicit Navigation Routes
Oct 5, 2026Jungho Kim, Hongjae Shin, Seunghoon Yu +11Autonomous Driving Datasets Autonomous Driving Benchmarks
Seoul National University, Republic of Korea

Sep 17, 2026J/K move · Enter open · S save

OPTED: On-Policy Fine-Tuning for End-to-End Driving using a Render-Free Teacher

Damiano Da Col, Maximilian Igl, Peter Karkus, Kashyap Chitta+4

KE:SAI · ETH Zürich · NVIDIA Research+2

End-to-End Autonomous Driving RL Post-Training RL Fine-Tuning

Open paper AskPDF arXiv

As scaling pre-training data alone yields diminishing returns, post-training is becoming increasingly important across physical AI domains such as autonomous driving. End-to-end driving policies are pre-trained in open loop with behavior cloning on human demonstrations. However, compounding errors during closed-loop deployment can take the vehicle outside the training data distribution, increasing the risk of safety-critical incidents. Closed-loop post-training can mitigate this risk but requires costly simulation for sensor-based policies. We propose OPTED (on-policy fine-tuning for end-to-end driving) which decouples reinforcement learning from the post-training of the end-to-end policy: a privileged teacher is trained using RL on vectorized inputs (HD-map and bounding boxes). This teacher then provides supervision to the pre-trained student during closed-loop post-training. We apply OPTED to two camera-based models, TransFuser and VaVAM, and fine-tune them in AlpaSim, using neural reconstructions (3DGS) of real driving logs. Driving scores increase by factors of 1.6

\times

and 9.5

\times

, respectively. In controlled experiments OPTED matches closed-loop performance with approximately three orders of magnitude fewer simulator interactions than direct RL post-training, while staying closer to the human prior. Project page: https://01dami23.github.io/opted/

Abstract

Explore similar work

OPTED: On-Policy Fine-Tuning for End-to-End Driving using a Render-Free Teacher

Rectify, Don't Regret: On-Policy Closed-Loop Training for Multimodal Trajectory Prediction

Odyssey: A Closed-Loop Benchmark for Long-Horizon Real-World Driving with Explicit Navigation Routes