GoalFlow：四、轨迹评分筛选模块（Trajectory Scorer, M3）

GoalFlow 核心算法组件 6.4轨迹评分筛选模块Trajectory Scorer, M3概述轨迹评分筛选模块Trajectory Scorer, M3是 GoalFlow 系统中负责从多条候选轨迹中选择最优轨迹的核心组件。该模块在推理阶段接收 M0基础流匹配模块生成的多条候选轨迹结合 M1目标点分解模块提供的导航目标点通过距离评分和进度评分的综合考量筛选出最优轨迹作为最终输出。核心目标从多条候选轨迹中筛选出最符合导航目标的轨迹平衡轨迹的导航准确性和前进进度提升轨迹预测的鲁棒性和安全性二、模块架构与核心组件2.1 模块位置与调用时机轨迹评分筛选模块位于GoalFlowTrajModel的推理阶段在 M0 生成多条候选轨迹之后执行# trajectory scorer ifself._config.use_nearestandnotself._config.fusion:distancestorch.norm(pred_trajs[:,:,8,:2]-navi,dim-1)scoresdistancesifself._config.ep_score_weight0.0:distances_normminmax(distances)progresstorch.norm(pred_trajs[:,:,8,:2],dim-1)progress_normminmax(progress)scores(1.-self._config.ep_score_weight)*distances_norm-self._config.ep_score_weight*progress_norm min_indextorch.argmin(scores,dim1)pred_trajspred_trajs[torch.arange(batch_size),min_index].unsqueeze(1)文件位置:navsim/agents/goalflow/goalflow_model_traj.py:3952.2 输入输出输入类型说明pred_trajs(B, anchor_size, num_poses, 3)M0 生成的多条候选轨迹navi(B, 1, 1, 2)M1 提供的导航目标点use_nearestbool是否启用轨迹评分筛选ep_score_weightfloat进度评分权重输出类型说明pred_trajs(B, 1, num_poses, 3)筛选后的最优轨迹2.3 核心数据结构# 候选轨迹张量结构pred_trajs:torch.Tensor# Shape: (batch_size, anchor_size, num_poses, 3)# - batch_size: 批次大小# - anchor_size: 每个样本生成的轨迹数量默认128# - num_poses: 轨迹点数默认12包含起始点# - 3: x, y, heading# 导航目标点navi:torch.Tensor# Shape: (batch_size, 1, 1, 2)# - 2: x, y 坐标三、轨迹评分算法3.1 基础距离评分基础评分策略是计算每条候选轨迹的终点与导航目标点之间的欧几里得距离# 提取轨迹第8个时间步的终点位置对应5.5s时间跨度中的关键预测点trajectory_endpointspred_trajs[:,:,8,:2]# (B, anchor_size, 2)# 计算与导航目标点的距离distancestorch.norm(trajectory_endpoints-navi,dim-1)# (B, anchor_size)原理说明选择第8个时间步约4.0s处作为评估点而非最后一个时间步这是因为中期预测更能反映轨迹的整体走向避免末端噪声对评分的影响与 M1 选择导航目标点的时间尺度一致3.2 综合评分距离 进度当ep_score_weight 0时系统采用综合评分策略同时考虑距离和进度defminmax(t_tensor):归一化函数将张量缩放到 [0, 1] 范围batch_mintorch.min(t_tensor,dim-1).values.unsqueeze(-1)batch_maxtorch.max(t_tensor,dim-1).values.unsqueeze(-1)return(t_tensor-batch_min)/(batch_max-batch_min)# 距离评分归一化distances_normminmax(distances)# (B, anchor_size)# 进度评分轨迹终点到原点的距离归一化progresstorch.norm(pred_trajs[:,:,8,:2],dim-1)# (B, anchor_size)progress_normminmax(progress)# (B, anchor_size)# 综合评分公式scores(1.-ep_score_weight)*distances_norm-ep_score_weight*progress_norm评分公式score(1−w)×distance_norm−w×progress_norm\text{score} (1 - w) \times \text{distance\_norm} - w \times \text{progress\_norm}score(1−w)×distance_norm−w×progress_norm其中wwwep_score_weight进度权重范围 [0, 1]distance_norm\text{distance\_norm}distance_norm 轨迹终点到导航目标点的归一化距离越小越好progress_norm\text{progress\_norm}progress_norm 轨迹终点到原点的归一化距离越大越好设计意图通过减去进度评分鼓励选择前进距离更远的轨迹距离评分确保轨迹朝向导航目标点进度评分确保车辆持续前进避免原地徘徊3.3 最优轨迹选择根据评分结果选择最优轨迹# 选择评分最小的轨迹距离最小或综合评分最优min_indextorch.argmin(scores,dim1)# (B,)# 从候选轨迹中提取最优轨迹pred_trajspred_trajs[torch.arange(batch_size),min_index].unsqueeze(1)# Shape: (B, 1, num_poses, 3)四、融合模式下的轨迹评分4.1 双轨迹生成机制在fusionTrue模式下系统生成两组轨迹条件轨迹Conditional Trajectory使用导航目标点作为条件无条件轨迹Non-Conditional Trajectory不使用导航目标点navi_dropoutTrue# 生成条件轨迹fort_curr,t_previnzip(t_shifted[:-1],t_shifted[1:]):net_output_condself.denoise(trajs,t_curr,global_feature)trajstrajs.detach().clone()net_output_cond*(step/self._config.infer_steps)diffusion_output_condself.denormalize_xy_rotation(trajs,Ngt_trajs.shape[-2],times10)# 生成无条件轨迹fort_curr,t_previnzip(t_shifted[:-1],t_shifted[1:]):net_output_nonaviself.denoise(trajs,t_curr,global_feature,navi_dropoutTrue)trajstrajs.detach().clone()net_output_nonavi*(step/self._config.infer_steps)diffusion_output_nonaviself.denormalize_xy_rotation(trajs,Ngt_trajs.shape[-2],times10)文件位置:navsim/agents/goalflow/goalflow_model_traj.py:3064.2 条件轨迹的评分筛选在融合模式下条件轨迹也需要进行评分筛选ifself._config.use_nearest:# 计算条件轨迹终点与导航目标点的距离distancestorch.norm(diffusion_output_cond[:,:,8,:2]-navi,dim-1)scoresdistancesifself._config.ep_score_weight0.0:distances_normminmax(distances)progresstorch.norm(diffusion_output_cond[:,:,8,:2],dim-1)progress_normminmax(progress)scores(1.-self._config.ep_score_weight)*distances_norm-self._config.ep_score_weight*progress_norm# 选择最优条件轨迹min_indextorch.argmin(scores,dim1)pred_trajs_conddiffusion_output_cond[torch.arange(batch_size),min_index].unsqueeze(1)else:# 直接取平均pred_trajs_condtorch.mean(diffusion_output_cond,dim1,keepdimTrue)文件位置:navsim/agents/goalflow/goalflow_model_traj.py:3184.3 双轨迹融合策略4.3.1 阈值融合当cond_threshold 0时根据无条件轨迹与导航目标点的偏离程度决定是否使用条件轨迹ifself._config.cond_threshold0.0:# 计算无条件轨迹终点与导航目标点的距离distancetorch.norm(pred_trajs_nonavi[:,:,8,:2]-navi,dim-1).squeeze()# 计算无条件轨迹的前进进度progresstorch.norm(pred_trajs_nonavi[:,:,8,:2],dim-1).squeeze()# 计算偏离比率deviation_ratiodistance/progress# 构建条件掩码偏离过大时使用无条件轨迹cond_mask(deviation_ratioself._config.cond_threshold)cond_maskcond_mask.view(-1,1,1,1)# 根据掩码选择轨迹pred_trajstorch.where(cond_mask,pred_trajs_nonavi,pred_trajs_cond)设计意图当无条件轨迹严重偏离导航目标时切换到条件轨迹否则使用无条件轨迹保持轨迹的多样性和鲁棒性4.3.2 加权融合当cond_threshold 0时采用加权平均融合pred_trajsself._config.beta*pred_trajs_cond(1.0-self._config.beta)*pred_trajs_nonavi其中beta是条件轨迹的权重系数范围 [0, 1]。五、导航目标点的 Top-k 选择5.1 目标点评分融合在 M0 训练阶段如果使用has_student_naviTrue系统会加载 M1 生成的评分文件并融合 IM 分数和 DAC 分数# 加载 IM 分数和 DAC 分数im_scorestorch.from_numpy(np.load(f{self._config.score_path}/im/{token[i]}.npy))dac_scorestorch.from_numpy(np.load(f{self._config.score_path}/dac/{token[i]}.npy))# 计算最终评分ifself._config.ep_point_weight0.0:# 加入距离惩罚goal_distancetorch.norm(cluster_points_tensor[...,:2],dim-1)goal_distance_normminmax(goal_distance)final_scores(0.1*torch.log(F.softmax(im_scores.squeeze(),dim-1)1e-7)self._config.theta*torch.log(torch.sigmoid(dac_scores.squeeze())1e-7)self._config.ep_point_weight*goal_distance_norm)else:final_scores(0.1*torch.log(F.softmax(im_scores.squeeze(),dim-1)1e-7)self._config.theta*torch.log(torch.sigmoid(dac_scores.squeeze())1e-7))文件位置:navsim/agents/goalflow/goalflow_model_traj.py:1985.2 Top-k 目标点选择# 选择 Top-k 评分最高的目标点topk_indicestorch.topk(final_scores,self._config.topk).indices topk_indicestopk_indices.unsqueeze(-1).expand(-1,-1,cluster_points_tensor.shape[-1])# 提取对应的目标点坐标navitorch.gather(cluster_points_tensor,dim1,indextopk_indices).mean(1)[...,:2].unsqueeze(1)说明topk参数决定选择多少个导航目标点多个目标点取平均作为最终导航目标这增加了导航目标的鲁棒性六、核心算法举例说明6.1 场景描述假设车辆在一条直道上行驶导航目标点为正前方 50m 处。M0 生成了 128 条候选轨迹其中大部分朝向目标点但存在一些异常轨迹轨迹 A准确朝向目标点终点距离目标点 2m轨迹 B略微偏右终点距离目标点 8m轨迹 C严重偏离终点距离目标点 25m轨迹 D朝向目标点但前进距离较短终点距离目标点 3m6.2 基础距离评分# 假设导航目标点 navi (50, 0)# 各轨迹第8步的终点位置# A: (48, 0), B: (50, 8), C: (40, 15), D: (30, 0)# 计算距离distancestorch.norm(pred_trajs[:,:,8,:2]-navi,dim-1)# distances [2.0, 8.0, 25.0, 20.0]# 选择距离最小的轨迹min_indextorch.argmin(distances)# index 0 (轨迹 A)评分结果轨迹终点位置距离目标点评分排名A(48, 0)2.0m2.01B(50, 8)8.0m8.02D(30, 0)20.0m20.03C(40, 15)25.0m25.04选择结果轨迹 A6.3 综合评分距离 进度假设ep_score_weight 0.3# 计算进度到原点的距离progresstorch.norm(pred_trajs[:,:,8,:2],dim-1)# progress [48.0, 50.79, 42.72, 30.0]# 归一化distances_normminmax(distances)# [0.0, 0.26, 1.0, 0.78]progress_normminmax(progress)# [0.65, 1.0, 0.48, 0.0]# 综合评分scores(1-0.3)*distances_norm-0.3*progress_norm# scores [0.0*0.7 - 0.65*0.3 -0.195]# [0.26*0.7 - 1.0*0.3 -0.118]# [1.0*0.7 - 0.48*0.3 0.556]# [0.78*0.7 - 0.0*0.3 0.546]评分结果轨迹距离评分进度评分综合评分排名B0.261.0-0.1181A0.00.65-0.1952D0.780.00.5463C1.00.480.5564选择结果轨迹 B虽然距离稍远但前进进度最大6.4 融合模式示例假设fusionTrue,beta0.6,cond_threshold0.5# 条件轨迹终点: (48, 0), 无条件轨迹终点: (45, 2)# 计算偏离比率distancetorch.norm(pred_trajs_nonavi[:,:,8,:2]-navi,dim-1)# 5.39progresstorch.norm(pred_trajs_nonavi[:,:,8,:2],dim-1)# 45.09deviation_ratiodistance/progress# 0.12# 判断是否超过阈值ifdeviation_ratio0.5:# 使用无条件轨迹pred_trajspred_trajs_nonavielse:# 使用加权融合pred_trajs0.6*pred_trajs_cond0.4*pred_trajs_nonavi# pred_trajs[8, :2] 0.6*(48,0) 0.4*(45,2) (46.8, 0.8)融合结果由于偏离比率0.12小于阈值0.5采用加权融合最终轨迹终点为 (46.8, 0.8)。七、与其他模块的协作7.1 模块协作关系图┌─────────────────────────────────────────────────────────────────────────────┐ │ GoalFlow 模块协作流程 │ ├─────────────────────────────────────────────────────────────────────────────┤ │ │ │ M1 (目标点分解) ───────────────────────────────────────────────────────► │ │ │ │ │ ▼ │ │ 导航目标点 (navi) ─────────────────────────────────────────────────────► │ │ │ │ │ │ M0 (基础流匹配) │ │ │ │ │ │ │ ▼ │ │ │ 多条候选轨迹 (anchor_size128) │ │ │ │ │ │ └─────────────────────────────────────────┼────────────────────────► │ │ ▼ │ │ M3 (轨迹评分筛选) │ │ │ │ │ ▼ │ │ 最优轨迹 (1条) │ │ │ │ │ ▼ │ │ 输出到控制器 │ │ │ └─────────────────────────────────────────────────────────────────────────────┘7.2 与 M0 的协作M0 生成anchor_size条候选轨迹默认128条M3 从这些候选轨迹中筛选出最优轨迹筛选结果反馈给 M0 的训练过程通过导航丢弃增强7.3 与 M1 的协作M1 提供导航目标点作为评分基准M1 的评分文件IM分数、DAC分数用于训练阶段的目标点选择M3 的评分结果可以间接验证 M1 的导航目标点质量八、关键配置参数8.1 轨迹评分相关参数dataclassclassGoalFlowConfig:# 轨迹评分参数use_nearest:boolFalse# 是否使用最近轨迹选择ep_score_weight:float0.0# 进度评分权重0~1ep_point_weight:float0.0# 导航点距离权重# 轨迹生成参数anchor_size:int128# 每个样本生成的轨迹数量infer_steps:int100# 扩散采样步数# 融合参数fusion:boolFalse# 是否启用双轨迹融合beta:float0.0# 条件轨迹权重cond_threshold:float0.0# 融合阈值# 导航目标点参数topk:int1# Top-k 目标点选择theta:float3.0# DAC 分数权重# 归一化参数train_scale:float0.1# 训练时噪声尺度test_scale:float0.1# 测试时噪声尺度文件位置:navsim/agents/goalflow/goalflow_config.py8.2 参数说明参数类型默认值说明use_nearestboolFalse是否启用轨迹评分筛选设为 True 时从候选轨迹中选择最优ep_score_weightfloat0.0进度评分权重越大越倾向于选择前进距离远的轨迹ep_point_weightfloat0.0导航点距离权重用于目标点选择时的距离惩罚anchor_sizeint128扩散采样生成的轨迹数量越大多样性越好但计算开销越大fusionboolFalse是否启用条件/无条件轨迹融合betafloat0.0融合时条件轨迹的权重cond_thresholdfloat0.0融合阈值大于0时启用阈值判断融合策略topkint1导航目标点的 Top-k 选择数量thetafloat3.0DAC 分数在目标点选择时的权重九、训练阶段的导航丢弃增强9.1 增强策略在 M0 训练阶段为了增强模型的鲁棒性系统采用导航丢弃navi_dropout策略ifself._config.has_navi:flagrandom.randint(1,3)ifflag1:# 正常训练predself.denoise(noisy_traj_points,timesteps,global_feature).reshape(batch_size,-1,30)elifflag2:# 强制丢弃场景特征置零predself.denoise(noisy_traj_points,timesteps,global_feature,force_dropoutTrue).reshape(batch_size,-1,30)elifflag3:# 导航丢弃导航特征置零predself.denoise(noisy_traj_points,timesteps,global_feature,navi_dropoutTrue).reshape(batch_size,-1,30)文件位置:navsim/agents/goalflow/goalflow_model_traj.py:2739.2 丢弃实现defdenoise(self,ego_trajectory,sigma,state_features,force_dropoutFalse,navi_dropoutFalse):state_features,state_type_embeddingstate_features# 导航丢弃将导航特征置零ifnavi_dropout:state_features[:,-1,:]*0# 强制丢弃将所有场景特征置零ifforce_dropout:all_featuresall_features*0文件位置:navsim/agents/goalflow/goalflow_model_traj.py:444增强效果正常训练模型学习在有导航目标时生成准确轨迹强制丢弃模型学习在没有场景信息时的鲁棒性导航丢弃模型学习在没有导航目标时的自主决策能力十、数据流程总结10.1 推理阶段流程传感器输入相机 LiDAR 状态 │ ▼ M0 扩散采样生成多条候选轨迹 (B, anchor_size, num_poses, 3) │ ▼ 加载 M1 评分文件IM分数、DAC分数 │ ▼ 选择导航目标点 (navi) │ ▼ ┌─────────────────────────────────────┐ │ M3 轨迹评分筛选 │ │ │ │ 1. 计算距离评分: │ │ distances norm(endpoint - navi)│ │ │ │ 2. 计算进度评分可选: │ │ progress norm(endpoint) │ │ │ │ 3. 综合评分: │ │ scores (1-w)*dist_norm - w*prog_norm│ │ │ │ 4. 选择最优轨迹: │ │ min_index argmin(scores) │ │ pred_trajs pred_trajs[min_index]│ └─────────────────────────────────────┘ │ ▼ 输出最优轨迹10.2 融合模式流程M0 生成条件轨迹 (pred_trajs_cond) │ ▼ M3 评分筛选条件轨迹 │ ▼ M0 生成无条件轨迹 (pred_trajs_nonavi) │ ▼ 计算偏离比率 (distance/progress) │ ├── 偏离 阈值 → 使用无条件轨迹 │ └── 偏离 ≤ 阈值 → 加权融合 pred_trajs beta * pred_trajs_cond (1-beta) * pred_trajs_nonavi │ ▼ 输出融合后的最优轨迹十一、技术特点11.1 多轨迹候选策略通过扩散模型生成多条候选轨迹anchor_size128增加了轨迹预测的多样性和鲁棒性避免单一轨迹可能的错误。11.2 综合评分机制同时考虑距离准确性和前进进度平衡了导航精度和行驶效率避免车辆过度追求目标点而忽视前进。11.3 双轨迹融合通过条件/无条件轨迹的融合结合了导航引导的精确性和自主决策的鲁棒性提升了复杂场景下的适应性。11.4 导航丢弃增强在训练阶段随机丢弃导航特征增强了模型在导航信号不可靠时的自主决策能力。11.5 可配置的评分策略通过ep_score_weight、fusion、beta等参数可以灵活调整评分策略适应不同场景需求。十二、核心代码清单文件路径核心功能navsim/agents/goalflow/goalflow_model_traj.pyGoalFlowTrajModel轨迹评分筛选的核心实现navsim/agents/goalflow/goalflow_config.pyGoalFlowConfig配置参数定义navsim/agents/goalflow/goalflow_model_navi.pyGoalFlowNaviModel生成 IM/DAC 评分供 M3 使用navsim/planning/script/config/common/agent/goalflow_agent_traj.yaml轨迹模型的配置文件