zhaoyuan/modules/metric/comfort.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
##################################################################
#
# Copyright (c) 2023 CICV, Inc. All Rights Reserved
#
##################################################################
"""
@Authors:           zhanghaiwen(zhanghaiwen@china-icv.cn), yangzihao(yangzihao@china-icv.cn)
@Data:              2023/06/25
@Last Modified:     2025/04/25
@Summary:           Comfort metrics
"""

import sys
import math
import scipy.signal
import pandas as pd
import numpy as np
import os
from pathlib import Path 
from typing import Dict, List, Any, Optional, Callable, Union, Tuple

from modules.lib.score import Score
from modules.lib.common import get_interpolation, get_frame_with_time
from modules.lib import data_process
from modules.lib.log_manager import LogManager
from modules.lib.chart_generator import generate_comfort_chart_data

# 更新COMFORT_INFO列表，添加车辆坐标系下的速度和加速度字段
COMFORT_INFO = [
    "simTime",
    "simFrame",
    "speedX",
    "speedY",
    "accelX",
    "accelY",
    "curvHor",
    "lightMask",
    "v",
    "lat_acc",
    "lon_acc",
    "time_diff",
    "lon_acc_diff",
    "lon_acc_roc",
    "speedH",
    "accelH",
    "posH",           # 航向角字段
    "lon_v_vehicle",   # 车辆坐标系下的纵向速度
    "lat_v_vehicle",   # 车辆坐标系下的横向速度
    "lon_acc_vehicle", # 车辆坐标系下的纵向加速度
    "lat_acc_vehicle"  # 车辆坐标系下的横向加速度
]
# ----------------------
# 独立指标计算函数
# ----------------------
def motionComfortIndex(data_processed) -> dict:
    """计算运动舒适度指数"""
    comfort = Comfort(data_processed)
    index = comfort.calculate_motion_comfort_index()
    return {"motionComfortIndex": float(index)}

def rideQualityScore(data_processed) -> dict:
    """计算乘坐质量评分"""
    comfort = Comfort(data_processed)
    score = comfort.calculate_ride_quality_score()
    return {"rideQualityScore": float(score)}
    
def calculate_motionsickness(data_processed) -> dict:
    """计算晕车概率指标"""
    comfort = ComfortCalculator(data_processed)
    motion_sickness_prob = comfort.calculate_motion_sickness_probability()
    return {"motionSickness": float(motion_sickness_prob)}

def calculate_vdv(data_processed) -> dict:
    """计算振动剂量值(Vibration Dose Value, VDV)指标"""
    comfort = ComfortCalculator(data_processed)
    vdv_value = comfort.calculate_vdv()
    return {"vdv": float(vdv_value)}

def calculate_ava_vav(data_processed) -> dict:
    """计算多维度综合加权加速度"""
    comfort = ComfortCalculator(data_processed)
    ava_vav_value = comfort.calculate_ava_vav()
    return {"ava_vav": float(ava_vav_value)}

def calculate_msdv(data_processed) -> dict:
    """计算晕动剂量值(MSDV)指标"""
    comfort = ComfortCalculator(data_processed)
    msdv_value = comfort.calculate_msdv()
    return {"msdv": float(msdv_value)}
    
def calculate_zigzag(data_processed) -> dict:
    """计算蛇行指标"""
    comfort = ComfortCalculator(data_processed)
    zigzag_count = comfort.calculate_zigzag_count()
    return {"zigzag": float(zigzag_count)}

def calculate_shake(data_processed) -> dict:
    """计算晃动指标"""
    comfort = ComfortCalculator(data_processed)
    shake_count = comfort.calculate_shake_count()
    return {"shake": float(shake_count)}

def calculate_cadence(data_processed) -> dict:
    """计算顿挫指标"""
    comfort = ComfortCalculator(data_processed)
    cadence_count = comfort.calculate_cadence_count()
    return {"cadence": float(cadence_count)}

def calculate_slambrake(data_processed) -> dict:
    """计算急刹车指标"""
    comfort = ComfortCalculator(data_processed)
    slam_brake_count = comfort.calculate_slam_brake_count()
    return {"slamBrake": float(slam_brake_count)}

def calculate_slamaccelerate(data_processed) -> dict:
    """计算急加速指标"""
    comfort = ComfortCalculator(data_processed)
    slam_accel_count = comfort.calculate_slam_accel_count()
    return {"slamAccelerate": float(slam_accel_count)}

# 装饰器保持不变
def peak_valley_decorator(method):
    def wrapper(self, *args, **kwargs):
        peak_valley = self._peak_valley_determination(self.df)
        pv_list = self.df.loc[peak_valley, ['simTime', 'speedH']].values.tolist()
        if len(pv_list) != 0:
            flag = True
            p_last = pv_list[0]

            for i in range(1, len(pv_list)):
                p_curr = pv_list[i]

                if self._peak_valley_judgment(p_last, p_curr):
                    # method(self, p_curr, p_last)
                    method(self, p_curr, p_last, flag, *args, **kwargs)
                else:
                    p_last = p_curr

            return method
        else:
            flag = False
            p_curr = [0, 0]
            p_last = [0, 0]
            method(self, p_curr, p_last, flag, *args, **kwargs)
            return method

    return wrapper


