#!/usr/bin/env python # -*- coding: utf-8 -*- ################################################################## # # Copyright (c) 2023 CICV, Inc. All Rights Reserved # ################################################################## """ @Authors: zhanghaiwen(zhanghaiwen@china-icv.cn) @Data: 2023/07/25 @Last Modified: 2023/07/26 @Summary: safe metrics """ import math import numpy as np import pandas as pd from modules.lib.score import Score from modules.config import config from collections import defaultdict import scipy.integrate as spi class Safe(object): def __init__(self, data_processed): # self.logger = log.get_logger() # 使用时再初始化 self.data_processed = data_processed self.df = data_processed.object_df.copy() self.ego_df = data_processed.ego_data self.obj_id_list = data_processed.obj_id_list self.metric_list = [ "TTC", "MTTC", "THW", "LatSD", "BTN", "collisionRisk", "collisionSeverity", ] # score data self.calculated_value = { "TTC": 1.0, "MTTC": 1.0, "THW": 1.0, # 'LonSD': 0.0, "LatSD": 0.0, # 'DRAC': 0.0, "BTN": 1.0, # 'STN': 1.0, "collisionRisk": 0.0, "collisionSeverity": 0.0, } # lists of drving control info 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 # no car following scene 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_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._safe_param_cal_new() def _safe_param_cal_new(self): Tc = 0.3 # 安全距离 rho = Tc # 驾驶员制动反应时间（单位：秒） 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" ] # 自车刹车最小减速度 ug 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"] # 构建双层字典数据结构 obj_dict = defaultdict(dict) # 构建字典列表，每个元素是一个字典，包含'start_time', 'end_time', 'start_frame', 'end_frame', 'type'五个键值对 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 # self.empty_flag = True for frame_num in self.frame_list: ego_data = obj_dict[frame_num][EGO_PLAYER_ID] v1 = ego_data["v"] # km/h to m/s x1 = ego_data["posX"] y1 = ego_data["posY"] h1 = ego_data["posH"] # len1 = ego_data['dimX'] # width1 = ego_data['dimY'] # o_x1 = ego_data['offX'] v_x1 = ego_data["speedX"] # km/h to m/s v_y1 = ego_data["speedY"] # km/h to m/s a_x1 = ego_data["accelX"] a_y1 = ego_data["accelY"] # a1 = ego_data['accel'] 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"] # km/h to m/s v_y2 = obj_data["speedY"] # km/h to m/s v2 = obj_data["v"] # km/h to m/s # h2 = obj_data['posH'] # len2 = obj_data['dimX'] # width2 = obj_data['dimY'] # o_x2 = obj_data['offX'] a_x2 = obj_data["accelX"] a_y2 = obj_data["accelY"] # a2 = obj_data['accel'] dx, dy = x2 - x1, y2 - y1 # 定义矢量A和x轴正向向量x A = np.array([dx, dy]) x = np.array([1, 0]) # 计算点积和向量长度 dot_product = np.dot(A, x) vector_length_A = np.linalg.norm(A) vector_length_x = np.linalg.norm(x) # 计算夹角的余弦值 cos_theta = dot_product / (vector_length_A * vector_length_x) # 将余弦值转换为角度值（弧度制） beta = np.arccos(cos_theta) # 如何通过theta正负确定方向 lon_d = dist * math.cos(beta - h1) lat_d = abs( dist * math.sin(beta - h1) ) # 需要增加左正右负的判断，但beta取值为[0,pi) obj_dict[frame_num][playerId]["lon_d"] = lon_d obj_dict[frame_num][playerId]["lat_d"] = lat_d # 代码注释，这里是筛选出车前100米，车后5米，车右边4米的目标车，用于计算安全距离 # print(f"lon_d: {lon_d}, 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 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) MTTC = self._cal_MTTC( TTC, vrel_projection_in_dist, arel_projection_in_dist ) THW = self._cal_THW(dist, v_ego_p) # 单车道时可用 # 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, ) # DRAC = self._cal_DRAC(dist, vrel_projection_in_dist, len1, len2, width1, width2, o_x1, o_x2) lon_a1 = a_x1 * math.cos(h1) + a_y1 * math.sin(h1) lon_a2 = a_x2 * math.cos(h1) + a_y2 * math.sin(h1) lon_a = abs(lon_a1 - lon_a2) lon_d = dist * abs(math.cos(beta - h1)) lon_v = v_x1 * math.cos(h1) + v_y1 * math.sin(h1) BTN = self._cal_BTN_new(lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max) lat_a1 = a_x1 * math.sin(h1) * -1 + a_y1 * math.cos(h1) lat_a2 = a_x2 * math.sin(h1) * -1 + a_y2 * math.cos(h1) lat_a = abs(lat_a1 - lat_a2) lat_d = dist * abs(math.sin(beta - h1)) lat_v = v_x1 * math.sin(h1) * -1 + v_y1 * math.cos(h1) # STN = self._cal_STN_new(TTC, lat_a1, lat_a, lat_d, lat_v, ego_decel_lat_max, width1, width2) obj_dict[frame_num][playerId]["lat_v_rel"] = v_x1 - v_x2 obj_dict[frame_num][playerId]["lon_v_rel"] = v_y1 - v_y2 TTC = None if (not TTC or TTC < 0) else TTC MTTC = None if (not MTTC or MTTC < 0) else MTTC THW = None if (not THW or THW < 0) else THW # 原DRAC计算已被注释，需移除相关代码引用 # DRAC = 10 if DRAC >= 10 else DRAC # 删除这行 # 修改后的指标赋值 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]['LonSD'] = LonSD obj_dict[frame_num][playerId]["LatSD"] = LatSD # obj_dict[frame_num][playerId]['DRAC'] = DRAC # 删除这行 obj_dict[frame_num][playerId]["BTN"] = abs(BTN) # obj_dict[frame_num][playerId]['STN'] = abs(STN) # 删除这行 # 确保以下变量已定义 collisionSeverity = ( collisionSeverity if "collisionSeverity" in locals() else 0 ) pr_death = pr_death if "pr_death" in locals() else 0 collisionRisk = collisionRisk if "collisionRisk" in locals() else 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 # print( # f"{playerId} {frame_num} {TTC} {MTTC} {THW} {LatSD} {BTN} {collisionSeverity} {pr_death} {collisionRisk}" # ) 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", "LatSD", "BTN", # 移除STN "collisionSeverity", "pr_death", "collisionRisk", ] if not self.empty_flag: self.df = df_safe[col_list].reset_index(drop=True) # self.df_safe是有问题的，因为后边统计的函数里是没有self.df_safe这个变量的,只有self.df, # 需要增加如果是空的情况下，self.df_safe的值为默认值。 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 # 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 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, ): 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", "LatSD"] max_list = ["BTN", "collisionRisk", "collisionSeverity"] # 移除STN for metric in min_list: if metric in self.metric_list: if metric in self.df.columns: self.calculated_value[metric] = self.df[metric].min() else: self.calculated_value[metric] = 10.0 for metric in max_list: if metric in self.metric_list: if metric in self.df.columns: self.calculated_value[metric] = self.df[metric].max() else: self.calculated_value[metric] = 10.0 return self.calculated_value def _safe_no_obj_statistic(self): # list_metric = ["TTC","MTTC","THW","LonSD","LatSD","DRAC","BTN","STN","collisionSeverity", "collisionRisk"] return self.calculated_value def report_statistic( self, ): safety_result = {} 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) # self.logger.info(f"安全性表现及得分情况:[{result}]") print("\n[安全性表现及得分情况]") return result