模拟防御性安全驾驶算法初探

模拟防御性安全驾驶算法初探

2026-07-04

这是一个纯算法实现的 Python 模块，仅依赖 math 和 numpy，不涉及任何外部网络/REST API。共包含 8 个核心模块，覆盖车辆动力学、运动平台控制、安全评估、训练自适应和驾驶员状态检测。

模块总览

#	模块	核心算法	类/函数	主要依赖
1	轮胎模型	魔术公式 + 摩擦椭圆	PacejkaParams, pacejka_*	math
2	运动提示	经典冲洗三通道滤波	ClassicalWashout	numpy
3	安全指标	运动学公式 + 加权评分	SafetyMetrics	—
4	危险感知	四维规则打分	HazardPerceptionScorer	numpy
5	行为评分	六维加权综合	DrivingBehaviorScorer	numpy
6	训练自适应	比例反馈调节	AdaptiveTrainer	numpy
7	疲劳检测	滑动窗口 PERCLOS	PERCLOSDetector	numpy
8	风格分类	规则阈值分类	DrivingStyleClassifier	numpy

---

1. Pacejka Magic Formula 轮胎模型

算法原理

Pacejka 魔术公式是汽车工业标准的经验轮胎模型，用统一的三角函数形式拟合轮胎力与滑移率/侧偏角的非线性关系：

F = D · sin( C · arctan( B·x − E·(B·x − arctan(B·x)) ) )

四个参数含义：

• B（刚度因子 Stiffness）：决定原点斜率
• C（形状因子 Shape）：决定曲线形状
• D（峰值因子 Peak）：决定最大值
• E（曲率因子 Curvature）：决定峰值附近形状

三个核心函数

函数	功能	输入 → 输出
pacejka_longitudinal	纵向力 Fx	滑移率 κ, 法向力 Fz → Fx (N)
pacejka_lateral	侧向力 Fy	侧偏角 α (rad), 法向力 Fz → Fy (N)
combined_slip	组合滑移力	κ, α, Fz → (Fx, Fy)

组合滑移先用前两式算出纯纵向/侧向力，再用摩擦椭圆约束做耦合修正：

约束: (Fx/Fmax)² + (Fy/Fmax)² ≤ 1
超界时按范数等比缩放: scale = 1 / √(ratio_x² + ratio_y²)

代码要点

# 纵向力计算
B, C, D, E = p.Bx, p.Cx, p.Dx, p.Ex
kappa = slip_ratio * 100          # 滑移率转为百分比
Fx = Fz * D * math.sin(C * math.atan(B * kappa - E * (B * kappa - math.atan(B * kappa))))

⚠️ 注意：使用可变默认参数 p: PacejkaParams = PacejkaParams()。dataclass 是可变对象，作为默认参数存在隐患（虽然此处不会被修改，但非最佳实践）。

---

2. Classical Washout 运动提示算法

算法原理

将车辆高动态运动映射到 6-DOF 运动平台有限的位移范围内，分三个独立通道：

通道	作用	滤波器类型	截止频率
高通平移	提取瞬时线加速度，二重积分得位移	2 阶高通	1.0 Hz
低通倾斜	提取持续加速度，倾斜平台靠重力分量模拟	2 阶低通	0.5 Hz
高通旋转	提取高频角加速度	2 阶高通	2.0 Hz

关键技术

(1) 双线性变换（Tustin 法）离散化

将连续传递函数 H(s) = s² / (s² + 2ζωs + ω²) 离散为差分方程：

# s = (2/dt)·(z-1)/(z+1) 代入并整理
a0 = 4 + 4*ζ*ω*dt + ω²*dt²
a1 = -8 + 2*ω²*dt²
a2 = 4 - 4*ζ*ω*dt + ω²*dt²

(2) 双二阶（biquad）IIR 滤波器

def _biquad_filter(self, x, state, a, b):
    y = b[0] * x + state[0]              # 当前输出
    state[0] = b[1] * x - a[1] * y + state[1]   # 状态更新1
    state[1] = b[2] * x - a[2] * y               # 状态更新2
    return y

(3) 倾斜通道的重力分量模拟

sustained_acc = self._biquad_filter(vehicle_acc[i], ...)
tilt_angles[i] = math.atan2(sustained_acc, g)   # 倾角 = atan(a/g)

平台前倾角度 θ 时，乘员感受到的重力水平分量 ≈ g·sin(θ) ≈ a，从而用有限角度模拟持续的纵向加速度。

(4) 输出限位 -

platform_cmd[3:6] = np.clip(platform_cmd[3:6], -self.max_rot, self.max_rot)

保证指令不超出机械行程（位移 ±0.3m，角度 ±25°）。

输出

6-DOF 平台指令 [x, y, z, roll, pitch, yaw]，其中 roll/pitch 来自倾斜通道，yaw 来自旋转通道。

---

3. TTC / THW 实时安全指标

算法原理

基于运动学的实时碰撞风险评估，无需机器学习，纯公式计算。

方法	含义	公式
compute_ttc	碰撞时间	TTC = 相对距离 / 接近速度
compute_thw	车头时距	THW = (前车尾−自车头) / 自车速度
lane_departure_warning	车道偏离	偏移量 > (车道宽−车宽)/2
collision_risk_score	综合风险评分	加权: TTC 50% + THW 30% + 车道偏离 20%

安全阈值

TTC_DANGER = 2.0      # TTC < 2s 危险
TTC_WARNING = 4.0     # TTC < 4s 警告
THW_DANGER = 1.0      # THW < 1s 危险
THW_WARNING = 2.0     # THW < 2s 警告

代码要点

# TTC 计算 
rel_dist = target_pos - ego_pos
rel_vel = ego_vel - target_vel        # 接近速度
if rel_vel <= 0.01:
    return float('inf')               # 未接近, 无碰撞风险
ttc = rel_dist / rel_vel
return max(ttc, 0.0)

风险评分采用线性衰减：ttc_risk = 100·(1 − ttc/10)，距离越近风险越高。

---

4. 危险感知评分模型

算法原理

规则驱动的四维加权打分，评估驾驶员发现和响应潜在危险的能力。每次危险事件满分 100 分。

四个评分维度

维度	分值	评分逻辑
反应时间	0–40	分段阈值：≤0.5s→40, ≤0.8s→35, ≤1.2s→28, ≤1.5s→20, ≤2.0s→10, 否则→0
注视分布	0–20	注视危险区域→20，否则→0
预判准确率	0–25	事件前预判到→25，否则→0
操作正确性	0–15	操作正确→15，否则→0

反应时间计算

# L377: 反应时间 = 响应时间戳 - 危险出现时间戳 (绝对时间相减)
reaction_time = driver_response_time - hazard_appear_time

📌 重要：两个参数都是绝对时间戳，相减得到反应时间。调用时必须传入绝对时间，而非相对反应时长。

综合评级

avg_score >= 85  → "优秀"
avg_score >= 70  → "良好"
avg_score >= 50  → "合格"
avg_score >= 30  → "待提高"
else             → "不合格"

get_final_score() 还输出注视率、预判率、正确操作率等统计指标。

---

5. 驾驶行为综合安全评分

