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:distances=torch.norm(pred_trajs[:,:,8,:2]-navi,dim=-1)scores=distancesifself._config.ep_score_weight>0.0:distances_norm=minmax(distances)progress=torch.norm(pred_trajs[:,:,8,:2],dim=-1)progress_norm=minmax(progress)scores=(1.-self._config.ep_score_weight)*distances_norm-self._config.ep_score_weight*progress_norm min_index=torch.argmin(scores,dim=1)pred_trajs=pred_trajs[torch.arange(batch_size),min_index].unsqueeze(1)

文件位置:navsim/agents/goalflow/goalflow_model_traj.py:395

2.2 输入输出

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_endpoints=pred_trajs[:,:,8,:2]# (B, anchor_size, 2)# 计算与导航目标点的距离distances=torch.norm(trajectory_endpoints-navi,dim=-1)# (B, anchor_size)

原理说明：

• 选择第8个时间步（约4.0s处）作为评估点，而非最后一个时间步
• 这是因为：
  1. 中期预测更能反映轨迹的整体走向
  2. 避免末端噪声对评分的影响
  3. 与 M1 选择导航目标点的时间尺度一致

3.2 综合评分（距离 + 进度）

当ep_score_weight > 0时，系统采用综合评分策略，同时考虑距离和进度：

defminmax(t_tensor):"""归一化函数：将张量缩放到 [0, 1] 范围"""batch_min=torch.min(t_tensor,dim=-1).values.unsqueeze(-1)batch_max=torch.max(t_tensor,dim=-1).values.unsqueeze(-1)return(t_tensor-batch_min)/(batch_max-batch_min)# 距离评分（归一化）distances_norm=minmax(distances)# (B, anchor_size)# 进度评分：轨迹终点到原点的距离（归一化）progress=torch.norm(pred_trajs[:,:,8,:2],dim=-1)# (B, anchor_size)progress_norm=minmax(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

其中：

• www=ep_score_weight（进度权重，范围 [0, 1]）
• distance_norm\text{distance\_norm}distance_norm= 轨迹终点到导航目标点的归一化距离（越小越好）
• progress_norm\text{progress\_norm}progress_norm= 轨迹终点到原点的归一化距离（越大越好）

设计意图：

• 通过减去进度评分，鼓励选择前进距离更远的轨迹
• 距离评分确保轨迹朝向导航目标点
• 进度评分确保车辆持续前进，避免原地徘徊

3.3 最优轨迹选择

根据评分结果选择最优轨迹：

# 选择评分最小的轨迹（距离最小或综合评分最优）min_index=torch.argmin(scores,dim=1)# (B,)# 从候选轨迹中提取最优轨迹pred_trajs=pred_trajs[torch.arange(batch_size),min_index].unsqueeze(1)# Shape: (B, 1, num_poses, 3)

四、融合模式下的轨迹评分

4.1 双轨迹生成机制

在fusion=True模式下，系统生成两组轨迹：

1. 条件轨迹（Conditional Trajectory）：使用导航目标点作为条件
2. 无条件轨迹（Non-Conditional Trajectory）：不使用导航目标点（navi_dropout=True）

# 生成条件轨迹fort_curr,t_previnzip(t_shifted[:-1],t_shifted[1:]):net_output_cond=self.denoise(trajs,t_curr,global_feature)trajs=trajs.detach().clone()+net_output_cond*(step/self._config.infer_steps)diffusion_output_cond=self.denormalize_xy_rotation(trajs,N=gt_trajs.shape[-2],times=10)# 生成无条件轨迹fort_curr,t_previnzip(t_shifted[:-1],t_shifted[1:]):net_output_nonavi=self.denoise(trajs,t_curr,global_feature,navi_dropout=True)trajs=trajs.detach().clone()+net_output_nonavi*(step/self._config.infer_steps)diffusion_output_nonavi=self.denormalize_xy_rotation(trajs,N=gt_trajs.shape[-2],times=10)

文件位置:navsim/agents/goalflow/goalflow_model_traj.py:306

4.2 条件轨迹的评分筛选

在融合模式下，条件轨迹也需要进行评分筛选：

