#!/usr/bin/env python # -*- coding: utf-8 -*- """ 安全指标计算模块 """ import numpy as np import pandas as pd import math from collections import defaultdict from typing import Dict, Any, List, Optional import ast from modules.lib.score import Score from modules.lib.log_manager import LogManager # 安全指标相关常量 SAFETY_INFO = [ "simTime", "simFrame", "playerId", "posX", "posY", "posH", "speedX", "speedY", "accelX", "accelY", "v", "type", "lane_width", "lane_type", "road_type", "lane_coords", "link_coords" ] # ---------------------- # 独立指标计算函数 # ---------------------- def calculate_ttc(data_processed) -> dict: """计算TTC (Time To Collision)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"TTC": None} try: safety = SafetyCalculator(data_processed) ttc_value = safety.get_ttc_value() LogManager().get_logger().info(f"安全指标[TTC]计算结果: {ttc_value}") return {"TTC": ttc_value} except Exception as e: LogManager().get_logger().error(f"TTC计算异常: {str(e)}", exc_info=True) return {"TTC": None} def calculate_mttc(data_processed) -> dict: """计算MTTC (Modified Time To Collision)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"MTTC": None} try: safety = SafetyCalculator(data_processed) mttc_value = safety.get_mttc_value() LogManager().get_logger().info(f"安全指标[MTTC]计算结果: {mttc_value}") return {"MTTC": mttc_value} except Exception as e: LogManager().get_logger().error(f"MTTC计算异常: {str(e)}", exc_info=True) return {"MTTC": None} def calculate_thw(data_processed) -> dict: """计算THW (Time Headway)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"THW": None} try: safety = SafetyCalculator(data_processed) thw_value = safety.get_thw_value() LogManager().get_logger().info(f"安全指标[THW]计算结果: {thw_value}") return {"THW": thw_value} except Exception as e: LogManager().get_logger().error(f"THW计算异常: {str(e)}", exc_info=True) return {"THW": None} def calculate_tlc(data_processed) -> dict: """计算TLC (Time to Line Crossing)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"TLC": None} try: safety = SafetyCalculator(data_processed) tlc_value = safety.get_tlc_value() LogManager().get_logger().info(f"安全指标[TLC]计算结果: {tlc_value}") return {"TLC": tlc_value} except Exception as e: LogManager().get_logger().error(f"TLC计算异常: {str(e)}", exc_info=True) return {"TLC": None} def calculate_ttb(data_processed) -> dict: """计算TTB (Time to Brake)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"TTB": None} try: safety = SafetyCalculator(data_processed) ttb_value = safety.get_ttb_value() LogManager().get_logger().info(f"安全指标[TTB]计算结果: {ttb_value}") return {"TTB": ttb_value} except Exception as e: LogManager().get_logger().error(f"TTB计算异常: {str(e)}", exc_info=True) return {"TTB": None} def calculate_tm(data_processed) -> dict: """计算TM (Time Margin)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"TM": None} try: safety = SafetyCalculator(data_processed) tm_value = safety.get_tm_value() LogManager().get_logger().info(f"安全指标[TM]计算结果: {tm_value}") return {"TM": tm_value} except Exception as e: LogManager().get_logger().error(f"TM计算异常: {str(e)}", exc_info=True) return {"TM": None} # def calculate_MPrTTC(data_processed) -> dict: # """计算MPrTTC (Model Predictive Time-to-Collision)""" # if data_processed is None or not hasattr(data_processed, 'object_df'): # return {"MPrTTC": None} # try: # safety = SafetyCalculator(data_processed) # mprttc_value = safety.get_mprttc_value() # LogManager().get_logger().info(f"安全指标[MPrTTC]计算结果: {mprttc_value}") # return {"MPrTTC": mprttc_value} # except Exception as e: # LogManager().get_logger().error(f"MPrTTC计算异常: {str(e)}", exc_info=True) # return {"MPrTTC": None} def calculate_dtc(data_processed) -> dict: """计算DTC (Distance to Collision)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"DTC": None} try: safety = SafetyCalculator(data_processed) dtc_value = safety.get_dtc_value() LogManager().get_logger().info(f"安全指标[DTC]计算结果: {dtc_value}") return {"DTC": dtc_value} except Exception as e: LogManager().get_logger().error(f"DTC计算异常: {str(e)}", exc_info=True) return {"DTC": None} def calculate_pet(data_processed) -> dict: """计算PET (Post Encroachment Time)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"PET": None} try: safety = SafetyCalculator(data_processed) pet_value = safety.get_dtc_value() LogManager().get_logger().info(f"安全指标[PET]计算结果: {pet_value}") return {"PET": pet_value} except Exception as e: LogManager().get_logger().error(f"PET计算异常: {str(e)}", exc_info=True) return {"PET": None} def calculate_psd(data_processed) -> dict: """计算PSD (Potential Safety Distance)""" if data_processed is None or not hasattr(data_processed, 'object_df'): return {"PSD": None} try: safety = SafetyCalculator(data_processed) psd_value = safety.get_psd_value() LogManager().get_logger().info(f"安全指标[PSD]计算结果: {psd_value}") return {"PSD": psd_value} except Exception as e: LogManager().get_logger().error(f"PSD计算异常: {str(e)}", exc_info=True) return {"PSD": None} def calculate_collisionrisk(data_processed) -> dict: """计算碰撞风险""" safety = SafetyCalculator(data_processed) collision_risk_value = safety.get_collision_risk_value() LogManager().get_logger().info(f"安全指标[collisionRisk]计算结果: {collision_risk_value}") return {"collisionRisk": collision_risk_value} def calculate_lonsd(data_processed) -> dict: """计算纵向安全距离""" safety = SafetyCalculator(data_processed) lonsd_value = safety.get_lonsd_value() LogManager().get_logger().info(f"安全指标[LonSD]计算结果: {lonsd_value}") return {"LonSD": lonsd_value} def calculate_latsd(data_processed) -> dict: """计算横向安全距离""" safety = SafetyCalculator(data_processed) latsd_value = safety.get_latsd_value() LogManager().get_logger().info(f"安全指标[LatSD]计算结果: {latsd_value}") return {"LatSD": latsd_value} def calculate_btn(data_processed) -> dict: """计算制动威胁数""" safety = SafetyCalculator(data_processed) btn_value = safety.get_btn_value() LogManager().get_logger().info(f"安全指标[BTN]计算结果: {btn_value}") return {"BTN": btn_value} def calculate_collisionseverity(data_processed) -> dict: """计算碰撞严重性""" safety = SafetyCalculator(data_processed) collision_severity_value = safety.get_collision_severity_value() LogManager().get_logger().info(f"安全指标[collisionSeverity]计算结果: {collision_severity_value}") return {"collisionSeverity": collision_severity_value} class SafetyRegistry: """安全指标注册器""" def __init__(self, data_processed): self.logger = LogManager().get_logger() self.data = data_processed self.safety_config = data_processed.safety_config["safety"] self.metrics = self._extract_metrics(self.safety_config) self._registry = self._build_registry() def _extract_metrics(self, config_node: dict) -> list: """从配置中提取指标名称""" metrics = [] def _recurse(node): if isinstance(node, dict): if 'name' in node and not any(isinstance(v, dict) for v in node.values()): metrics.append(node['name']) for v in node.values(): _recurse(v) _recurse(config_node) self.logger.info(f'评比的安全指标列表:{metrics}') return metrics def _build_registry(self) -> dict: """构建指标函数注册表""" registry = {} for metric_name in self.metrics: func_name = f"calculate_{metric_name.lower()}" if func_name in globals(): registry[metric_name] = globals()[func_name] else: self.logger.warning(f"未实现安全指标函数: {func_name}") return registry def batch_execute(self) -> dict: """批量执行指标计算""" results = {} for name, func in self._registry.items(): try: result = func(self.data) results.update(result) except Exception as e: self.logger.error(f"{name} 执行失败: {str(e)}", exc_info=True) results[name] = None self.logger.info(f'安全指标计算结果:{results}') return results class SafeManager: """安全指标管理类""" def __init__(self, data_processed): self.data = data_processed self.registry = SafetyRegistry(self.data) def report_statistic(self): """计算并报告安全指标结果""" safety_result = self.registry.batch_execute() return safety_result class SafetyCalculator: """安全指标计算类 - 兼容Safe类风格""" def __init__(self, data_processed): self.logger = LogManager().get_logger() self.data_processed = data_processed self.df = data_processed.object_df.copy() self.ego_df = data_processed.ego_data.copy() # 使用copy()避免修改原始数据 self.obj_id_list = data_processed.obj_id_list self.metric_list = [ 'TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'TM', 'DTC', 'PET', 'PSD', 'LonSD', 'LatSD', 'BTN', 'collisionRisk', 'collisionSeverity' ] # 初始化默认值 self.calculated_value = { "TTC": 10.0, "MTTC": 10.0, "THW": 10.0, "TLC": 10.0, "TTB": 10.0, "TM": 10.0, # "MPrTTC": 10.0, "PET": 10.0, "DTC": 10.0, "PSD": 10.0, "LatSD": 3.0, "BTN": 1.0, "collisionRisk": 0.0, "collisionSeverity": 0.0, } self.time_list = self.ego_df['simTime'].values.tolist() self.frame_list = self.ego_df['simFrame'].values.tolist() self.collisionRisk = 0 self.empty_flag = True self.logger.info("SafetyCalculator初始化完成，场景中包含自车的目标物一共为: %d", len(self.obj_id_list)) if len(self.obj_id_list) > 1: self.unsafe_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.unsafe_time_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.unsafe_dist_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.unsafe_acce_drac_df = pd.DataFrame( columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.unsafe_acce_xtn_df = pd.DataFrame( columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.unsafe_prob_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type']) self.most_dangerous = {} self.pass_percent = {} self.logger.info("开始执行安全参数计算 _safe_param_cal_new") self._safe_param_cal_new() self.logger.info("安全参数计算完成") def _safe_param_cal_new(self): self.logger.debug("进入 _safe_param_cal_new 方法") # 直接复用Safe类的实现 Tc = 0.3 # 安全距离 rho = self.data_processed.vehicle_config["RHO"] ego_accel_max = self.data_processed.vehicle_config["EGO_ACCEL_MAX"] obj_decel_max = self.data_processed.vehicle_config["OBJ_DECEL_MAX"] ego_decel_min = self.data_processed.vehicle_config["EGO_DECEL_MIN"] ego_decel_lon_max = self.data_processed.vehicle_config["EGO_DECEL_LON_MAX"] ego_decel_lat_max = self.data_processed.vehicle_config["EGO_DECEL_LAT_MAX"] driver_reaction_time = self.data_processed.vehicle_config["RHO"] ego_decel_max = np.sqrt(ego_decel_lon_max ** 2 + ego_decel_lat_max ** 2) x_relative_start_dist = self.ego_df["x_relative_dist"] obj_dict = defaultdict(dict) obj_data_dict = self.df.to_dict('records') for item in obj_data_dict: obj_dict[item['simFrame']][item['playerId']] = item df_list = [] EGO_PLAYER_ID = 1 ramp_poss = self.ego_df[self.ego_df["link_type"] == 19]["link_coords"].drop_duplicates().tolist() # 寻找匝道的位置坐标 lane_poss = self.ego_df[self.ego_df["lane_type"] == 2]["lane_coords"].drop_duplicates().tolist() # 寻找匝道的位置坐标 for frame_num in self.frame_list: ego_data = obj_dict[frame_num][EGO_PLAYER_ID] v1 = ego_data['v'] x1 = ego_data['posX'] y1 = ego_data['posY'] h1 = ego_data['posH'] laneOffset = ego_data["laneOffset"] v_x1 = ego_data['speedX'] v_y1 = ego_data['speedY'] a_x1 = ego_data['accelX'] a_y1 = ego_data['accelY'] a1 = np.sqrt(a_x1 ** 2 + a_y1 ** 2) for playerId in self.obj_id_list: if playerId == EGO_PLAYER_ID: continue try: obj_data = obj_dict[frame_num][playerId] except KeyError: continue x2 = obj_data['posX'] y2 = obj_data['posY'] dist = self.dist(x1, y1, x2, y2) obj_data['dist'] = dist v_x2 = obj_data['speedX'] v_y2 = obj_data['speedY'] v2 = obj_data['v'] a_x2 = obj_data['accelX'] a_y2 = obj_data['accelY'] a2 = np.sqrt(a_x2 ** 2 + a_y2 ** 2) dx, dy = x2 - x1, y2 - y1 # 计算目标车相对于自车的方位角 beta = math.atan2(dy, dx) # 将全局坐标系下的相对位置向量转换到自车坐标系 # 自车坐标系：车头方向为x轴正方向，车辆左侧为y轴正方向 h1_rad = math.radians(90 - h1) # 转换为与x轴的夹角 # 坐标变换 lon_d = dx * math.cos(h1_rad) + dy * math.sin(h1_rad) # 纵向距离（前为正，后为负） lat_d = abs(-dx * math.sin(h1_rad) + dy * math.cos(h1_rad)) # 横向距离（取绝对值） obj_dict[frame_num][playerId]['lon_d'] = lon_d obj_dict[frame_num][playerId]['lat_d'] = lat_d if lon_d > 100 or lon_d < -5 or lat_d > 4: continue self.empty_flag = False vx, vy = v_x1 - v_x2, v_y1 - v_y2 ax, ay = a_x2 - a_x1, a_y2 - a_y1 relative_v = np.sqrt(vx ** 2 + vy ** 2) v_ego_p = self._cal_v_ego_projection(dx, dy, v_x1, v_y1) v_obj_p = self._cal_v_ego_projection(dx, dy, v_x2, v_y2) vrel_projection_in_dist = self._cal_v_projection(dx, dy, vx, vy) arel_projection_in_dist = self._cal_a_projection(dx, dy, vx, vy, ax, ay, x1, y1, x2, y2, v_x1, v_y1, v_x2, v_y2) obj_dict[frame_num][playerId]['vrel_projection_in_dist'] = vrel_projection_in_dist obj_dict[frame_num][playerId]['arel_projection_in_dist'] = arel_projection_in_dist obj_dict[frame_num][playerId]['v_ego_projection_in_dist'] = v_ego_p obj_dict[frame_num][playerId]['v_obj_projection_in_dist'] = v_obj_p obj_type = obj_data['type'] TTC = self._cal_TTC(dist, vrel_projection_in_dist) if abs(vrel_projection_in_dist) > 1e-6 else None MTTC = self._cal_MTTC(dist, vrel_projection_in_dist, arel_projection_in_dist) THW = self._cal_THW(dist, v_ego_p) if abs(v_ego_p) > 1e-6 else None TLC = self._cal_TLC(v1, h1, laneOffset) TTB = self._cal_TTB(x_relative_start_dist, relative_v, ego_decel_max) TM = self._cal_TM(x_relative_start_dist, v2, a2, v1, a1) DTC = self._cal_DTC(vrel_projection_in_dist, arel_projection_in_dist, driver_reaction_time) # MPrTTC = self._cal_MPrTTC(x_relative_start_dist) # PET = self._cal_PET(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, ego_posx, ego_posy, obj_posx, obj_posy, lane_width, delta_t, v1, v2, a1, a2) PET = None for lane_pos in lane_poss: lane_posx1 = ast.literal_eval(lane_pos)[0][0] lane_posy1 = ast.literal_eval(lane_pos)[0][1] lane_posx2 = ast.literal_eval(lane_pos)[-1][0] lane_posy2 = ast.literal_eval(lane_pos)[-1][1] for ramp_pos in ramp_poss: ramp_posx1 = ast.literal_eval(ramp_pos)[0][0] ramp_posy1 = ast.literal_eval(ramp_pos)[0][1] ramp_posx2 = ast.literal_eval(ramp_pos)[-1][0] ramp_posy2 = ast.literal_eval(ramp_pos)[-1][1] ego_posx = x1 ego_posy = y1 obj_posx = x2 obj_posy = y2 delta_t = self._cal_reaction_time_to_avgspeed(self.ego_df) lane_width = self.ego_df["lane_width"].iloc[0] PET = self._cal_PET(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, ego_posx, ego_posy, obj_posx, obj_posy, lane_width, delta_t, v1, v2, a1, a2) PSD = self._cal_PSD(x_relative_start_dist, v1, ego_decel_lon_max) LonSD = self._cal_longitudinal_safe_dist(v_ego_p, v_obj_p, rho, ego_accel_max, ego_decel_min, obj_decel_max) lat_dist = 0.5 v_right = v1 v_left = v2 a_right_lat_brake_min = 1 a_left_lat_brake_min = 1 a_lat_max = 5 LatSD = self._cal_lateral_safe_dist(lat_dist, v_right, v_left, rho, a_right_lat_brake_min, a_left_lat_brake_min, a_lat_max) # 使用自车坐标系下的纵向加速度 lon_a1 = a_x1 * math.cos(h1_rad) + a_y1 * math.sin(h1_rad) lon_a2 = a_x2 * math.cos(h1_rad) + a_y2 * math.sin(h1_rad) lon_a = abs(lon_a1 - lon_a2) lon_d = max(0.1, lon_d) # 确保纵向距离为正 lon_v = v_x1 * math.cos(h1_rad) + v_y1 * math.sin(h1_rad) BTN = self._cal_BTN_new(lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max) # 使用自车坐标系下的横向加速度 lat_a1 = -a_x1 * math.sin(h1_rad) + a_y1 * math.cos(h1_rad) lat_a2 = -a_x2 * math.sin(h1_rad) + a_y2 * math.cos(h1_rad) lat_a = abs(lat_a1 - lat_a2) lat_v = -v_x1 * math.sin(h1_rad) + v_y1 * math.cos(h1_rad) obj_dict[frame_num][playerId]['lat_v_rel'] = lat_v - (-v_x2 * math.sin(h1_rad) + v_y2 * math.cos(h1_rad)) obj_dict[frame_num][playerId]['lon_v_rel'] = lon_v - (v_x2 * math.cos(h1_rad) + v_y2 * math.sin(h1_rad)) TTC = None if (TTC is None or TTC < 0) else TTC MTTC = None if (MTTC is None or MTTC < 0) else MTTC THW = None if (THW is None or THW < 0) else THW TLC = None if (TLC is None or TLC < 0) else TLC TTB = None if (TTB is None or TTB < 0) else TTB TM = None if (TM is None or TM < 0) else TM DTC = None if (DTC is None or DTC < 0) else DTC PET = None if (PET is None or PET < 0) else PET PSD = None if (PSD is None or PSD < 0) else PSD obj_dict[frame_num][playerId]['TTC'] = TTC obj_dict[frame_num][playerId]['MTTC'] = MTTC obj_dict[frame_num][playerId]['THW'] = THW obj_dict[frame_num][playerId]['TLC'] = TLC obj_dict[frame_num][playerId]['TTB'] = TTB obj_dict[frame_num][playerId]['TM'] = TM obj_dict[frame_num][playerId]['DTC'] = DTC obj_dict[frame_num][playerId]['PET'] = PET obj_dict[frame_num][playerId]['PSD'] = PSD obj_dict[frame_num][playerId]['LonSD'] = LonSD obj_dict[frame_num][playerId]['LatSD'] = LatSD obj_dict[frame_num][playerId]['BTN'] = abs(BTN) collisionSeverity = 0 pr_death = 0 collisionRisk = 0 obj_dict[frame_num][playerId]['collisionSeverity'] = collisionSeverity * 100 obj_dict[frame_num][playerId]['pr_death'] = pr_death * 100 obj_dict[frame_num][playerId]['collisionRisk'] = collisionRisk * 100 df_fnum = pd.DataFrame(obj_dict[frame_num].values()) df_list.append(df_fnum) df_safe = pd.concat(df_list) col_list = ['simTime', 'simFrame', 'playerId', 'TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'TM', 'DTC', 'PET', 'PSD', 'LonSD', 'LatSD', 'BTN', 'collisionSeverity', 'pr_death', 'collisionRisk'] self.df_safe = df_safe[col_list].reset_index(drop=True) # 计算车辆从非匀速达到匀速的反应时间 def _cal_reaction_time_to_avgspeed(self, ego_df, threshold = 0.01): ego_df = ego_df.reset_index(drop=True) ego_df['v_change'] = ego_df['v'].diff() # 初始化结果列表 uniform_speed_segments = [] start_index = 0 # 遍历数据，找出匀速部分 for i in range(1, len(ego_df)): if ego_df['v_change'].iloc[i] > threshold: if i - start_index > 1: # 至少有两个数据点才能形成一个匀速段 uniform_speed_segments.append( (ego_df.iloc[start_index]['simTime'], ego_df.iloc[i - 1]['simTime'], ego_df.iloc[start_index]['v'])) start_index = i # 检查最后一个段 if len(ego_df) - start_index > 1: uniform_speed_segments.append( (ego_df.iloc[start_index]['simTime'], ego_df.iloc[-1]['simTime'], ego_df.iloc[start_index]['v'])) return uniform_speed_segments[-1][0] - ego_df['simTime'].iloc[0] def _cal_v_ego_projection(self, dx, dy, v_x1, v_y1): # 计算 AB 连线的向量 AB # dx = x2 - x1 # dy = y2 - y1 # 计算 AB 连线的模长 |AB| AB_mod = math.sqrt(dx ** 2 + dy ** 2) # 计算 AB 连线的单位向量 U_AB U_ABx = dx / AB_mod U_ABy = dy / AB_mod # 计算 A 在 AB 连线上的速度 V1_on_AB V1_on_AB = v_x1 * U_ABx + v_y1 * U_ABy return V1_on_AB def _cal_v_projection(self, dx, dy, vx, vy): # 计算 AB 连线的向量 AB # dx = x2 - x1 # dy = y2 - y1 # 计算 AB 连线的模长 |AB| AB_mod = math.sqrt(dx ** 2 + dy ** 2) # 计算 AB 连线的单位向量 U_AB U_ABx = dx / AB_mod U_ABy = dy / AB_mod # 计算 A 相对于 B 的速度 V_relative # vx = vx1 - vx2 # vy = vy1 - vy2 # 计算 A 相对于 B 在 AB 连线上的速度 V_on_AB V_on_AB = vx * U_ABx + vy * U_ABy return V_on_AB def _cal_a_projection(self, dx, dy, vx, vy, ax, ay, x1, y1, x2, y2, v_x1, v_y1, v_x2, v_y2): # 计算 AB 连线的向量 AB # dx = x2 - x1 # dy = y2 - y1 # 计算 θ V_mod = math.sqrt(vx ** 2 + vy ** 2) AB_mod = math.sqrt(dx ** 2 + dy ** 2) if V_mod == 0 or AB_mod == 0: return 0 cos_theta = (vx * dx + vy * dy) / (V_mod * AB_mod) theta = math.acos(cos_theta) # 计算 AB 连线的模长 |AB| AB_mod = math.sqrt(dx ** 2 + dy ** 2) # 计算 AB 连线的单位向量 U_AB U_ABx = dx / AB_mod U_ABy = dy / AB_mod # 计算 A 相对于 B 的加速度 a_relative # ax = ax1 - ax2 # ay = ay1 - ay2 # 计算 A 相对于 B 在 AB 连线上的加速度 a_on_AB a_on_AB = ax * U_ABx + ay * U_ABy VA = np.array([v_x1, v_y1]) VB = np.array([v_x2, v_y2]) D_A = np.array([x1, y1]) D_B = np.array([x2, y2]) V_r = VA - VB V = np.linalg.norm(V_r) w = self._cal_relative_angular_v(theta, D_A, D_B, VA, VB) a_on_AB_back = self._calculate_derivative(a_on_AB, w, V, theta) return a_on_AB_back # 计算相对加速度 def _calculate_derivative(self, a, w, V, theta): # 计算(V×cos(θ))'的值 # derivative = a * math.cos(theta) - w * V * math.sin(theta)theta derivative = a - w * V * math.sin(theta) return derivative def _cal_relative_angular_v(self, theta, A, B, VA, VB): dx = A[0] - B[0] dy = A[1] - B[1] dvx = VA[0] - VB[0] dvy = VA[1] - VB[1] # (dx * dvy - dy * dvx) angular_velocity = math.sqrt(dvx ** 2 + dvy ** 2) * math.sin(theta) / math.sqrt(dx ** 2 + dy ** 2) return angular_velocity def _death_pr(self, obj_type, v_relative): if obj_type == 5: p_death = 1 / (1 + np.exp(7.723 - 0.15 * v_relative)) else: p_death = 1 / (1 + np.exp(8.192 - 0.12 * v_relative)) return p_death def _cal_collisionRisk_level(self, obj_type, v_relative, collisionSeverity): if obj_type == 5: p_death = 1 / (1 + np.exp(7.723 - 0.15 * v_relative)) else: p_death = 1 / (1 + np.exp(8.192 - 0.12 * v_relative)) collisionRisk = 0.4 * p_death + 0.6 * collisionSeverity return collisionRisk # 求两车之间当前距离 def dist(self, x1, y1, x2, y2): dist = np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2) return dist # 求车辆与道路之间的横向距离 def horizontal_distance(self, posx1, posy1, posx2, posy2, posx, posy): dist = np.sqrt((posx2 - posx1)(posy - posy1) - (posy2 - posy1)(posx - posy1))/np.sqrt((posx2 - posx1)**2 + (posy2 - posy1)**2) return dist # 求车辆与道路之间的纵向距离 def _is_alone_the_road(self, posx1, posy1, posx2, posy2, posx, posy): pro_car = (posx2 - posx1)(posx - posx1) + (posy2 - posy1)(posy - posy1) if pro_car > 0: return True else: return False def _is_in_the_road(self, posx1, posy1, posx2, posy2, posx, posy): pro_obj1 = (posx2 - posx1)(posx - posx1) + (posy2 - posy1)(posy - posy1) pro_obj2 = (posx1 - posx2)(posx - posx2) + (posy1 - posy2)(posy - posy2) if pro_obj1 > 0 and pro_obj2 > 0: return True else: return False # TTC (time to collision) def _cal_TTC(self, dist, vrel_projection_in_dist): if vrel_projection_in_dist == 0: return math.inf TTC = dist / vrel_projection_in_dist return TTC def _cal_MTTC(self, dist, vrel_projection_in_dist, arel_projection_in_dist): MTTC = math.nan if arel_projection_in_dist != 0: tmp = vrel_projection_in_dist ** 2 + 2 * arel_projection_in_dist * dist if tmp < 0: return math.nan t1 = (-1 * vrel_projection_in_dist - math.sqrt(tmp)) / arel_projection_in_dist t2 = (-1 * vrel_projection_in_dist + math.sqrt(tmp)) / arel_projection_in_dist if t1 > 0 and t2 > 0: if t1 >= t2: MTTC = t2 elif t1 < t2: MTTC = t1 elif t1 > 0 and t2 <= 0: MTTC = t1 elif t1 <= 0 and t2 > 0: MTTC = t2 if arel_projection_in_dist == 0 and vrel_projection_in_dist > 0: MTTC = dist / vrel_projection_in_dist return MTTC # THW (time headway) def _cal_THW(self, dist, v_ego_projection_in_dist): if not v_ego_projection_in_dist: THW = None else: THW = dist / v_ego_projection_in_dist return THW # TLC (time to line crossing) def _cal_TLC(self, ego_v, ego_yaw, laneOffset): TLC = laneOffset/ego_v/np.sin(ego_yaw) if ((ego_v != 0) and (np.sin(ego_yaw) != 0)) else 10.0 if TLC < 0: TLC = None return TLC def _cal_TTB(self, x_relative_start_dist, relative_v, ego_decel_max): if len(x_relative_start_dist): return None if (ego_decel_max == 0) or (relative_v == 0): return self.calculated_value["TTB"] else: x_relative_start_dist0 = x_relative_start_dist.tolist()[0] TTB = (x_relative_start_dist0 + relative_v * relative_v/2/ego_decel_max)/relative_v return TTB def _cal_TM(self, x_relative_start_dist, v2, a2, v1, a1): if len(x_relative_start_dist): return None if (a2 == 0) or (v1 == 0): return self.calculated_value["TM"] if a1 == 0: return None x_relative_start_dist0 = x_relative_start_dist.tolist()[0] TM = (x_relative_start_dist0 + v2**2/(2*a2) - v1**2/(2*a1)) / v1 return TM # def _cal_MPrTTC(self, T=5, c = False, collision_dist = 5.99): # time_interval = self.ego_df['simTime'].tolist()[1] - self.ego_df['simTime'].tolist()[0] # # for i in range(len(self.obj_id_list)): # for j in range(T): # MPrTTC = j * time_interval def _cal_DTC(self, v_on_dist, a_on_dist, t): if a_on_dist == 0: return None DTC = v_on_dist * t + v_on_dist ** 2 / a_on_dist return DTC def _cal_PET(self, lane_posx1, lane_posy1, lane_posx2, lane_posy2, ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, ego_posx, ego_posy, obj_posx, obj_posy, lane_width, delta_t, v1, v2, a1, a2): dist1 = self.horizontal_distance(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ego_posx, ego_posy) dist2 = self.horizontal_distance(ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, obj_posx, obj_posy) if ((dist1 <= lane_width/2) and (self._is_alone_the_road(lane_posx1, lane_posy1, lane_posx2, lane_posy2, ego_posx, ego_posy)) and (self._is_in_the_road(ramp_posx1, ramp_posy1, ramp_posx2, ramp_posy2, obj_posx, obj_posy)) and (dist2 <= lane_width/2) and (a1 != 0) and (a2 != 0)): dist_ego = np.sqrt((ego_posx - lane_posx1)**2 + (ego_posy-lane_posy1)**2) dist_obj = np.sqrt((obj_posx - ramp_posx2)**2 + (obj_posy-ramp_posy2)**2) PET = (-2*v2 + np.sqrt((4* v2**2)-8*a2*(v2*delta_t - dist_obj)))/ 2/ a2 - (2*v1 + np.sqrt((4* v1**2)-8*a1*dist_ego))/ 2/ a1 + delta_t return PET else: return None def _cal_PSD(self, x_relative_start_dist, v1, ego_decel_lon_max): if v1 == 0: return None else: if len(x_relative_start_dist) > 0: x_relative_start_dist0 = x_relative_start_dist.tolist()[0] PSD = x_relative_start_dist0 * 2 * ego_decel_lon_max / v1 return PSD else: return None def velocity(self, v_x, v_y): v = math.sqrt(v_x ** 2 + v_y ** 2) * 3.6 return v def _cal_longitudinal_safe_dist(self, v_ego_p, v_obj_p, rho, ego_accel_max, ego_decel_min, ego_decel_max): lon_dist_min = v_ego_p * rho + ego_accel_max * (rho ** 2) / 2 + (v_ego_p + rho * ego_accel_max) ** 2 / ( 2 * ego_decel_min) - v_obj_p ** 2 / (2 * ego_decel_max) return lon_dist_min def _cal_lateral_safe_dist(self, lat_dist, v_right, v_left, rho, a_right_lat_brake_min, a_left_lat_brake_min, a_lat_max): # 检查除数是否为零 if a_right_lat_brake_min == 0 or a_left_lat_brake_min == 0: return self._default_value('LatSD') # 返回默认值 v_right_rho = v_right + rho * a_lat_max v_left_rho = v_left + rho * a_lat_max dist_min = lat_dist + ( (v_right + v_right_rho) * rho / 2 + v_right_rho**2 / a_right_lat_brake_min / 2 + ((v_left + v_right_rho) * rho / 2) + v_left_rho**2 / a_left_lat_brake_min / 2 ) return dist_min # DRAC (decelerate required avoid collision) def _cal_DRAC(self, dist, vrel_projection_in_dist, len1, len2, width1, width2, o_x1, o_x2): dist_length = dist - (len2 / 2 - o_x2 + len1 / 2 + o_x1) # 4.671 if dist_length < 0: dist_width = dist - (width2 / 2 + width1 / 2) if dist_width < 0: return math.inf else: d = dist_width else: d = dist_length DRAC = vrel_projection_in_dist ** 2 / (2 * d) return DRAC # BTN (brake threat number) def _cal_BTN_new(self, lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max): BTN = (lon_a1 + lon_a - lon_v ** 2 / (2 * lon_d)) / ego_decel_lon_max # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值 return BTN # STN (steer threat number) def _cal_STN_new(self, ttc, lat_a1, lat_a, lat_d, lat_v, ego_decel_lat_max, width1, width2): STN = (lat_a1 + lat_a + 2 / ttc ** 2 * (lat_d + abs(ego_decel_lat_max * lat_v) * ( width1 + width2) / 2 + abs(lat_v * ttc))) / ego_decel_lat_max return STN # BTN (brake threat number) def cal_BTN(self, a_y1, ay, dy, vy, max_ay): BTN = (a_y1 + ay - vy ** 2 / (2 * dy)) / max_ay # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值 return BTN # STN (steer threat number) def cal_STN(self, ttc, a_x1, ax, dx, vx, max_ax, width1, width2): STN = (a_x1 + ax + 2 / ttc ** 2 * (dx + np.sign(max_ax * vx) * (width1 + width2) / 2 + vx * ttc)) / max_ax return STN # 追尾碰撞风险 def _normal_distribution(self, x): mean = 1.32 std_dev = 0.26 return (1 / (math.sqrt(std_dev * 2 * math.pi))) * math.exp(-0.5 * (x - mean) ** 2 / std_dev) def continuous_group(self, df): time_list = df['simTime'].values.tolist() frame_list = df['simFrame'].values.tolist() group_time = [] group_frame = [] sub_group_time = [] sub_group_frame = [] for i in range(len(frame_list)): if not sub_group_time or frame_list[i] - frame_list[i - 1] <= 1: sub_group_time.append(time_list[i]) sub_group_frame.append(frame_list[i]) else: group_time.append(sub_group_time) group_frame.append(sub_group_frame) sub_group_time = [time_list[i]] sub_group_frame = [frame_list[i]] group_time.append(sub_group_time) group_frame.append(sub_group_frame) group_time = [g for g in group_time if len(g) >= 2] group_frame = [g for g in group_frame if len(g) >= 2] # 输出图表值 time = [[g[0], g[-1]] for g in group_time] frame = [[g[0], g[-1]] for g in group_frame] unfunc_time_df = pd.DataFrame(time, columns=['start_time', 'end_time']) unfunc_frame_df = pd.DataFrame(frame, columns=['start_frame', 'end_frame']) unfunc_df = pd.concat([unfunc_time_df, unfunc_frame_df], axis=1) return unfunc_df # 统计最危险的指标 def _safe_statistic_most_dangerous(self): min_list = ['TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'LonSD', 'LatSD', 'TM', 'PET', 'PSD'] max_list = ['DTC', 'BTN', 'collisionRisk', 'collisionSeverity'] result = {} for metric in min_list: if metric in self.metric_list: if metric in self.df.columns: val = self.df[metric].min() result[metric] = float(val) if pd.notnull(val) else self._default_value(metric) else: result[metric] = self._default_value(metric) for metric in max_list: if metric in self.metric_list: if metric in self.df.columns: val = self.df[metric].max() result[metric] = float(val) if pd.notnull(val) else self._default_value(metric) else: result[metric] = self._default_value(metric) return result def _safe_no_obj_statistic(self): # 仅有自车时的默认值 result = {metric: self._default_value(metric) for metric in self.metric_list} return result def _default_value(self, metric): # 统一默认值 defaults = { 'TTC': 10.0, 'MTTC': 4.2, 'THW': 2.1, 'TLC': 10.0, 'TTB': 10.0, 'TM': 10.0, 'DTC': 10.0, 'PET': 10.0, 'PSD': 10.0, 'LonSD': 10.0, 'LatSD': 2.0, 'BTN': 1.0, 'collisionRisk': 0.0, 'collisionSeverity': 0.0 } return defaults.get(metric, None) def report_statistic(self): if len(self.obj_id_list) == 1: safety_result = self._safe_no_obj_statistic() else: safety_result = self._safe_statistic_most_dangerous() evaluator = Score(self.data_processed.safety_config) result = evaluator.evaluate(safety_result) print("\n[安全性表现及得分情况]") return result def get_ttc_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('TTC') ttc_values = self.df_safe['TTC'].dropna() return float(ttc_values.min()) if not ttc_values.empty else self._default_value('TTC') def get_mttc_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('MTTC') mttc_values = self.df_safe['MTTC'].dropna() return float(mttc_values.min()) if not mttc_values.empty else self._default_value('MTTC') def get_thw_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('THW') thw_values = self.df_safe['THW'].dropna() return float(thw_values.min()) if not thw_values.empty else self._default_value('THW') def get_tlc_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('TLC') tlc_values = self.df_safe['TLC'].dropna() return float(tlc_values.min()) if not tlc_values.empty else self._default_value('TLC') def get_ttb_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('TTB') ttb_values = self.df_safe['TTB'].dropna() return float(ttb_values.min()) if not ttb_values.empty else self._default_value('TTB') def get_tm_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('TM') tm_values = self.df_safe['TM'].dropna() return float(tm_values.min()) if not tm_values.empty else self._default_value('TM') def get_dtc_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('DTC') dtc_values = self.df_safe['DTC'].dropna() return float(dtc_values.min()) if not dtc_values.empty else self._default_value('DTC') def get_pet_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('PET') pet_values = self.df_safe['PET'].dropna() return float(pet_values.min()) if not pet_values.empty else self._default_value('PET') def get_psd_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('PSD') psd_values = self.df_safe['PSD'].dropna() return float(psd_values.min()) if not psd_values.empty else self._default_value('PSD') def get_lonsd_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('LonSD') lonsd_values = self.df_safe['LonSD'].dropna() return float(lonsd_values.mean()) if not lonsd_values.empty else self._default_value('LonSD') def get_latsd_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('LatSD') latsd_values = self.df_safe['LatSD'].dropna() # 使用最小值而非平均值，与safety1.py保持一致 return float(latsd_values.min()) if not latsd_values.empty else self._default_value('LatSD') def get_btn_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('BTN') btn_values = self.df_safe['BTN'].dropna() return float(btn_values.max()) if not btn_values.empty else self._default_value('BTN') def get_collision_risk_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('collisionRisk') risk_values = self.df_safe['collisionRisk'].dropna() return float(risk_values.max()) if not risk_values.empty else self._default_value('collisionRisk') def get_collision_severity_value(self) -> float: if self.empty_flag or self.df_safe is None: return self._default_value('collisionSeverity') severity_values = self.df_safe['collisionSeverity'].dropna() return float(severity_values.max()) if not severity_values.empty else self._default_value('collisionSeverity')