算法原理

最具工程含量的模块，从 6 个维度加权评估一段驾驶的整体安全性，满分 100。

六维评分体系

维度	权重	扣分逻辑
速度控制	0.20	超速占比×60 + 急加减速占比×40（阈值 3 m/s²）
转向稳定性	0.15	方向盘角速度标准差×0.3 + 急转向占比×50（阈值 180°/s）
跟车安全	0.20	TTC 危险占比×70 + TTC 警告占比×30
车道保持	0.15	偏离次数占比×70 + 平均偏移×30
危险响应	0.20	正确率 × 反应时间因子
规则遵守	0.10	违规严重度累计×25

关键代码

(1) 转向平顺性

steering_rate = np.diff(steering)   # 方向盘角速度序列
jerk = np.std(steering_rate)        # 角速度标准差, 反映操作平顺性

(2) 危险响应因子

if avg_rt <= 0.8:   rt_factor = 1.0     # 反应快, 满分
elif avg_rt <= 1.5: rt_factor = 0.7
elif avg_rt <= 2.0: rt_factor = 0.4
else:               rt_factor = 0.1     # 反应慢, 大幅折减
scores['hazard_response'] = correct_rate * 100 * rt_factor

(3) 加权综合

total_score = sum(scores[k] * self.WEIGHTS[k] for k in self.WEIGHTS)

评级

>= 90 → "A级 (安全)"
>= 75 → "B级 (良好)"
>= 60 → "C级 (合格)"
>= 40 → "D级 (待提高)"
else  → "E级 (不合格)"

---

6. 自适应训练难度调节

算法原理

基于绩效误差的比例反馈调节（类似控制论中的比例控制器 / 梯度下降思想），目标是把学员维持在”最近发展区”（目标成功率 75%）。

四个难度参数

PARAM_RANGES = {
    'traffic_density':    (0.1, 1.0),   # 交通密度
    'hazard_frequency':   (0.1, 1.0),   # 危险事件频率
    'weather_complexity': (0.0, 1.0),   # 天气复杂度
    'scenario_complexity': (0.1, 1.0)   # 场景复杂度
}

核心调节逻辑

# 1. 计算绩效误差 (实际成功率 - 目标成功率)
success_rate = scenario_score / 100.0
error = success_rate - self.TARGET_SUCCESS_RATE   # 0.75

# 2. 比例调整: 表现好(error>0)→增难度, 表现差→降难度
adjustment = self.LEARNING_RATE * error           # 学习率 0.08

# 3. 各参数按其范围宽度等比调整, 并限幅
for param in self.difficulty:
    lo, hi = self.PARAM_RANGES[param]
    self.difficulty[param] += adjustment * (hi - lo)
    self.difficulty[param] = np.clip(self.difficulty[param], lo, hi)

弱点追踪

得分 < 60 的场景类型会被累计计数，get_training_plan() (L701) 据此给出薄弱环节和训练建议：

if trend < 40:  "建议降低难度, 重新练习基础场景"
elif trend < 60: "建议保持当前难度, 巩固薄弱环节"
elif trend < 80: "表现良好, 可以适度提升难度"
else:            "表现优秀, 建议挑战高难度场景"

📌 说明：注释标为 “RL-based”（强化学习），但实际是比例反馈控制，无 Q 表或策略网络。

---

7. PERCLOS 疲劳检测

算法原理

PERCLOS（Percentage of Eye Closure，眼睑闭合时间占比） 是行业公认的疲劳检测指标。

在时间窗口内，眼睑闭合超过 80% 的时间占比即为 PERCLOS 值。

核心步骤

(1) 滑动窗口数据管理

WINDOW_SIZE = 60   # 60秒滑动窗口

def update(self, eye_closure_ratio, timestamp):
    self.eye_data.append((timestamp, eye_closure_ratio))
    # 移除过期数据 (60秒前)
    cutoff = timestamp - self.WINDOW_SIZE
    self.eye_data = [(t, e) for t, e in self.eye_data if t >= cutoff]

(2) 眨眼检测

# 检测"快速闭合"边沿: 上一帧张开(<0.5) → 当前闭合(>0.8)
if prev < 0.5 and eye_closure_ratio > 0.8:
    self.blink_times.append(timestamp)

(3) PERCLOS 计算

closed_count = sum(1 for _, e in self.eye_data if e > self.EYE_CLOSURE_THRESHOLD)
perclos = closed_count / len(self.eye_data)

多指标融合判定

PERCLOS 值	等级	评分
≥ 0.30 (30%)	严重疲劳	100
≥ 0.15 (15%)	疲劳	70
≥ 0.08 (8%)	轻度疲劳	40
< 0.08	清醒	0

修正项：眨眼频率异常（<8 或 >30 次/分钟）时，评分 +15。

---

8. 驾驶风格分类器

算法原理

基于规则的阈值分类（注释明确说明是 LSTM 的基线版本，当前未用神经网络），提取 4 类统计特征，加权计算”激进度评分”（0–100）。

四个特征因素（合计 100 分）

因素	权重	激进判据
速度比（车速/限速）	30%	>0.95→30, >0.85→20, >0.75→10
纵向加速度标准差	30%	>2.0→30, >1.2→20, >0.8→10
转向角速度标准差	20%	>100→20, >60→12, >30→6
平均车头时距 THW	20%	<1.0→20, <1.5→12, <2.0→6

三分类规则

if aggressiveness >= 65:   style = "激进型 (Aggressive)"
elif aggressiveness >= 35: style = "平稳型 (Moderate)"
else:                      style = "保守型 (Conservative)"

置信度计算

# 激进型: 越远离 65 越确信
confidence = min((aggressiveness - 65) / 35 + 0.6, 1.0)
# 平稳型: 越接近 50 越确信
confidence = 1.0 - abs(aggressiveness - 50) / 50
# 保守型: 越远离 35 越确信
confidence = min((35 - aggressiveness) / 35 + 0.6, 1.0)

完整代码

模块清单:
  1. Pacejka Magic Formula 轮胎模型
  2. Classical Washout 运动提示算法
  3. TTC / THW 实时安全指标计算
  4. 危险感知 (Hazard Perception) 评分模型
  5. 驾驶行为综合安全评分模型
  6. 自适应训练难度调节 (RL-based)
  7. PERCLOS 疲劳检测模型
  8. 驾驶风格分类器 (LSTM-based)

import math
import numpy as np
from dataclasses import dataclass, field
from enum import IntEnum
from typing import Optional


# ===========================================================================
# 1. Pacejka Magic Formula 轮胎模型
#    行业标准的经验轮胎模型，描述轮胎力与滑移率之间的关系
#    F = D * sin(C * arctan(B*κ - E*(B*κ - arctan(B*κ))))
# ===========================================================================

@dataclass
class PacejkaParams:
    """Pacejka Magic Formula 参数集"""
    # 纵向力参数
    Bx: float = 10.0    # 刚度因子 (Stiffness)
    Cx: float = 1.65    # 形状因子 (Shape)
    Dx: float = 1.0     # 峰值因子 (Peak) — 单位法向力下的最大纵向力
    Ex: float = 0.97    # 曲率因子 (Curvature)
    # 侧向力参数
    By: float = 8.0
    Cy: float = 1.3
    Dy: float = 1.0
    Ey: float = 0.97
    # 组合滑移修正
    px: float = 0.5     # 纵向对侧向的影响系数
    py: float = 0.5     # 侧向对纵向的影响系数