class ComfortRegistry:
    """舒适性指标注册器"""
    
    def __init__(self, data_processed):
        self.logger = LogManager().get_logger()  # 获取全局日志实例
        self.data = data_processed
        self.comfort_config = data_processed.comfort_config["comfort"]
        self.metrics = self._extract_metrics(self.comfort_config)
        self._registry = self._build_registry()
        self.output_dir = None  # 图表数据输出目录
    
    def _extract_metrics(self, config_node: dict) -> list:
        """DFS遍历提取指标"""
        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()}"
            try:
                registry[metric_name] = globals()[func_name]
            except KeyError:
                self.logger.error(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)
                # 新增：将每个指标的结果写入日志
                self.logger.info(f'舒适性指标[{name}]计算结果: {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 ComfortCalculator:
    """舒适性指标计算类 - 提供核心计算功能"""
    
    def generate_metric_chart(self, metric_name: str) -> None:
        """
        生成指标图表
        
        Args:
            metric_name: 指标名称
        """
        # 设置输出目录
        if not hasattr(self, 'output_dir') or not self.output_dir:
            self.output_dir = os.path.join(os.getcwd(), 'data')
            os.makedirs(self.output_dir, exist_ok=True)
        
        # 调用chart_generator中的函数生成图表
        chart_path = generate_comfort_chart_data(self, metric_name, self.output_dir)
        if chart_path:
            self.logger.info(f"{metric_name}图表已生成: {chart_path}")
    
    def __init__(self, data_processed):
        self.data_processed = data_processed
        self.logger = LogManager().get_logger()
        
        self.data = data_processed.ego_data
        self.ego_df = pd.DataFrame()
        self.discomfort_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
        
        # 统计指标
        self.calculated_value = {
            'zigzag': 0, 
            'shake': 0, 
            'cadence': 0,
            'slamBrake': 0, 
            'slamAccelerate': 0,
            'ava_vav': 0,  # 添加新指标的默认值
            'msdv': 0,     # 添加MSDV指标的默认值
            'motionSickness': 0,  # 添加晕车概率指标的默认值
            'vdt:': 0,
            'motionComfortIndex': 0,  # 新增指标
            'rideQualityScore': 0     # 新增指标
        }
        
        self.time_list = self.data['simTime'].values.tolist()
        self.frame_list = self.data['simFrame'].values.tolist()
        
        self.zigzag_count = 0
        self.shake_count = 0
        self.cadence_count = 0
        self.slam_brake_count = 0
        self.slam_accel_count = 0
        
        self.zigzag_time_list = []
        self.zigzag_stre_list = []
        self.shake_events = []  # 用于存储晃动事件数据
        
        self._initialize_data()
    
    def _initialize_data(self):
        """初始化数据"""
        self.ego_df = self.data[COMFORT_INFO].copy()
        self.df = self.ego_df.reset_index(drop=True)
        self._prepare_comfort_parameters()
    
    def _prepare_comfort_parameters(self):
        """准备舒适性计算所需参数"""
        # 检查是否已经有车身坐标系下的加速度和速度数据
        # 这些数据应该在data_process.py的process_ego_data方法中已经计算好
        if 'lon_acc_vehicle' in self.ego_df.columns and 'lat_acc_vehicle' in self.ego_df.columns:
            # 直接使用已转换的数据
            self.logger.info("使用data_process中预先计算的车身坐标系加速度和速度数据")
        elif 'posH' in self.ego_df.columns:
            # 如果没有预先计算的数据，但有航向角，则进行计算
            # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
            self.logger.warning("未找到预先计算的车身坐标系数据，进行实时计算")
            self.ego_df['heading_rad'] = np.deg2rad(90 - self.ego_df['posH'])  # 转换为与x轴的夹角
            
            # 转换加速度到车身坐标系 - 使用与data_process.py相同的转换方法
            self.ego_df['lon_acc_vehicle'] = self.ego_df['accelX'] * np.cos(self.ego_df['heading_rad']) + \
                                          self.ego_df['accelY'] * np.sin(self.ego_df['heading_rad'])
            self.ego_df['lat_acc_vehicle'] = -self.ego_df['accelX'] * np.sin(self.ego_df['heading_rad']) + \
                                          self.ego_df['accelY'] * np.cos(self.ego_df['heading_rad'])
            
            # 转换速度到车身坐标系
            self.ego_df['lon_v_vehicle'] = self.ego_df['speedX'] * np.cos(self.ego_df['heading_rad']) + \
                                        self.ego_df['speedY'] * np.sin(self.ego_df['heading_rad'])
            self.ego_df['lat_v_vehicle'] = -self.ego_df['speedX'] * np.sin(self.ego_df['heading_rad']) + \
                                        self.ego_df['speedY'] * np.cos(self.ego_df['heading_rad'])
        else:
            self.logger.warning("缺少航向角数据，无法将数据转换为车身坐标系")
        
        # 计算加减速阈值 - 使用车辆坐标系下的纵向速度代替合速度
        speed_field = 'lon_v_vehicle' if 'lon_v_vehicle' in self.ego_df.columns else 'v'
        self.logger.info(f"加减速阈值计算使用的速度字段: {speed_field}")
        
        self.ego_df['ip_acc'] = self.ego_df[speed_field].apply(get_interpolation, point1=[18, 4], point2=[72, 2])
        self.ego_df['ip_dec'] = self.ego_df[speed_field].apply(get_interpolation, point1=[18, -5], point2=[72, -3.5])
        
        # 使用车辆坐标系下的纵向加速度计算急刹车和急加速
        acc_field = 'lon_acc_vehicle' if 'lon_acc_vehicle' in self.ego_df.columns else 'lon_acc'
        self.logger.info(f"急刹车和急加速检测使用的加速度字段: {acc_field}")
        
        # 使用车辆坐标系下的纵向加速度与阈值比较，判断急刹车和急加速
        self.ego_df['slam_brake'] = (self.ego_df[acc_field] - self.ego_df['ip_dec']).apply(
            lambda x: 1 if x < 0 else 0)
        self.ego_df['slam_accel'] = (self.ego_df[acc_field] - self.ego_df['ip_acc']).apply(
            lambda x: 1 if x > 0 else 0)
        
        # 确保cadence列使用车辆坐标系下的纵向加速度计算
        self.ego_df['cadence'] = self.ego_df.apply(
            lambda row: self._cadence_process_new(row[acc_field], row['ip_acc'], row['ip_dec']), axis=1)

    def _apply_frequency_weighting(self, acceleration_data, weighting_type='Wk', fs=100):
        """应用ISO 2631-1:1997标准的频率加权滤波
        
        参数:
            acceleration_data: 加速度时间序列数据
            weighting_type: 加权类型，可选值包括:
                - 'Wk': 垂直方向(Z轴)加权
                - 'Wd': 水平方向(X和Y轴)加权
                - 'Wf': 运动病相关加权
            fs: 采样频率(Hz)
            
        返回:
            加权后的加速度数据
        """
        # 检查数据有效性
        if acceleration_data.empty or acceleration_data.isna().all():
            return acceleration_data
            
        # 根据ISO 2631-1:1997标准设计滤波器
        # 这些参数来自标准文档，用于构建数字滤波器
        if weighting_type == 'Wk':  # 垂直方向(Z轴)
            # Wk滤波器参数
            f1 = 0.4
            f2 = 100.0
            f3 = 12.5
            f4 = 12.5
            Q1 = 0.63
            Q2 = 0.5
            Q3 = 0.63
            Q4 = 0.63
            K = 0.4
        elif weighting_type == 'Wd':  # 水平方向(X和Y轴)
            # Wd滤波器参数
            f1 = 0.4
            f2 = 100.0
            f3 = 2.0
            f4 = 2.0
            Q1 = 0.63
            Q2 = 0.5
            Q3 = 0.63
            Q4 = 0.63
            K = 0.4
        elif weighting_type == 'Wf':  # 运动病相关
            # Wf滤波器参数
            f1 = 0.08
            f2 = 0.63
            f3 = 0.25
            f4 = 0.8
            Q1 = 0.63
            Q2 = 0.86
            Q3 = 0.8
            Q4 = 0.8
            K = 1.0
        else:
            self.logger.warning(f"未知的加权类型: {weighting_type}，使用原始数据")
            return acceleration_data
        
        # 将频率转换为角频率
        w1 = 2 * np.pi * f1
        w2 = 2 * np.pi * f2
        w3 = 2 * np.pi * f3
        w4 = 2 * np.pi * f4
        
        # 设计高通滤波器(s域)
        b1 = [K * w1**2, 0]
        a1 = [1, w1/Q1, w1**2]
        
        # 设计低通滤波器(s域)
        b2 = [K, 0, 0]
        a2 = [1, w2/Q2, w2**2]
        
        # 设计加速度-速度转换滤波器(s域)
        b3 = [K, 0]
        a3 = [1, w3/Q3, w3**2]
        
        # 设计上升滤波器(s域)
        b4 = [K, 0, 0]
        a4 = [1, w4/Q4, w4**2]
        
        # 使用双线性变换将s域滤波器转换为z域
        b1_z, a1_z = scipy.signal.bilinear(b1, a1, fs)
        b2_z, a2_z = scipy.signal.bilinear(b2, a2, fs)
        b3_z, a3_z = scipy.signal.bilinear(b3, a3, fs)
        b4_z, a4_z = scipy.signal.bilinear(b4, a4, fs)
        
        # 应用滤波器链
        data_np = acceleration_data.to_numpy()
        filtered_data = scipy.signal.lfilter(b1_z, a1_z, data_np)
        filtered_data = scipy.signal.lfilter(b2_z, a2_z, filtered_data)
        filtered_data = scipy.signal.lfilter(b3_z, a3_z, filtered_data)
        filtered_data = scipy.signal.lfilter(b4_z, a4_z, filtered_data)
        
        return pd.Series(filtered_data, index=acceleration_data.index)
    
    def calculate_vdv(self):
        """计算振动剂量值(Vibration Dose Value, VDV)
    
        VDV更强调"冲击"或"突发"振动事件对整体舒适度的影响，
        常用于评估包含较多瞬态冲击或颠簸的振动信号。
        
        计算公式: VDV = (∫[0,T] |a_ω(t)|⁴ dt)^(1/4)
        
        相较于MSDV的二次累积，VDV的四次累积使其对高幅值短时冲击更为敏感，
        能够更准确地反映剧烈颠簸对乘员舒适度的不利影响。
        如你正在开发自动驾驶体验分析模块，建议：

        MSDV → 用于标记低频摇摆造成的晕车风险

        VDV → 用于标记车内座椅或底盘振动强度（比如悬挂问题、路面冲击）
        
        Returns:
            float: 振动剂量值
        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelX' not in df.columns or 'accelY' not in df.columns:
            self.logger.warning("缺少计算振动剂量值所需的数据列")
            return self.calculated_value['vdv']
        
        # 将东北天坐标系下的加速度转换为车身坐标系下的加速度
        if 'posH' not in df.columns:
            self.logger.warning("缺少航向角数据，无法进行坐标转换")
            return self.calculated_value['vdv']
            
        # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
        df['posH_rad'] = np.radians(df['posH'])
        
        # 转换加速度到车身坐标系
        df['a_x_body'] = df['accelX'] * np.sin(df['posH_rad']) + df['accelY'] * np.cos(df['posH_rad'])
        df['a_y_body'] = df['accelX'] * np.cos(df['posH_rad']) - df['accelY'] * np.sin(df['posH_rad'])
        
        # Z方向加速度，如果没有则假设为0
        df['a_z_body'] = df['accelZ'] if 'accelZ' in df.columns else pd.Series(np.zeros(len(df)))
        
        # 计算时间差
        df['time_diff'] = df['simTime'].diff().fillna(0)
        
        # 应用ISO 2631-1:1997标准的频率加权滤波
        # 估计采样频率 - 假设数据是均匀采样的
        if len(df) > 1:
            time_diff = df['simTime'].diff().median()
            fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
        else:
            fs = 100  # 默认采样频率
        
        # 对各方向加速度应用适当的频率加权
        # 对于VDV评估，使用与MSDV相同的加权滤波器
        a_x_weighted = self._apply_frequency_weighting(df['a_x_body'], 'Wd', fs)  # 水平方向使用Wd
        a_y_weighted = self._apply_frequency_weighting(df['a_y_body'], 'Wd', fs)  # 水平方向使用Wd
        a_z_weighted = self._apply_frequency_weighting(df['a_z_body'], 'Wk', fs)  # 垂直方向使用Wk
        
        # 计算加权均方根值 (r.m.s.)
        a_x_rms = np.sqrt(np.mean(a_x_weighted**2))
        a_y_rms = np.sqrt(np.mean(a_y_weighted**2))
        a_z_rms = np.sqrt(np.mean(a_z_weighted**2))
        
        # 记录r.m.s.值用于参考
        self.logger.info(f"X方向加权均方根值: {a_x_rms}")
        self.logger.info(f"Y方向加权均方根值: {a_y_rms}")
        self.logger.info(f"Z方向加权均方根值: {a_z_rms}")
        
        # 计算VDV - 对加速度四次方进行时间积分，再开四次方根
        # 对于X方向（前后方向）
        vdv_x = np.power(np.sum(np.power(np.abs(a_x_weighted), 4) * df['time_diff']), 0.25)
        
        # 对于Y方向（左右方向）
        vdv_y = np.power(np.sum(np.power(np.abs(a_y_weighted), 4) * df['time_diff']), 0.25)
        
        # 对于Z方向（上下方向）
        vdv_z = np.power(np.sum(np.power(np.abs(a_z_weighted), 4) * df['time_diff']), 0.25)
        
        # 综合VDV - 可以使用向量和或加权和
        # 根据ISO 2631标准，垂直方向(Z)的权重通常更高
        vdv = np.sqrt(vdv_x**2 + vdv_y**2 + (1.4 * vdv_z)**2)
        
        # 记录计算结果
        self.calculated_value['vdv'] = vdv
        self.logger.info(f"振动剂量值(VDV)计算结果: {vdv}")
        self.logger.info(f"X方向VDV: {vdv_x}, Y方向VDV: {vdv_y}, Z方向VDV: {vdv_z}")
        
        return vdv

    def calculate_motion_sickness_probability(self):
        """计算基于运动参数的晕车概率模型
        
        该模型综合考虑三轴加速度和加加速度(Jerk)对乘客晕车感的影响，
        通过非线性指数函数将计算结果映射到0-100%的概率范围。
        
        计算公式: P = 100 * [1 - exp(-(α*(ax²+ay²+az²) + β*Jerk_RMS) / γ)]
        
        其中:
        - ax, ay, az: 三轴加速度，表征车辆纵向、横向、垂向运动强度
        - Jerk_RMS: 加速度变化率的均方根值，反映运动突兀性
        - α: 加速度权重(默认0.1 s⁴/m²)
        - β: Jerk权重(默认0.5 s²/m²)
        - γ: 归一化因子(默认10 m²/s⁴)
        
        Returns:
            float: 晕车概率(0-100%)

        晕车概率 (%)	晕车程度分级	         建议动作
        0 - 10%	    无不适感（Comfortable） 	无需干预
        10% - 30%	轻度不适（Slight）	        可接受
        30% - 50%	中度不适（Moderate）	    需关注驾驶方式
        > 50%	    明显不适（Discomfort）	    应考虑优化轨迹、减速、减小加加速度变化率
        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelX' not in df.columns or 'accelY' not in df.columns:
            self.logger.warning("缺少计算晕车概率所需的数据列")
            return self.calculated_value.get('motionSickness', 0)
        
        # 将东北天坐标系下的加速度转换为车身坐标系下的加速度
        if 'posH' not in df.columns:
            self.logger.warning("缺少航向角数据，无法进行坐标转换")
            return self.calculated_value.get('motionSickness', 0)
            
        # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
        df['posH_rad'] = np.radians(df['posH'])
        
        # 转换加速度到车身坐标系
        df['a_x_body'] = df['accelX'] * np.sin(df['posH_rad']) + df['accelY'] * np.cos(df['posH_rad'])
        df['a_y_body'] = df['accelX'] * np.cos(df['posH_rad']) - df['accelY'] * np.sin(df['posH_rad'])
        
        # Z方向加速度，如果没有则假设为0
        df['a_z_body'] = df['accelZ'] if 'accelZ' in df.columns else pd.Series(np.zeros(len(df)))
        
        # 计算时间差
        df['time_diff'] = df['simTime'].diff().fillna(0)
        
        # 应用ISO 2631-1:1997标准的频率加权滤波
        # 估计采样频率 - 假设数据是均匀采样的
        if len(df) > 1:
            time_diff = df['simTime'].diff().median()
            fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
        else:
            fs = 100  # 默认采样频率
        
        # 对各方向加速度应用适当的频率加权
        a_x_weighted = self._apply_frequency_weighting(df['a_x_body'], 'Wf', fs)  # 使用Wf滤波器(晕动相关)
        a_y_weighted = self._apply_frequency_weighting(df['a_y_body'], 'Wf', fs)
        a_z_weighted = self._apply_frequency_weighting(df['a_z_body'], 'Wf', fs)
        
        # 计算加加速度(Jerk) - 加速度的导数
        # 使用中心差分法计算导数
        df['jerk_x'] = a_x_weighted.diff() / df['time_diff']
        df['jerk_y'] = a_y_weighted.diff() / df['time_diff']
        df['jerk_z'] = a_z_weighted.diff() / df['time_diff']
        
        # 填充NaN值
        df[['jerk_x', 'jerk_y', 'jerk_z']] = df[['jerk_x', 'jerk_y', 'jerk_z']].fillna(0)
        
        # 计算Jerk的均方根值(RMS)
        jerk_squared_sum = df['jerk_x']**2 + df['jerk_y']**2 + df['jerk_z']**2
        jerk_rms = np.sqrt(np.mean(jerk_squared_sum))
        
        # 计算加速度平方和的均值
        accel_squared_sum = a_x_weighted**2 + a_y_weighted**2 + a_z_weighted**2
        accel_squared_mean = np.mean(accel_squared_sum)
        
        # 设置模型参数
        alpha = 0.1  # 加速度权重(s⁴/m²)
        beta = 0.5   # Jerk权重(s²/m²)
        gamma = 10.0  # 归一化因子(m²/s⁴)
        
        # 计算晕车概率
        acceleration_term = alpha * accel_squared_mean
        jerk_term = beta * jerk_rms
        score = (acceleration_term + jerk_term) / gamma
        probability = 100 * (1 - np.exp(-score))
        
        # 限制在0-100%范围内
        probability = np.clip(probability, 0, 100)
        
        # 记录计算结果
        self.calculated_value['motionSickness'] = probability
        self.logger.info(f"晕车概率(Motion Sickness Probability)计算结果: {probability:.2f}%")
        self.logger.info(f"加速度平方和均值: {accel_squared_mean:.4f} m²/s⁴, Jerk均方根值: {jerk_rms:.4f} m/s³")
        
        return probability

    def calculate_motion_comfort_index(self):
        """计算运动舒适度指数
        
        该指数综合考虑加速度、加加速度和角速度对乘客舒适感的影响，
        通过加权平均的方式得出一个0-10的舒适度评分，其中10表示最舒适。
        
        Returns:
            float: 运动舒适度指数(0-10)

        | 舒适指数 `comfort_index` | 等级   | 描述           |
| -------------------- | ---- | ------------ |
| **8 - 10**           | 非常舒适 | 几乎无晃动        |
| **6 - 8**            | 舒适   | 偶有轻微晃动       |
| **4 - 6**            | 一般   | 有一定晃动感       |
| **2 - 4**            | 不适   | 明显晃动、影响乘坐感受  |
| **0 - 2**            | 极度不适 | 强烈晃动，需优化控制系统 |

        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelX' not in df.columns or 'accelY' not in df.columns:
            self.logger.warning("缺少计算运动舒适度指数所需的数据列")
            return self.calculated_value.get('motionComfortIndex', 5.0)
        
        # 计算合成加速度
        df['accel_magnitude'] = np.sqrt(df['accelX']**2 + df['accelY']**2)
        if 'accelZ' in df.columns:
            df['accel_magnitude'] = np.sqrt(df['accel_magnitude']**2 + df['accelZ']**2)
        
        # 计算加加速度(Jerk)
        df['time_diff'] = df['simTime'].diff().fillna(0.01)
        df['jerk_x'] = df['accelX'].diff() / df['time_diff']
        df['jerk_y'] = df['accelY'].diff() / df['time_diff']
        df['jerk_magnitude'] = np.sqrt(df['jerk_x']**2 + df['jerk_y']**2)
        if 'accelZ' in df.columns:
            df['jerk_z'] = df['accelZ'].diff() / df['time_diff']
            df['jerk_magnitude'] = np.sqrt(df['jerk_magnitude']**2 + df['jerk_z']**2)
        
        # 计算角速度
        if 'rollRate' in df.columns and 'pitchRate' in df.columns:
            df['angular_velocity'] = np.sqrt(df['rollRate']**2 + df['pitchRate']**2)
            if 'speedH' in df.columns:  # 使用航向角速度作为偏航角速度
                df['angular_velocity'] = np.sqrt(df['angular_velocity']**2 + df['speedH']**2)
        else:
            df['angular_velocity'] = 0
        
        # 计算各指标的均方根值
        accel_rms = np.sqrt(np.mean(df['accel_magnitude']**2))
        jerk_rms = np.sqrt(np.mean(df['jerk_magnitude']**2))
        angular_rms = np.sqrt(np.mean(df['angular_velocity']**2))
        
        # 设置阈值和权重
        accel_threshold = 2.0  # m/s²，超过此值舒适度开始下降
        jerk_threshold = 1.0   # m/s³，超过此值舒适度开始下降
        angular_threshold = 0.2 # rad/s，超过此值舒适度开始下降
        
        accel_weight = 0.5     # 加速度权重
        jerk_weight = 0.3      # 加加速度权重
        angular_weight = 0.2   # 角速度权重
        
        # 计算各分量的舒适度得分(0-10)
        accel_score = 10 * np.exp(-max(0, accel_rms - accel_threshold) / accel_threshold)
        jerk_score = 10 * np.exp(-max(0, jerk_rms - jerk_threshold) / jerk_threshold)
        angular_score = 10 * np.exp(-max(0, angular_rms - angular_threshold) / angular_threshold)
        
        # 计算加权平均得分
        comfort_index = (accel_weight * accel_score + 
                         jerk_weight * jerk_score + 
                         angular_weight * angular_score)
        
        # 限制在0-10范围内
        comfort_index = np.clip(comfort_index, 0, 10)
        
        # 记录计算结果
        self.calculated_value['motionComfortIndex'] = comfort_index
        self.logger.info(f"运动舒适度指数(Motion Comfort Index)计算结果: {comfort_index:.2f}/10")
        self.logger.info(f"加速度RMS: {accel_rms:.4f} m/s², 加加速度RMS: {jerk_rms:.4f} m/s³, 角速度RMS: {angular_rms:.4f} rad/s")
        
        return comfort_index
        
    def calculate_ride_quality_score(self):
        """计算乘坐质量评分
        
        该评分基于ISO 2631标准，综合考虑振动频率、振幅和持续时间对人体的影响，
        评估车辆在不同路况下的乘坐舒适性。
        
        Returns:
            float: 乘坐质量评分(0-100)
        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelZ' not in df.columns:
            self.logger.warning("缺少计算乘坐质量评分所需的垂直加速度数据")
            return self.calculated_value.get('rideQualityScore', 70.0)
        
        # 估计采样频率
        if len(df) > 1:
            time_diff = df['simTime'].diff().median()
            fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
        else:
            fs = 100  # 默认采样频率
        
        # 应用ISO 2631-1:1997标准的频率加权滤波
        a_z_weighted = self._apply_frequency_weighting(df['accelZ'], 'Wk', fs)
        
        # 计算垂直方向加速度的均方根值
        a_z_rms = np.sqrt(np.mean(a_z_weighted**2))
        
        # 根据ISO 2631-1:1997标准的舒适度评价
        # < 0.315 m/s² - 不感到不适
        # 0.315-0.63 m/s² - 轻微不适
        # 0.5-1.0 m/s² - 有些不适
        # 0.8-1.6 m/s² - 不适
        # 1.25-2.5 m/s² - 非常不适
        # > 2.0 m/s² - 极度不适
        
        # 将RMS值映射到0-100的评分
        if a_z_rms < 0.315:
            base_score = 90
        elif a_z_rms < 0.63:
            base_score = 80
        elif a_z_rms < 1.0:
            base_score = 70
        elif a_z_rms < 1.6:
            base_score = 60
        elif a_z_rms < 2.5:
            base_score = 40
        else:
            base_score = 20
        
        # 考虑振动持续时间的影响
        duration_factor = min(1.0, 10.0 / (df['simTime'].max() - df['simTime'].min()))
        
        # 考虑振动频率分布的影响
        # 计算功率谱密度
        if len(a_z_weighted) > 50:  # 确保有足够的数据点进行频谱分析
            f, psd = self._calculate_psd(a_z_weighted, fs)
            
            # 计算人体敏感频率范围(4-8Hz)的能量占比
            sensitive_mask = (f >= 4) & (f <= 8)
            sensitive_energy = np.sum(psd[sensitive_mask])
            total_energy = np.sum(psd)
            
            frequency_factor = 1.0 - 0.3 * (sensitive_energy / total_energy if total_energy > 0 else 0)
        else:
            frequency_factor = 1.0
        
        # 计算最终评分
        ride_quality_score = base_score * duration_factor * frequency_factor
        
        # 限制在0-100范围内
        ride_quality_score = np.clip(ride_quality_score, 0, 100)
        
        # 记录计算结果
        self.calculated_value['rideQualityScore'] = ride_quality_score
        self.logger.info(f"乘坐质量评分(Ride Quality Score)计算结果: {ride_quality_score:.2f}/100")
        self.logger.info(f"垂直加速度RMS: {a_z_rms:.4f} m/s²")
        
        return ride_quality_score
    
    def _calculate_psd(self, signal, fs):
        """计算信号的功率谱密度
        
        Args:
            signal: 输入信号
            fs: 采样频率
            
        Returns:
            tuple: 频率和对应的功率谱密度
        """
        # 使用Welch方法计算PSD
        from scipy import signal as sp_signal
        f, psd = sp_signal.welch(signal, fs, nperseg=min(256, len(signal)//2))
        return f, psd

    def calculate_ava_vav(self):
        """计算多维度综合加权加速度
        
        基于ISO 2631-1:1997标准，综合考虑车辆在三个平移方向和三个旋转方向的加速度或角速度
        
        Returns:
            float: 多维度综合加权加速度值
        """
        # 定义各方向的权重系数
        k_x = 1.0  # X方向加速度权重
        k_y = 1.0  # Y方向加速度权重
        k_z = 1.0  # Z方向加速度权重
        k_roll = 0.63  # 横滚角速度权重
        k_pitch = 0.8  # 俯仰角速度权重
        k_yaw = 0.5  # 偏航角速度权重
        
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelX' not in df.columns or 'accelY' not in df.columns:
            self.logger.warning("缺少计算多维度综合加权加速度所需的数据列")
            return self.calculated_value['ava_vav']
        
        # 将东北天坐标系下的加速度转换为车身坐标系下的加速度
        # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
        if 'posH' not in df.columns:
            self.logger.warning("缺少航向角数据，无法进行坐标转换")
            return self.calculated_value['ava_vav']
            
        df['posH_rad'] = np.radians(df['posH'])
        
        # 转换加速度到车身坐标系
        # 注意：posH是航向角，北向为0度，顺时针为正
        # 车身X轴 = 东向*sin(posH) + 北向*cos(posH)
        # 车身Y轴 = 东向*cos(posH) - 北向*sin(posH)
        df['a_x_body'] = df['accelX'] * np.sin(df['posH_rad']) + df['accelY'] * np.cos(df['posH_rad'])
        df['a_y_body'] = df['accelX'] * np.cos(df['posH_rad']) - df['accelY'] * np.sin(df['posH_rad'])
        
        # Z方向加速度，如果没有则假设为0
        df['a_z_body'] = df['accelZ'] if 'accelZ' in df.columns else pd.Series(np.zeros(len(df)))
        
        # 角速度数据，如果没有则使用角速度变化率代替
        # 注意：speedH是航向角速度，需要转换为车身坐标系下的偏航角速度
        omega_roll = df['rollRate'] if 'rollRate' in df.columns else pd.Series(np.zeros(len(df)))
        omega_pitch = df['pitchRate'] if 'pitchRate' in df.columns else pd.Series(np.zeros(len(df)))
        omega_yaw = df['speedH']  # 使用航向角速度作为偏航角速度
        
        # 应用ISO 2631-1:1997标准的频率加权滤波
        # 估计采样频率 - 假设数据是均匀采样的
        if len(df) > 1:
            time_diff = df['simTime'].diff().median()
            fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
        else:
            fs = 100  # 默认采样频率
        
        # 对各方向加速度应用适当的频率加权
        a_x_weighted = self._apply_frequency_weighting(df['a_x_body'], 'Wd', fs)
        a_y_weighted = self._apply_frequency_weighting(df['a_y_body'], 'Wd', fs)
        a_z_weighted = self._apply_frequency_weighting(df['a_z_body'], 'Wk', fs)
        
        # 对角速度也应用适当的频率加权
        # 注意：ISO标准没有直接指定角速度的加权，这里使用简化处理
        omega_roll_weighted = omega_roll  # 可以根据需要应用适当的滤波
        omega_pitch_weighted = omega_pitch
        omega_yaw_weighted = omega_yaw
        
        # 计算加权均方根值 (r.m.s.)
        # 对每个方向的加速度/角速度平方后求平均，再开平方根
        a_x_rms = np.sqrt(np.mean(a_x_weighted**2))
        a_y_rms = np.sqrt(np.mean(a_y_weighted**2))
        a_z_rms = np.sqrt(np.mean(a_z_weighted**2))
        omega_roll_rms = np.sqrt(np.mean(omega_roll_weighted**2))
        omega_pitch_rms = np.sqrt(np.mean(omega_pitch_weighted**2))
        omega_yaw_rms = np.sqrt(np.mean(omega_yaw_weighted**2))
        
        # 计算综合加权加速度
        ava_vav = np.sqrt(
            k_x * a_x_rms**2 + 
            k_y * a_y_rms**2 + 
            k_z * a_z_rms**2 + 
            k_roll * omega_roll_rms**2 + 
            k_pitch * omega_pitch_rms**2 + 
            k_yaw * omega_yaw_rms**2
        )
        
        # 记录计算结果
        self.calculated_value['ava_vav'] = ava_vav
        self.logger.info(f"多维度综合加权加速度(ava_vav)计算结果: {ava_vav}")
        
        return ava_vav

    def calculate_msdv(self):
        """计算晕动剂量值(Motion Sickness Dose Value, MSDV)
        
        MSDV用于量化乘员因持续振动而产生的晕动风险，其物理意义是
        "频率加权后的加速度有效值的平方对时间的累积"，
        能够反映乘员在一定时间内受到振动刺激的总量。
        
        计算公式: MSDV = √(∫[0,T] a_ω(t)² dt)
        
        Returns:
            float: 晕动剂量值
        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelX' not in df.columns or 'accelY' not in df.columns:
            self.logger.warning("缺少计算晕动剂量值所需的数据列")
            return self.calculated_value['msdv']
        
        # 将东北天坐标系下的加速度转换为车身坐标系下的加速度
        if 'posH' not in df.columns:
            self.logger.warning("缺少航向角数据，无法进行坐标转换")
            return self.calculated_value['msdv']
            
        # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
        df['posH_rad'] = np.radians(df['posH'])
        
        # 转换加速度到车身坐标系
        # 注意：posH是航向角，北向为0度，顺时针为正
        # 车身X轴 = 东向*sin(posH) + 北向*cos(posH)
        # 车身Y轴 = 东向*cos(posH) - 北向*sin(posH)
        df['a_x_body'] = df['accelX'] * np.sin(df['posH_rad']) + df['accelY'] * np.cos(df['posH_rad'])
        df['a_y_body'] = df['accelX'] * np.cos(df['posH_rad']) - df['accelY'] * np.sin(df['posH_rad'])
        
        # Z方向加速度，如果没有则假设为0
        df['a_z_body'] = df['accelZ'] if 'accelZ' in df.columns else pd.Series(np.zeros(len(df)))
        
        # 计算时间差
        df['time_diff'] = df['simTime'].diff().fillna(0)
        total_time = df['time_diff'].sum()
        
        # 应用ISO 2631-1:1997标准的频率加权滤波
        # 估计采样频率 - 假设数据是均匀采样的
        if len(df) > 1:
            time_diff = df['simTime'].diff().median()
            fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
        else:
            fs = 100  # 默认采样频率
        
        # 对各方向加速度应用适当的频率加权
        # 对于晕动评估，使用Wf加权滤波器
        a_x_weighted = self._apply_frequency_weighting(df['a_x_body'], 'Wf', fs)
        a_y_weighted = self._apply_frequency_weighting(df['a_y_body'], 'Wf', fs)
        a_z_weighted = self._apply_frequency_weighting(df['a_z_body'], 'Wf', fs)
        
        # 先计算加权均方根值 (r.m.s.)
        a_x_rms = np.sqrt(np.sum(a_x_weighted**2 * df['time_diff']) / total_time)
        a_y_rms = np.sqrt(np.sum(a_y_weighted**2 * df['time_diff']) / total_time)
        a_z_rms = np.sqrt(np.sum(a_z_weighted**2 * df['time_diff']) / total_time)
        
        # 记录r.m.s.值用于参考
        self.logger.info(f"X方向加权均方根值: {a_x_rms}")
        self.logger.info(f"Y方向加权均方根值: {a_y_rms}")
        self.logger.info(f"Z方向加权均方根值: {a_z_rms}")
        
        # 计算MSDV - 基于r.m.s.值和总时间
        msdv_x = a_x_rms * np.sqrt(total_time)
        msdv_y = a_y_rms * np.sqrt(total_time)
        msdv_z = a_z_rms * np.sqrt(total_time)
        
        # 综合MSDV - 可以使用向量和或加权和
        # 根据ISO 2631标准，垂直方向(Z)的权重通常更高
        msdv = np.sqrt(msdv_x**2 + msdv_y**2 + (1.4 * msdv_z)**2)
        
        # 记录计算结果
        self.calculated_value['msdv'] = msdv
        self.logger.info(f"晕动剂量值(MSDV)计算结果: {msdv}")
        self.logger.info(f"X方向MSDV: {msdv_x}, Y方向MSDV: {msdv_y}, Z方向MSDV: {msdv_z}")
        
        return msdv
    
    def _cal_cur_ego_path(self, row):
        """计算车辆轨迹曲率"""
        try:
            divide = (row['speedX'] ** 2 + row['speedY'] ** 2) ** (3 / 2)
            if not divide:
                res = None
            else:
                res = (row['speedX'] * row['accelY'] - row['speedY'] * row['accelX']) / divide
        except:
            res = None
        return res
    
    def _peak_valley_determination(self, df):
        """确定角速度的峰谷"""
        peaks, _ = scipy.signal.find_peaks(
            df['speedH'], height=2.3, distance=3, 
            prominence=2.3, width=1)
        valleys, _ = scipy.signal.find_peaks(
            -df['speedH'], height=2.3, distance=3, 
            prominence=2.3, width=1)
        return sorted(list(peaks) + list(valleys))
    
    def _peak_valley_judgment(self, p_last, p_curr, tw=100, avg=4.6):
        """判断峰谷是否满足蛇行条件"""
        t_diff = p_curr[0] - p_last[0]
        v_diff = abs(p_curr[1] - p_last[1])
        s = p_curr[1] * p_last[1]

        if t_diff < tw and v_diff > avg and s < 0:
            if [p_last[0], p_curr[0]] not in self.zigzag_time_list:
                self.zigzag_time_list.append([p_last[0], p_curr[0]])
            return True
        return False
    
    def _cadence_process_new(self, lon_acc, ip_acc, ip_dec):
        """处理顿挫数据
        
        使用车辆坐标系下的纵向加速度判断顿挫
        
        Args:
            lon_acc: 纵向加速度（车辆坐标系）
            ip_acc: 加速阈值
            ip_dec: 减速阈值
            
        Returns:
            int/float: nan表示不符合顿挫条件，1表示加速顿挫，-1表示减速顿挫，0表示正常
        """
        if abs(lon_acc) < 1 or lon_acc > ip_acc or lon_acc < ip_dec:
            return np.nan
        elif abs(lon_acc) == 0:
            return 0
        elif lon_acc > 0 and lon_acc < ip_acc:
            return 1
        elif lon_acc < 0 and lon_acc > ip_dec:
            return -1
        else:
            return 0
    
    @peak_valley_decorator
    def _zigzag_detector(self, p_curr, p_last, flag=True):
        """检测蛇行事件"""
        if flag:
            # 记录蛇行事件的起止时间和帧号
            start_time = p_last[0]
            end_time = p_curr[0]
            start_frame = get_frame_with_time(self.time_list, self.frame_list, start_time)
            end_frame = get_frame_with_time(self.time_list, self.frame_list, end_time)
            
            # 计算事件持续时间
            duration = end_time - start_time
            
            # 设置最小持续时间阈值
            min_duration = 0.5  # 秒
            
            if duration >= min_duration:
                # 更新蛇行计数
                self.zigzag_count += 1
                
                # 添加到不舒适事件表
                new_row = pd.DataFrame([{
                    'start_time': start_time,
                    'end_time': end_time,
                    'start_frame': start_frame,
                    'end_frame': end_frame,
                    'type': 'zigzag'
                }])
                self.discomfort_df = pd.concat([self.discomfort_df, new_row], ignore_index=True)
                
                # 记录事件信息到zigzag_time_list
                self.zigzag_time_list.append({
                    'start_time': start_time,
                    'end_time': end_time,
                    'start_frame': start_frame,
                    'end_frame': end_frame,
                    'duration': duration
                })
        else:
            self.zigzag_count += 0
    
    @peak_valley_decorator
    def _cal_zigzag_strength(self, p_curr, p_last, flag=True):
        """计算蛇行强度"""
        if flag:
            v_diff = abs(p_curr[1] - p_last[1])
            t_diff = p_curr[0] - p_last[0]
            if t_diff > 0:
                self.zigzag_stre_list.append(v_diff / t_diff)  # 平均角加速度
        else:
            self.zigzag_stre_list = []
    
    def _get_zigzag_times(self):
        """获取所有蛇行时间点"""
        all_times = []
        for time_range in self.zigzag_time_list:
            start, end = time_range
            # 获取这个时间范围内的所有时间点
            times_in_range = self.ego_df[(self.ego_df['simTime'] >= start) & 
                                         (self.ego_df['simTime'] <= end)]['simTime'].tolist()
            all_times.extend(times_in_range)
        return all_times
    
    def calculate_zigzag_count(self):
        """计算蛇行指标"""
        self._zigzag_detector()
        
        # 生成蛇行指标图表
        self.generate_metric_chart('zigzag')
        
        return self.zigzag_count
    
    def calculate_shake_count(self):
        """计算晃动指标"""
        self._shake_detector()
        
        # 生成晃动指标图表
        self.generate_metric_chart('shake')
        
        return self.shake_count
    
    def calculate_cadence_count(self):
        """计算顿挫指标"""
        # 调用顿挫检测器
        cadence_time_ranges = self._cadence_detector()
        
        # 记录检测结果
        self.calculated_value['cadence'] = self.cadence_count
        
        # 生成顿挫指标图表
        self.generate_metric_chart('cadence')
        
        return self.cadence_count
    
    def calculate_slam_brake_count(self):
        """计算急刹车指标"""
        self._slam_brake_detector()
        
        # 生成急刹车指标图表
        self.generate_metric_chart('slamBrake')
        
        return self.slam_brake_count
        
    
    def calculate_slam_accel_count(self):
        """计算急加速指标"""
        self._slam_accel_detector()
        
        # 生成急加速指标图表
        self.generate_metric_chart('slamAccelerate')
        
        return self.slam_accel_count
        

    def calculate_vdv(self):
        """计算振动剂量值(Vibration Dose Value, VDV)指标"""
        vdv_value = super().calculate_vdv()
        
        # 生成VDV指标图表
        self.generate_metric_chart('vdv')
        
        return vdv_value

    def calculate_ava_vav(self):
        """计算多维度综合加权加速度"""
        ava_vav_value = super().calculate_ava_vav()
        
        # 生成AVA_VAV指标图表
        self.generate_metric_chart('ava_vav')
        
        return ava_vav_value

    def calculate_msdv(self):
        """计算晕动剂量值(Motion Sickness Dose Value, MSDV)"""
        msdv_value = super().calculate_msdv()
        
        # 生成MSDV指标图表
        self.generate_metric_chart('msdv')
        
        return msdv_value
    
    def _shake_detector(self, T_diff=0.5):
        """检测晃动事件 - 改进版本（使用向量化操作）
        
        该函数通过以下步骤检测车辆晃动事件：
        1. 计算横向加速度变化率和横摆角速度变化率
        2. 分析横摆角速度的短期变化特性
        3. 设置基于车速的动态阈值
        4. 综合多个条件判断晃动事件
        5. 对连续帧进行分组处理
        6. 保存晃动事件数据到CSV文件
        7. 将保存的CSV数据可视化并为晃动事件发生的时间段添加背景颜色
        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 检查是否有必要的列
        if 'lat_acc' not in df.columns or 'posH' not in df.columns:
            self.logger.warning("缺少计算晃动指标所需的数据列")
            return
            
        # 将东北天坐标系下的数据转换为车身坐标系
        # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
        df['posH_rad'] = np.radians(df['posH'])
        
        # 转换横向加速度到车身坐标系
        df['lat_acc_body'] = df['lat_acc'] * np.cos(df['posH_rad']) - df['lon_acc'] * np.sin(df['posH_rad'])
        
        # 转换横摆角速度到车身坐标系
        # speedH已经是车身坐标系下的横摆角速度，不需要转换
        df['speedH_body'] = df['speedH']
        
        # 1. 计算横向加速度变化率（使用车身坐标系下的横向加速度）
        df['lat_acc_rate'] = df['lat_acc_body'].diff() / df['simTime'].diff()
        
        # 2. 计算横摆角速度变化率（使用车身坐标系下的横摆角速度）
        df['speedH_rate'] = df['speedH_body'].diff() / df['simTime'].diff()
        
        # 3. 计算横摆角速度的短期变化特性
        window_size = 10  # 10帧窗口
        df['speedH_std'] = df['speedH'].rolling(window=window_size, min_periods=2).std()
        
        # 4. 基于车速的动态阈值
        v0 = 20 * 5/18        # ≈5.56 m/s
        k  = 0.008 * 3.6      # =0.0288 per m/s
        df['lat_acc_threshold'] = df['v'].apply(
            lambda speed: max(
                1.0,                                   # 下限 1.0 m/s²
                min(
                    1.8,                               # 上限 1.8 m/s²
                    1.8 - k * (speed - v0)             # 线性递减
                )
            )
        )
        
        df['speedH_threshold'] = df['v'].apply(
            lambda speed: max(1.5, min(3.0, 2.0 * (1 + (speed - 20) / 60)))
        )
        
        # 5. 综合判断晃动条件
        # 条件A: 横向加速度超过阈值
        condition_A = df['lat_acc'].abs() > df['lat_acc_threshold']
        
        # 条件B: 横向加速度变化率超过阈值
        lat_acc_rate_threshold = 0.5  # 横向加速度变化率阈值 (m/s³)
        condition_B = df['lat_acc_rate'].abs() > lat_acc_rate_threshold
        
        # 条件C: 横摆角速度有明显变化但不呈现周期性
        condition_C = (df['speedH_std'] > df['speedH_threshold'])
        
        # 综合条件: 满足条件A，且满足条件B或条件C
        shake_condition = condition_A & (condition_B | condition_C)
        
        # 6. 使用向量化操作检测连续事件
        event_groups = (shake_condition != shake_condition.shift()).cumsum()
        shake_events = []
        
        for _, group in df[shake_condition].groupby(event_groups):
            if len(group) >= 2:  # 至少2帧才算一次晃动
                start_time = group['simTime'].iloc[0]
                end_time = group['simTime'].iloc[-1]
                duration = end_time - start_time
                
                if duration >= T_diff:  # 只记录持续时间超过阈值的事件
                    shake_events.append({
                        'start_time': start_time,
                        'end_time': end_time,
                        'start_frame': group['simFrame'].iloc[0],
                        'end_frame': group['simFrame'].iloc[-1],
                        'duration': duration,
                        'max_lat_acc': group['lat_acc'].abs().max()
                    })
                    
                    # 添加到不舒适事件表
                    new_row = pd.DataFrame([{
                        'start_time': start_time,
                        'end_time': end_time,
                        'start_frame': group['simFrame'].iloc[0],
                        'end_frame': group['simFrame'].iloc[-1],
                        'type': 'shake'
                    }])
                    self.discomfort_df = pd.concat([self.discomfort_df, new_row], ignore_index=True)
        
        # 更新晃动计数
        self.shake_count = len(shake_events)
        self.logger.info(f"检测到 {self.shake_count} 次晃动事件")
        
        # 更新ego_df中的相关列
        self.ego_df = df.copy()
        
        # 保存晃动事件数据到CSV文件，用于后续图表生成
        if shake_events:
            try:
                # 保存用于绘图的数据
                self.shake_events = shake_events
                self.logger.info(f"检测到 {len(shake_events)} 个晃动事件，已保存用于图表生成")
            except Exception as e:
                self.logger.error(f"保存晃动事件数据失败: {str(e)}", exc_info=True)
        
        return shake_events
    
    
    def _cadence_detector(self):
        """
        顿挫检测：短时间内出现多次加速度突变（cadence），例如频繁加减速。
        - 特征点定义为 cadence 变化值非 0 的时刻；
        - 聚类这些特征点，1s 内聚为一组，去除小于 1 个点的组；
        - 统计强度与频率，记录不适事件。
        """
        # 检查必要字段是否存在
        required_fields = ['simTime', 'simFrame', 'cadence']
        acc_field = 'lon_acc_vehicle' if 'lon_acc_vehicle' in self.ego_df.columns else 'lon_acc'
        
        if not all(field in self.ego_df.columns for field in required_fields + [acc_field]):
            missing_fields = [field for field in required_fields + [acc_field] if field not in self.ego_df.columns]
            self.logger.warning(f"顿挫检测缺少必要字段: {missing_fields}，无法进行检测")
            self.cadence_count = 0
            return []
            
        # 提取必要字段
        df = self.ego_df[['simTime', 'simFrame', acc_field, 'cadence']].copy()
        # 重命名列以保持代码一致性
        df.rename(columns={acc_field: 'acc_used'}, inplace=True)
        
        # 滤除无效cadence值
        df = df[df['cadence'].notna()].copy()
        df['cadence_diff'] = df['cadence'].diff()
        df.dropna(subset=['cadence_diff'], inplace=True)
        df = df[df['cadence_diff'] != 0]

        if df.empty:
            self.logger.info("未检测到明显cadence变化，未触发顿挫事件")
            self.cadence_count = 0
            return []

        # 提取突变点信息
        time_list = df['simTime'].tolist()
        frame_list = df['simFrame'].tolist()

        # 聚类突变点：按时间差小于 TIME_RANGE 分组
        TIME_RANGE = 1.0  # 秒
        grouped_times, grouped_frames = [], []
        temp_times, temp_frames = [], []

        for i in range(len(time_list)):
            if not temp_times or (time_list[i] - temp_times[-1] <= TIME_RANGE):
                temp_times.append(time_list[i])
                temp_frames.append(frame_list[i])
            else:
                if len(temp_times) >= 1:
                    grouped_times.append(temp_times)
                    grouped_frames.append(temp_frames)
                temp_times, temp_frames = [time_list[i]], [frame_list[i]]

        if len(temp_times) >= 1:
            grouped_times.append(temp_times)
            grouped_frames.append(temp_frames)

        # 只保留有效顿挫组
        cadence_time_ranges = [[g[0], g[-1]] for g in grouped_times]
        cadence_frame_ranges = [[g[0], g[-1]] for g in grouped_frames]

        # 输出结果到 discomfort_df
        if cadence_time_ranges:
            discomfort_df = pd.DataFrame({
                'start_time': [g[0] for g in cadence_time_ranges],
                'end_time': [g[1] for g in cadence_time_ranges],
                'start_frame': [g[0] for g in cadence_frame_ranges],
                'end_frame': [g[1] for g in cadence_frame_ranges],
                'type': 'cadence'
            })
            self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)

        # 计算强度与频率（用于量化）
        stre_list, freq_list = [], []
        for group_times in grouped_times:
            g_df = df[df['simTime'].isin(group_times)]
            strength = g_df['acc_used'].abs().mean()
            stre_list.append(strength)

            if len(group_times) > 1:
                t_delta = group_times[-1] - group_times[0]
                freq = len(group_times) / t_delta if t_delta > 0 else 0
                freq_list.append(freq)

        # 存储检测统计
        self.cadence_count = len(cadence_time_ranges)
        cadence_strength = np.mean(stre_list) if stre_list else 0
        self.logger.info(f"检测到 {self.cadence_count} 次顿挫事件，平均强度：{cadence_strength:.2f}")
        
        # 记录使用的加速度字段
        self.logger.info(f"顿挫检测使用的加速度字段: {acc_field}")

        return cadence_time_ranges

    def _slam_brake_detector(self):
        """检测急刹车事件"""
        # 获取数据
        df = self.ego_df.copy()
        
        # 检查是否有必要的列
        if 'slam_brake' not in df.columns:
            self.logger.warning("缺少计算急刹车指标所需的数据列")
            return
            
        # 设置急刹车检测参数
        min_duration = 0.5  # 最小持续时间 秒
        
        # 检测连续的急刹车事件
        slam_brake_events = []
        in_event = False
        start_idx = 0
        
        for i, row in df.iterrows():
            if row['slam_brake'] == 1 and not in_event:
                # 开始新的急刹车事件
                in_event = True
                start_idx = i
            elif row['slam_brake'] == 0 and in_event:
                # 结束当前急刹车事件
                in_event = False
                end_idx = i - 1
                
                # 计算事件持续时间
                start_time = df.loc[start_idx, 'simTime']
                end_time = df.loc[end_idx, 'simTime']
                duration = end_time - start_time
                
                # 如果持续时间超过阈值，记录为有效急刹车事件
                if duration >= min_duration:
                    # 确定使用的加速度字段
                    acc_field = 'lon_acc_vehicle' if 'lon_acc_vehicle' in df.columns else 'lon_acc'
                    
                    slam_brake_events.append({
                        'start_time': start_time,
                        'end_time': end_time,
                        'start_frame': df.loc[start_idx, 'simFrame'],
                        'end_frame': df.loc[end_idx, 'simFrame'],
                        'duration': duration,
                        'min_lon_acc': df.loc[start_idx:end_idx, acc_field].min()
                    })
                    
                    # 添加到不舒适事件表
                    new_row = pd.DataFrame([{
                        'start_time': start_time,
                        'end_time': end_time,
                        'start_frame': df.loc[start_idx, 'simFrame'],
                        'end_frame': df.loc[end_idx, 'simFrame'],
                        'type': 'slam_brake'
                    }])
                    self.discomfort_df = pd.concat([self.discomfort_df, new_row], ignore_index=True)
        
        # 如果最后一个事件没有结束，检查它
        if in_event:
            end_idx = len(df) - 1
            start_time = df.loc[start_idx, 'simTime']
            end_time = df.loc[end_idx, 'simTime']
            duration = end_time - start_time
            
            if duration >= min_duration:
                # 确定使用的加速度字段
                acc_field = 'lon_acc_vehicle' if 'lon_acc_vehicle' in df.columns else 'lon_acc'
                
                slam_brake_events.append({
                    'start_time': start_time,
                    'end_time': end_time,
                    'start_frame': df.loc[start_idx, 'simFrame'],
                    'end_frame': df.loc[end_idx, 'simFrame'],
                    'duration': duration,
                    'min_lon_acc': df.loc[start_idx:end_idx, acc_field].min()
                })
                
                # 添加到不舒适事件表
                new_row = pd.DataFrame([{
                    'start_time': start_time,
                    'end_time': end_time,
                    'start_frame': df.loc[start_idx, 'simFrame'],
                    'end_frame': df.loc[end_idx, 'simFrame'],
                    'type': 'slam_brake'
                }])
                self.discomfort_df = pd.concat([self.discomfort_df, new_row], ignore_index=True)
        
        # 更新急刹车计数
        self.slam_brake_count = len(slam_brake_events)
        self.logger.info(f"检测到 {self.slam_brake_count} 次急刹车事件")
    
    def _slam_accel_detector(self):
        """检测急加速事件"""
        # 获取数据
        df = self.ego_df.copy()
        
        # 检查是否有必要的列
        if 'slam_accel' not in df.columns:
            self.logger.warning("缺少计算急加速指标所需的数据列")
            return
            
        # 设置急加速检测参数
        min_duration = 0.5  # 最小持续时间 秒
        
        # 检测连续的急加速事件
        slam_accel_events = []
        in_event = False
        start_idx = 0
        
        for i, row in df.iterrows():
            if row['slam_accel'] == 1 and not in_event:
                # 开始新的急加速事件
                in_event = True
                start_idx = i
            elif row['slam_accel'] == 0 and in_event:
                # 结束当前急加速事件
                in_event = False
                end_idx = i - 1
                
                # 计算事件持续时间
                start_time = df.loc[start_idx, 'simTime']
                end_time = df.loc[end_idx, 'simTime']
                duration = end_time - start_time
                
                # 如果持续时间超过阈值，记录为有效急加速事件
                if duration >= min_duration:
                    # 确定使用的加速度字段
                    acc_field = 'lon_acc_vehicle' if 'lon_acc_vehicle' in df.columns else 'lon_acc'
                    
                    slam_accel_events.append({
                        'start_time': start_time,
                        'end_time': end_time,
                        'start_frame': df.loc[start_idx, 'simFrame'],
                        'end_frame': df.loc[end_idx, 'simFrame'],
                        'duration': duration,
                        'max_lon_acc': df.loc[start_idx:end_idx, acc_field].max()
                    })
                    
                    # 添加到不舒适事件表
                    new_row = pd.DataFrame([{
                        'start_time': start_time,
                        'end_time': end_time,
                        'start_frame': df.loc[start_idx, 'simFrame'],
                        'end_frame': df.loc[end_idx, 'simFrame'],
                        'type': 'slam_accel'
                    }])
                    self.discomfort_df = pd.concat([self.discomfort_df, new_row], ignore_index=True)
        
        # 如果最后一个事件没有结束，检查它
        if in_event:
            end_idx = len(df) - 1
            start_time = df.loc[start_idx, 'simTime']
            end_time = df.loc[end_idx, 'simTime']
            duration = end_time - start_time
            
            if duration >= min_duration:
                # 确定使用的加速度字段
                acc_field = 'lon_acc_vehicle' if 'lon_acc_vehicle' in df.columns else 'lon_acc'
                
                slam_accel_events.append({
                    'start_time': start_time,
                    'end_time': end_time,
                    'start_frame': df.loc[start_idx, 'simFrame'],
                    'end_frame': df.loc[end_idx, 'simFrame'],
                    'duration': duration,
                    'max_lon_acc': df.loc[start_idx:end_idx, acc_field].max()
                })
                
                # 添加到不舒适事件表
                new_row = pd.DataFrame([{
                    'start_time': start_time,
                    'end_time': end_time,
                    'start_frame': df.loc[start_idx, 'simFrame'],
                    'end_frame': df.loc[end_idx, 'simFrame'],
                    'type': 'slam_accel'
                }])
                self.discomfort_df = pd.concat([self.discomfort_df, new_row], ignore_index=True)
        
        # 更新急加速计数
        self.slam_accel_count = len(slam_accel_events)
        self.logger.info(f"检测到 {self.slam_accel_count} 次急加速事件")
class ComfortManager:
    """舒适性指标计算主类"""
    
    def __init__(self, data_processed):
        self.data = data_processed
        self.logger = LogManager().get_logger()
        self.registry = ComfortRegistry(self.data)

    def report_statistic(self):
        """生成舒适性评分报告"""
        comfort_result = self.registry.batch_execute()
        
        return comfort_result