GoalFlow 核心算法组件 6.3可行驶区域感知模块DAC Score, M2概述可行驶区域感知模块Drivable Area Compliance Score, DAC Score, M2是 GoalFlow 系统中负责可行驶区域合规性评估的核心组件。该模块通过 PDMPrediction-based Decision Making模拟器预先计算每个 VOCVector of Candidates候选目标点的可行驶性标签为 M1目标点分解模块提供监督信号确保导航目标点落在合法的可行驶区域内。核心目标构建高精度的可行驶区域地图评估轨迹终点是否在可行驶区域内为 M1 模块提供 DAC 监督标签确保自动驾驶车辆始终在合法道路上行驶二、可行驶区域地图构建PDMDrivableMap2.1 类定义与继承关系classPDMDrivableMap(PDMOccupancyMap):def__init__(self,tokens:List[str],map_types:List[SemanticMapLayer],geometries:npt.NDArray[np.object_],node_capacity:int10,):super().__init__(tokenstokens,geometriesgeometries,node_capacitynode_capacity)self._map_typesmap_types文件位置:navsim/planning/simulation/planner/pdm_planner/observation/pdm_occupancy_map.py:1012.2 语义地图层类型PDMDrivableMap 从 nuPlan 地图 API 中提取以下语义层语义层类型说明ROADBLOCK道路区块ROADBLOCK_CONNECTOR道路区块连接LANE车道LANE_CONNECTOR车道连接器INTERSECTION交叉路口CARPARK_AREA停车场区域2.3 地图构建流程classmethoddeffrom_simulation(cls,map_api:AbstractMap,ego_state:EgoState,map_radius:float50)-PDMDrivableMap:roadblock_layers[SemanticMapLayer.ROADBLOCK,SemanticMapLayer.ROADBLOCK_CONNECTOR]drivable_map_layers[SemanticMapLayer.INTERSECTION,SemanticMapLayer.CARPARK_AREA]position:Point2Dego_state.center.point drivable_areamap_api.get_proximal_map_objects(position,map_radius,roadblock_layersdrivable_map_layers)# 提取各类多边形polygons,polygon_tokens,polygon_types[],[],[]# 1. Roadblock Polygons# 2. Lane Lane-Connector Polygons (从 roadblock 的 interior_edges 提取)# 3. Other drivable area polygons (INTERSECTION, CARPARK_AREA)returnPDMDrivableMap(polygon_tokens,polygon_types,polygons)文件位置:navsim/planning/simulation/planner/pdm_planner/observation/pdm_occupancy_map.py:1362.4 空间查询方法2.4.1 points_in_polygons检查一组点是否在可行驶区域多边形内defpoints_in_polygons(self,points:npt.NDArray[np.float64])-npt.NDArray[np.bool_]:input_shapepoints.shape[:-1]flattened_pointspoints.reshape(-1,2)outputnp.zeros((len(self._geometries),len(flattened_points)),dtypebool)fori,polygoninenumerate(self._geometries):output[i]shapely.vectorized.contains(polygon,flattened_points[:,0],flattened_points[:,1])output_shape(len(self._geometries),)input_shapereturnoutput.reshape(output_shape)文件位置:navsim/planning/simulation/planner/pdm_planner/observation/pdm_occupancy_map.py:208输出维度:(num_polygons, num_proposals, num_horizon, num_points)2.4.2 is_in_layer检查单个点是否在指定语义层内defis_in_layer(self,point:Point2D,layer:SemanticMapLayer)-bool:polygons_indicesself._str_tree.query(Point(point.x,point.y),predicatewithin)polygons_types[self._map_types[polygon_idx]forpolygon_idxinpolygons_indices]returnlayerinpolygons_types文件位置:navsim/planning/simulation/planner/pdm_planner/observation/pdm_occupancy_map.py:228三、可行驶区域合规性评分PDMScorer3.1 DAC 评分专用方法PDMScorer 提供了score_dac方法专门用于计算可行驶区域合规性分数defscore_dac(self,states:npt.NDArray[np.float64],observation:PDMObservation,centerline:PDMPath,route_lane_ids:List[str],drivable_area_map:PDMDrivableMap,)-npt.NDArray[np.float64]:self._reset(states,observation,centerline,route_lane_ids,drivable_area_map)self._calculate_ego_area()self._calculate_drivable_area_compliance_lastpoints()returnself._multi_metrics[MultiMetricIndex.DRIVABLE_AREA]文件位置:navsim/planning/simulation/planner/pdm_planner/scoring/pdm_scorer.py:1683.2 车辆区域计算_calculate_ego_area方法计算车辆在每个时刻的区域状态def_calculate_ego_area(self)-None:n_proposals,n_horizon,n_points,_self._ego_coords.shape in_polygonsself._drivable_area_map.points_in_polygons(self._ego_coords)in_polygonsin_polygons.transpose(1,2,0,3)# 获取各类可行驶区域的索引drivable_area_idcsself._drivable_area_map.get_indices_of_map_type([SemanticMapLayer.ROADBLOCK,SemanticMapLayer.INTERSECTION,SemanticMapLayer.DRIVABLE_AREA,SemanticMapLayer.CARPARK_AREA,])drivable_lane_idcsself._drivable_area_map.get_indices_of_map_type([SemanticMapLayer.LANE,SemanticMapLayer.LANE_CONNECTOR])# 计算三种区域状态# 1. MULTIPLE_LANES: 车辆跨越多条车道# 2. NON_DRIVABLE_AREA: 车辆在不可行驶区域# 3. ONCOMING_TRAFFIC: 车辆在对向车道文件位置:navsim/planning/simulation/planner/pdm_planner/scoring/pdm_scorer.py:2933.3 终点可行驶区域合规性检查_calculate_drivable_area_compliance_lastpoints方法检查轨迹终点是否在可行驶区域内def_calculate_drivable_area_compliance_lastpoints(self)-None:drivable_area_compliance_scoresnp.ones(self._num_proposals,dtypenp.float64)# 检查轨迹最后一个时间点的 NON_DRIVABLE_AREA 状态off_road_maskself._ego_areas[:,-1,EgoAreaIndex.NON_DRIVABLE_AREA]drivable_area_compliance_scores[off_road_mask]0.0self._multi_metrics[MultiMetricIndex.DRIVABLE_AREA]drivable_area_compliance_scores文件位置:navsim/planning/simulation/planner/pdm_planner/scoring/pdm_scorer.py:442评分规则1.0轨迹终点在可行驶区域内0.0轨迹终点不在可行驶区域内四、DAC 分数预计算流程4.1 预计算脚本run_dac_score.py脚本用于批量预计算所有场景下 VOC 候选点的 DAC 标签defrun_pdm_score(args:List[Dict[str,Union[List[str],DictConfig]]])-List[Dict[str,Any]]:# 1. 加载 VOC 候选点cluster_pointsnp.load(cfg.agent.config.voc_path)# shape: (8192, 3)# 2. 创建轨迹采样配置traj_samplingTrajectorySampling(time_horizon4.0,interval_length4.0)# 3. 为每个 VOC 点创建单步轨迹traj_list[Trajectory(cluster_points[i,None,:],traj_sampling)foriinrange(len(cluster_points))]# 4. 遍历所有场景 tokenfortokenintokens_to_evaluate:# 加载 metric cachemetric_cachepickle.load(f)# 计算 DAC 分数drivable_area_compliancedac_score(metric_cachemetric_cache,model_trajectorytraj_list,future_samplingtraj_sampling_sim,simulatorsimulator,scorerscorer,)# 保存结果np.savez_compressed(gz_path,array1drivable_area_compliance)文件位置:navsim/planning/script/run_dac_score.py:814.2 DAC 分数计算函数dac_score函数在pdm_score.py中定义defdac_score(metric_cache:MetricCache,model_trajectory:List[Trajectory],future_sampling:TrajectorySampling,simulator:PDMSimulator,scorer:PDMScorer)-PDMResults:initial_ego_statemetric_cache.ego_state# 将多条轨迹转换为全局坐标系trajectory_list[transform_trajectory(model_trajectory[i],initial_ego_state)foriinrange(traj_num)]# 获取轨迹状态数组states_list[get_trajectory_as_array(trajectory_list[i],future_sampling,initial_ego_state.time_point)foriinrange(traj_num)]trajectory_statesnp.concatenate([states[None,...]forstatesinstates_list],axis0)# 计算 DAC 分数scoresscorer.score_dac(trajectory_states,metric_cache.observation,metric_cache.centerline,metric_cache.route_lane_ids,metric_cache.drivable_area_map,)returnscores文件位置:navsim/evaluate/pdm_score.py:1604.3 数据流向图┌─────────────────────────────────────────────────────────────────────────────┐ │ DAC 分数预计算流程 │ ├─────────────────────────────────────────────────────────────────────────────┤ │ │ │ VOC 文件 (.npy) │ │ │ │ │ ▼ │ │ cluster_points (8192, 3) │ │ │ │ │ ▼ │ │ [Trajectory] × 8192 ──────────────────────────────────────────────────► │ │ │ │ │ ▼ │ │ transform_trajectory() → 全局坐标系转换 │ │ │ │ │ ▼ │ │ get_trajectory_as_array() → 状态数组 (num_proposals, num_poses1, 9) │ │ │ │ │ ▼ │ │ scorer.score_dac() │ │ │ │ │ ├── _calculate_ego_area() → 计算车辆区域状态 │ │ │ │ │ └── _calculate_drivable_area_compliance_lastpoints() → DAC 分数 │ │ │ │ ▼ │ │ drivable_area_compliance (8192,) ──► np.savez_compressed() │ │ │ │ │ ▼ │ │ {token}.gz.npz 文件 │ │ │ └─────────────────────────────────────────────────────────────────────────────┘五、核心算法举例说明5.1 场景描述假设车辆位于一个十字路口前VOC 中有 3 个候选目标点目标点 A左转弯车道终点在可行驶区域内目标点 B直行车道终点在可行驶区域内目标点 C人行道区域不在可行驶区域内5.2 可行驶区域地图构建# 从 nuPlan 地图 API 查询周围 50m 范围内的可行驶区域drivable_areamap_api.get_proximal_map_objects(positionego_pose.point,radius50,layers[ROADBLOCK,ROADBLOCK_CONNECTOR,INTERSECTION,CARPARK_AREA])# 提取多边形polygons[]# - ROADBLOCK: 主干道多边形# - LANE: 各车道多边形从 ROADBLOCK 的 interior_edges 提取# - INTERSECTION: 交叉路口多边形# - CARPARK_AREA: 停车场多边形如果有# 构建 PDMDrivableMapdrivable_mapPDMDrivableMap(polygon_tokens,polygon_types,polygons)5.3 DAC 分数计算过程步骤 1创建单步轨迹# 时间跨度 4.0s间隔 4.0s即只有起始点和终点traj_samplingTrajectorySampling(time_horizon4.0,interval_length4.0)# 为每个 VOC 点创建轨迹traj_ATrajectory(np.array([[x_A,y_A,heading_A]]),traj_sampling)traj_BTrajectory(np.array([[x_B,y_B,heading_B]]),traj_sampling)traj_CTrajectory(np.array([[x_C,y_C,heading_C]]),traj_sampling)步骤 2转换到全局坐标系# 将相对坐标系的轨迹转换为全局坐标系global_traj_Atransform_trajectory(traj_A,initial_ego_state)global_traj_Btransform_trajectory(traj_B,initial_ego_state)global_traj_Ctransform_trajectory(traj_C,initial_ego_state)步骤 3计算车辆区域状态# 获取轨迹终点的车辆位置ego_coordsstate_array_to_coords_array(states,vehicle_parameters)# 检查终点四个角点是否在可行驶区域内in_polygonsdrivable_map.points_in_polygons(ego_coords[:,-1])# 只检查最后一个时间点# 统计在可行驶区域内的角点数量corner_countin_polygons[:,drivable_area_idcs].sum(axis-1)步骤 4判定可行驶性# 规则如果少于 4 个角点在可行驶区域内则判定为不在可行驶区域drivable_maskcorner_count4# DAC 分数dac_scoresnp.where(drivable_mask,1.0,0.0)5.4 计算结果示例目标点终点位置角点在可行驶区域内数量DAC 分数A (左转)左转弯车道内41.0B (直行)直行车道内41.0C (人行道)人行道上00.05.5 结果保存# 将 8192 个 VOC 点的 DAC 分数保存为压缩文件dac_labelsnp.array([1.0,1.0,0.0,...])# shape: (8192,)np.savez_compressed(f{token}.gz.npz,array1dac_labels)六、与 M1 模块的协作6.1 M1 中的 DAC-MLP 使用在 M1目标点分解模块中DAC-MLP 学习预测预计算的 DAC 标签classGoalFlowNaviModel(nn.Module):def__init__(self,config:GoalFlowConfig):# ... 其他组件 ...# DAC-MLP 评分网络self.dac_mlpnn.Sequential(nn.Linear(config.d_model,config.d_model),nn.ReLU(),nn.Linear(config.d_model,config.d_model),nn.ReLU(),nn.Linear(config.d_model,1),)defforward(self,batch:Dict[str,torch.Tensor],dac_label_path:strNone):# ... 获取特征 ...# DAC 评分dac_scoresself.dac_mlp(navi_feature.unsqueeze(1))# (B, 8192, 1)# 如果是训练模式加载预计算的 DAC 标签ifself.traininganddac_label_pathisnotNone:dac_labelself._load_dac_label(batch[token],dac_label_path)dac_lossF.binary_cross_entropy_with_logits(dac_scores.squeeze(),dac_label)else:dac_lossNonereturn{dac_scores:dac_scores,dac_loss:dac_loss,# ...}文件位置:navsim/agents/goalflow/goalflow_model_navi.py6.2 DAC 损失函数# DAC 损失二元交叉熵dac_lossF.binary_cross_entropy_with_logits(predictions[dac_scores].squeeze(),# (B, 8192)targets[dac_label],# (B, 8192) - 预计算的标签)6.3 推理阶段的分数融合在推理阶段M1 将 IM 分数和 DAC 分数融合# 分数融合公式final_scores(0.1*torch.log(F.softmax(im_scores.squeeze(),dim-1)1e-7)theta*torch.log(torch.sigmoid(dac_scores.squeeze())1e-7))# 选择 Top-k 目标点top_k_indicestorch.topk(final_scores,kconfig.top_k,dim-1).indices说明thetaDAC 分数的权重系数在配置文件中设置通过对数空间融合可以有效抑制 DAC 分数为 0 的候选点DAC 分数为 0 的点不在可行驶区域内会被严重惩罚七、关键配置参数7.1 PDMScorerConfigdataclassclassPDMScorerConfig:# 加权指标权重progress_weight:float5.0ttc_weight:float5.0comfortable_weight:float2.0# 阈值driving_direction_horizon:float1.0# 驾驶方向评估时间范围 [s]driving_direction_compliance_threshold:float2.0# 合规阈值 [m]driving_direction_violation_threshold:float6.0# 违规阈值 [m]stopped_speed_threshold:float5e-03# 停止速度阈值 [m/s]progress_distance_threshold:float0.1# 进度距离阈值 [m]文件位置:navsim/planning/simulation/planner/pdm_planner/scoring/pdm_scorer.py:497.2 训练配置# goalflow_training.yamldac_label_path:null# DAC 标签路径训练时需要设置7.3 预计算配置# default_run_pdm_score.yamlmetric_cache_path:${oc.env:NAVSIM_EXP_ROOT}/metric_cachedac_label_path:null# 输出路径八、数据流程总结8.1 预计算阶段VOC 候选点 (.npy) │ ▼ 为每个 VOC 点创建单步轨迹 │ ▼ PDM 模拟器模拟轨迹 │ ▼ PDMScorer 评估终点可行驶性 │ ▼ 生成 {token}.gz.npz DAC 标签文件8.2 训练阶段M1 使用场景数据 DAC 标签文件 │ ▼ GoalFlowNaviModel 前向传播 │ ├── IM-MLP 计算模仿分数 └── DAC-MLP 计算可行驶性分数 │ ▼ 损失计算imitation_loss dac_loss │ ▼ 反向传播更新权重8.3 推理阶段传感器输入相机 LiDAR 状态 │ ▼ GoalFlowNaviModel 前向传播 │ ├── IM-MLP → im_scores (B, 8192) └── DAC-MLP → dac_scores (B, 8192) │ ▼ 分数融合 → final_scores (B, 8192) │ ▼ 选择 Top-k 目标点 → 传递给 M0九、技术特点9.1 离线预计算DAC 分数在训练前预先计算避免了在线计算的巨大开销。每个场景 token 对应一个.gz.npz文件包含 8192 个 VOC 点的可行驶性标签。9.2 精确的空间查询使用 shapely 的 STRtree 进行高效的空间索引和查询能够快速判断大量点是否在多边形内。9.3 终点判定策略DAC 分数只检查轨迹的最后一个时间点终点而不是整个轨迹。这种设计简化了计算同时确保目标点本身是可达的。9.4 与 M1 的协同M2 为 M1 提供监督信号确保 M1 学习到的导航策略不仅模仿专家行为IM 分数还符合物理约束DAC 分数。9.5 多层语义融合PDMDrivableMap 融合了多种语义地图层包括车道、交叉路口、停车场等提供了全面的可行驶区域定义。十、核心代码清单文件路径核心功能navsim/planning/simulation/planner/pdm_planner/observation/pdm_occupancy_map.pyPDMDrivableMap 类可行驶区域地图构建与查询navsim/planning/simulation/planner/pdm_planner/scoring/pdm_scorer.pyPDMScorer 类可行驶区域合规性评分navsim/evaluate/pdm_score.pydac_score 函数DAC 分数计算接口navsim/planning/script/run_dac_score.pyDAC 分数预计算脚本navsim/agents/goalflow/goalflow_model_navi.pyGoalFlowNaviModelM1 模块使用 DAC-MLPnavsim/agents/goalflow/goalflow_loss.py损失函数定义包含 DAC 损失