def pacejka_longitudinal(slip_ratio: float, Fz: float, p: PacejkaParams = PacejkaParams()) -> float:
    """
    计算纵向轮胎力
    :param slip_ratio: 滑移率 κ = (ω·R - V) / V,  制动时为负, 驱动时为正
    :param Fz: 法向力 (N)
    :param p: Pacejka参数
    :return: 纵向力 Fx (N)
    """
    B, C, D, E = p.Bx, p.Cx, p.Dx, p.Ex
    kappa = slip_ratio * 100  # 转换为百分比
    Fx = Fz * D * math.sin(C * math.atan(B * kappa - E * (B * kappa - math.atan(B * kappa))))
    return Fx


def pacejka_lateral(slip_angle: float, Fz: float, p: PacejkaParams = PacejkaParams()) -> float:
    """
    计算侧向轮胎力
    :param slip_angle: 侧偏角 α (rad)
    :param Fz: 法向力 (N)
    :param p: Pacejka参数
    :return: 侧向力 Fy (N)
    """
    B, C, D, E = p.By, p.Cy, p.Dy, p.Ey
    alpha = slip_angle * 180 / math.pi  # 转换为角度
    Fy = Fz * D * math.sin(C * math.atan(B * alpha - E * (B * alpha - math.atan(B * alpha))))
    return Fy


def combined_slip(slip_ratio: float, slip_angle: float, Fz: float,
                  p: PacejkaParams = PacejkaParams()) -> tuple:
    """
    组合滑移工况下的轮胎力计算 (纵向+侧向耦合)
    使用摩擦椭圆修正
    :return: (Fx, Fy) 纵向力和侧向力
    """
    Fx0 = pacejka_longitudinal(slip_ratio, Fz, p)
    Fy0 = pacejka_lateral(slip_angle, Fz, p)

    # 摩擦椭圆修正
    Fmax = Fz * p.Dx  # 最大可用摩擦力
    ratio_x = Fx0 / Fmax if Fmax > 0 else 0
    ratio_y = Fy0 / Fmax if Fmax > 0 else 0

    # 椭圆约束: (Fx/Fx_max)^2 + (Fy/Fy_max)^2 <= 1
    scale = 1.0
    norm = ratio_x**2 + ratio_y**2
    if norm > 1.0:
        scale = 1.0 / math.sqrt(norm)

    Fx = Fx0 * scale
    Fy = Fy0 * scale
    return Fx, Fy


# ===========================================================================
# 2. Classical Washout 运动提示算法 (Motion Cueing Algorithm)
#    将车辆高动态运动映射到运动平台有限位移范围内
#    分三个通道: 高通平移、低通倾斜、高通旋转
# ===========================================================================

class ClassicalWashout:
    """
    经典冲洗运动提示算法

    原理:
    - 高通通道: 提取高频平移加速度 (瞬时加速度), 经冲洗滤波回到中立位
    - 低通通道: 提取低频持续加速度, 通过倾斜平台利用重力分量模拟
    - 旋转通道: 提取高频角速度/角加速度, 经冲洗滤波

    输出: 6-DOF 运动平台指令 (x, y, z, roll, pitch, yaw)
    """

    def __init__(self, dt: float = 0.01):
        self.dt = dt  # 仿真步长 (s)

        # === 高通平移滤波器参数 ===
        # 2阶高通: H(s) = s^2 / (s^2 + 2*ζ*ω*s + ω^2)
        self.hp_omega = 2.0 * math.pi * 1.0   # 截止频率 1.0 Hz
        self.hp_zeta = 0.7                     # 阻尼比

        # === 低通倾斜滤波器参数 ===
        self.lp_omega = 2.0 * math.pi * 0.5   # 截止频率 0.5 Hz
        self.lp_zeta = 1.0

        # === 高通旋转滤波器参数 ===
        self.hp_rot_omega = 2.0 * math.pi * 2.0
        self.hp_rot_zeta = 0.7

        # 状态变量 (离散化状态空间)
        self._init_filters()

        # 平台位置状态
        self.pos = np.zeros(3)       # x, y, z 位移
        self.rot = np.zeros(3)       # roll, pitch, yaw 角度
        self.vel = np.zeros(3)       # 平台速度
        self.rot_vel = np.zeros(3)   # 平台角速度

        # 限位参数
        self.max_pos = np.array([0.3, 0.3, 0.15])      # 最大位移 (m)
        self.max_rot = np.array([0.44, 0.44, 0.44])    # 最大角度 (rad) ≈ ±25°

    def _init_filters(self):
        """初始化离散滤波器状态"""
        dt = self.dt

        # 高通平移 (2阶)
        wh = self.hp_omega
        zh = self.hp_zeta
        # 离散化 (双线性变换)
        a0 = 4 + 4 * zh * wh * dt + wh**2 * dt**2
        a1 = -8 + 2 * wh**2 * dt**2
        a2 = 4 - 4 * zh * wh * dt + wh**2 * dt**2
        b0 = 4 / a0
        b1 = -8 / a0
        b2 = 4 / a0
        self.hp_a = np.array([1, a1 / a0, a2 / a0])
        self.hp_b = np.array([b0, b1, b2])
        self.hp_state_pos = np.zeros((3, 2))   # 3轴, 每轴2个历史状态
        self.hp_state_acc = np.zeros((3, 2))

        # 低通倾斜
        wl = self.lp_omega
        zl = self.lp_zeta
        a0l = 4 + 4 * zl * wl * dt + wl**2 * dt**2
        a1l = -8 + 2 * wl**2 * dt**2
        a2l = 4 - 4 * zl * wl * dt + wl**2 * dt**2
        b0l = wl**2 * dt**2 / a0l
        b1l = 2 * wl**2 * dt**2 / a0l
        b2l = wl**2 * dt**2 / a0l
        self.lp_a = np.array([1, a1l / a0l, a2l / a0l])
        self.lp_b = np.array([b0l, b1l, b2l])
        self.lp_state = np.zeros((2, 2))  # x, y 两轴

        # 高通旋转
        wr = self.hp_rot_omega
        zr = self.hp_rot_zeta
        a0r = 4 + 4 * zr * wr * dt + wr**2 * dt**2
        a1r = -8 + 2 * wr**2 * dt**2
        a2r = 4 - 4 * zr * wr * dt + wr**2 * dt**2
        self.hp_rot_a = np.array([1, a1r / a0r, a2r / a0r])
        self.hp_rot_b = np.array([4 / a0r, -8 / a0r, 4 / a0r])
        self.hp_rot_state = np.zeros((3, 2))

    def _biquad_filter(self, x: float, state: np.ndarray,
                       a: np.ndarray, b: np.ndarray) -> float:
        """双二阶滤波器"""
        y = b[0] * x + state[0]
        state[0] = b[1] * x - a[1] * y + state[1]
        state[1] = b[2] * x - a[2] * y
        return y

    def update(self, vehicle_acc: np.ndarray, vehicle_rot_rate: np.ndarray,
               vehicle_rot_acc: np.ndarray) -> np.ndarray:
        """
        单步更新运动平台指令

        :param vehicle_acc: 车辆线加速度 [ax, ay, az] (m/s²)
        :param vehicle_rot_rate: 车辆角速度 [wx, wy, wz] (rad/s)
        :param vehicle_rot_acc: 车辆角加速度 [dwx, dwy, dwz] (rad/s²)
        :return: 平台指令 [x, y, z, roll, pitch, yaw] (m, rad)
        """
        g = 9.81

        # === 1. 高通平移通道 ===
        hp_pos = np.zeros(3)
        for i in range(3):
            # 滤波加速度
            acc_filtered = self._biquad_filter(
                vehicle_acc[i], self.hp_state_acc[i], self.hp_a, self.hp_b)
            # 二重积分得到位移
            self.vel[i] += acc_filtered * self.dt
            self.pos[i] += self.vel[i] * self.dt
            # 限位
            self.pos[i] = np.clip(self.pos[i], -self.max_pos[i], self.max_pos[i])

        # === 2. 低通倾斜通道 (利用重力分量模拟持续加速度) ===
        # 纵向加速度 → pitch倾斜, 横向加速度 → roll倾斜
        tilt_angles = np.zeros(2)
        for i in range(2):  # x, y
            sustained_acc = self._biquad_filter(
                vehicle_acc[i], self.lp_state[i], self.lp_a, self.lp_b)
            # 倾斜角度 = atan(a_sustained / g)
            tilt_angles[i] = math.atan2(sustained_acc, g)

        # === 3. 高通旋转通道 ===
        for i in range(3):
            self.rot[i] = self._biquad_filter(
                vehicle_rot_acc[i], self.hp_rot_state[i],
                self.hp_rot_a, self.hp_rot_b)

        # === 组合输出 ===
        platform_cmd = np.zeros(6)
        platform_cmd[0:3] = self.pos              # x, y, z 位移
        platform_cmd[3] = tilt_angles[1]          # roll (横向加速度引起)
        platform_cmd[4] = -tilt_angles[0]         # pitch (纵向加速度引起, 符号取反)
        platform_cmd[5] = self.rot[2]             # yaw (偏航)

        # 限位
        platform_cmd[3:6] = np.clip(platform_cmd[3:6], -self.max_rot, self.max_rot)

        return platform_cmd


