From 9299ed5c3fd732091566b3aa3e877e4c83f5d115 Mon Sep 17 00:00:00 2001 From: 22268 <2226804607@qq.com> Date: Sun, 21 Dec 2025 10:58:13 +0800 Subject: [PATCH] 1 --- src/minicarsim/README.md | 35 ++- src/minicarsim/main.py | 589 ++++++++++++++++++++++++++++++++------- 2 files changed, 519 insertions(+), 105 deletions(-) diff --git a/src/minicarsim/README.md b/src/minicarsim/README.md index 71e8269a02..e506e23ed1 100644 --- a/src/minicarsim/README.md +++ b/src/minicarsim/README.md @@ -14,6 +14,8 @@ 6. **温度监控**:模拟车内温度变化并可视化 7. **智能空调系统**:根据温度自动开启/关闭空调调节温度 8. **温度调节可视化**:生成多种温度调节相关图表 +9. **故障自检系统**:实时监测传感器和执行器状态 +10. **故障记录与可视化**:自动生成故障报告和趋势图 ## 环境要求 @@ -31,12 +33,16 @@ minicarsim/ │ └── simple_car.xml # 小车和环境模型 └── output/ # 输出数据目录 ├── lidar/ # LiDAR点云数据 - └── annotations/ # 物体检测标注 - └── visualization/ # 可视化图表 - ├── temperature_trend.png # 温度变化趋势图 - ├── temperature_summary.png # 温度调节汇总图 - ├── ac_control_XXXX.png # 空调控制状态图 - └── thermal_map_XXXX.png # 环境温度分布图 + ├── annotations/ # 物体检测标注 + ├── visualization/ # 可视化图表 + │ ├── temperature_trend.png # 温度变化趋势图 + │ ├── temperature_summary.png # 温度调节汇总图 + │ ├── ac_control_XXXX.png # 空调控制状态图 + │ └── thermal_map_XXXX.png # 环境温度分布图 + └── fault_reports/ # 故障监测报告 + ├── frame_XXXX.json # 故障详细报告(JSON) + ├── fault_status_XXXX.png # 故障状态图 + └── health_trend.png # 系统健康趋势图 ``` ## 快速开始 @@ -67,6 +73,14 @@ python main.py - `visualize_ac_control()`: 生成空调控制状态图 - `generate_temperature_trend()`: 生成温度变化趋势图 - `generate_temperature_summary_chart()`: 生成温度调节汇总图 + - `generate_fault_report()`: 生成故障报告 + - `visualize_fault_status()`: 生成故障状态图 + - `generate_health_trend()`: 生成健康趋势图 + +- `FaultMonitor`: 故障监测类 + - `check_system_health()`: 检查系统整体健康状态 + - `_check_sensor_health()`: 检查传感器健康状态 + - `_check_actuator_health()`: 检查执行器健康状态 ### 配置参数 @@ -74,6 +88,7 @@ python main.py - `LIDAR_PARAMS`: LiDAR传感器参数 - `TEMPERATURE_PARAMS`: 温度监控参数 +- `FAULT_MONITORING_PARAMS`: 故障监测参数 - `SIMULATION_FRAMES`: 仿真总帧数 ## 模型说明 @@ -88,7 +103,7 @@ python main.py ## 数据输出 -仿真运行后会产生三类数据: +仿真运行后会产生四类数据: 1. **LiDAR点云数据**:保存为`.npy`格式的NumPy数组 2. **物体检测标注**:保存为`.json`格式的标注文件 @@ -97,6 +112,10 @@ python main.py - 温度调节汇总图 (`temperature_summary.png`) - 空调控制状态图 (`ac_control_XXXX.png`) - 环境温度分布图 (`thermal_map_XXXX.png`) +4. **故障监测报告**: + - 故障详细报告 (`frame_XXXX.json`) + - 故障状态图 (`fault_status_XXXX.png`) + - 系统健康趋势图 (`health_trend.png`) ## 扩展建议 @@ -106,6 +125,7 @@ python main.py 4. 实现自主导航功能 5. 添加更多的车辆控制方式 6. 增强空调控制算法(PID控制等) +7. 增加更多类型的故障检测(如通信故障、电源故障等) ## 故障排除 @@ -115,6 +135,7 @@ python main.py 2. **无法显示可视化窗口**:确保已正确安装MuJoCo 3. **没有检测到物体**:检查小车与物体之间的距离 4. **温度调节效果不明显**:检查空调功率系数和舒适温度范围设置 +5. **没有生成故障报告**:检查故障检测阈值设置 ### 支持 diff --git a/src/minicarsim/main.py b/src/minicarsim/main.py index ac7f7aeeaf..e87a5dc4f0 100644 --- a/src/minicarsim/main.py +++ b/src/minicarsim/main.py @@ -10,6 +10,7 @@ import time import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D +from collections import defaultdict, deque # -------------------------- 配置参数 -------------------------- # 场景文件路径 @@ -38,6 +39,12 @@ "ac_power": 1.0 # 空调功率系数 } +# 添加故障监测参数 +FAULT_MONITORING_PARAMS = { + "sensor_fault_threshold": 0.1, # 传感器故障阈值 + "actuator_fault_threshold": 0.15, # 执行器故障阈值 + "health_check_interval": 20, # 健康检查间隔(帧) +} # ------------------------------------------------------------- @@ -50,6 +57,7 @@ def __init__(self, xml_path, output_dir): os.makedirs(f"{output_dir}/annotations", exist_ok=True) os.makedirs(f"{output_dir}/visualization", exist_ok=True) # 新增可视化目录 os.makedirs(f"{output_dir}/distance_analysis", exist_ok=True) # 新增距离分析目录 + os.makedirs(f"{output_dir}/fault_reports", exist_ok=True) # 新增故障报告目录 # 加载MuJoCo模型和数据 self.model = mujoco.MjModel.from_xml_path(xml_path) @@ -67,6 +75,11 @@ def __init__(self, xml_path, output_dir): self.comfort_min_temp = TEMPERATURE_PARAMS["comfort_range"][0] self.comfort_max_temp = TEMPERATURE_PARAMS["comfort_range"][1] + # 初始化故障监测系统 + self.fault_monitor = FaultMonitor(self.model, self.data) + self.fault_history = [] # 存储故障历史记录 + self.health_scores = [] # 存储健康评分历史 + def get_world_pose(self, body_name): """ 获取指定物体的世界位姿 @@ -241,9 +254,9 @@ def calculate_avoidance_control(self, lidar_data, detected_objects): # 简单避障策略:向相反方向转弯 if angle_to_obstacle > 0: # 障碍物在左侧 - steering_angle = -5.0 # 向右转 + steering_angle = -0.5 # 向右转 else: # 障碍物在右侧 - steering_angle = 5.0 # 向左转 + steering_angle = 0.5 # 向左转 # 如果非常接近,减速 if closest_obj['distance'] < 3.0: @@ -423,37 +436,37 @@ def simulate_temperature_data(self): # 基础环境温度 temperature = TEMPERATURE_PARAMS["ambient_temp"] - + # 遍历热源物体,计算对温度的影响 for heat_source in TEMPERATURE_PARAMS["heat_sources"]: try: # 获取热源位置 body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, heat_source) heat_pos = self.data.xpos[body_id].copy() - + # 计算传感器与热源的距离 distance = np.linalg.norm(sensor_pos - heat_pos) - + # 根据距离计算温度影响(假设热源散发热量遵循平方反比定律) # 距离越近,温度越高 temp_increase = TEMPERATURE_PARAMS["temp_variation"] / (distance + 1) # 避免除零 temperature += temp_increase - + except Exception: # 如果找不到热源,跳过 continue - + # 添加随机噪声模拟真实传感器 noise = np.random.normal(0, 0.5) # 均值为0,标准差为0.5的高斯噪声 temperature += noise - + # 如果空调开启,调整温度 if self.ac_status: # 空调效果:逐渐向目标温度靠近 temp_diff = self.ac_target_temp - temperature ac_effect = temp_diff * TEMPERATURE_PARAMS["ac_power"] * 0.05 # 空调效果系数 temperature += ac_effect - + return temperature def visualize_temperature_data(self, temperature, detected_objects): @@ -461,34 +474,34 @@ def visualize_temperature_data(self, temperature, detected_objects): 生成温度分布可视化图 """ fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 7)) - + # 左侧图:温度随时间变化趋势 # 由于我们是单帧数据,这里展示当前温度信息 ax1.set_xlim(0, 10) - ax1.set_ylim(TEMPERATURE_PARAMS["ambient_temp"] - 5, + ax1.set_ylim(TEMPERATURE_PARAMS["ambient_temp"] - 5, TEMPERATURE_PARAMS["ambient_temp"] + TEMPERATURE_PARAMS["temp_variation"] + 5) - ax1.axhline(y=TEMPERATURE_PARAMS["ambient_temp"], color='b', linestyle='--', + ax1.axhline(y=TEMPERATURE_PARAMS["ambient_temp"], color='b', linestyle='--', label=f'环境温度: {TEMPERATURE_PARAMS["ambient_temp"]:.1f}°C') - ax1.bar([5], [temperature], width=2, color='r', alpha=0.7, + ax1.bar([5], [temperature], width=2, color='r', alpha=0.7, label=f'测量温度: {temperature:.1f}°C') ax1.set_xlabel('时间') ax1.set_ylabel('温度 (°C)') ax1.set_title(f'温度监测 - 帧 {self.frame_count:04d}') ax1.legend() ax1.grid(True) - + # 右侧图:温度与物体距离关系 if detected_objects: distances = [obj['distance'] for obj in detected_objects] object_names = [obj['name'] for obj in detected_objects] - + # 计算每个物体附近的预期温度 expected_temps = [] try: sensor_pos = self.data.site_xpos[ mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "lidar_site") ].copy() - + for obj in detected_objects: obj_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, obj['name']) obj_pos = self.data.xpos[obj_id].copy() @@ -498,13 +511,13 @@ def visualize_temperature_data(self, temperature, detected_objects): expected_temps.append(expected_temp) except: # 如果出错,使用简化计算 - expected_temps = [TEMPERATURE_PARAMS["ambient_temp"] + + expected_temps = [TEMPERATURE_PARAMS["ambient_temp"] + TEMPERATURE_PARAMS["temp_variation"] / (d + 1) for d in distances] - + x_pos = range(len(distances)) ax2.bar(x_pos, expected_temps, alpha=0.7, color='orange', label='预期温度') ax2.axhline(y=temperature, color='r', linestyle='-', label=f'实测温度: {temperature:.1f}°C') - + ax2.set_xlabel('物体') ax2.set_ylabel('温度 (°C)') ax2.set_title('物体距离与温度关系') @@ -513,15 +526,15 @@ def visualize_temperature_data(self, temperature, detected_objects): ax2.legend() ax2.grid(True) else: - ax2.text(0.5, 0.5, '无检测到物体', horizontalalignment='center', + ax2.text(0.5, 0.5, '无检测到物体', horizontalalignment='center', verticalalignment='center', transform=ax2.transAxes) ax2.set_title('物体距离与温度关系') - + plt.tight_layout() plt.savefig(f"{self.output_dir}/visualization/temp_frame_{self.frame_count:04d}.png", dpi=300, bbox_inches='tight') plt.close() - + print(f"已生成温度可视化图: temp_frame_{self.frame_count:04d}.png") def generate_thermal_map(self, temperature, detected_objects): @@ -529,25 +542,25 @@ def generate_thermal_map(self, temperature, detected_objects): 生成热力图(二维温度分布图) """ fig, ax = plt.subplots(figsize=(10, 8)) - + # 创建网格点用于绘制热力图 grid_size = 50 x_range = np.linspace(-10, 15, grid_size) y_range = np.linspace(-8, 8, grid_size) X, Y = np.meshgrid(x_range, y_range) - + # 计算每个网格点的温度值 Z = np.zeros_like(X) sensor_height = 0.8 # 传感器高度 - + for i in range(grid_size): for j in range(grid_size): # 当前网格点位置 point_pos = np.array([X[j, i], Y[j, i], sensor_height]) - + # 基础环境温度 temp = TEMPERATURE_PARAMS["ambient_temp"] - + # 遍历热源计算温度贡献 for heat_source in TEMPERATURE_PARAMS["heat_sources"]: try: @@ -558,51 +571,51 @@ def generate_thermal_map(self, temperature, detected_objects): temp += temp_increase except: continue - + Z[j, i] = temp - + # 绘制热力图 im = ax.contourf(X, Y, Z, levels=50, cmap='hot') plt.colorbar(im, ax=ax, label='温度 (°C)') - + # 绘制车辆位置 try: vehicle_pos, _ = self.get_world_pose("vehicle") ax.plot(vehicle_pos[0], vehicle_pos[1], 'bo', markersize=10, label='车辆') except: pass - + # 绘制障碍物位置 colors = ['red', 'green', 'blue', 'yellow', 'purple'] for i, obstacle in enumerate(TEMPERATURE_PARAMS["heat_sources"]): try: body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, obstacle) pos = self.data.xpos[body_id].copy() - ax.plot(pos[0], pos[1], 's', color=colors[i % len(colors)], + ax.plot(pos[0], pos[1], 's', color=colors[i % len(colors)], markersize=8, label=obstacle) except: continue - + # 绘制温度传感器测量值 try: lidar_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "lidar_site") if lidar_site_id >= 0: sensor_pos = self.data.site_xpos[lidar_site_id].copy() - ax.plot(sensor_pos[0], sensor_pos[1], 'wo', markersize=6, + ax.plot(sensor_pos[0], sensor_pos[1], 'wo', markersize=6, markeredgecolor='black', label=f'传感器({temperature:.1f}°C)') except: pass - + ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_title(f'环境温度分布图 - 帧 {self.frame_count:04d}') ax.legend() ax.grid(True) - + plt.savefig(f"{self.output_dir}/visualization/thermal_map_{self.frame_count:04d}.png", dpi=300, bbox_inches='tight') plt.close() - + print(f"已生成温度分布热力图: thermal_map_{self.frame_count:04d}.png") def check_and_control_ac(self, temperature): @@ -622,7 +635,7 @@ def check_and_control_ac(self, temperature): if self.ac_status: self.ac_status = False print(f"❄️ 空调已关闭,当前温度: {temperature:.1f}°C,处于舒适范围内") - + return self.ac_status def visualize_ac_control(self, temperature, ac_status): @@ -632,43 +645,43 @@ def visualize_ac_control(self, temperature, ac_status): :param ac_status: 空调状态 """ fig, ax = plt.subplots(figsize=(10, 6)) - + # 绘制温度范围 comfort_min = self.comfort_min_temp comfort_max = self.comfort_max_temp ambient_temp = TEMPERATURE_PARAMS["ambient_temp"] - + # 绘制舒适温度区域 ax.axvspan(comfort_min, comfort_max, alpha=0.3, color='green', label='舒适温度区间') - + # 绘制环境温度线 ax.axvline(ambient_temp, color='blue', linestyle='--', linewidth=1, label=f'环境温度 ({ambient_temp}°C)') - + # 绘制当前温度 color = 'red' if ac_status else 'orange' status_label = '空调运行中' if ac_status else '空调关闭' ax.scatter(temperature, 1, s=100, color=color, label=f'当前温度 {temperature:.1f}°C ({status_label})') - + # 绘制目标温度(如果空调开启) if ac_status: - ax.axvline(self.ac_target_temp, color='purple', linestyle='-.', linewidth=1, + ax.axvline(self.ac_target_temp, color='purple', linestyle='-.', linewidth=1, label=f'目标温度 ({self.ac_target_temp}°C)') - + ax.set_xlim(ambient_temp - 10, ambient_temp + 10) ax.set_ylim(0, 2) ax.set_xlabel('温度 (°C)') ax.set_title(f'空调控制系统状态 - 帧 {self.frame_count:04d}') ax.legend() ax.grid(True, alpha=0.3) - + # 移除y轴刻度 ax.set_yticks([]) - + plt.tight_layout() plt.savefig(f"{self.output_dir}/visualization/ac_control_{self.frame_count:04d}.png", dpi=300, bbox_inches='tight') plt.close() - + print(f"已生成空调控制状态图: ac_control_{self.frame_count:04d}.png") def generate_temperature_trend(self): @@ -680,17 +693,17 @@ def generate_temperature_trend(self): for file in os.listdir(f"{self.output_dir}/annotations"): if file.startswith("temp_frame_") and file.endswith(".json"): temp_files.append(file) - + if not temp_files: return - + # 按帧排序 temp_files.sort() - + frames = [] temperatures = [] ac_statuses = [] - + # 读取温度数据 for file in temp_files: with open(f"{self.output_dir}/annotations/{file}", "r") as f: @@ -698,38 +711,38 @@ def generate_temperature_trend(self): frames.append(temp_data["frame"]) temperatures.append(temp_data["temperature"]) ac_statuses.append(temp_data["ac_status"]) - + # 绘制温度变化趋势图 fig, ax = plt.subplots(figsize=(12, 6)) - + # 绘制温度曲线 ax.plot(frames, temperatures, 'o-', color='red', linewidth=2, markersize=4, label='实测温度') - + # 绘制舒适温度区间 - ax.axhspan(self.comfort_min_temp, self.comfort_max_temp, alpha=0.2, color='green', + ax.axhspan(self.comfort_min_temp, self.comfort_max_temp, alpha=0.2, color='green', label='舒适温度区间') - + # 绘制环境温度线 - ax.axhline(TEMPERATURE_PARAMS["ambient_temp"], color='blue', linestyle='--', + ax.axhline(TEMPERATURE_PARAMS["ambient_temp"], color='blue', linestyle='--', label=f'环境温度 ({TEMPERATURE_PARAMS["ambient_temp"]}°C)') - + # 标记空调开启的时间点 ac_on_frames = [frames[i] for i in range(len(frames)) if ac_statuses[i]] ac_on_temps = [temperatures[i] for i in range(len(temperatures)) if ac_statuses[i]] if ac_on_frames: - ax.scatter(ac_on_frames, ac_on_temps, color='purple', s=50, marker='^', + ax.scatter(ac_on_frames, ac_on_temps, color='purple', s=50, marker='^', label='空调运行中', zorder=5) - + ax.set_xlabel('帧序号') ax.set_ylabel('温度 (°C)') ax.set_title('温度变化趋势与空调控制状态') ax.legend() ax.grid(True, alpha=0.3) - + plt.tight_layout() plt.savefig(f"{self.output_dir}/visualization/temperature_trend.png", dpi=300, bbox_inches='tight') plt.close() - + print("已生成温度变化趋势图: temperature_trend.png") def generate_temperature_summary_chart(self): @@ -741,18 +754,18 @@ def generate_temperature_summary_chart(self): for file in os.listdir(f"{self.output_dir}/annotations"): if file.startswith("temp_frame_") and file.endswith(".json"): temp_files.append(file) - + if not temp_files: return - + # 按帧排序 temp_files.sort() - + frames = [] temperatures = [] ac_statuses = [] target_temps = [] - + # 读取温度数据 for file in temp_files: with open(f"{self.output_dir}/annotations/{file}", "r") as f: @@ -761,67 +774,67 @@ def generate_temperature_summary_chart(self): temperatures.append(temp_data["temperature"]) ac_statuses.append(temp_data["ac_status"]) target_temps.append(temp_data.get("ac_target_temp", None)) - + # 创建汇总图 fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(15, 10)) - + # 第一个子图:温度变化和空调状态 ax1.plot(frames, temperatures, 'o-', color='red', linewidth=2, markersize=4, label='实测温度') - ax1.axhspan(self.comfort_min_temp, self.comfort_max_temp, alpha=0.2, color='green', + ax1.axhspan(self.comfort_min_temp, self.comfort_max_temp, alpha=0.2, color='green', label='舒适温度区间') - ax1.axhline(TEMPERATURE_PARAMS["ambient_temp"], color='blue', linestyle='--', + ax1.axhline(TEMPERATURE_PARAMS["ambient_temp"], color='blue', linestyle='--', label=f'环境温度 ({TEMPERATURE_PARAMS["ambient_temp"]}°C)') - + # 标记空调开启的时间点 ac_on_frames = [frames[i] for i in range(len(frames)) if ac_statuses[i]] ac_on_temps = [temperatures[i] for i in range(len(temperatures)) if ac_statuses[i]] if ac_on_frames: - ax1.scatter(ac_on_frames, ac_on_temps, color='purple', s=50, marker='^', + ax1.scatter(ac_on_frames, ac_on_temps, color='purple', s=50, marker='^', label='空调运行中', zorder=5) - + ax1.set_ylabel('温度 (°C)') ax1.set_title('温度变化与空调控制状态') ax1.legend() ax1.grid(True, alpha=0.3) - + # 第二个子图:空调工作时的目标温度和调节效果 # 只显示空调开启时的数据点 ac_frames = [] ac_temps = [] ac_targets = [] temp_differences = [] - + for i in range(len(frames)): if ac_statuses[i] and target_temps[i] is not None: ac_frames.append(frames[i]) ac_temps.append(temperatures[i]) ac_targets.append(target_temps[i]) temp_differences.append(abs(temperatures[i] - target_temps[i])) - + if ac_frames: ax2.plot(ac_frames, ac_temps, 'o-', color='red', linewidth=2, markersize=4, label='实测温度') ax2.plot(ac_frames, ac_targets, 's-', color='purple', linewidth=2, markersize=4, label='目标温度') - + # 添加温度差值的柱状图 ax3 = ax2.twinx() bars = ax3.bar(ac_frames, temp_differences, alpha=0.3, color='orange', width=1.0, label='温度差值') ax3.set_ylabel('温度差值 (°C)', color='orange') ax3.tick_params(axis='y', labelcolor='orange') - + # 添加数值标签 for bar, diff in zip(bars, temp_differences): height = bar.get_height() - ax3.annotate(f'{diff:.1f}', + ax3.annotate(f'{diff:.1f}', xy=(bar.get_x() + bar.get_width()/2, height), xytext=(0, 3), # 3 points vertical offset textcoords="offset points", ha='center', va='bottom', fontsize=8, color='orange') - + ax2.set_xlabel('帧序号') ax2.set_ylabel('温度 (°C)') ax2.set_title('空调调节效果分析') ax2.grid(True, alpha=0.3) - + # 合并图例 if ac_frames: lines2, labels2 = ax2.get_legend_handles_labels() @@ -829,13 +842,92 @@ def generate_temperature_summary_chart(self): ax2.legend(lines2 + lines3, labels2 + labels3, loc='upper left') else: ax2.legend() - + plt.tight_layout() plt.savefig(f"{self.output_dir}/visualization/temperature_summary.png", dpi=300, bbox_inches='tight') plt.close() - + print("已生成温度调节汇总图: temperature_summary.png") + def visualize_real_time_avoidance(self, lidar_data, detected_objects, left_speed, right_speed, steering_angle): + """ + 生成实时避障图 + """ + fig, ax = plt.subplots(figsize=(12, 10)) + + # 绘制LiDAR点云数据 + if len(lidar_data) > 0: + ax.scatter(lidar_data[:, 0], lidar_data[:, 1], + c='blue', s=1, alpha=0.6, label='LiDAR点云') + + # 绘制检测到的物体 + colors = ['red', 'green', 'orange', 'purple', 'brown'] + for i, obj in enumerate(detected_objects): + pos = np.array(obj['position']) + # 绘制物体中心点 + ax.scatter(pos[0], pos[1], + c=colors[i % len(colors)], s=100, marker='o', + label=f"{obj['name']} (距离: {obj['distance']:.1f}m)") + + # 绘制安全距离圆 + circle = plt.Circle((pos[0], pos[1]), 3.0, fill=False, + color=colors[i % len(colors)], linestyle='--', alpha=0.7) + ax.add_patch(circle) + + # 尝试绘制小车 + try: + vehicle_pos, _ = self.get_world_pose("vehicle") + # 绘制小车位置 + ax.scatter(vehicle_pos[0], vehicle_pos[1], + c='cyan', s=200, marker='s', label='小车') + + # 绘制小车方向 (简化表示) + direction_length = 2.0 + ax.arrow(vehicle_pos[0], vehicle_pos[1], + direction_length * np.cos(steering_angle*2), + direction_length * np.sin(steering_angle*2), + head_width=0.3, head_length=0.3, fc='cyan', ec='cyan') + + except ValueError: + # 如果无法获取小车位置,则不绘制 + pass + + # 绘制避障决策信息 + ax.text(0.02, 0.98, f'左轮速度: {left_speed:.1f}', transform=ax.transAxes, + verticalalignment='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8)) + ax.text(0.02, 0.93, f'右轮速度: {right_speed:.1f}', transform=ax.transAxes, + verticalalignment='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8)) + ax.text(0.02, 0.88, f'转向角度: {steering_angle:.1f}', transform=ax.transAxes, + verticalalignment='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8)) + + # 根据转向角度确定转向方向文字 + if steering_angle > 0: + turn_direction = "左转" + elif steering_angle < 0: + turn_direction = "右转" + else: + turn_direction = "直行" + + ax.text(0.02, 0.83, f'转向方向: {turn_direction}', transform=ax.transAxes, + verticalalignment='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8)) + + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_title(f'实时避障图 - 帧 {self.frame_count:04d}') + ax.legend(loc='lower right') + ax.grid(True, alpha=0.3) + ax.set_aspect('equal', adjustable='box') + + # 确保目录存在 + os.makedirs(f"{self.output_dir}/visualization", exist_ok=True) + + # 保存可视化图像 + plt.savefig(f"{self.output_dir}/visualization/avoidance_{self.frame_count:04d}.png", + dpi=300, bbox_inches='tight') + plt.close() + + print(f"已生成实时避障图: avoidance_{self.frame_count:04d}.png") + def run_simulation(self): """运行MuJoCo仿真并生成数据""" print("开始仿真...") @@ -853,16 +945,19 @@ def run_simulation(self): prev_detected_count = 0 + # 初始化控制变量 + left_speed, right_speed, steering_angle = 5.0, 5.0, 0.0 + for i in range(SIMULATION_FRAMES): # 每20帧生成和保存一次数据 if i % 20 == 0: # 生成传感器数据和标注 lidar_data = self.generate_realistic_lidar_data() annotations = self.generate_annotations() - + # 新增:模拟温度数据 temperature = self.simulate_temperature_data() - + # 检查并控制空调 ac_status = self.check_and_control_ac(temperature) @@ -889,16 +984,20 @@ def run_simulation(self): # 生成识别效果图 self.visualize_detection(lidar_data, annotations) - + + # 新增:生成实时避障图 + self.visualize_real_time_avoidance(lidar_data, annotations["objects"], + left_speed, right_speed, steering_angle) + # 新增:生成温度可视化图 self.visualize_temperature_data(temperature, annotations["objects"]) - + # 新增:生成温度分布热力图 self.generate_thermal_map(temperature, annotations["objects"]) - + # 新增:生成空调控制状态图 self.visualize_ac_control(temperature, ac_status) - + # 在保存数据时也保存温度信息 temp_data = { "frame": self.frame_count, @@ -911,12 +1010,6 @@ def run_simulation(self): json.dump(temp_data, f, indent=4) print(f"已仿真 {i}/{SIMULATION_FRAMES} 帧") - else: - # 使用上一帧的控制指令 - try: - left_speed, right_speed, steering_angle - except NameError: - left_speed, right_speed, steering_angle = 5.0, 5.0, 0.0 # 设置控制输入 if rear_left_idx >= 0: @@ -939,14 +1032,314 @@ def run_simulation(self): # 控制仿真速度以便观察 time.sleep(0.01) + # 新增:故障监测 + if i % FAULT_MONITORING_PARAMS["health_check_interval"] == 0: + health_report = self.fault_monitor.check_system_health() + self.health_scores.append((self.frame_count, health_report["overall_health"])) + + # 记录故障 + if health_report["faults"]: + self.fault_history.append({ + "frame": self.frame_count, + "timestamp": time.time(), + "report": health_report + }) + + # 生成故障报告和图表 + self.generate_fault_report(health_report) + self.visualize_fault_status(health_report) + self.generate_health_trend() + + # 打印故障信息 + self.print_fault_info(health_report) + # 生成最终的温度趋势图 self.generate_temperature_trend() - + # 生成温度调节汇总图 self.generate_temperature_summary_chart() - + + # 生成最终的健康趋势图 + self.generate_health_trend() + print(f"仿真完成!数据已保存到:{self.output_dir}") + def print_fault_info(self, health_report): + """打印故障信息""" + faults = health_report["faults"] + if faults: + print(f"🚨 检测到 {len(faults)} 个故障:") + for fault in faults: + print(f" - {fault['component']}: {fault['description']} (严重程度: {fault['severity']})") + else: + print("✅ 系统健康状态良好") + + def generate_fault_report(self, health_report): + """生成故障报告JSON文件""" + fault_data = { + "frame": self.frame_count, + "timestamp": time.time(), + "health_report": health_report + } + + with open(f"{self.output_dir}/fault_reports/frame_{self.frame_count:04d}.json", "w") as f: + json.dump(fault_data, f, indent=4, ensure_ascii=False) + + print(f"已生成故障报告: frame_{self.frame_count:04d}.json") + + def visualize_fault_status(self, health_report): + """生成故障状态可视化图""" + fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6)) + + # 左侧图:各组件健康状态 + components = list(health_report["component_health"].keys()) + health_scores = list(health_report["component_health"].values()) + + # 使用不同颜色表示健康状态 + colors = [] + for score in health_scores: + if score > 0.8: + colors.append('green') # 健康 + elif score > 0.5: + colors.append('orange') # 警告 + else: + colors.append('red') # 严重 + + bars = ax1.bar(range(len(components)), health_scores, color=colors) + ax1.set_xlabel('组件') + ax1.set_ylabel('健康评分 (0-1)') + ax1.set_title(f'组件健康状态 - 帧 {self.frame_count:04d}') + ax1.set_xticks(range(len(components))) + ax1.set_xticklabels(components, rotation=45, ha='right') + ax1.set_ylim(0, 1.1) + + # 添加数值标签 + for i, (bar, score) in enumerate(zip(bars, health_scores)): + ax1.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 0.01, + f'{score:.2f}', ha='center', va='bottom') + + # 右侧图:故障详情 + faults = health_report["faults"] + if faults: + fault_names = [f"{fault['component']}" for fault in faults] + severities = [fault['severity'] for fault in faults] + + # 严重程度颜色映射 + severity_colors = [] + for severity in severities: + if severity > 0.7: + severity_colors.append('red') + elif severity > 0.4: + severity_colors.append('orange') + else: + severity_colors.append('yellow') + + bars = ax2.bar(range(len(faults)), severities, color=severity_colors) + ax2.set_xlabel('故障组件') + ax2.set_ylabel('严重程度 (0-1)') + ax2.set_title(f'检测到的故障 - 帧 {self.frame_count:04d}') + ax2.set_xticks(range(len(faults))) + ax2.set_xticklabels(fault_names, rotation=45, ha='right') + ax2.set_ylim(0, 1.1) + + # 添加数值标签和描述 + for i, (bar, fault) in enumerate(zip(bars, faults)): + ax2.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 0.01, + f"{fault['description']}", ha='center', va='bottom', fontsize=8) + else: + ax2.text(0.5, 0.5, '当前无故障', horizontalalignment='center', + verticalalignment='center', transform=ax2.transAxes, fontsize=14) + ax2.set_title(f'故障状态 - 帧 {self.frame_count:04d}') + + plt.tight_layout() + plt.savefig(f"{self.output_dir}/fault_reports/fault_status_{self.frame_count:04d}.png", + dpi=300, bbox_inches='tight') + plt.close() + + print(f"已生成故障状态图: fault_status_{self.frame_count:04d}.png") + + def generate_health_trend(self): + """生成系统健康趋势图""" + if not self.health_scores: + return + + frames, scores = zip(*self.health_scores) + + plt.figure(figsize=(12, 6)) + plt.plot(frames, scores, 'o-', linewidth=2, markersize=6, color='blue') + plt.xlabel('帧序号') + plt.ylabel('整体健康评分 (0-1)') + plt.title('系统健康状态趋势') + plt.grid(True, alpha=0.3) + plt.ylim(0, 1.1) + + # 添加健康状态区域 + plt.axhspan(0.8, 1.0, alpha=0.2, color='green', label='健康 (>0.8)') + plt.axhspan(0.5, 0.8, alpha=0.2, color='yellow', label='警告 (0.5-0.8)') + plt.axhspan(0.0, 0.5, alpha=0.2, color='red', label='严重 (<0.5)') + + plt.legend() + + # 标注故障点 + fault_frames = [record["frame"] for record in self.fault_history] + fault_scores = [record["report"]["overall_health"] for record in self.fault_history] + if fault_frames: + plt.scatter(fault_frames, fault_scores, color='red', s=50, zorder=5, label='检测到故障') + + plt.tight_layout() + plt.savefig(f"{self.output_dir}/fault_reports/health_trend.png", dpi=300, bbox_inches='tight') + plt.close() + + print("已生成健康趋势图: health_trend.png") + + +class FaultMonitor: + """故障监测系统""" + + def __init__(self, model, data): + self.model = model + self.data = data + self.sensor_names = [] + self.actuator_names = [] + + # 获取传感器列表 + for i in range(model.nsensor): + sensor_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i) + if sensor_name: + self.sensor_names.append(sensor_name) + + # 获取执行器列表 + for i in range(model.nu): + actuator_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) + if actuator_name: + self.actuator_names.append(actuator_name) + + # 存储历史数据用于异常检测 + self.sensor_history = defaultdict(lambda: deque(maxlen=50)) + self.actuator_history = defaultdict(lambda: deque(maxlen=50)) + + def check_system_health(self): + """检查系统整体健康状态""" + faults = [] + component_health = {} + + # 检查传感器健康状态 + sensor_health = self._check_sensor_health(faults) + component_health.update(sensor_health) + + # 检查执行器健康状态 + actuator_health = self._check_actuator_health(faults) + component_health.update(actuator_health) + + # 计算整体健康评分 + if component_health: + overall_health = sum(component_health.values()) / len(component_health) + else: + overall_health = 1.0 + + return { + "overall_health": overall_health, + "component_health": component_health, + "faults": faults + } + + def _check_sensor_health(self, faults): + """检查传感器健康状态""" + sensor_health = {} + + for sensor_name in self.sensor_names: + try: + # 获取传感器索引 + sensor_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR, sensor_name) + if sensor_id < 0: + continue + + # 获取传感器数据 + sensor_data = self.data.sensordata[sensor_id] + + # 存储历史数据 + self.sensor_history[sensor_name].append(sensor_data) + + # 计算健康评分(基于数据变化) + if len(self.sensor_history[sensor_name]) > 5: + recent_data = list(self.sensor_history[sensor_name])[-5:] + # 计算数据变化率 + data_changes = [abs(recent_data[i] - recent_data[i-1]) + for i in range(1, len(recent_data))] + avg_change = sum(data_changes) / len(data_changes) + + # 健康评分(变化率越小越健康,但不能为0) + health_score = max(0.1, 1.0 - avg_change) + + # 检查是否有故障 + if avg_change < FAULT_MONITORING_PARAMS["sensor_fault_threshold"]: + faults.append({ + "component": f"传感器:{sensor_name}", + "description": "传感器数据无变化,可能存在故障", + "severity": 1.0 - health_score + }) + else: + health_score = 1.0 + + sensor_health[f"传感器:{sensor_name}"] = health_score + + except Exception as e: + # 传感器读取失败 + sensor_health[f"传感器:{sensor_name}"] = 0.0 + faults.append({ + "component": f"传感器:{sensor_name}", + "description": f"传感器读取失败: {str(e)}", + "severity": 1.0 + }) + + return sensor_health + + def _check_actuator_health(self, faults): + """检查执行器健康状态""" + actuator_health = {} + + for actuator_name in self.actuator_names: + try: + # 获取执行器索引 + actuator_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name) + if actuator_id < 0: + continue + + # 获取执行器数据 + actuator_data = self.data.ctrl[actuator_id] + + # 存储历史数据 + self.actuator_history[actuator_name].append(actuator_data) + + # 计算健康评分(基于数据有效性) + if len(self.actuator_history[actuator_name]) > 5: + recent_data = list(self.actuator_history[actuator_name])[-5:] + # 检查是否有NaN或无穷大值 + invalid_count = sum(1 for x in recent_data if np.isnan(x) or np.isinf(x)) + health_score = 1.0 - (invalid_count / len(recent_data)) + + # 检查是否有故障 + if invalid_count > 0: + faults.append({ + "component": f"执行器:{actuator_name}", + "description": f"执行器数据异常 ({invalid_count}/{len(recent_data)} 帧)", + "severity": invalid_count / len(recent_data) + }) + else: + health_score = 1.0 + + actuator_health[f"执行器:{actuator_name}"] = health_score + + except Exception as e: + # 执行器读取失败 + actuator_health[f"执行器:{actuator_name}"] = 0.0 + faults.append({ + "component": f"执行器:{actuator_name}", + "description": f"执行器读取失败: {str(e)}", + "severity": 1.0 + }) + + return actuator_health if __name__ == "__main__": print("正在初始化仿真器...")