Paper deep dive
Beyond Waypoint Regression: Query-Based Cost Learning over Reachable Ego Futures for End-to-End Driving
Ahmed Abouelazm, Rupert Polley, Qingyuan Zhang, Yin Wu, Philip Schörner, Carl Esselborn, J. Marius Zöllner
Intelligence
Status: not_run | Model: - | Prompt: - | Confidence: 0%
Entities (0)
Relation Signals (0)
No relation signals yet.
Cypher Suggestions (0)
No Cypher suggestions yet.
Abstract
Abstract:End-to-end planners based on waypoint regression achieve strong open-loop accuracy, but they primarily learn to mimic expert geometry and remain difficult to adapt to deployment-time safety constraints. We propose a query-based cost-learning framework that estimates bounded costs for dynamically reachable ego trajectory queries, rather than dense BEV cells or a small regressed trajectory set. Compact joint scene tokens capture coherent multimodal agent futures, while contingency-aware cost aggregation and cost-guided intra-cluster MPPI mixing convert the learned cost topology into feasible ego plans. On nuScenes, our method improves over prior cost-estimation planners such as ST-P3 and NMP, outperforms most regression baselines in collision rate, while remaining competitive in L2, and retaining an interpretable cost interface. On real-world driving logs, the proposed planner reduces collision rates compared with SparseDrive and Alpamayo without fine-tuning, while maintaining a diverse set of candidate trajectories.
Tags
Links
- Source: https://arxiv.org/abs/2610.08123v1
- Canonical: https://arxiv.org/abs/2610.08123v1
Trouble viewing inline? Open PDF directly →
Full Text
82,019 characters extracted from source content.
Expand or collapse full text
Beyond Waypoint Regression: Query-Based Cost Learning over Reachable Ego Futures for End-to-End Driving Ahmed Abouelazm Affiliation: FZI Research Center for Information Technology () E-mail abouelazm@fzi.de Affiliation: Karlsruhe Institute of Technology Rupert Polley Affiliation: FZI Research Center for Information Technology () E-mail abouelazm@fzi.de Affiliation: Karlsruhe Institute of Technology Qingyuan Zhang Affiliation: FZI Research Center for Information Technology () E-mail abouelazm@fzi.de Yin Wu Affiliation: Karlsruhe Institute of Technology Affiliation: CARIAD SE Philip Schörner Affiliation: FZI Research Center for Information Technology () E-mail abouelazm@fzi.de Carl Esselborn Affiliation: CARIAD SE J. Marius Zöllner Affiliation: FZI Research Center for Information Technology () E-mail abouelazm@fzi.de Affiliation: Karlsruhe Institute of Technology Abstract End-to-end planners based on waypoint regression achieve strong open-loop accuracy, but they primarily learn to mimic expert geometry and remain difficult to adapt to deployment-time safety constraints. We propose a query-based cost-learning framework that estimates bounded costs for dynamically reachable ego trajectory queries, rather than dense BEV cells or a small regressed trajectory set. Compact joint scene tokens capture coherent multimodal agent futures, while contingency-aware cost aggregation and cost-guided intra-cluster MPPI mixing convert the learned cost topology into feasible ego plans. On nuScenes, our method improves over prior cost-estimation planners such as ST-P3 and NMP, outperforms most regression baselines in collision rate, while remaining competitive in L2, and retaining an interpretable cost interface. On real-world driving logs, the proposed planner reduces collision rates compared with SparseDrive and Alpamayo without fine-tuning, while maintaining a diverse set of candidate trajectories. Keywords: End-to-end driving Decision Making Motion Planning 1 Introduction End-to-End (E2E) driving has progressed from direct sensor-to-control learning [11, 2] to unified systems that jointly learn perception, prediction, and planning [5, 10]. Recent sparse representations make these systems more scalable by allocating capacity to compact task- or instance-level tokens instead of dense raster features [13, 24, 12]. However, the final planning interface remains a bottleneck: decisions should remain risk-aware, interpretable, physically feasible, and adaptable to deployment-time safety constraints. Waypoint regression approaches are effective because they directly predict a short horizon of future ego motions. However, imitation objectives primarily reward spatial similarity to expert demonstrations and do not explicitly distinguish safe behavior from collision-causing hard negatives [26]. Consequently, regression planners are difficult to inspect or repair, may produce infeasible behavior under distribution shift, and weakly incorporate safety through post-hoc rescoring of a limited top-k set. Cost maps offer a more abstract alternative by exposing a risk landscape that can be optimized, repaired, and combined with external costs at runtime. Existing E2E cost-map planners such as Neural Motion Planner (NMP) [31] and ST-P3 [9], however, estimate dense rasterized costs over many unreachable cells, spreading supervision away from feasible trajectories. Their flexibility has therefore often come at the cost of lower open-loop planning accuracy than direct regression. Moreover, many planners rely on marginal motion modes, which can ignore cross-agent correlations and miss scene-consistent interaction outcomes [23]. This motivates a cost representation that preserves the optimization benefits of cost maps while avoiding dense supervision over irrelevant space. We propose a vectorized, query-based E2E cost planner that narrows this gap by learning a cost estimation over the reachable ego trajectories rather than over a dense spatial grid. The model estimates mode-conditioned costs for sampled ego trajectory queries from compact joint scene tokens built from vectorized map and agent representations. It then aggregates costs over coherent future scene modes with a contingency strategy and refines trajectories through cluster-based MPPI-style mixing guided by the learned cost topology. This design retains the flexibility of cost optimization while approaching regression-style accuracy and improving transfer to real-world driving. The key contributions of this work are: • Query-based cost estimation over sampled reachable ego trajectories instead of dense raster grids. • Compact joint scene tokens that condition ego costs on coherent multimodal futures for contingency planning. • Cost-guided intra-cluster mixing for continuous reachable-space coverage beyond top-k selection. • Ranking-based cost learning objective driven by expert trajectories, collision risk, and off-road violations. Figure 1: Multi-view images feed a sparse perception encoder with temporal instance memory. The proposed planner combines joint scene representations and reachable ego queries to estimate bounded costs and refine ego plans through intra-cluster mixing. 2 Related Work E2E autonomous driving has advanced along two closely related directions: richer scene representations and more structured planning interfaces. Our work lies at their intersection, using compact sparse scene tokens while addressing the limitations of direct regression by estimating costs over reachable ego trajectories. 2.1 Sparse and vectorized end-to-end planning. E2E driving has moved from intermediate-representation and sensor-fusion models, such as DSDNet [32] and TransFuser [5], toward planning-oriented architectures such as UniAD [10] and DriveTransformer [12]. In parallel, vectorized and sparse representations have reduced the computation cost of dense BEV representations. MapTR introduced structured queries for online map reconstruction [19]; VAD, SparseDrive, and SparseAD extend this idea to end-to-end driving with compact map and agent queries [13, 24, 33]. While these methods improve scene encoding and planning efficiency, the final decision is commonly obtained through trajectory regression. They also often reason over marginal agent modes, which can fail to capture coherent scene understanding and complex interactive futures [23]. In contrast, our planner learns a cost landscape over reachable ego trajectories conditioned on joint scene tokens, so each action is scored against coherent multimodal scene hypotheses rather than independently selected futures. 2.2 Cost-based and safety-aware planning. Cost-based planners provide an interpretable interface between perception and action by exposing a cost landscape that can be optimized or repaired. Neural Motion Planner [31], QUAD [1], and MP3 [4] estimate costs from BEV or map-conditioned features and score sampled ego trajectories, while ST-P3 learns a spatial-temporal BEV cost volume for multi-camera planning [9]. These works demonstrate the value of learned costs, but dense BEV cost supervision disperses learning over large regions that are irrelevant to the ego vehicle’s reachable motion. This can make optimization less stable, blur the distinction between reachable and unreachable costs, and has often led to weaker planning performance than direct regression. In addition, cost learning is connected to Inverse Reinforcement Learning (IRL), which treats expert demonstrations as evidence of an underlying reward or cost function. Maximum-entropy IRL formalizes this idea by recovering costs whose induced trajectory distribution matches expert behavior [30], with later extensions to spatial-temporal costmaps [16, 15, 25]. However, these methods usually supervise costs indirectly through expert likelihood or policy matching, often requiring an inner planning or policy-inference loop. On the other hand, purely supervised trajectory regression has safety limitations: Wang et al. [26] show that distance-to-expert losses fail to separate safe and collision-prone alternatives. We instead learn a query-based cost landscape in which expert behavior, collision risk, and off-road violations supervise the ordering of reachable ego queries. 3 Methodology 3.1 Overview and Problem Formulation Given a history of multi-view images ℐ=It,n∈ℝ3×H0×W0t=−Thist0,n=1,…,NII=\I_t,n ^3× H_0× W_0\_t=-T_hist^0,n=1,…,N_I, the current ego state 0x_0, and a high-level navigation command c, the planner outputs a future ego trajectory τ^=^tt=1Tplan τ=\ x_t\_t=1^T_plan in the ego coordinate frame. Trajectory-regression planners learn this mapping directly, producing a small set of probabilistic ego futures. In contrast, we formulate planning as cost estimation over dynamically reachable ego trajectory queries. This preserves the interpretability of cost-based planning while avoiding dense supervision over regions the ego vehicle cannot execute. Figure 1 summarizes the proposed architecture, which follows four stages. First, a sparse perception encoder extracts compact agent and map tokens from the image queue. Second, a joint scene prediction module summarizes multimodal agent futures into joint scene tokens =(j,πj)j=1NsS=\(s_j, _j)\_j=1^N_s, where js_j denotes a future scene-mode token, πj _j its probability, and NsN_s the number of scene modes. Third, conditioned on the ego state 0x_0 and navigation command c, a kinematic rollout sampler generates a clustered reachable query set =(τi,gi)i=1NQ=\( _i,g_i)\_i=1^N, where τi _i is a feasible ego trajectory and gig_i denotes an intention cluster induced by the sampler. Each trajectory is then embedded as an ego query. Finally, the model predicts a bounded cost for each ego query, scene mode, and future timestep, θ=[Cθ(τi,j,t)]∈[Cmin,Cmax]N×Ns×Tplan.C_θ= [C_θ( _i,s_j,t) ]∈[C_ ,C_ ]^N× N_s× T_plan. (1) where θ denotes the learned model parameters. We refer to θC_θ as a query-based cost estimate: rather than assigning costs to dense BEV cells, it defines a cost landscape over the reachable ego trajectory manifold. The final plan is obtained by aggregating costs over time and scene modes with a contingency-aware risk criterion, refining low-cost candidates through cost-guided trajectory mixing within intention clusters, and selecting the refined trajectory with the lowest resulting cost. 3.2 Vectorized Map and Agent Encoding To efficiently encode the image queue ℐI into compact map and agent features, we build on the SparseDrive image encoder and symmetric sparse perception module [24]. At each timestep, a ResNet-FPN [20] image encoder maps the multi-view images to multi-scale image features ℱ=r∈ℝNI×D×Hr×Wrr=1NF,F=\F_r ^N_I× D× H_r× W_r\_r=1^N_F, (2) where D is the feature dimension and NFN_F is the number of feature scales. The sparse perception module then learns two distinct instance sets. Static map elements are encoded as map features and polyline anchors (,)(M,L), with ∈ℝNm×DM ^N_m× D and ∈ℝNm×P×2L ^N_m× P× 2. Dynamic agents are encoded as agent features and 3D anchors (,)(A,B), with ∈ℝNa×DA ^N_a× D and ∈ℝNa×11B ^N_a× 11. Unlike BEV-based encoders [9], SparseDrive does not lift image features into a dense BEV volume. Instead, each sparse instance uses its anchor to gather image evidence from ℱF through deformable feature aggregation. After confidence-based selection, the map tokens M and agent tokens A provide the initial vectorized context for the joint prediction and cost estimation modules. 3.3 Joint Scene Representation Starting from the selected map and agent tokens, temporal propagation and attention-based interactions refine the features with latent temporal and local scene context. However, cost-based planning also requires compact hypotheses about how the scene may evolve. Predicting agents independently can produce futures that are individually likely but mutually inconsistent [28]. Accordingly, we propose NsN_s joint scene modes, each assigned a scene-level probability, so the cost estimator can reason over coherent scene contingencies rather than independent marginal futures. Each scene mode is represented by a learnable embedding j∈ℝDe_j ^D shared across all agents. We form mode-conditioned agent tokens by adding this embedding to each selected agent token, ~a,j=a+j a_a,j=a_a+e_j, yielding ~∈ℝNa×Ns×D A ^N_a× N_s× D. A scene-level token is produced by cross-attending the mode embedding to its mode-conditioned agent tokens: j=FFN(MHCA(Q=j,K=~a,ja=1Na,V=~a,ja=1Na)).s_j=FFN\! (MHCA (Q=e_j,\;K=\ a_a,j\_a=1^N_a,\;V=\ a_a,j\_a=1^N_a ) ). (3) The resulting token js_j summarizes one coherent future scene mode, and its probability is predicted by πj=softmaxj(ψscene(j)) _j=softmax_j( _scene(s_j)), where ψscene _scene is a lightweight scene-logit head. To encourage these scene tokens to represent jointly consistent futures, the mode-conditioned agent tokens regress mode-specific agent trajectories, while the scene tokens predict the mixture weights over modes [22]. Let =a,tY=\y_a,t\ denote the future positions of the selected agents for t=1,…,Tpredt=1,…,T_pred. We model their joint future distribution as p(∣ℐ)=∑j=1Nsπj∏a=1Na∏t=1Tpred(a,t,a,j,t,a,j,t).p(Y )= _j=1^N_s _j _a=1^N_a _t=1^T_predN\! (y_a,t; μ_a,j,t, _a,j,t ). (4) The joint prediction branch is trained with the negative log-likelihood of Eq. 4, encouraging the scene-mode probabilities to assign mass to the hypotheses that best explain the observed joint future. 3.4 Reachable Ego Queries and Cost Estimation Figure 2: Detailed view of the proposed planner. Joint scene tokens encode multimodal futures, while the ego branch samples and encodes reachable trajectory queries. A lightweight head predicts scene-conditioned costs with contingency aggregation, optional inference-time external-cost repair, and cost-guided intra-cluster mixing. Figure 2 details how joint scene tokens and reachable ego queries are combined to estimate and refine query costs. The previous modules provide a compact description of the current scene and its plausible joint futures. We build on this representation to define the planning interface itself: instead of regressing a few ego motions, we learn costs over explicit reachable ego queries. The ego branch builds a context token for the current decision by pooling an ego feature from the front-camera feature map and refining it through temporal attention, agent interaction, and cross-attention to selected map tokens. We then add a learned embedding of the navigation command c, yielding an ego context token e∈ℝDh_e ^D that encodes the current ego state, scene context, and route intent. Reachable Query Sampling. To avoid the diversity collapse often associated with top-k selection, we generate reachable queries with intention-aware clustering. Given the current ego state 0x_0 and command c, a kinematic rollout sampler produces =(τi,gi)i=1NQ=\( _i,g_i)\_i=1^N, where τi=i,tt=1Tplan _i=\x_i,t\_t=1^T_plan is a dynamically feasible ego trajectory and gi∈1,…,Ngg_i∈\1,…,N_g\ denotes its intention cluster. The clusters preserve coverage over distinct maneuver families, while the samples within each cluster explore local motion variations. Each trajectory contributes its geometry and temporal structure through a coordinate and time embedding, i,t=ϕτ(PE(i,t))+t,r_i,t= _τ\! (PE(x_i,t) )+ γ_t, (5) where ϕτ _τ is a lightweight projection and t γ_t denotes the timestep embedding. For cost estimation, the three inputs play complementary roles: eh_e provides the current ego-scene context, i,tr_i,t provides the spatial location and timing of a candidate ego motion, and js_j provides a compact hypothesis of the surrounding scene future. We combine them additively to form a cost query for trajectory i, scene mode j, and timestep t: i,j,tcost=i,t+e+j.z^cost_i,j,t=r_i,t+h_e+s_j. (6) A lightweight cost head maps these queries to bounded costs, Cθ(τi,j,t)=Cmin+σ(ψcost(i,j,tcost))(Cmax−Cmin),C_θ( _i,s_j,t)=C_ +σ\! ( _cost(z^cost_i,j,t) )(C_ -C_ ), (7) where ψcost _cost is implemented as a small prediction head shared across trajectory queries. This defines a query-based cost map only over executable ego trajectories, avoiding dense BEV cells unreachable by the vehicle. Risk-aware Aggregation. The per-mode costs must be collapsed without prematurely committing to a single future scene. This is important in interactive settings, where early ego actions should remain safe under multiple plausible agent responses, while later costs can account for the probability of each contingency [17]. We therefore use a risk-aware contingency aggregation. Let b=⌊Tb/Δt⌋b= T_b/ t denote the branching index, where TbT_b is the branching time in seconds and Δt t is the planning timestep. Before this index, the planner uses the worst scene-mode cost; afterward, it follows the learned scene probabilities: C¯i,t=maxjCθ(τi,j,t),t≤b,∑j=1NsπjCθ(τi,j,t),t>b. C_i,t= cases _jC_θ( _i,s_j,t),&t≤ b,\\ _j=1^N_s _j\,C_θ( _i,s_j,t),&t>b. cases (8) Cost-guided Intra-cluster Mixing. The aggregated costs define a local cost topology over the sampled reachable set. Pure sampling provides broad coverage, but the hard selection remains limited by the resolution of the sampled set and can therefore suffer from high displacement error. Selecting only the global lowest-cost samples can also collapse to a single maneuver family. We address these limitations with cost-guided intra-cluster mixing: intention clusters preserve global maneuver diversity, while the learned risk-aware costs refine each cluster toward its low-cost region. Let q=i:gi=qJ_q=\i:g_i=q\ denote the samples in intention cluster q. For mixing, we use a flexible cost profile Ri,tR_i,t derived from C¯i,t C_i,t. This profile uses average trajectory cost by default, with per-timestep cost as an alternative. Within each cluster, this profile is converted into MPPI-style weights [29], wi,t(q)=exp(−(Ri,t−Rq,tmin)/λ)∑m∈qexp(−(Rm,t−Rq,tmin)/λ),Rq,tmin=minm∈qRm,t,w_i,t^(q)= \! (-(R_i,t-R_q,t )/λ ) _m _q \! (-(R_m,t-R_q,t )/λ ), R_q,t = _m _qR_m,t, (9) where λ controls how strongly the refinement follows the lowest-cost samples. Given the sampled control sequence i,tu_i,t associated with each trajectory, the refined control for cluster q is computed directly as ~q,t=∑i∈qwi,t(q)i,t,τ~q=fkin(~q,tt=1Tplan), u_q,t= _i _qw_i,t^(q)u_i,t, τ_q=f_kin(\ u_q,t\_t=1^T_plan), (10) where fkinf_kin denotes the ego kinematic rollout. The resulting set τ~q\ τ_q\ retains one candidate per intention while using the learned safety-aware costs to refine controls beyond the original samples. The same weights define a cluster-desirability proxy for the mixed-trajectory cost, C~q=1Tplan∑t=1Tplan∑i∈qwi,t(q)Ri,t. C_q= 1T_plan _t=1^T_plan _i _qw_i,t^(q)R_i,t. (11) This proxy preserves a single differentiable path through which trajectory ranking, classification, and regression shape sample costs and fusion, without rescoring the mixed trajectory. For final candidate selection and classification supervision, we use the negative cluster costs as logits, pqego=softmaxq(−C~q)p_q^ego=softmax_q(- C_q), such that lower-cost refined candidates receive higher probability. 3.5 Cost Ranking and Candidate Supervision During training, we retain the standard SparseDrive losses for map and object detection and supervise the joint scene modes with the negative log-likelihood in Eq. 4. We then add a planner-specific objective for the query-based cost estimation. This planning objective supervises two complementary parts of the planner: the cost topology over reachable queries and the final candidate selected after intra-cluster refinement. We propose the following planning loss as ℒplan=λrankℒrank+λclsℒcls+λregℒreg.L_plan= _rank\,L_rank+ _cls\,L_cls+ _reg\,L_reg. (12) Here, ℒrankL_rank trains the cost map before final selection, while ℒclsL_cls and ℒregL_reg supervise the refined candidates τ~qq=1Ng\ τ_q\_q=1^N_g. Safety-aware cost ranking. Imitation losses based only on expert distance may assign similar supervision to safe alternatives and unsafe hard negatives [26]. We therefore train the learned costs to separate the expert trajectory τ⋆τ from sampled reachable trajectories using a novel safety-aware margin. The expert trajectory is queried only during training, not at inference. Unlike ST-P3- or NMP-style objectives [9, 31], which mainly rank candidates through expert distance, our margin uses collision and boundary violations to determine the required separation between learned costs. For a sampled trajectory τi _i, let C⋆C and CiC_i denote the predicted costs of the expert and sampled trajectory under the chosen cost aggregation. The ranking violation and loss are vi=[C⋆−Ci+mi]+,ℒrank=1|h|∑i∈hvi.v_i= [C -C_i+m_i ]_+, _rank= 1|K_h| _i _hv_i. (13) where mi=m(di,ρicol,ρibd)m_i=m(d_i, _i^col, _i^bd) is a bounded margin that grows with the distance did_i to the expert and increases sharply when τi _i collides or leaves the drivable boundary. Intuitively, the loss enforces Ci≥C⋆+miC_i≥ C +m_i: candidates farther from the expert require a higher cost, and collision or boundary violations increase the margin even when the trajectory remains geometrically close to the expert. Rather than using only the single largest violation, hK_h denotes the indices of the top-khk_h violations in a scene. This spreads gradients over informative negatives, producing a smoother topology and providing more stable guidance for the cost-weighted cluster mixing. Candidate selection and refinement. After intra-cluster mixing, each intention cluster yields one refined candidate τ~q τ_q. The classification loss encourages the cluster-cost logits to select the candidate closest to the expert, while the regression loss pulls the selected candidate toward the expert trajectory. Together, the ranking loss shapes the intra-cluster cost landscape used for mixing, and the classification and regression losses preserve accurate inter-cluster selection while allowing the refined candidates to continuously cover the expert distribution. 3.6 Inference-time Cost Repair At inference time, we propose a repair mechanism that incorporates external safety or rule signals without retraining the model. Since our planner predicts bounded costs over explicit ego queries, these signals can directly override unsafe regions of the learned cost landscape. Let Ei,t∈[0,1]E_i,t∈[0,1] denote an external penalty for trajectory τi _i at timestep t, obtained, for example, from a traffic-rule monitor or vehicle-to-everything (V2X) traffic-light data. We perform cost repair by saturating externally unsafe query states toward the maximum cost, C¯i,trepair=(1−Ei,t)C¯i,t+Ei,tCmax. C^\,repair_i,t=(1-E_i,t)\, C_i,t+E_i,t\,C_ . (14) The repaired costs are then used in the same cluster-wise mixing described above. Thus, trajectories marked unsafe by external signals receive low mixing weights, and intention clusters whose trajectories are consistently penalized obtain high cluster costs and are unlikely to be selected. This gives the planner a practical repair mechanism for correcting learned costs at deployment while preserving the reachable-query structure. 4 Experimental Setup Dataset and protocol. We evaluate on the nuScenes dataset [3], which contains 1000 driving scenes with six surround-view cameras and keyframe annotations at 2Hz. Using the train/validation split, the model observes a queue of four multi-view frames and produces a 3s future ego plan for open-loop evaluation. For zero-shot transfer, we use a subset of 465 driving logs from FZI-AURA [21], each lasting 20s, totaling 2.58 hours of driving, about 3× the duration of the nuScenes validation split. The logs were collected with the CoCar NextGen research vehicle [7] in Germany across different times of day, lighting conditions, and traffic densities. To reduce domain shift, we select six surround-view cameras and process the logs following a nuScenes-inspired protocol with manually annotated 3D bounding boxes and semantic segmentation labels. The vehicle uses a sensor rig similar in layout but not identical to nuScenes, resulting in slightly different camera intrinsics and extrinsics. Implementation details. Our model uses SparseDrive-S [24] as the sparse perception backbone with a ResNet-50 FPN image encoder, 900 agent anchors, 100 map anchors, and 256-dimensional instance features. All variants of our model are trained with AdamW using a learning rate of 3×10−43× 10^-4, weight decay of 10−310^-3, and an effective batch size of 48 on two NVIDIA RTX 6000 Ada GPUs. The query-cost planner uses Ns=6N_s=6 joint scene modes, N=512N=512 sampled ego trajectories, and Ng=16N_g=16 intention clusters with 32 samples per cluster. Costs are bounded to [0,100][0,100], contingency aggregation uses a TbT_b of 1s, and the safety-ranking loss averages the top-kh=32k_h=32 violations. Metrics and baselines. We report displacement error and collision rate at 1s, 2s, and 3s, together with their averages. For zero-shot real-world transfer, we additionally report candidate diversity for all models. Diversity is measured by the mean pairwise average distance and endpoint distance among candidate trajectories, capturing separation between maneuver intentions. For variants of our end-to-end model, we also report cost AUC and cost gap, which measure how well the learned costs separate unsafe candidates from safe ones. Unsafe candidates are those that collide or leave the drivable area. An AUC of 0.50.5 corresponds to random ordering and 1.01.0 to perfect separation. The cost gap is the difference between the mean cost of unsafe candidates and the mean cost of safe candidates; larger positive values indicate stronger safety-aligned separation. 5 Evaluation 5.1 Comparison to State of the Art Table 1 compares open-loop planning performance on nuScenes. Compared with prior cost-based planners, our method substantially improves both displacement error and collision rate. Our query cost representation concentrates learning on reachable ego motions, the safety-margin loss separates unsafe hard negatives, and cost-guided cluster mixing converts the learned topology into accurate final plans. Against ST-P3, the average L2 error drops from 2.11m to 0.71m, while the average collision rate drops from 0.71% to 0.07%. This closes much of the gap to recent regression-based planners while preserving the advantages of an explicit cost interface. Notably, our method achieves a lower average collision rate than SparseDrive-S and remains close to the larger SparseDrive-B variant, while also outperforming most regression baselines such as VAD [13] and DriveTransformer [12], suggesting that learning costs over reachable queries provides a useful balance between accurate imitation and safety-aware planning. Table 1: Open-loop planning performance on nuScenes under the UniAD metrics. † denotes LiDAR-based methods. Method L2 (m) ↓ Collision (%) ↓ 1s 2s 3s Avg. 1s 2s 3s Avg. Cost-based methods NMP [31]† – – 2.31 – – – 1.92 – SA-NMP [31]† – – 2.05 – – – 1.59 – ST-P3 [9] 1.33 2.11 2.90 2.11 0.23 0.62 1.27 0.71 Ours 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 Regression-based methods F [8]† 0.55 1.20 2.54 1.43 0.06 0.17 1.07 0.43 EO [14]† 0.67 1.36 2.78 1.60 0.04 0.09 0.88 0.33 UniAD [10] 0.48 0.74 1.07 0.76 0.12 0.13 0.28 0.17 VAD-Tiny [13] 0.46 0.76 1.12 0.78 0.21 0.35 0.58 0.38 VAD-Base [13] 0.41 0.70 1.05 0.72 0.07 0.17 0.41 0.22 BEVPlanner [18] 0.27 0.54 0.90 0.57 – – – – DriveTransformer [12] 0.19 0.34 0.66 0.40 0.03 0.10 0.21 0.11 SparseDrive-S [24] 0.29 0.58 0.96 0.61 0.01 0.05 0.18 0.08 SparseDrive-B [24] 0.29 0.55 0.91 0.58 0.01 0.02 0.13 0.06 5.2 Zero-shot Real-world Transfer Table 2 evaluates zero-shot transfer to real-world driving logs collected in Germany, without fine-tuning any method on this data. Alpamayo 1.5 [27] is a large vision-language driving model trained on substantially broader data, so we use it as a strong imitation-oriented reference rather than a lightweight planner baseline. Compared with SparseDrive-S, our method reduces the average L2 distance from 4.22m to 2.31m and the average collision rate from 2.73% to 0.68%. Alpamayo achieves the lowest displacement error, but uses a 10B-parameter model and requires 1.87s per frame on an NVIDIA RTX 3090 GPU. In contrast, our 86.2M-parameter planner achieves the lowest collision rate, runs in real time, and maintains higher candidate diversity for downstream repair and selection. These results suggest that reachable-query cost learning provides a favorable balance between safety, diversity, and efficiency under real-world domain shift. Table 2: Zero-shot real-world transfer. No method is fine-tuned on the real-world logs. Latency is measured on a workstation with a RTX 3090 GPU. Method L2 (m) ↓ Collision (%) ↓ Diversity (m) ↑ Params. Latency (ms) ↓ 1s 2s 3s Avg. 1s 2s 3s Avg. Avg. Endpoint SparseDrive-S [24] 2.38 4.17 6.11 4.22 0.53 2.57 5.08 2.73 1.77 3.41 85.9M 115 Alpamayo 1.5 [27] 0.64 1.41 2.39 1.48 0.03 0.90 1.79 0.87 1.47 3.25 10.0B 1878 Ours 0.85 2.12 3.98 2.31 0.01 0.40 1.50 0.68 5.54 11.88 86.2M 104 5.3 Safety Supervision and Cost Topology We next isolate how the safety margin shapes the learned cost topology. Table 3 first studies how the collision term in the margin is computed during training. All variants use the same architecture and cost planner at inference; only the collision signal used to compute the training margin changes. At test time, the margin itself is not required, since its effect is absorbed into the learned cost topology. Ground-truth collision provides an oracle target, while top-1 predicted collision supervises the margin using only the most likely future scene. In contrast, contingency collision risk is computed over multimodal predicted scene futures, allowing lower-probability but safety-critical futures to influence the learned costs. The results show a tradeoff between accuracy and safety. Top-1 predicted collision achieves the lowest displacement error, suggesting that supervising with the most likely predicted future is well aligned with imitation accuracy, but it also gives the highest average collision rate. Using predicted contingency risk slightly increases L2 error, but reduces the average collision rate from 0.12% to 0.07%. This indicates that multimodal collision supervision encourages the latent scene tokens to encode collision-relevant futures and yields a cost topology that is better aligned with safety-aware planning. Table 3: Ablation of collision supervision for the safety margin on nuScenes. Collision supervision L2 (m) ↓ Collision (%) ↓ 1s 2s 3s Avg. 1s 2s 3s Avg. Ground-truth collision 0.27 0.63 1.16 0.69 0.00 0.06 0.25 0.10 Top-1 predicted collision 0.25 0.59 1.10 0.64 0.02 0.07 0.26 0.12 Contingency collision risk 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 Having chosen contingency collision risk as the default supervision, Tables 4 and 5 evaluate whether the resulting cost topology is specific to the proposed ranking objective. All variants use the same architecture, reachable-query planner, candidate losses, and inference procedure; only ℒrankL_rank is replaced. We first compare against ranking losses adapted from NMP [31] and ST-P3 [9], and then ablate the sources used to form our safety margin. Table 4 shows that the choice of cost supervision mainly changes the safety structure of the learned topology. The ST-P3-style loss achieves the lowest nuScenes L2 loss, but its weaker cost separation results in a higher collision rate than the proposed margin. In contrast, our safety-margin objective improves nuScenes collision from 0.12% to 0.07% relative to the ST-P3-style loss, while increasing cost AUC from 0.79 to 0.86 and cost gap from 7.80 to 45.70. The same trend holds under real-world transfer: the proposed objective yields the lowest collision rate and the largest cost gap, indicating that the learned safety ordering generalizes beyond the training domain. Table 4: Comparison of cost-supervision objectives. Objective ℒrankL_rank nuScenes Real-world L2 ↓ Coll. ↓ Cost AUC ↑ Cost Gap ↑ L2 ↓ Coll. ↓ Cost AUC ↑ Cost Gap ↑ NMP-style loss [31] 0.76 0.33 0.70 6.60 2.29 0.84 0.59 3.99 ST-P3-style loss [9] 0.61 0.12 0.79 7.80 3.26 0.83 0.61 4.24 Proposed safety margin 0.71 0.07 0.86 45.70 2.31 0.68 0.63 16.18 Table 5 further decomposes the proposed margin. This evaluates the contribution of expert-distance and safety terms, and whether combining them yields a cost topology that is both safety-aware and useful for final planning. The distance-only margin keeps the trajectories close to the expert but leaves many unsafe hard negatives weakly separated, resulting in higher collision rates on both nuScenes and real-world logs. Using only collision and off-road terms produces the strongest raw separation, but it also degrades displacement and does not translate into the best final collision rate. Combining distance with collision and off-road terms gives the best planning tradeoff: the learned costs remain anchored to expert-like reachable motions while still penalizing unsafe alternatives, yielding the lowest collision rate in both domains. Table 5: Ablation of safety-margin sources. Margin source nuScenes Real-world L2 ↓ Coll. ↓ Cost AUC ↑ Cost Gap ↑ L2 ↓ Coll. ↓ Cost AUC ↑ Cost Gap ↑ Distance 0.72 0.20 0.71 10.51 2.36 1.71 0.56 4.89 Collision + off-road 0.88 0.11 0.87 51.25 2.34 1.48 0.66 23.57 All 0.71 0.07 0.86 45.70 2.31 0.68 0.63 16.18 5.4 Qualitative Results of External Cost Repair Figure 3 illustrates the inference-time cost repair introduced in Section 3.6. As an example of external costs, we use the conflict area of a red traffic light received from V2X infrastructure. SparseDrive produces a low-diversity proposal set in which all candidates enter this area (Fig. 3(b)). Since the proposals already violate the rule, post-hoc rescoring alone cannot recover a rule-compliant trajectory. Our planner instead applies the external penalty to the aggregated query-cost profile before intra-cluster mixing. This changes the cost topology used to form the final candidates, suppressing unsafe queries and producing safe low-cost candidates, highlighted in green in Fig. 3(c), that respect the traffic-light rule. 5.5 Qualitative Robustness to Misaligned Navigation Commands Figure 4 qualitatively evaluates the behavior of the planners under a temporally misaligned high-level navigation command. The scene contains an S-bend before a highway merge: the route will soon require a right turn, but the vehicle must first follow a left-hand bend bounded by lane dividers. Thus, the command is not semantically wrong, but executing the right-turn intent immediately would cut into the divider. SparseDrive prematurely shifts its selected trajectory toward the upcoming right maneuver and collides with the divider, as seen in Fig. 4(b). This reflects a limitation of command-conditioned trajectory regression: the generated proposal follows the route intent in geometry, but does not explicitly optimize a local road-compliance cost. Our planner samples command-conditioned reachable trajectories with the kinematic rollout sampler and evaluates them using the learned query-cost landscape. Trajectories that conflict with the current lane geometry receive high learned cost, so the selected trajectory follows the low-cost, road-compliant option through the left bend before later committing to the rightward route intent, as illustrated in Fig. 4(c). This example shows that query-based cost estimation can use navigation commands while still selecting the safest locally feasible trajectory in the candidate set. (a) Camera view (b) SparseDrive BEV (c) Ours BEV Figure 3: Qualitative comparison using the conflict area of a red traffic light received from a V2X unit. SparseDrive’s low-diversity proposals already violate the rule, so post-hoc rescoring cannot recover a rule-compliant trajectory. Our planner applies the external penalty to the aggregated query-cost profile, producing safe low-cost candidates. (a) Projected final trajectory candidates (b) SparseDrive BEV (c) Ours BEV Figure 4: Qualitative example under a temporally premature right-turn command. Panel (a) shows the final selected trajectory candidates projected into the camera views, with ours in green and SparseDrive in red. SparseDrive follows the premature command into the lane divider, while our planner selects the locally safe left bend. 6 Conclusion This work revisits cost-based planning as an end-to-end driving interface by learning bounded costs over dynamically reachable ego trajectories rather than dense BEV grids. This shifts supervision toward actions the vehicle can actually execute, while retaining a cost representation that can be inspected, optimized, and repaired. Compact joint scene tokens, safety-aware ranking, and cost-guided cluster mixing support reasoning over coherent future contingencies and produce safe ego plans. On nuScenes, the approach substantially improves over previous cost-based planners and remains competitive with strong regression-based approaches. Zero-shot transfer to FZI-AURA demonstrates effectiveness without fine-tuning, while the cost interface provides a mechanism for inference-time repair using external safety signals. Acknowledgements The research leading to these results is funded by the German Federal Ministry for Economic Affairs and Energy within the project “NXT GEN AI METHODS – Generative Methoden für Perzeption, Prädiktion und Planung". The authors would like to thank the consortium for the successful cooperation. The authors acknowledge support by the state of Baden-Württemberg through bwHPC. References [1] S. Biswas, S. Casas, Q. Sykora, B. Agro, A. Sadat, and R. Urtasun (2024) QUAD: query-based interpretable neural motion planning for autonomous driving. In 2024 IEEE International Conference on Robotics and Automation (ICRA), p. 14236–14243. Cited by: §2.2. [2] M. Bojarski, P. Yeres, A. Choromanska, K. Choromanski, B. Firner, L. Jackel, and U. Muller (2017) Explaining how a deep neural network trained with end-to-end learning steers a car. arXiv preprint arXiv:1704.07911. Cited by: §1. [3] H. Caesar, V. Bankiti, A. H. Lang, S. Vora, V. E. Liong, Q. Xu, A. Krishnan, Y. Pan, G. Baldan, and O. Beijbom (2020) NuScenes: a multimodal dataset for autonomous driving. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, p. 11621–11631. Cited by: §4. [4] S. Casas, A. Sadat, and R. Urtasun (2021) MP3: a unified model to map, perceive, predict and plan. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, p. 14398–14407. Cited by: §2.2. [5] K. Chitta, A. Prakash, B. Jaeger, Z. Yu, K. Renz, and A. Geiger (2022) TransFuser: imitation with transformer-based sensor fusion for autonomous driving. IEEE Transactions on Pattern Analysis and Machine Intelligence 45 (11), p. 12878–12895. Cited by: §1, §2.1. [6] T. Fawcett (2006) An introduction to roc analysis. Pattern recognition letters 27 (8), p. 861–874. Cited by: §0.B.4. [7] M. Heinrich, M. Zipfl, M. Uecker, S. Ochs, M. Gontscharow, T. Fleck, J. Doll, P. Schörner, C. Hubschneider, M. R. Zofka, et al. (2024) Cocar nextgen: a multi-purpose platform for connected autonomous driving research. In 2024 IEEE 27th International Conference on Intelligent Transportation Systems (ITSC), p. 482–489. Cited by: §4. [8] P. Hu, A. Huang, J. Dolan, D. Held, and D. Ramanan (2021) Safe local motion planning with self-supervised freespace forecasting. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, p. 12732–12741. Cited by: Table 1. [9] S. Hu, L. Chen, P. Wu, H. Li, J. Yan, and D. Tao (2022) ST-p3: end-to-end vision-based autonomous driving via spatial-temporal feature learning. In European Conference on Computer Vision, p. 533–549. Cited by: §0.A.1, §1, §2.2, §3.2, §3.5, §5.3, Table 1, Table 4. [10] Y. Hu, J. Yang, L. Chen, K. Li, C. Sima, X. Zhu, S. Chai, S. Du, T. Lin, W. Wang, et al. (2023) Planning-oriented autonomous driving. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, p. 17853–17862. Cited by: §0.B.4, §1, §2.1, Table 1. [11] C. Hubschneider, A. Bauer, M. Weber, and J. M. Zöllner (2017) Adding navigation to the equation: turning decisions for end-to-end vehicle control. In 2017 IEEE 20th international conference on intelligent transportation systems (ITSC), p. 1–8. Cited by: §1. [12] X. Jia, J. You, Z. Zhang, and J. Yan (2025) DriveTransformer: unified transformer for scalable end-to-end autonomous driving. arXiv preprint arXiv:2503.07656. Cited by: §1, §2.1, §5.1, Table 1. [13] B. Jiang, S. Chen, Q. Xu, B. Liao, J. Chen, H. Zhou, Q. Zhang, W. Liu, C. Huang, and X. Wang (2023) VAD: vectorized scene representation for efficient autonomous driving. In Proceedings of the IEEE/CVF International Conference on Computer Vision, p. 8340–8350. Cited by: §1, §2.1, §5.1, Table 1, Table 1. [14] T. Khurana, P. Hu, A. Dave, J. Ziglar, D. Held, and D. Ramanan (2022) Differentiable raycasting for self-supervised occupancy forecasting. In European Conference on Computer Vision, p. 353–369. Cited by: Table 1. [15] K. Lee, D. Isele, E. A. Theodorou, and S. Bae (2022) Risk-sensitive mpcs with deep distributional inverse rl for autonomous driving. In 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 7635–7642. Cited by: §2.2. [16] K. Lee, D. Isele, E. A. Theodorou, and S. Bae (2022) Spatiotemporal costmap inference for mpc via deep inverse reinforcement learning. IEEE Robotics and Automation Letters 7 (2), p. 3194–3201. Cited by: §2.2. [17] T. Li, L. Zhang, S. Liu, and S. Shen (2025) Multimodal integrated prediction and decision-making with adaptive interaction modality explorations. In 2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 13674–13681. Cited by: §3.4. [18] Z. Li, Z. Yu, S. Lan, J. Li, J. Kautz, T. Lu, and J. M. Alvarez (2024) Is ego status all you need for open-loop end-to-end autonomous driving?. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, p. 14864–14873. Cited by: Table 1. [19] B. Liao, S. Chen, X. Wang, T. Cheng, Q. Zhang, W. Liu, and C. Huang (2022) MapTR: structured modeling and learning for online vectorized hd map construction. arXiv preprint arXiv:2208.14437. Cited by: §2.1. [20] T. Lin, P. Dollár, R. Girshick, K. He, B. Hariharan, and S. Belongie (2017) Feature pyramid networks for object detection. In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, p. 2117–2125. Cited by: §3.2. [21] R. Polley, M. Heinrich, P. Schörner, M. Uecker, S. Ochs, T. Fleck, M. R. Zofka, and J. M. Zöllner (2026) FZI-AURA: A Multimodal Autonomous Driving Dataset. FZI Forschungszentrum Informatik. External Links: Link Cited by: §4. [22] S. Shi, L. Jiang, D. Dai, and B. Schiele (2024) Mtr++: multi-agent motion prediction with symmetric scene modeling and guided intention querying. IEEE Transactions on Pattern Analysis and Machine Intelligence 46 (5), p. 3955–3971. Cited by: §3.3. [23] Q. Sun, X. Huang, J. Gu, B. C. Williams, and H. Zhao (2022) M2I: from factored marginal trajectory prediction to interactive prediction. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), p. 6543–6552. Cited by: §1, §2.1. [24] W. Sun, X. Lin, Y. Shi, C. Zhang, H. Wu, and S. Zheng (2025) SparseDrive: end-to-end autonomous driving via sparse scene representation. In 2025 IEEE International Conference on Robotics and Automation (ICRA), p. 8795–8801. Cited by: §1, §2.1, §3.2, §4, Table 1, Table 1, Table 2. [25] S. Triest, M. G. Castro, P. Maheshwari, M. Sivaprakasam, W. Wang, and S. Scherer (2023) Learning risk-aware costmaps via inverse reinforcement learning for off-road navigation. In 2023 IEEE International Conference on Robotics and Automation (ICRA), p. 924–930. Cited by: §2.2. [26] J. Wang, Z. Hua, X. Liu, Z. Xing, H. Tian, K. Ma, H. Ye, G. Chen, L. Chen, and Q. Zhang (2026) Beyond imitation: learning safe end-to-end autonomous driving from hard negatives. arXiv preprint arXiv:2605.19771. Cited by: §1, §2.2, §3.5. [27] Y. Wang, W. Luo, J. Bai, Y. Cao, T. Che, K. Chen, Y. Chen, J. Diamond, Y. Ding, W. Ding, et al. (2025) Alpamayo-r1: bridging reasoning and action prediction for generalizable autonomous driving in the long tail. arXiv preprint arXiv:2511.00088. Cited by: §0.B.3, §5.2, Table 2. [28] E. Weng, H. Hoshino, D. Ramanan, and K. Kitani (2023) Joint metrics matter: a better standard for trajectory forecasting. In Proceedings of the IEEE/CVF International Conference on Computer Vision, p. 20315–20326. Cited by: §3.3. [29] G. Williams, A. Aldrich, and E. A. Theodorou (2017) Model predictive path integral control: from theory to parallel computation. Journal of Guidance, Control, and Dynamics 40 (2), p. 344–357. Cited by: §3.4. [30] M. Wulfmeier, P. Ondruska, and I. Posner (2015) Maximum entropy deep inverse reinforcement learning. arXiv preprint arXiv:1507.04888. Cited by: §2.2. [31] W. Zeng, W. Luo, S. Suo, A. Sadat, B. Yang, S. Casas, and R. Urtasun (2019) End-to-end interpretable neural motion planner. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, p. 8660–8669. Cited by: §0.A.1, §1, §2.2, §3.5, §5.3, Table 1, Table 1, Table 4. [32] W. Zeng, S. Wang, R. Liao, Y. Chen, B. Yang, and R. Urtasun (2020) DSDNet: deep structured self-driving network. In European Conference on Computer Vision, p. 156–172. Cited by: §2.1. [33] D. Zhang, G. Wang, R. Zhu, J. Zhao, X. Chen, S. Zhang, J. Gong, Q. Zhou, W. Zhang, N. Wang, et al. (2024) SparseAD: sparse query-centric paradigm for efficient end-to-end autonomous driving. arXiv preprint arXiv:2404.06892. Cited by: §2.1. Supplementary Material for Beyond Waypoint Regression: Query-Based Cost Learning over Reachable Ego Futures for End-to-End Driving Appendix 0.A Additional Method Details 0.A.1 Reachable Query Sampling The proposed method uses a clustered set of reachable ego trajectories =(τi,gi)i=1NQ=\( _i,g_i)\_i=1^N as the queries of the learned cost landscape. Here, we describe how these queries are generated before cost evaluation. The goal is to cover distinct maneuver intentions while keeping every query tied to a physically executable control sequence. Following sampling-based cost planners such as NMP [31] and ST-P3 [9], we use sampled ego trajectories as the support for cost estimation, but organize them as command-conditioned reachable control clusters. The sampler first defines coarse reference controls for a set of intention clusters, then samples temporally smooth control perturbations around these references. Given the current ego speed v0v_0 and navigation command c, we first define a uniform grid of NgN_g reference intentions over target final speeds and heading changes. The target final speeds vtarq\v_tar^q\ span a bounded interval starting from zero up to a command-independent speed limit around the current velocity. The heading changes Δψq\ _q\ are command-conditioned: Δψq∈[−ψmax,0],c=right,[0,ψmax],c=left,[−ψmax/2,ψmax/2],c=straight. _q∈ cases[- _ ,0],&c=right,\\ [0, _ ],&c=left,\\ [- _ /2, _ /2],&c=straight. cases (15) Each grid point defines a reference intention. Rather than using it as a geometric anchor, we convert it into constant reference controls over the planning horizon: aq0=vtarq−v0TplanΔt,ωq0=ΔψqTplanΔt,q,t0=(ωq0,aq0).a_q^0= v_tar^q-v_0T_plan t, _q^0= _qT_plan t, _q,t^0=( _q^0,a_q^0). (16) Thus, each cluster represents a coarse control intention, such as maintaining speed, slowing down, or turning with different heading changes. To improve low-speed coverage, the sampler also includes one auxiliary slow-motion cluster. In total, intra-cluster mixing operates over N~g=Ng+1 N_g=N_g+1 clusters. To explore local variations around each intention, we draw Np−1N_p-1 perturbed control sequences around each cluster control reference and use the auxiliary cluster for the remaining low-speed samples, giving N=NgNpN=N_g\,N_p total queries. For trajectory i assigned to cluster gig_i, the sampled control at timestep t is i,t=gi,t0+ϵi,t,i,t=(ωi,t,ai,t).u_i,t=u_g_i,t^0+ ε_i,t, _i,t=( _i,t,a_i,t). (17) We use temporally correlated perturbations instead of independent per-step noise, which produces smoother control sequences and avoids unrealistically jittery trajectories: ϵi,t=αϵi,t−1+1−α2⊙i,t,α=exp(−Δt/tc), ε_i,t=α\, ε_i,t-1+ 1-α^2\, σ _i,t, α= (- t/t_c), (18) where i,t∼(,)z_i,t (0,I), σ sets the yaw-rate and acceleration noise scales, and tct_c controls the correlation time. Finally, each sampled control sequence is rolled out with the kinematic model, τi=fkin(i,tt=1Tplan),τi=i,tt=1Tplan. _i=f_kin\! (\u_i,t\_t=1^T_plan ), _i=\x_i,t\_t=1^T_plan. (19) For clarity, the rollout can be written as a unicycle update for t=1,…,Tplant=1,…,T_plan, initialized at the current ego state: vi,t=vi,t−1+ai,tΔt,ψi,t=ψi,t−1+ωi,tΔt,v_i,t=v_i,t-1+a_i,t t, _i,t= _i,t-1+ _i,t t, (20) i,t=i,t−1+vi,t−1[cosψi,t−1sinψi,t−1]Δt+12ai,t[cosψi,t−1sinψi,t−1]Δt2.x_i,t=x_i,t-1+v_i,t-1 bmatrix _i,t-1\\ _i,t-1 bmatrix t+ 12a_i,t bmatrix _i,t-1\\ _i,t-1 bmatrix t^2. (21) This corresponding closed-form control integration for nonzero yaw rate is used. The resulting query set is =(τi,i,gi)i=1N,N=NgNp,gi∈1,…,N~g,Q=\( _i,u_i,g_i)\_i=1^N, N=N_g\,N_p, g_i∈\1,…, N_g\, (22) which is passed to the query-cost planner. Figure 5 visualizes the resulting command-conditioned query sets for representative straight, left, and right commands from the nuScenes dataset. (a) Straight command (b) Left command (c) Right command Figure 5: Command-conditioned reachable query sampling. The kinematic sampler generates clustered, dynamically executable ego trajectories that cover the diverse intentions implied by the navigation command while preserving local control variations within each cluster. 0.A.2 Cost-Guided Intra-cluster Mixing Intra-cluster mixing can be written as a weighted average of sampled controls. Here, we give a more detailed view, which is equivalent to the main paper but makes the MPPI structure explicit. After contingency aggregation, each sampled trajectory has a per-timestep cost profile C¯i,t C_i,t and belongs to a mixing cluster q=i:gi=qJ_q=\i:g_i=q\, with q=1,…,N~gq=1,…, N_g. For mixing, we define a score profile Ri,tR_i,t from the learned costs. In the trajectory-level variant, the same score is used at every timestep, Ri,t=1Tplan∑t′=1TplanC¯i,t′,∀t,R_i,t= 1T_plan _t =1^T_plan C_i,t , ∀ t, (23) whereas in the timestep-level variant we use Ri,t=C¯i,tR_i,t= C_i,t directly. The first option follows the overall quality of each sampled trajectory, while the second recomputes weights at each timestep, so different samples can dominate different parts of the fused control sequence. Within each cluster, this score profile is converted into the same MPPI-style softmax weights as defined in the main paper, with the cluster-wise minimum score subtracted for numerical stability. The temperature λ controls the sharpness of the update: smaller values concentrate the refinement around the lowest-cost samples, while larger values average over more of the local cost topology. Rather than averaging trajectory coordinates, our method fuses control perturbations around the reference control of each cluster, then rolls out the fused controls with the ego kinematic model to keep the refined candidate feasible. Δi,t=i,t−gi,t0 _i,t=u_i,t-u_g_i,t^0 (24) denote its deviation from the reference control of its intention cluster. The refined control sequence for cluster q is then ~q,t=q,t0+∑i∈qwi,t(q)Δi,t. u_q,t=u_q,t^0+ _i _qw_i,t^(q) _i,t. (25) Because the weights sum to one within each cluster and timestep, this residual form is equivalent to directly averaging the sampled controls with the same MPPI weights. The residual form simply highlights that the update is performed around the command-conditioned control intention. Finally, the refined candidate is obtained by rolling out the fused controls, τ~q=fkin(~q,tt=1Tplan). τ_q=f_kin (\ u_q,t\_t=1^T_plan ). (26) This produces one continuous candidate per intention cluster, preserving maneuver diversity while using the learned cost topology to reduce the discretization error of the sampled query set. 0.A.3 Cost-Guided Inference and Repair Algorithm Algorithm 1 summarizes how predicted query costs become a selected ego plan. Contingency aggregation combines scene-mode costs, and optional external penalties modify them before fusion. Mean trajectory costs determine within-cluster weights used to mix controls, roll out candidates, and compute cluster-desirability scores for selection. T=TplanT=T_plan, b=⌊Tb/Δt⌋b= T_b/ t , and Ei,t=0E_i,t=0 disables external repair. Algorithm 1 Cost aggregation, repair, and intra-cluster fusion 1: Sampled controls i,tu_i,t, cluster IDs gig_i, query costs Ci,j,tC_i,j,t, scene probabilities πj _j, optional penalties Ei,tE_i,t 2: Horizon T, branch index b, temperature λ>0λ>0, cost bound CmaxC_ 3: Selected plan τ τ and candidates (τ~q,C~q)\( τ_q, C_q)\ 4: for each query i and timestep t=1,…,Tt=1,…,T do 5: C¯i,t←maxjCi,j,t,t≤b,∑jπjCi,j,t,t>b C_i,t← cases _jC_i,j,t,&t≤ b,\\ _j _jC_i,j,t,&t>b cases 6: C¯i,t←(1−Ei,t)C¯i,t+Ei,tCmax C_i,t←(1-E_i,t) C_i,t+E_i,tC_ 7: end for 8: Ri←T−1∑t=1TC¯i,tR_i← T^-1 _t=1^T C_i,t for every query i 9: for each nonempty cluster q do 10: q←i:gi=qJ_q←\i:g_i=q\, Rqmin←mini∈qRiR_q ← _i _qR_i 11: wi(q)←exp(−(Ri−Rqmin)/λ)∑m∈qexp(−(Rm−Rqmin)/λ)w_i^(q)← (-(R_i-R_q )/λ) _m _q (-(R_m-R_q )/λ), i∈qi _q 12: ~q,t←∑i∈qwi(q)i,t u_q,t← _i _qw_i^(q)u_i,t for all t 13: τ~q←fkin(~q,tt=1T) τ_q← f_kin(\ u_q,t\_t=1^T) 14: C~q←∑i∈qwi(q)Ri C_q← _i _qw_i^(q)R_i 15: end for 16: q⋆←argminqC~q ← _q C_q 17: return τ^=τ~q⋆ τ= τ_q , (τ~q,C~q)\( τ_q, C_q)\ The rollout uses the sampler’s initialization and kinematic model. We fuse controls rather than trajectory coordinates, and use the cluster-desirability proxy C~q C_q without re-evaluating fused trajectories through the learned cost head. External penalties therefore influence both candidate construction and selection without retraining. The expert trajectory is used only during training. 0.A.4 Network Architecture Details. We provide the implementation details of the lightweight cost head used to score the reachable ego queries. Given trajectory embeddings i,tr_i,t, ego context eh_e, and scene tokens jj=1Ns\s_j\_j=1^N_s, the model forms the cost-query tensor by broadcasting cost∈ℝB×N×Ns×Tplan×D,i,j,tcost=i,t+e+j.z^cost ^B× N× N_s× T_plan× D, ^cost_i,j,t=r_i,t+h_e+s_j. The shared cost head is a two-layer MLP applied to every trajectory, scene mode, and timestep: ψcost()=2ReLU(1+1)+2. _cost(z)=W_2\,ReLU(W_1z+b_1)+b_2. Its scalar output is mapped to [Cmin,Cmax][C_ ,C_ ] using the bounded sigmoid cost mapping described in the method; in our experiments, we set Cmin=0C_ =0 and Cmax=100C_ =100. Appendix 0.B Additional Experimental Setup Details 0.B.1 Real World Driving Logs Details. The real-world evaluation set contains 465 driving logs from FZI-AURA collected with the CoCar NextGen research vehicle in Germany across different times of day, lighting conditions, and traffic densities. Each log has a duration of 20s, resulting in 2.58 h of raw driving data and 93,000 raw samples at 10 Hz. The logs cover different locations, times of day, lighting conditions, and traffic densities. The logs include synchronized camera images, ego poses, calibrated sensor transformations, and both 3D bounding-box annotations and semantic labels at 2Hz keyframes. We match the nuScenes planning protocol and evaluate on 2Hz keyframes. Across the 465 logs, the dataset contains 725,459 annotated 3D boxes. The labeled keyframes contain an average of 40 boxes, a median of 33, and a range of 1 to 415. The dominant categories are cars, bicycles, persons, and trucks. Frames near the end of a log are excluded from full-horizon planning metrics when the required 3 s of future trajectory are unavailable. 0.B.2 SparseDrive-format preprocessing. SparseDrive and our method are evaluated with six surround-view cameras, mapped to the nuScenes-style camera slots. The five forward and side-facing cameras have a native resolution of 1920×12001920× 1200, while the rear wide-angle camera has a native resolution of 2592×20482592× 2048. All image inputs are resized and cropped to 704×256704× 256 pixels. To reduce visual domain shift caused by different camera mounting heights and fields of view, we use a fixed vertical crop offset. This crop choice aligns the horizon and road layout more closely with the nuScenes image distribution. Since SparseDrive is trained in the nuScenes top-lidar reference convention, we define a virtual top-lidar frame on the research vehicle to provide the same planner coordinate interface. This frame follows the nuScenes axis convention, with the x axis pointing right and the y axis pointing forward; the translation from the virtual top-lidar frame to the vehicle base frame is (0.94,0.00,1.84)(0.94,0.00,1.84) m. High-level navigation commands are generated from the recorded future ego motion using the same virtual frame. For each evaluated frame, we transform the ego displacement after 3 s into the virtual top-lidar frame and apply the SparseDrive lateral-threshold rule. Displacements greater than 2 m are labeled right, displacements below −2-2 m are labeled left, and the remaining frames are labeled straight. This yields a command distribution of 8% left, 83.5% straight, and 8.5% right. For Alpamayo, we convert the same command labels into text prompts so both methods receive the same high-level intent. 0.B.3 Alpamayo Inference Details. We evaluate Alpamayo 1.5 [27] as a large imitation-oriented vision-language driving reference rather than as a lightweight real-time planner baseline. Alpamayo is evaluated using only the front-medium camera. It receives four image history frames and 16 ego-history states sampled at 10 Hz, corresponding to 1.6 s of ego-state history. In contrast, SparseDrive and our method receive four multi-view frames at 2 Hz. The same navigation-command heuristic is used for Alpamayo as for the SparseDrive-format methods, with the discrete labels converted into natural-language instructions: “Turn left”, “Go straight”, or “Turn right”. Alpamayo inference is run on the 10 Hz raw logs with stride 5, giving the same 2 Hz evaluation rate as the labeled keyframes. Predicted trajectories are generated for a 3 s horizon and exported into the same SparseDrive-style result format used by the other methods. Alpamayo 1.5 can sample one or more future trajectories, but the released model does not expose a model-defined trajectory-level cost, confidence, likelihood, or score for ranking those samples. Therefore, L2 and collision are computed using the first decoded trajectory. For candidate-diversity evaluation, we collect 16 sampled trajectories per frame. The samples are generated sequentially due to GPU memory limitations. For the real-world transfer table in the main paper, latency is measured with a single generated trajectory per frame. 0.B.4 Evaluation Metric Details. We report the standard open-loop L2 and collision metrics at 1s, 2s, and 3s. Unless otherwise stated, these metrics are computed on the final selected trajectory for each planner. SparseDrive selects the trajectory with the highest planning score, whereas our method selects the refined candidate with the lowest aggregated query cost. Following the uniAD [10] protocol, each reported collision value is cumulative over the 0.5s evaluation steps up to the corresponding horizon. Collision is evaluated by intersecting the predicted ego footprint with future annotated 3D bounding boxes in the ego-aligned evaluation frame. Map-based road-compliance signals are used outside the reported open-loop L2 and collision metrics. On nuScenes, boundary violations are used during training to construct the safety-aware ranking margin: a violation is counted when the predicted ego trajectory, evaluated with footprint offsets, crosses drivable-area boundary polylines extracted from lane and road-segment polygons. On the real-world logs, where the same HD-map protocol is not available, we build an accumulated semantic-lidar road-support map for qualitative visualization and sanity checks. Additionally, we report candidate-level metrics for diversity and learned cost separation. For a fixed scene s, let τ~qq=1Ks\ τ_q\_q=1^K_s denote the refined candidate trajectories, where τ~q=~q,tt=1Tplan τ_q=\ x_q,t\_t=1^T_plan. Candidate diversity. We measure candidate diversity as the pairwise displacement between candidates produced for the same scene. We report both an average-displacement version and an endpoint-displacement version: DivAvg(s)=2Ks(Ks−1)∑q<q′1Tplan∑t=1Tplan‖~q,t−~q′,t‖2,Div_Avg(s)= 2K_s(K_s-1) _q<q 1T_plan _t=1^T_plan\| x_q,t- x_q ,t\|_2, DivEndpoint(s)=2Ks(Ks−1)∑q<q′‖~q,Tplan−~q′,Tplan‖2.Div_Endpoint(s)= 2K_s(K_s-1) _q<q \| x_q,T_plan- x_q ,T_plan\|_2. We average both values over scenes. DivAvgDiv_Avg captures the average spread of the candidates over the full planning horizon, while DivEndpointDiv_Endpoint emphasizes endpoint separation and more directly reflects diversity between maneuver intentions. Cost separation. For cost-based variants, we evaluate whether the learned costs separate unsafe candidates from safe candidates. A candidate is unsafe if it collides with another actor or leaves the drivable area at any valid timestep. Let sU_s and ℛsR_s denote the unsafe and safe refined-candidate index sets in scene s, respectively, and let C~q C_q denote the final cluster cost of refined candidate τ~q τ_q. Following the standard probabilistic interpretation of ROC-AUC [6], cost AUC measures how often an unsafe candidate is ranked above a safe candidate. Since higher costs correspond to less desirable trajectories, it estimates Pr(C~u>C~r)+12Pr(C~u=C~r) ( C_u> C_r)+ 12 ( C_u= C_r), where u∼su _s and r∼ℛsr _s. For scenes containing at least one safe and one unsafe candidate, we compute this empirical AUC using the following ranking form: AUCs=1|s||ℛs|∑u∈s∑r∈ℛs[C~u>C~r+12 1C~u=C~r].AUC_s= 1|U_s||R_s| _u _s _r _s [ 1_\ C_u> C_r\+ 12\, 1_\ C_u= C_r\ ]. The equality term follows the standard half-credit convention for ties in AUC; exact equality is typically rare for continuous learned costs, but the term makes the metric well-defined for bounded or numerically equal scores. Thus, an AUC of 0.50.5 indicates random ordering, while 1.01.0 indicates perfect separation. We also report the cost gap, Gaps=1|s|∑u∈sC~u−1|ℛs|∑r∈ℛsC~r.Gap_s= 1|U_s| _u _s C_u- 1|R_s| _r _s C_r. Larger positive gaps indicate that unsafe candidates receive substantially higher costs than safe candidates on average. The reported AUC and gap are averaged over scenes where both safe and unsafe candidates exist. Appendix 0.C Additional Experiments 0.C.1 Branching Time. Table 6 evaluates the branching time that separates worst-case from probability-weighted scene-cost aggregation. At Tb=0T_b=0, probability-weighted aggregation is used throughout, while at Tb=3T_b=3 s, worst-case aggregation covers the full planning horizon. Pure expectation can underweight low-probability, high-cost futures, whereas a long worst-case interval can favor conservative responses even when the corresponding scene mode is unlikely. The default Tb=1T_b=1 s achieves both the lowest average L2 error (0.710.71 m) and the lowest average collision rate (0.07%0.07\%) among the ablated branching times. Extending the worst-case interval to two or three seconds degrades both metrics, so more conservative cost aggregation does not automatically yield fewer collisions. These results support a short worst-case prefix for near-term decisions, followed by probability-weighted costs that distinguish the likelihood of later outcomes. Table 6: Ablation of Branching-time on nuScenes. TbT_b (s) L2 (m) ↓ Collision (%) ↓ 1s 2s 3s Avg. 1s 2s 3s Avg. 0 0.33 0.80 1.50 0.88 0.02 0.14 0.35 0.17 1 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 2 0.30 0.69 1.28 0.76 0.02 0.07 0.29 0.13 3 0.32 0.78 1.47 0.86 0.03 0.16 0.37 0.19 0.C.2 Plan Selection and Rescoring. Table 7 compares global lowest-cost selection, best-per-cluster selection, MPPI mixing, and MPPI mixing with learned rescoring of the fused trajectories. Global selection chooses the lowest-cost sample without preserving a candidate for each intention, while best-per-cluster selection maintains a representative sample from each cluster. MPPI instead uses the learned safety-aware costs to mix controls within each intention cluster and rolls out the fused controls with the kinematic model, allowing refinement beyond individual samples without averaging distinct maneuver families. Compared with global selection, MPPI reduces average L2 from 0.770.77 to 0.710.71 m and collision from 0.13%0.13\% to 0.07%0.07\%. Best-per-cluster selection achieves lower L2 (0.680.68 m), but its higher collision rate (0.11%0.11\%) highlights the separation between matching the expert trajectory and avoiding collisions. The mixing results therefore support using the learned cost topology for safety-aware refinement rather than only selecting fixed samples. The rescoring experiment tests whether evaluating the fused trajectory with the learned cost head improves selection over the cluster-desirability proxy. The proxy combines sample costs using the fusion weights, whereas rescoring directly evaluates the resulting trajectory. Rescoring increases average L2 from 0.710.71 to 0.730.73 m and average collision from 0.07%0.07\% to 0.15%0.15\% relative to the default. Empirical analysis shows that the cluster-desirability proxy is strongly correlated with the learned cost evaluated directly on the mixed trajectory, with Pearson and Spearman correlation coefficients of 0.9210.921 and 0.9230.923, respectively. Table 7: Ablation of Plan selection and rescoring on nuScenes. Variant L2 (m) ↓ Collision (%) ↓ 1s 2s 3s Avg. 1s 2s 3s Avg. Global selection 0.29 0.70 1.30 0.77 0.02 0.07 0.31 0.13 Best per cluster 0.26 0.63 1.14 0.68 0.01 0.05 0.28 0.11 MPPI 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 MPPI + rescoring 0.28 0.67 1.22 0.73 0.01 0.12 0.31 0.15 0.C.3 Planning Loss Components. Table 8 ablates the planner-specific losses while keeping the same reachable-query architecture. The safety margin corresponds to ℒrankL_rank, while candidate classification and candidate regression correspond to ℒclsL_cls and ℒregL_reg of the training objective ℒplanL_plan. The safety-margin ranking loss alone already learns a meaningful safe-versus-unsafe ordering, as reflected by a cost AUC of 0.81 and a cost gap of 36.17, but the final plan remains limited by weak cluster-level supervision. Adding candidate classification improves displacement error by encouraging the planner to select a more expert-like intention cluster, although this alone does not improve collision. Adding candidate regression reduces collision relative to the safety-margin-only variant, but without the classification loss, it does not reliably select the best cluster and therefore hurts displacement. Using all three losses gives the best overall behavior: the ranking loss shapes the cost topology, classification improves inter-cluster selection, and regression refines the selected candidate geometry. Table 8: Ablation of planning loss components on nuScenes. Variant L2 (m) ↓ Collision (%) ↓ Cost AUC ↑ Cost Gap ↑ 1s 2s 3s Avg. 1s 2s 3s Avg. Safety margin 0.35 0.89 1.72 0.99 0.00 0.151 0.41 0.19 0.81 36.17 + candidate classification 0.34 0.82 1.54 0.90 0.04 0.19 0.44 0.22 0.81 38.64 + candidate regression 0.38 1.00 1.91 1.10 0.04 0.11 0.31 0.15 0.82 35.76 + classification + regression 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 0.86 45.70 0.C.4 Hard-Negative Aggregation. Table 9 studies two choices in the safety-ranking loss: the collision signal used to define the margin, and how hard negatives are aggregated. The max variant backpropagates only through the single largest ranking violation in each scene, while the Top-k variant averages the largest khk_h violations. Thus, max aggregation focuses on the hardest negative but can produce sparse gradients, whereas Top-k spreads supervision over multiple unsafe or near-unsafe candidates. For ground-truth collision supervision, Top-k aggregation consistently improves the learned cost topology and final planning, reducing L2 from 0.76 to 0.69 and increasing the cost gap from 41.67 to 46.66. A similar trend appears for contingency collision risk, where Top-k aggregation reduces the average collision rate from 0.13% to 0.07% and improves both cost AUC and cost gap. This suggests that when the collision signal is reliable or contingency-aware, spreading gradients across several hard negatives yields a smoother and more useful cost topology than optimizing only the worst single violation. The top-1 predicted collision signal behaves differently: max aggregation yields a lower collision rate than Top-k, likely because a single predicted future provides a narrower, noisier safety target. In that setting, averaging many hard negatives can overemphasize candidates that are unsafe only under the most likely predicted future, rather than under the broader multimodal risk. Overall, the best result is obtained by combining contingency collision risk with Top-k aggregation, supporting our default choice of supervising the ranking loss with multiple informative negatives under multimodal scene risk. Table 9: Ablation of hard-negative aggregation for safety-margin training on nuScenes. Collision signal Negative agg. L2 (m) ↓ Collision (%) ↓ Cost AUC ↑ Cost Gap ↑ 1s 2s 3s Avg. 1s 2s 3s Avg. Ground-truth collision Max 0.29 0.69 1.30 0.76 0.01 0.06 0.27 0.11 0.85 41.67 Ground-truth collision Top-k 0.27 0.63 1.16 0.69 0.00 0.06 0.25 0.10 0.86 46.66 Top-1 predicted collision Max 0.27 0.62 1.16 0.68 0.00 0.04 0.19 0.08 0.88 42.56 Top-1 predicted collision Top-k 0.27 0.61 1.12 0.67 0.04 0.12 0.23 0.13 0.87 45.71 Contingency collision risk Max 0.28 0.66 1.23 0.72 0.01 0.10 0.28 0.13 0.84 41.22 Contingency collision risk Top-k 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 0.86 45.70 0.C.5 Cost Profile for Cluster Mixing. Table 10 evaluates which learned cost profile is used to compute the MPPI-style cluster weights. The mean trajectory cost first averages the cost over the horizon and uses a single score for each sampled trajectory, giving the refinement a trajectory-level preference. The per-timestep cost instead computes weights independently at each future step, which is more local but can combine controls from different samples at different timesteps. The two variants obtain the same cost AUC, indicating a similar safe-versus-unsafe ordering. The difference appears in how this topology is used for refinement: the mean trajectory cost reduces the average L2 from 0.77 to 0.71 and the average collision from 0.14% to 0.07%. This suggests that intra-cluster mixing benefits from a temporally consistent trajectory-level preference. Per-timestep weighting is more flexible, but in a finite sampled cluster, it can assemble a control sequence from samples that are locally low-cost at different times but do not form a coherent maneuver. We therefore use the mean trajectory cost as the default profile for cost-guided intra-cluster mixing. Table 10: Ablation of cost profiles for cost-guided cluster mixing on nuScenes. Mixing score L2 (m) ↓ Collision (%) ↓ Cost AUC ↑ Cost Gap ↑ 1s 2s 3s Avg. 1s 2s 3s Avg. Mean trajectory cost 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 0.86 45.70 Per-timestep cost 0.30 0.71 1.32 0.77 0.02 0.10 0.30 0.14 0.86 42.99 0.C.6 Scene-Cost Aggregation. Table 11 ablates how the scene-mode costs are aggregated before cluster mixing. Top-1 aggregation uses only the most likely scene mode, worst-case aggregation takes the maximum over scene modes, mean aggregation treats all modes uniformly, and probability-weighted aggregation follows the predicted scene probabilities. The contingency strategy combines these views temporally: it uses worst-case costs before the branching time and probability-weighted costs afterward. The aggregation strategy has a clear effect on both planning accuracy and safety. Top-1 and probability-weighted aggregation commit strongly to likely scene modes and can therefore underemphasize low-probability but safety-critical futures. Worst-case aggregation avoids this optimism, but applying the maximum over all scene modes for the full horizon can be overly conservative and does not improve collision in this setting. Uniform mean aggregation provides a moderate compromise, reducing collision relative to Top-1 and worst-case aggregation, but it ignores the different roles of early and late planning timesteps. The contingency aggregation performs best across all metrics. By using worst-case costs before the branching time, it keeps early ego actions safe under multiple plausible futures. After the branching time, probability-weighted aggregation allows the planner to account for the likelihood of each scene mode. This temporal structure improves average L2 from 0.85–0.88 to 0.71 and reduces collision from 0.13–0.19% to 0.07%, while producing the strongest cost separation. Table 11: Ablation of scene-cost aggregation on nuScenes. Aggregation L2 (m) ↓ Collision (%) ↓ Cost AUC ↑ Cost Gap ↑ 1s 2s 3s Avg. 1s 2s 3s Avg. Top-1 0.33 0.79 1.50 0.87 0.03 0.17 0.37 0.19 0.81 33.99 Worst Case 0.32 0.78 1.47 0.86 0.03 0.16 0.37 0.19 0.81 37.81 Mean 0.32 0.77 1.45 0.85 0.00 0.09 0.30 0.13 0.82 38.97 Probability-weighted 0.33 0.80 1.50 0.88 0.02 0.14 0.35 0.17 0.81 37.97 Contingency 0.28 0.65 1.21 0.71 0.00 0.02 0.18 0.07 0.86 45.70 Appendix 0.D Additional Qualitative Results 0.D.1 nuScenes Examples. Figure 6 shows representative scenes covering car following, yielding, lane-change, and overtaking behavior from nuScenes. These examples complement the quantitative results by illustrating how the reachable-query planner evaluates a diverse set of ego futures in interactive traffic scenes and selects low-cost candidates that remain consistent with the local road geometry and surrounding agents. (a) Following a leading vehicle (b) Yielding to oncoming traffic (c) Lane change Maneuver (d) Overtaking Maneuver Figure 6: Qualitative nuScenes examples. The camera views and BEV visualizations show representative interactive scenes in which the learned cost topology guides selection among reachable ego candidates. 0.D.2 Zero-shot Real-World Examples Figure 7 shows representative zero-shot transfer examples on our real-world driving logs, covering stop-and-go traffic, interaction with vulnerable road users, intersection turning, and highway following. These examples complement the quantitative real-world evaluation by illustrating that the learned cost topology remains meaningful under a different sensor setup, camera domain, and road layout. Each row shows the surround camera inputs, the BEV cost visualization over top-k reachable ego candidates, and the ground-truth ego trajectory together with semantic map support. In the middle BEV panel, the selected final trajectory candidate is highlighted in dark green with a black outline. (a) Reacting to a stopping lead vehicle (b) Reacting to a pedestrian (c) Turning left at an intersection (d) Stop-and-go traffic on a highway Figure 7: Zero-shot qualitative examples on real-world driving logs. Each row shows surround-view camera predictions, the BEV cost visualization with the selected trajectory, and ground-truth boxes with ego trajectory and semantic map support.