# ===========================================================================
# 3. TTC / THW 实时安全指标计算
# ===========================================================================

class SafetyMetrics:
    """
    实时安全指标计算器

    TTC (Time to Collision): 碰撞时间 = 相对距离 / 相对接近速度
    THW (Time Headway): 车头时距 = 车间距 / 自车速度
    """

    # 安全阈值
    TTC_DANGER = 2.0       # TTC < 2s 为危险
    TTC_WARNING = 4.0      # TTC < 4s 为警告
    THW_DANGER = 1.0       # THW < 1s 为危险
    THW_WARNING = 2.0      # THW < 2s 为警告

    @staticmethod
    def compute_ttc(ego_pos: float, ego_vel: float,
                    target_pos: float, target_vel: float) -> float:
        """
        计算碰撞时间 TTC
        :return: TTC (s), 若不收敛返回 float('inf')
        """
        rel_dist = target_pos - ego_pos
        rel_vel = ego_vel - target_vel  # 接近速度

        if rel_vel <= 0.01:
            return float('inf')  # 没有在接近

        ttc = rel_dist / rel_vel
        return max(ttc, 0.0)

    @staticmethod
    def compute_thw(ego_pos: float, ego_vel: float,
                    lead_pos: float, vehicle_length: float = 4.5) -> float:
        """
        计算车头时距 THW
        :param vehicle_length: 前车长度, 用于计算实际间距
        :return: THW (s)
        """
        gap = lead_pos - ego_pos - vehicle_length  # 实际间距
        if ego_vel < 0.01:
            return float('inf')
        return max(gap / ego_vel, 0.0)

    @staticmethod
    def lane_departure_warning(lateral_offset: float, lane_width: float = 3.75,
                               vehicle_width: float = 1.8) -> tuple:
        """
        车道偏离检测
        :return: (is_departure, deviation_ratio)  偏离比例 0~1
        """
        half_lane = lane_width / 2
        half_vehicle = vehicle_width / 2
        threshold = half_lane - half_vehicle
        deviation = abs(lateral_offset)
        is_departure = deviation > threshold
        ratio = min(deviation / threshold, 1.0) if threshold > 0 else 1.0
        return is_departure, ratio

    @staticmethod
    def collision_risk_score(ttc: float, thw: float,
                             lane_deviation: float = 0.0) -> float:
        """
        综合碰撞风险评分 (0-100, 100为最高风险)
        """
        # TTC 风险
        if ttc == float('inf') or ttc > 10:
            ttc_risk = 0
        else:
            ttc_risk = max(0, 100 * (1 - ttc / 10))

        # THW 风险
        if thw == float('inf') or thw > 5:
            thw_risk = 0
        else:
            thw_risk = max(0, 100 * (1 - thw / 5))

        # 车道偏离风险
        ld_risk = min(abs(lane_deviation) * 50, 100)

        # 加权综合
        risk = 0.5 * ttc_risk + 0.3 * thw_risk + 0.2 * ld_risk
        return min(risk, 100.0)


# ===========================================================================
# 4. 危险感知 (Hazard Perception) 评分模型
# ===========================================================================

