1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174
| """ FCW/AEB联动控制器 根据驾驶员状态动态调整警告时机 """
import numpy as np from typing import Dict, Tuple
class FCWController: """ FCW控制器 根据驾驶员状态调整FCW触发距离 """ def __init__(self): self.base_ttc_threshold = 2.7 self.base_distance_threshold = 50 self.sensitivity_multipliers = { 'NORMAL': 1.0, 'DISTRACTED': 1.5, 'DROWSY': 2.0, 'IMPAIRED': 2.5, 'UNRESPONSIVE': 3.0 } def compute_fcw_threshold(self, driver_state: str, vehicle_speed: float) -> Tuple[float, float]: """ 计算FCW触发阈值 Args: driver_state: 驾驶员状态 vehicle_speed: 车速 (m/s) Returns: ttc_threshold: TTC阈值 distance_threshold: 距离阈值 """ multiplier = self.sensitivity_multipliers.get(driver_state, 1.0) ttc_threshold = self.base_ttc_threshold * multiplier distance_threshold = vehicle_speed * ttc_threshold return ttc_threshold, distance_threshold def check_fcw_trigger(self, ttc: float, distance: float, driver_state: str, vehicle_speed: float) -> bool: """ 检查是否触发FCW Args: ttc: 当前TTC distance: 当前距离 driver_state: 驾驶员状态 vehicle_speed: 车速 Returns: trigger: 是否触发 """ ttc_threshold, distance_threshold = self.compute_fcw_threshold( driver_state, vehicle_speed ) return ttc < ttc_threshold or distance < distance_threshold
class AEBController: """ AEB控制器 根据驾驶员状态调整AEB触发条件 """ def __init__(self): self.aeb_ttc_thresholds = { 'NORMAL': 1.5, 'DISTRACTED': 2.0, 'DROWSY': 2.2, 'IMPAIRED': 2.5, 'UNRESPONSIVE': 3.0 } self.max_decel = 10.0 def compute_aeb_intervention(self, ttc: float, driver_state: str, relative_speed: float) -> Tuple[bool, float]: """ 计算AEB干预 Args: ttc: 当前TTC driver_state: 驾驶员状态 relative_speed: 相对速度 Returns: should_brake: 是否刹车 deceleration: 减速度 """ threshold = self.aeb_ttc_thresholds.get(driver_state, 1.5) if ttc < threshold: required_decel = relative_speed / ttc deceleration = min(required_decel * 1.2, self.max_decel) return True, deceleration return False, 0.0
class LKAController: """ LKA控制器 根据驾驶员状态调整车道保持辅助 """ def __init__(self): self.lane_offset_thresholds = { 'NORMAL': 0.3, 'DISTRACTED': 0.25, 'DROWSY': 0.2, 'IMPAIRED': 0.15, 'UNRESPONSIVE': 0.1 } self.max_steering_torque = 3.0 def compute_lka_intervention(self, lane_offset: float, driver_state: str, lane_change_intent: bool) -> Tuple[bool, float]: """ 计算LKA干预 Args: lane_offset: 车道偏移 driver_state: 驾驶员状态 lane_change_intent: 是否有变道意图 Returns: should_intervene: 是否介入 steering_torque: 转向力矩 """ if lane_change_intent: return False, 0.0 threshold = self.lane_offset_thresholds.get(driver_state, 0.3) if abs(lane_offset) > threshold: torque = np.sign(lane_offset) * min( abs(lane_offset) * 5, self.max_steering_torque ) return True, torque return False, 0.0
|