ifself._config.use_nearest:# 计算条件轨迹终点与导航目标点的距离distances=torch.norm(diffusion_output_cond[:,:,8,:2]-navi,dim=-1)scores=distancesifself._config.ep_score_weight>0.0:distances_norm=minmax(distances)progress=torch.norm(diffusion_output_cond[:,:,8,:2],dim=-1)progress_norm=minmax(progress)scores=(1.-self._config.ep_score_weight)*distances_norm-self._config.ep_score_weight*progress_norm# 选择最优条件轨迹min_index=torch.argmin(scores,dim=1)pred_trajs_cond=diffusion_output_cond[torch.arange(batch_size),min_index].unsqueeze(1)else:# 直接取平均pred_trajs_cond=torch.mean(diffusion_output_cond,dim=1,keepdim=True)

文件位置:navsim/agents/goalflow/goalflow_model_traj.py:318

4.3 双轨迹融合策略

4.3.1 阈值融合

当cond_threshold > 0时，根据无条件轨迹与导航目标点的偏离程度决定是否使用条件轨迹：

ifself._config.cond_threshold>0.0:# 计算无条件轨迹终点与导航目标点的距离distance=torch.norm(pred_trajs_nonavi[:,:,8,:2]-navi,dim=-1).squeeze()# 计算无条件轨迹的前进进度progress=torch.norm(pred_trajs_nonavi[:,:,8,:2],dim=-1).squeeze()# 计算偏离比率deviation_ratio=distance/progress# 构建条件掩码：偏离过大时使用无条件轨迹cond_mask=(deviation_ratio>self._config.cond_threshold)cond_mask=cond_mask.view(-1,1,1,1)# 根据掩码选择轨迹pred_trajs=torch.where(cond_mask,pred_trajs_nonavi,pred_trajs_cond)

设计意图：

• 当无条件轨迹严重偏离导航目标时，切换到条件轨迹
• 否则使用无条件轨迹，保持轨迹的多样性和鲁棒性

4.3.2 加权融合

当cond_threshold <= 0时，采用加权平均融合：

pred_trajs=self._config.beta*pred_trajs_cond+(1.0-self._config.beta)*pred_trajs_nonavi

其中beta是条件轨迹的权重系数，范围 [0, 1]。

五、导航目标点的 Top-k 选择

5.1 目标点评分融合

在 M0 训练阶段，如果使用has_student_navi=True，系统会加载 M1 生成的评分文件，并融合 IM 分数和 DAC 分数：

# 加载 IM 分数和 DAC 分数im_scores=torch.from_numpy(np.load(f'{self._config.score_path}/im/{token[i]}.npy'))dac_scores=torch.from_numpy(np.load(f'{self._config.score_path}/dac/{token[i]}.npy'))# 计算最终评分ifself._config.ep_point_weight>0.0:# 加入距离惩罚goal_distance=torch.norm(cluster_points_tensor[...,:2],dim=-1)goal_distance_norm=minmax(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:198

5.2 Top-k 目标点选择

# 选择 Top-k 评分最高的目标点topk_indices=torch.topk(final_scores,self._config.topk).indices topk_indices=topk_indices.unsqueeze(-1).expand(-1,-1,cluster_points_tensor.shape[-1])# 提取对应的目标点坐标navi=torch.gather(cluster_points_tensor,dim=1,index=topk_indices).mean(1)[...,:2].unsqueeze(1)

说明：

• topk参数决定选择多少个导航目标点
• 多个目标点取平均作为最终导航目标
• 这增加了导航目标的鲁棒性

六、核心算法举例说明

6.1 场景描述

假设车辆在一条直道上行驶，导航目标点为正前方 50m 处。M0 生成了 128 条候选轨迹，其中大部分朝向目标点，但存在一些异常轨迹：

• 轨迹 A：准确朝向目标点，终点距离目标点 2m
• 轨迹 B：略微偏右，终点距离目标点 8m
• 轨迹 C：严重偏离，终点距离目标点 25m
• 轨迹 D：朝向目标点但前进距离较短，终点距离目标点 3m

6.2 基础距离评分

# 假设导航目标点 navi = (50, 0)# 各轨迹第8步的终点位置：# A: (48, 0), B: (50, 8), C: (40, 15), D: (30, 0)# 计算距离distances=torch.norm(pred_trajs[:,:,8,:2]-navi,dim=-1)# distances = [2.0, 8.0, 25.0, 20.0]# 选择距离最小的轨迹min_index=torch.argmin(distances)# index = 0 (轨迹 A)