class HazardPerceptionScorer:
    """
    危险感知评分模型

    评估驾驶员发现和响应潜在危险的能力:
    - 反应时间 (Reaction Time): 从危险出现到驾驶员操作响应的时间
    - 注视分布 (Gaze Distribution): 视线是否覆盖关键危险区域
    - 预判准确率 (Prediction Accuracy): 是否提前预判危险
    - 操作正确性 (Action Correctness): 采取的操作是否正确
    """

    def __init__(self):
        self.events: list = []
        self.scores: list = []

    def record_hazard_event(self,
                            hazard_appear_time: float,
                            driver_response_time: float,
                            gaze_on_hazard: bool,
                            predicted_before_event: bool,
                            action_correct: bool,
                            action_type: str = "brake"):
        """
        记录一次危险事件并评分

        :param hazard_appear_time: 危险出现时间戳
        :param driver_response_time: 驾驶员响应时间戳
        :param gaze_on_hazard: 驾驶员是否注视了危险区域
        :param predicted_before_event: 是否在事件发生前预判到
        :param action_correct: 采取的操作是否正确
        :param action_type: 操作类型 (brake/steer/brake+steer)
        """
        reaction_time = driver_response_time - hazard_appear_time

        # === 1. 反应时间评分 (0-40分) ===
        # 正常反应时间 0.5-1.5s, 优秀 <0.8s, 差 >2.0s
        if reaction_time <= 0.5:
            rt_score = 40
        elif reaction_time <= 0.8:
            rt_score = 35
        elif reaction_time <= 1.2:
            rt_score = 28
        elif reaction_time <= 1.5:
            rt_score = 20
        elif reaction_time <= 2.0:
            rt_score = 10
        else:
            rt_score = 0

        # === 2. 注视分布评分 (0-20分) ===
        gaze_score = 20 if gaze_on_hazard else 0

        # === 3. 预判准确率评分 (0-25分) ===
        pred_score = 25 if predicted_before_event else 0

        # === 4. 操作正确性评分 (0-15分) ===
        action_score = 15 if action_correct else 0

        total = rt_score + gaze_score + pred_score + action_score

        event_record = {
            'reaction_time': reaction_time,
            'rt_score': rt_score,
            'gaze_score': gaze_score,
            'pred_score': pred_score,
            'action_score': action_score,
            'total_score': total,
            'action_type': action_type
        }
        self.events.append(event_record)
        self.scores.append(total)

    def get_final_score(self) -> dict:
        """获取综合危险感知评分"""
        if not self.scores:
            return {'hp_score': 0, 'level': '未评估', 'avg_rt': 0}

        avg_score = np.mean(self.scores)
        avg_rt = np.mean([e['reaction_time'] for e in self.events])
        gaze_rate = np.mean([e['gaze_score'] > 0 for e in self.events])
        pred_rate = np.mean([e['pred_score'] > 0 for e in self.events])
        action_rate = np.mean([e['action_score'] > 0 for e in self.events])

        # 评级
        if avg_score >= 85:
            level = "优秀"
        elif avg_score >= 70:
            level = "良好"
        elif avg_score >= 50:
            level = "合格"
        elif avg_score >= 30:
            level = "待提高"
        else:
            level = "不合格"

        return {
            'hp_score': round(avg_score, 1),
            'level': level,
            'avg_rt': round(avg_rt, 3),
            'gaze_rate': round(gaze_rate * 100, 1),
            'prediction_rate': round(pred_rate * 100, 1),
            'correct_action_rate': round(action_rate * 100, 1),
            'total_events': len(self.events)
        }


# ===========================================================================
# 5. 驾驶行为综合安全评分模型
# ===========================================================================

class DrivingBehaviorScorer:
    """
    驾驶行为综合安全评分模型

    从多维度评估驾驶安全性:
    - 速度控制 (Speed Control): 超速、急加速/急减速频率
    - 转向稳定性 (Steering Stability): 方向盘操作平顺性、急转向
    - 跟车安全 (Following Safety): TTC/THW 违规次数
    - 车道保持 (Lane Keeping): 车道偏离次数
    - 危险响应 (Hazard Response): 危险事件中的操作正确性
    - 规则遵守 (Rule Compliance): 闯红灯、违规变道等
    """

    # 各维度权重
    WEIGHTS = {
        'speed_control':    0.20,
        'steering_stability': 0.15,
        'following_safety': 0.20,
        'lane_keeping':     0.15,
        'hazard_response':  0.20,
        'rule_compliance':  0.10
    }

    def __init__(self):
        self.speed_data: list = []
        self.steering_data: list = []
        self.acceleration_data: list = []
        self.ttc_data: list = []
        self.thw_data: list = []
        self.lane_deviation_data: list = []
        self.hazard_events: list = []
        self.violations: list = []

    def update_telemetry(self, speed: float, steering_angle: float,
                         acceleration: float, ttc: float, thw: float,
                         lane_deviation: float, timestamp: float):
        """更新遥测数据"""
        self.speed_data.append((timestamp, speed))
        self.steering_data.append((timestamp, steering_angle))
        self.acceleration_data.append((timestamp, acceleration))
        self.ttc_data.append((timestamp, ttc))
        self.thw_data.append((timestamp, thw))
        self.lane_deviation_data.append((timestamp, lane_deviation))

    def add_violation(self, violation_type: str, timestamp: float, severity: float):
        """记录违规事件"""
        self.violations.append({
            'type': violation_type,
            'timestamp': timestamp,
            'severity': severity  # 0-1
        })

    def add_hazard_response(self, correct: bool, reaction_time: float):
        """记录危险响应事件"""
        self.hazard_events.append({
            'correct': correct,
            'reaction_time': reaction_time
        })

    def compute_score(self, speed_limit: float = 60) -> dict:
        """计算综合安全评分 (0-100)"""
        scores = {}

        # === 1. 速度控制 (0-100) ===
        if self.speed_data:
            speeds = [s for _, s in self.speed_data]
            over_speed_count = sum(1 for s in speeds if s > speed_limit * 1.1)
            over_speed_ratio = over_speed_count / len(speeds)

            # 急加速/急减速次数
            hard_acc = sum(1 for a in [a for _, a in self.acceleration_data]
                          if abs(a) > 3.0)  # >3 m/s²
            hard_acc_ratio = hard_acc / max(len(self.acceleration_data), 1)

            speed_score = 100 - over_speed_ratio * 60 - hard_acc_ratio * 40
            scores['speed_control'] = max(0, min(100, speed_score))
        else:
            scores['speed_control'] = 100

        # === 2. 转向稳定性 (0-100) ===
        if len(self.steering_data) > 1:
            steering = [s for _, s in self.steering_data]
            # 方向盘角速度标准差
            steering_rate = np.diff(steering)
            jerk = np.std(steering_rate)
            # 急转向次数 (角速度 > 180°/s)
            sharp_turns = sum(1 for r in steering_rate if abs(r) > 180)
            sharp_ratio = sharp_turns / max(len(steering_rate), 1)

            steer_score = 100 - min(jerk * 0.3, 50) - sharp_ratio * 50
            scores['steering_stability'] = max(0, min(100, steer_score))
        else:
            scores['steering_stability'] = 100

        # === 3. 跟车安全 (0-100) ===
        if self.ttc_data:
            danger_ttc = sum(1 for _, t in self.ttc_data if t < SafetyMetrics.TTC_DANGER)
            warn_ttc = sum(1 for _, t in self.ttc_data if t < SafetyMetrics.TTC_WARNING)
            total = len(self.ttc_data)
            danger_ratio = danger_ttc / total
            warn_ratio = warn_ttc / total

            follow_score = 100 - danger_ratio * 70 - warn_ratio * 30
            scores['following_safety'] = max(0, min(100, follow_score))
        else:
            scores['following_safety'] = 100

        # === 4. 车道保持 (0-100) ===
        if self.lane_deviation_data:
            deviations = [abs(d) for _, d in self.lane_deviation_data]
            departure_count = sum(1 for d in deviations if d > 1.0)  # 偏离 > 1m
            departure_ratio = departure_count / len(deviations)
            avg_deviation = np.mean(deviations)

            lane_score = 100 - departure_ratio * 70 - min(avg_deviation * 30, 30)
            scores['lane_keeping'] = max(0, min(100, lane_score))
        else:
            scores['lane_keeping'] = 100

        # === 5. 危险响应 (0-100) ===
        if self.hazard_events:
            correct_rate = sum(1 for e in self.hazard_events if e['correct']) / len(self.hazard_events)
            avg_rt = np.mean([e['reaction_time'] for e in self.hazard_events])
            # 反应时间评分
            if avg_rt <= 0.8:
                rt_factor = 1.0
            elif avg_rt <= 1.5:
                rt_factor = 0.7
            elif avg_rt <= 2.0:
                rt_factor = 0.4
            else:
                rt_factor = 0.1

            scores['hazard_response'] = correct_rate * 100 * rt_factor
        else:
            scores['hazard_response'] = 100

        # === 6. 规则遵守 (0-100) ===
        if self.violations:
            total_severity = sum(v['severity'] for v in self.violations)
            violation_penalty = min(total_severity * 25, 100)
            scores['rule_compliance'] = max(0, 100 - violation_penalty)
        else:
            scores['rule_compliance'] = 100

        # === 加权综合 ===
        total_score = sum(scores[k] * self.WEIGHTS[k] for k in self.WEIGHTS)

        # 评级
        if total_score >= 90:
            level = "A级 (安全)"
        elif total_score >= 75:
            level = "B级 (良好)"
        elif total_score >= 60:
            level = "C级 (合格)"
        elif total_score >= 40:
            level = "D级 (待提高)"
        else:
            level = "E级 (不合格)"

        return {
            'total_score': round(total_score, 1),
            'level': level,
            'dimension_scores': {k: round(v, 1) for k, v in scores.items()},
            'violation_count': len(self.violations),
            'hazard_event_count': len(self.hazard_events)
        }


# ===========================================================================
# 6. 自适应训练难度调节 (RL-based Adaptive Training)
# ===========================================================================

class AdaptiveTrainer:
    """
    自适应训练难度调节器

    根据学员表现动态调整训练场景的难度参数:
    - 交通密度 (traffic_density): 0.1 ~ 1.0
    - 危险事件频率 (hazard_frequency): 0.1 ~ 1.0
    - 天气复杂度 (weather_complexity): 0.0 ~ 1.0
    - 场景复杂度 (scenario_complexity): 0.1 ~ 1.0

    使用基于绩效的动态调整策略:
    - 表现优秀 → 增加难度
    - 表现差 → 降低难度
    - 维持在"最近发展区" (Zone of Proximal Development)
    """

    # 难度参数范围
    PARAM_RANGES = {
        'traffic_density':   (0.1, 1.0),
        'hazard_frequency':  (0.1, 1.0),
        'weather_complexity': (0.0, 1.0),
        'scenario_complexity': (0.1, 1.0)
    }

    # 调整速率
    LEARNING_RATE = 0.08
    # 目标成功率 (维持适度挑战)
    TARGET_SUCCESS_RATE = 0.75

    def __init__(self):
        self.difficulty = {
            'traffic_density': 0.3,
            'hazard_frequency': 0.2,
            'weather_complexity': 0.1,
            'scenario_complexity': 0.2
        }
        self.history: list = []
        self.weak_areas: dict = {}

    def update(self, scenario_score: float, scenario_type: str,
               performance_details: Optional[dict] = None) -> dict:
        """
        根据场景表现更新难度

        :param scenario_score: 场景得分 0-100
        :param scenario_type: 场景类型
        :param performance_details: 详细表现数据
        :return: 更新后的难度参数
        """
        self.history.append({
            'score': scenario_score,
            'type': scenario_type,
            'difficulty': self.difficulty.copy()
        })

        # 计算绩效误差
        success_rate = scenario_score / 100.0
        error = success_rate - self.TARGET_SUCCESS_RATE

        # 调整难度: 表现好(error>0)→增加难度, 表现差(error<0)→降低难度
        adjustment = self.LEARNING_RATE * error

        for param in self.difficulty:
            lo, hi = self.PARAM_RANGES[param]
            self.difficulty[param] += adjustment * (hi - lo)
            self.difficulty[param] = np.clip(self.difficulty[param], lo, hi)

        # 弱点追踪
        if scenario_score < 60:
            self.weak_areas[scenario_type] = self.weak_areas.get(scenario_type, 0) + 1

        return self.difficulty.copy()

    def get_training_plan(self) -> dict:
        """获取训练计划建议"""
        # 找出最薄弱的场景类型
        if self.weak_areas:
            weakest = max(self.weak_areas, key=self.weak_areas.get)
        else:
            weakest = None

        # 最近5个场景的平均分趋势
        recent = self.history[-5:] if len(self.history) >= 5 else self.history
        if recent:
            trend = np.mean([h['score'] for h in recent])
        else:
            trend = 0

        return {
            'current_difficulty': self.difficulty,
            'weak_area': weakest,
            'recent_avg_score': round(trend, 1),
            'total_scenarios': len(self.history),
            'recommendation': self._generate_recommendation(trend, weakest)
        }

    def _generate_recommendation(self, trend: float, weak_area: Optional[str]) -> str:
        """生成训练建议"""
        if trend < 40:
            base = "建议降低难度, 重新练习基础场景"
        elif trend < 60:
            base = "建议保持当前难度, 巩固薄弱环节"
        elif trend < 80:
            base = "表现良好, 可以适度提升难度"
        else:
            base = "表现优秀, 建议挑战高难度场景"

        if weak_area:
            base += f"; 重点关注 '{weak_area}' 类型场景的训练"

        return base


# ===========================================================================
# 7. PERCLOS 疲劳检测模型
# ===========================================================================

class PERCLOSDetector:
    """
    PERCLOS (Percentage of Eye Closure) 疲劳检测

    原理: 在一定时间窗口内, 眼睑闭合超过80%的时间占比
    PERCLOS > 0.15 (15%) → 疲劳预警
    PERCLOS > 0.30 (30%) → 严重疲劳告警

    结合眨眼频率和注视稳定性进行综合判断
    """

    WINDOW_SIZE = 60  # 60秒滑动窗口
    EYE_CLOSURE_THRESHOLD = 0.8  # 眼睑闭合80%判定为闭合
    FATIGUE_WARN = 0.08  # 8% 预警
    FATIGUE_ALERT = 0.15  # 15% 告警
    FATIGUE_SEVERE = 0.30  # 30% 严重

    def __init__(self):
        self.eye_data: list = []  # [(timestamp, eye_closure_ratio)]
        self.blink_times: list = []

    def update(self, eye_closure_ratio: float, timestamp: float):
        """更新眼动数据"""
        self.eye_data.append((timestamp, eye_closure_ratio))

        # 检测眨眼 (快速闭合→打开)
        if len(self.eye_data) >= 2:
            prev = self.eye_data[-2][1]
            if prev < 0.5 and eye_closure_ratio > 0.8:
                self.blink_times.append(timestamp)

        # 移除过期数据
        cutoff = timestamp - self.WINDOW_SIZE
        self.eye_data = [(t, e) for t, e in self.eye_data if t >= cutoff]
        self.blink_times = [t for t in self.blink_times if t >= cutoff]

    def compute_fatigue_level(self) -> dict:
        """计算疲劳等级"""
        if len(self.eye_data) < 10:
            return {'perclos': 0, 'level': '数据不足', 'blink_rate': 0}

        # 计算 PERCLOS
        closed_count = sum(1 for _, e in self.eye_data
                          if e > self.EYE_CLOSURE_THRESHOLD)
        perclos = closed_count / len(self.eye_data)

        # 眨眼频率 (次/分钟)
        blink_rate = len(self.blink_times) / (self.WINDOW_SIZE / 60)

        # 平均注视稳定性 (眼动速度的标准差, 疲劳时降低)
        if len(self.eye_data) > 2:
            closures = [e for _, e in self.eye_data]
            gaze_stability = 1.0 - min(np.std(closures) * 2, 1.0)
        else:
            gaze_stability = 1.0

        # 综合疲劳判定
        if perclos >= self.FATIGUE_SEVERE:
            level = "严重疲劳"
            score = 100
        elif perclos >= self.FATIGUE_ALERT:
            level = "疲劳"
            score = 70
        elif perclos >= self.FATIGUE_WARN:
            level = "轻度疲劳"
            score = 40
        else:
            level = "清醒"
            score = 0

        # 眨眼频率修正 (正常 10-20次/分钟)
        if blink_rate < 8 or blink_rate > 30:
            score = min(score + 15, 100)

        return {
            'perclos': round(perclos, 4),
            'level': level,
            'fatigue_score': score,
            'blink_rate': round(blink_rate, 1),
            'gaze_stability': round(gaze_stability, 3)
        }