评分结果：

选择结果：轨迹 A

6.3 综合评分（距离 + 进度）

假设ep_score_weight = 0.3：

# 计算进度（到原点的距离）progress=torch.norm(pred_trajs[:,:,8,:2],dim=-1)# progress = [48.0, 50.79, 42.72, 30.0]# 归一化distances_norm=minmax(distances)# [0.0, 0.26, 1.0, 0.78]progress_norm=minmax(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]

评分结果：

选择结果：轨迹 B（虽然距离稍远，但前进进度最大）

6.4 融合模式示例

假设fusion=True,beta=0.6,cond_threshold=0.5：

# 条件轨迹终点: (48, 0), 无条件轨迹终点: (45, 2)# 计算偏离比率distance=torch.norm(pred_trajs_nonavi[:,:,8,:2]-navi,dim=-1)# = 5.39progress=torch.norm(pred_trajs_nonavi[:,:,8,:2],dim=-1)# = 45.09deviation_ratio=distance/progress# = 0.12# 判断是否超过阈值ifdeviation_ratio>0.5:# 使用无条件轨迹pred_trajs=pred_trajs_nonavielse:# 使用加权融合pred_trajs=0.6*pred_trajs_cond+0.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_size=128) │ │ │ │ │ │ └─────────────────────────────────────────┼────────────────────────► │ │ ▼ │ │ 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:bool=False# 是否使用最近轨迹选择ep_score_weight:float=0.0# 进度评分权重（0~1）ep_point_weight:float=0.0# 导航点距离权重# 轨迹生成参数anchor_size:int=128# 每个样本生成的轨迹数量infer_steps:int=100# 扩散采样步数# 融合参数fusion:bool=False# 是否启用双轨迹融合beta:float=0.0# 条件轨迹权重cond_threshold:float=0.0# 融合阈值# 导航目标点参数topk:int=1# Top-k 目标点选择theta:float=3.0# DAC 分数权重# 归一化参数train_scale:float=0.1# 训练时噪声尺度test_scale:float=0.1# 测试时噪声尺度

文件位置:navsim/agents/goalflow/goalflow_config.py

8.2 参数说明

九、训练阶段的导航丢弃增强

9.1 增强策略

在 M0 训练阶段，为了增强模型的鲁棒性，系统采用导航丢弃（navi_dropout）策略：

ifself._config.has_navi:flag=random.randint(1,3)ifflag==1:# 正常训练pred=self.denoise(noisy_traj_points,timesteps,global_feature).reshape(batch_size,-1,30)elifflag==2:# 强制丢弃（场景特征置零）pred=self.denoise(noisy_traj_points,timesteps,global_feature,force_dropout=True).reshape(batch_size,-1,30)elifflag==3:# 导航丢弃（导航特征置零）pred=self.denoise(noisy_traj_points,timesteps,global_feature,navi_dropout=True).reshape(batch_size,-1,30)

文件位置:navsim/agents/goalflow/goalflow_model_traj.py:273

9.2 丢弃实现

defdenoise(self,ego_trajectory,sigma,state_features,force_dropout=False,navi_dropout=False):state_features,state_type_embedding=state_features# 导航丢弃：将导航特征置零ifnavi_dropout:state_features[:,-1,:]*=0# 强制丢弃：将所有场景特征置零ifforce_dropout:all_features=all_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_size=128），增加了轨迹预测的多样性和鲁棒性，避免单一轨迹可能的错误。

11.2 综合评分机制

同时考虑距离准确性和前进进度，平衡了导航精度和行驶效率，避免车辆过度追求目标点而忽视前进。

11.3 双轨迹融合

通过条件/无条件轨迹的融合，结合了导航引导的精确性和自主决策的鲁棒性，提升了复杂场景下的适应性。

11.4 导航丢弃增强

在训练阶段随机丢弃导航特征，增强了模型在导航信号不可靠时的自主决策能力。

11.5 可配置的评分策略

通过ep_score_weight、fusion、beta等参数，可以灵活调整评分策略，适应不同场景需求。

十二、核心代码清单