# ===========================================================================
# 8. 驾驶风格分类器 (基于简单规则, 可扩展为LSTM)
# ===========================================================================

class DrivingStyleClassifier:
    """
    驾驶风格三分类: 保守型 / 平稳型 / 激进型

    基于驾驶操作特征进行分类:
    - 平均车速 vs 限速比
    - 纵向加速度标准差 (加减速激烈程度)
    - 方向盘角速度标准差 (转向激烈程度)
    - 矛盾驾驶行为次数 (急加速后急减速等)
    - 车头时距分布 (跟车距离偏好)

    注: 完整实现可使用 LSTM 时序网络, 这里提供基于规则的基线版本
    """

    def __init__(self):
        self.features: list = []

    def add_features(self, speed: float, speed_limit: float,
                     accel: float, steering_rate: float, thw: float):
        """添加驾驶特征数据点"""
        speed_ratio = speed / max(speed_limit, 1)
        self.features.append({
            'speed_ratio': speed_ratio,
            'accel': accel,
            'steering_rate': steering_rate,
            'thw': thw
        })

    def classify(self) -> dict:
        """分类驾驶风格"""
        if len(self.features) < 30:
            return {'style': '数据不足', 'confidence': 0}

        speed_ratios = [f['speed_ratio'] for f in self.features]
        accels = [f['accel'] for f in self.features]
        steer_rates = [f['steering_rate'] for f in self.features]
        thws = [f['thw'] for f in self.features if f['thw'] < float('inf')]

        # 提取统计特征
        avg_speed_ratio = np.mean(speed_ratios)
        accel_std = np.std(accels)
        steer_std = np.std(steer_rates)
        avg_thw = np.mean(thws) if thws else 3.0
        hard_events = sum(1 for a in accels if abs(a) > 2.5)

        # 激进度评分 (0-100, 越高越激进)
        aggressiveness = 0

        # 速度因素 (30%)
        if avg_speed_ratio > 0.95:
            aggressiveness += 30
        elif avg_speed_ratio > 0.85:
            aggressiveness += 20
        elif avg_speed_ratio > 0.75:
            aggressiveness += 10

        # 加减速因素 (30%)
        if accel_std > 2.0:
            aggressiveness += 30
        elif accel_std > 1.2:
            aggressiveness += 20
        elif accel_std > 0.8:
            aggressiveness += 10

        # 转向因素 (20%)
        if steer_std > 100:
            aggressiveness += 20
        elif steer_std > 60:
            aggressiveness += 12
        elif steer_std > 30:
            aggressiveness += 6

        # 跟车距离因素 (20%)
        if avg_thw < 1.0:
            aggressiveness += 20
        elif avg_thw < 1.5:
            aggressiveness += 12
        elif avg_thw < 2.0:
            aggressiveness += 6

        # 分类
        if aggressiveness >= 65:
            style = "激进型 (Aggressive)"
            confidence = min((aggressiveness - 65) / 35 + 0.6, 1.0)
        elif aggressiveness >= 35:
            style = "平稳型 (Moderate)"
            confidence = 1.0 - abs(aggressiveness - 50) / 50
        else:
            style = "保守型 (Conservative)"
            confidence = min((35 - aggressiveness) / 35 + 0.6, 1.0)

        return {
            'style': style,
            'aggressiveness': round(aggressiveness, 1),
            'confidence': round(confidence, 3),
            'avg_speed_ratio': round(avg_speed_ratio, 3),
            'accel_std': round(accel_std, 3),
            'steer_std': round(steer_std, 2),
            'avg_thw': round(avg_thw, 2),
            'hard_events': hard_events,
            'data_points': len(self.features)
        }


# ===========================================================================
# 演示与验证
# ===========================================================================

def demo_all():
    """运行所有算法的演示"""

    print("=" * 70)
    print("  防御性安全驾驶模拟器 — 核心算法演示")
    print("=" * 70)

    # === 1. Pacejka 轮胎模型 ===
    print("\n[1] Pacejka Magic Formula 轮胎模型")
    print("-" * 50)
    Fz = 4000  # 法向力 4000N (约400kg载荷)
    print(f"  法向力 Fz = {Fz} N")
    for kappa in [-0.3, -0.2, -0.1, -0.05, 0, 0.05, 0.1, 0.2, 0.3]:
        Fx = pacejka_longitudinal(kappa, Fz)
        bar = "█" * int(abs(Fx) / Fz * 40)
        print(f"  κ={kappa:+.2f}  Fx={Fx:+8.1f} N  ({Fx/Fz:+.3f} μ)  {bar}")
    alpha = 0.1  # 侧偏角 0.1 rad ≈ 5.7°
    Fy = pacejka_lateral(alpha, Fz)
    print(f"  侧偏角 α={math.degrees(alpha):.1f}°  Fy={Fy:.1f} N")

    # === 2. 运动提示算法 ===
    print("\n[2] Classical Washout 运动提示算法")
    print("-" * 50)
    mca = ClassicalWashout(dt=0.01)
    # 模拟紧急制动: 纵向减速度 6 m/s²
    print("  模拟紧急制动 (纵向 -6 m/s², 持续 2s):")
    for step in range(200):  # 2秒, 100Hz
        t = step * 0.01
        acc = np.array([-6.0, 0, 0])  # 纵向制动
        rot_rate = np.array([0, 0, 0])
        rot_acc = np.array([0, 0, 0])
        cmd = mca.update(acc, rot_rate, rot_acc)
        if step % 40 == 0 or step == 199:
            print(f"  t={t:.2f}s  位移=[{cmd[0]:+.3f},{cmd[1]:+.3f},{cmd[2]:+.3f}]m  "
                  f"姿态=[{math.degrees(cmd[3]):+.1f},{math.degrees(cmd[4]):+.1f},{math.degrees(cmd[5]):+.1f}]°")

    # === 3. 安全指标 ===
    print("\n[3] TTC / THW 安全指标")
    print("-" * 50)
    # 场景: 自车 60km/h, 前车 40km/h, 距离 30m
    ego_v = 60 / 3.6  # m/s
    lead_v = 40 / 3.6
    dist = 30.0
    ttc = SafetyMetrics.compute_ttc(0, ego_v, dist, lead_v)
    thw = SafetyMetrics.compute_thw(0, ego_v, dist)
    risk = SafetyMetrics.collision_risk_score(ttc, thw)
    print(f"  场景: 自车 {ego_v:.1f}m/s, 前车 {lead_v:.1f}m/s, 距离 {dist}m")
    print(f"  TTC = {ttc:.2f}s  ({'危险!' if ttc < 2 else '警告' if ttc < 4 else '安全'})")
    print(f"  THW = {thw:.2f}s  ({'危险!' if thw < 1 else '警告' if thw < 2 else '安全'})")
    print(f"  碰撞风险评分 = {risk:.1f}/100")

    # === 4. 危险感知评分 ===
    print("\n[4] 危险感知 (Hazard Perception) 评分")
    print("-" * 50)
    hp = HazardPerceptionScorer()
    # 模拟5次危险事件
    # 每条: (危险出现时间戳, 驾驶员响应时间戳, 注视, 预判, 操作正确)
    # 响应时间戳 = 出现时间戳 + 反应时间, 反应时间由二者相减得到
    events = [
        (0.0,  0.6,  True,  True,  True),   # 反应 0.6s — 优秀
        (5.0,  5.9,  True,  False, True),   # 反应 0.9s — 良好
        (10.0, 11.3, True,  False, True),   # 反应 1.3s — 合格
        (15.0, 15.7, True,  True,  True),   # 反应 0.7s — 优秀
        (20.0, 22.2, False, False, False),  # 反应 2.2s — 不合格
    ]
    for e in events:
        hp.record_hazard_event(*e)
    result = hp.get_final_score()
    print(f"  HP综合评分: {result['hp_score']}/100  ({result['level']})")
    print(f"  平均反应时间: {result['avg_rt']:.3f}s")
    print(f"  注视率: {result['gaze_rate']}%  预判率: {result['prediction_rate']}%")
    print(f"  正确操作率: {result['correct_action_rate']}%  事件数: {result['total_events']}")

    # === 5. 驾驶行为评分 ===
    print("\n[5] 驾驶行为综合安全评分")
    print("-" * 50)
    scorer = DrivingBehaviorScorer()
    # 模拟一段驾驶数据
    np.random.seed(42)
    for i in range(200):
        t = i * 0.1
        speed = 55 + np.random.normal(0, 5)  # 略低于限速60
        steer = np.random.normal(0, 30)       # 方向盘角度
        accel = np.random.normal(0, 1.5)      # 加速度
        ttc_val = np.random.uniform(2, 10)
        thw_val = np.random.uniform(1, 3)
        lane_dev = np.random.normal(0, 0.3)
        scorer.update_telemetry(speed, steer, accel, ttc_val, thw_val, lane_dev, t)

    scorer.add_violation("超速", 10.0, 0.3)
    scorer.add_hazard_response(True, 0.8)
    scorer.add_hazard_response(False, 1.8)

    score = scorer.compute_score(speed_limit=60)
    print(f"  综合评分: {score['total_score']}/100  {score['level']}")
    print(f"  各维度:")
    for dim, val in score['dimension_scores'].items():
        bar = "█" * int(val / 5)
        print(f"    {dim:25s} {val:5.1f}  {bar}")
    print(f"  违规次数: {score['violation_count']}  危险事件: {score['hazard_event_count']}")

    # === 6. 自适应训练 ===
    print("\n[6] 自适应训练难度调节")
    print("-" * 50)
    trainer = AdaptiveTrainer()
    # 模拟10次场景训练
    scenario_scores = [45, 52, 58, 65, 70, 68, 75, 80, 78, 85]
    scenario_types = ["路口冲突", "跟车安全", "行人避让", "恶劣天气",
                      "高速场景", "路口冲突", "行人避让", "夜间驾驶",
                      "应急避险", "高速场景"]
    for i, (s, t) in enumerate(zip(scenario_scores, scenario_types)):
        diff = trainer.update(s, t)
    plan = trainer.get_training_plan()
    print(f"  训练场景数: {plan['total_scenarios']}")
    print(f"  最近平均分: {plan['recent_avg_score']}")
    print(f"  薄弱场景: {plan['weak_area'] or '无'}")
    print(f"  当前难度参数:")
    for k, v in plan['current_difficulty'].items():
        bar = "█" * int(v * 30)
        print(f"    {k:25s} {v:.3f}  {bar}")
    print(f"  建议: {plan['recommendation']}")

    # === 7. PERCLOS 疲劳检测 ===
    print("\n[7] PERCLOS 疲劳检测")
    print("-" * 50)
    detector = PERCLOSDetector()
    # 模拟60秒眼动数据, 前40秒清醒, 后20秒疲劳
    np.random.seed(123)
    for i in range(600):  # 60秒, 10Hz
        t = i * 0.1
        if t < 40:
            # 清醒: 眼睑开度 0.1-0.3, 偶尔眨眼
            closure = np.random.uniform(0.05, 0.2)
            if np.random.random() < 0.02:
                closure = 0.9  # 眨眼
        else:
            # 疲劳: 眼睑开度增大, 闭合时间增长
            closure = np.random.uniform(0.3, 0.7)
            if np.random.random() < 0.08:
                closure = 0.85  # 疲劳性闭眼
        detector.update(closure, t)

    fatigue = detector.compute_fatigue_level()
    print(f"  PERCLOS: {fatigue['perclos']:.1%}")
    print(f"  疲劳等级: {fatigue['level']}")
    print(f"  疲劳评分: {fatigue['fatigue_score']}/100")
    print(f"  眨眼频率: {fatigue['blink_rate']:.1f} 次/分钟 (正常 10-20)")
    print(f"  注视稳定性: {fatigue['gaze_stability']:.3f}")

    # === 8. 驾驶风格分类 ===
    print("\n[8] 驾驶风格分类")
    print("-" * 50)
    classifier = DrivingStyleClassifier()
    np.random.seed(456)
    # 模拟激进型驾驶员
    for i in range(100):
        speed = 62 + np.random.normal(0, 4)   # 略超速
        accel = np.random.normal(0, 2.5)       # 激烈加减速
        steer_rate = np.random.normal(0, 80)   # 激烈转向
        thw = abs(np.random.normal(1.2, 0.3))  # 跟车近
        classifier.add_features(speed, 60, accel, steer_rate, thw)

    style = classifier.classify()
    print(f"  驾驶风格: {style['style']}")
    print(f"  激进度评分: {style['aggressiveness']}/100")
    print(f"  置信度: {style['confidence']:.1%}")
    print(f"  平均速度比: {style['avg_speed_ratio']:.2f} (限速比)")
    print(f"  加速度标准差: {style['accel_std']:.2f} m/s²")
    print(f"  转向角速度标准差: {style['steer_std']:.1f} °/s")
    print(f"  平均车头时距: {style['avg_thw']:.2f} s")

    print("\n" + "=" * 70)
    print("  所有算法演示完成!")
    print("=" * 70)


if __name__ == "__main__":
    demo_all()