diff --git a/src/Active-Lane-Keeping-Assistant/lane.py b/src/Active-Lane-Keeping-Assistant/lane.py index ddfc45eab6..9a7cd67f09 100644 --- a/src/Active-Lane-Keeping-Assistant/lane.py +++ b/src/Active-Lane-Keeping-Assistant/lane.py @@ -12,10 +12,6 @@ class Lane: HLS_L_THRESHOLD_MIN = 150 HLS_L_THRESHOLD_MAX = 255 XM_PER_PIX = 3.7 / 781 - ROI_BOTTOM = 350 - ROI_TOP = 260 - ROI_LEFT = 200 - ROI_RIGHT = 440 def __init__(self, height:int, width:int, save:bool=False, save_folder:str=join('img', 'examples')) -> None: """Constructor @@ -70,11 +66,8 @@ def get_lines(self, img:np.ndarray) -> np.ndarray: return binary_blured - def extract_roi(self, img: np.ndarray, - bottom: int = ROI_BOTTOM, - top: int = ROI_TOP, - left: int = ROI_LEFT, - right: int = ROI_RIGHT) -> tuple[np.ndarray, np.ndarray]: + def extract_roi(self, img:np.ndarray, bottom:int=350, top:int=260, + left:int=200, right:int=440) -> tuple[np.ndarray, np.ndarray]: """Extract the Region of Interest Extracts the region of interest and transforms it to fit the original diff --git a/src/Embodied_Human_Arm_Modeling/arm_model_full_hand.xml b/src/Embodied_Human_Arm_Modeling/arm_model_full_hand.xml new file mode 100644 index 0000000000..13ab5ec0c9 --- /dev/null +++ b/src/Embodied_Human_Arm_Modeling/arm_model_full_hand.xml @@ -0,0 +1,208 @@ + + + + \ No newline at end of file diff --git a/src/Embodied_Human_Arm_Modeling/simulate_arm.py b/src/Embodied_Human_Arm_Modeling/simulate_arm.py new file mode 100644 index 0000000000..b907272946 --- /dev/null +++ b/src/Embodied_Human_Arm_Modeling/simulate_arm.py @@ -0,0 +1,59 @@ +import os +import sys +os.environ['MUJOCO_GL'] = 'egl' +# 关键设置:获取当前脚本文件的绝对目录,并设置为工作目录 +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +os.chdir(SCRIPT_DIR) +# simulate_arm.py +import mujoco +import mujoco.viewer +import numpy as np +import time + +def main(): + # 1. 加载模型 + model = mujoco.MjModel.from_xml_path('arm_model.xml') + data = mujoco.MjData(model) + + # 2. 设置初始关节位置(例如,让肘部稍微弯曲) + data.qpos[model.joint('elbow').id] = -np.radians(45) # -45度 + + # 3. 使用 viewer 进行交互式仿真 + print("正在启动仿真查看器...") + print("提示:") + print(" - 在查看器窗口按 [Space] 暂停/继续物理仿真。") + print(" - 在左侧‘关节’面板,可以拖拽滑块实时驱动电机,观察手臂运动。") + + with mujoco.viewer.launch(model, data) as viewer: + # 设置一个简单的摆动目标 + target_positions = np.array([0.3, 0.1, -0.5, 0.0]) # 对应四个电机的目标位置 + time_per_cycle = 5.0 # 摆动周期(秒) + start_time = time.time() + + # 保持查看器窗口开启 + while viewer.is_running(): + step_start = time.time() + + # 计算基于时间的周期性目标 + t = time.time() - start_time + phase = 2 * np.pi * t / time_per_cycle + current_target = target_positions * np.sin(phase) + + # 将目标位置设置为电机的控制信号(简单的位置控制) + data.ctrl[:] = current_target + + # 执行一步物理仿真 + mujoco.mj_step(model, data) + + # 同步查看器 + viewer.sync() + + # 粗略的时间同步,避免占用过多CPU + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(min(time_until_next_step, model.opt.timestep)) + + print("仿真结束。") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/Embodied_Human_Arm_Modeling/simulate_full_hand.py b/src/Embodied_Human_Arm_Modeling/simulate_full_hand.py new file mode 100644 index 0000000000..130e6222b3 --- /dev/null +++ b/src/Embodied_Human_Arm_Modeling/simulate_full_hand.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +"""完整五指手掌仿真""" +import os +import sys +import time +import numpy as np +import mujoco +import mujoco.viewer + + +def setup_environment(): + """设置环境""" + os.environ['MUJOCO_GL'] = os.getenv('MUJOCO_GL', 'egl') + return mujoco, mujoco.viewer + + +class FullHandController: + """五指手掌控制器""" + + def __init__(self, model, data): + self.model = model + self.data = data + self.mode = "idle" + self.nu = model.nu + + # 设置初始位置 + self.set_default_positions() + + # 运动模式参数 + self.wave_phase = 0.0 + self.grasp_strength = 0.0 + + def set_default_positions(self): + """设置手的默认姿态(放松状态)""" + # 手臂位置 + if self.nu >= 3: + self.data.ctrl[0] = 0.1 # 肩部屈曲 + self.data.ctrl[1] = 0.05 # 肩部外展 + self.data.ctrl[2] = 0.0 # 肩部旋转 + + if self.nu >= 4: + self.data.ctrl[3] = -0.3 # 肘部弯曲 + + if self.nu >= 6: + self.data.ctrl[4] = 0.0 # 腕部屈曲 + self.data.ctrl[5] = 0.0 # 腕部外展 + + # 手指位置(放松状态) + finger_indices = list(range(6, min(self.nu, 21))) + for i in finger_indices: + self.data.ctrl[i] = 0.1 # 轻微弯曲 + + def update_wave(self, t): + """波浪形手指运动""" + if self.nu >= 20: + # 拇指 + self.data.ctrl[6] = 0.3 + 0.2 * np.sin(2 * np.pi * 0.5 * t) + self.data.ctrl[7] = 0.2 + 0.1 * np.sin(2 * np.pi * 0.5 * t + 0.5) + self.data.ctrl[8] = 0.1 + 0.1 * np.sin(2 * np.pi * 0.5 * t + 1.0) + + # 食指 + self.data.ctrl[9] = 0.2 + 0.3 * np.sin(2 * np.pi * 0.8 * t) + self.data.ctrl[10] = 0.1 + 0.2 * np.sin(2 * np.pi * 0.8 * t + 0.3) + self.data.ctrl[11] = 0.1 + 0.1 * np.sin(2 * np.pi * 0.8 * t + 0.6) + + # 中指 + self.data.ctrl[12] = 0.2 + 0.3 * np.sin(2 * np.pi * 1.0 * t) + self.data.ctrl[13] = 0.1 + 0.2 * np.sin(2 * np.pi * 1.0 * t + 0.3) + self.data.ctrl[14] = 0.1 + 0.1 * np.sin(2 * np.pi * 1.0 * t + 0.6) + + # 无名指 + self.data.ctrl[15] = 0.15 + 0.25 * np.sin(2 * np.pi * 1.2 * t) + self.data.ctrl[16] = 0.1 + 0.15 * np.sin(2 * np.pi * 1.2 * t + 0.3) + self.data.ctrl[17] = 0.05 + 0.1 * np.sin(2 * np.pi * 1.2 * t + 0.6) + + # 小指 + self.data.ctrl[18] = 0.1 + 0.2 * np.sin(2 * np.pi * 1.5 * t) + self.data.ctrl[19] = 0.05 + 0.1 * np.sin(2 * np.pi * 1.5 * t + 0.3) + + def update_grasp(self, strength): + """抓握动作""" + # 拇指 + if self.nu >= 9: + self.data.ctrl[6] = 0.4 * strength # 拇指CMC + self.data.ctrl[7] = 0.6 * strength # 拇指MCP + self.data.ctrl[8] = 0.4 * strength # 拇指IP + + # 其他手指(协同弯曲) + finger_groups = [(9, 12), (10, 13), (11, 14)] # 食指 + for start, end in finger_groups: + if end < self.nu: + for i in range(start, end + 1): + self.data.ctrl[i] = 0.7 * strength + + def update_idle(self, t): + """空闲状态轻微运动""" + if self.nu >= 6: + # 手臂轻微摆动 + self.data.ctrl[0] = 0.05 * np.sin(2 * np.pi * 0.1 * t) + self.data.ctrl[3] = -0.3 + 0.05 * np.sin(2 * np.pi * 0.15 * t + 0.5) + + # 手指轻微抖动 + for i in range(6, min(self.nu, 21)): + self.data.ctrl[i] = 0.1 + 0.02 * np.sin(2 * np.pi * 0.2 * t + i * 0.1) + + def update(self, t, mode="idle", strength=0.0): + """根据模式更新控制""" + self.mode = mode + + if mode == "wave": + self.update_wave(t) + elif mode == "grasp": + self.update_grasp(strength) + else: # idle + self.update_idle(t) + + +class FullHandSimulation: + """完整五指仿真""" + + def __init__(self, model_path="arm_model_full_hand.xml"): + self.mujoco, self.viewer = setup_environment() + + print(f"加载完整五指模型: {model_path}") + self.model = self.mujoco.MjModel.from_xml_path(model_path) + self.data = self.mujoco.MjData(self.model) + + self.controller = FullHandController(self.model, self.data) + self.sim_time = 0.0 + self.paused = False + self.mode = "idle" + self.grasp_strength = 0.0 + + print(f"模型信息: {self.model.nu} 个执行器, {self.model.nv} 个自由度") + + def step(self): + """执行仿真步""" + if self.paused: + return + + # 更新控制器 + if self.sim_time < 5.0: + self.mode = "idle" + elif self.sim_time < 10.0: + self.mode = "wave" + else: + self.mode = "grasp" + self.grasp_strength = 0.5 + 0.3 * np.sin(2 * np.pi * 0.3 * self.sim_time) + + self.controller.update(self.sim_time, self.mode, self.grasp_strength) + + # 物理步进 + self.mujoco.mj_step(self.model, self.data) + self.sim_time = self.data.time + + def print_status(self): + """打印状态信息""" + if int(self.sim_time * 10) % 20 == 0: # 每2秒打印一次 + print(f"\n时间: {self.sim_time:.1f}s | 模式: {self.mode}") + print(f"拇指位置: {self.data.qpos[10]:.3f}, 食指: {self.data.qpos[13]:.3f}") + + def run_interactive(self): + """交互式运行""" + print("\n" + "=" * 60) + print("完整五指手掌仿真 - 交互模式") + print("=" * 60) + print("控制指令:") + print(" [空格] 暂停/继续") + print(" [1] 空闲模式") + print(" [2] 波浪模式") + print(" [3] 抓握模式") + print(" [ESC] 退出") + print("=" * 60) + + with self.viewer.launch(self.model, self.data) as viewer: + viewer.cam.distance = 2.0 + viewer.cam.elevation = -15 + viewer.cam.azimuth = 120 + + last_print_time = 0 + + while viewer.is_running(): + # 处理按键 + if viewer.is_key_down(self.mujoco.mjtKey.mjKEY_SPACE): + self.paused = not self.paused + time.sleep(0.2) + + if viewer.is_key_down(ord('1')): + self.mode = "idle" + print("切换到空闲模式") + + if viewer.is_key_down(ord('2')): + self.mode = "wave" + print("切换到波浪模式") + + if viewer.is_key_down(ord('3')): + self.mode = "grasp" + print("切换到抓握模式") + + # 仿真步进 + self.step() + + # 打印状态 + if self.sim_time - last_print_time > 2.0: + self.print_status() + last_print_time = self.sim_time + + # 同步查看器 + viewer.sync() + + # 控制帧率 + time.sleep(0.001) + + print("\n仿真完成") + + +def main(): + """主函数""" + print("🚀 启动完整五指手掌仿真") + + try: + sim = FullHandSimulation() + sim.run_interactive() + except Exception as e: + print(f"错误: {e}") + import traceback + traceback.print_exc() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index cbd7071750..6e218282e5 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -40,7 +40,6 @@ def is_port_used(port): else: raise RuntimeError("CARLA连接超时(3次重试失败),请检查模拟器是否启动") - # ========== 修复核心错误:从World获取蓝图库(而非Map) ========== self.blueprint_library = self.world.get_blueprint_library() # ========== 同步模式配置 ========== @@ -124,20 +123,6 @@ def is_port_used(port): self.max_steps = 800 # 单回合最大步数(调整为800) self.current_step = 0 # 当前回合已执行步数 - # ========== 新增:档位检测核心配置 ========== - self.gear_config = { - # 车速区间(km/h) : 推荐档位 - 0: 1, # 0-10km/h → 1挡 - 10: 2, # 10-20km/h → 2挡 - 20: 3, # 20-30km/h → 3挡 - 30: 4, # 30-40km/h → 4挡 - 40: 5 # ≥40km/h → 5挡 - } - self.gear_correct_reward = 0.1 # 档位正确加分 - self.last_speed_range = 0 # 记录上一次的车速区间(用于判断是否跨档位) - self.last_gear = 1 # 记录上一步档位 - self.is_initial_gear_logged = False # 标记初始档位日志是否已输出 - # 基础配置(完全保留) self.spawn_retry_times = 20 self.spawn_safe_radius = 2.0 @@ -161,62 +146,6 @@ def is_port_used(port): self.view_distance = 8.0 self.z_offset = 0.5 - # ========== 新增:档位奖励计算(初始输出一次 + 跨区间输出) ========== - def _calculate_gear_reward(self): - if not self.vehicle or not self.vehicle.is_alive: - return 0.0 - - # 1. 计算当前车速(m/s → km/h) - velocity = self.vehicle.get_velocity() - speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) - speed_kmh = round(speed_m_s * 3.6, 1) - - # 2. 判断当前车速区间 - current_speed_range = 0 - if speed_kmh < 0: # 倒车 - current_speed_range = -1 - recommended_gear = 'R' - else: - # 匹配前进车速区间 - for range_val in sorted(self.gear_config.keys(), reverse=True): - if speed_kmh >= range_val: - current_speed_range = range_val - recommended_gear = self.gear_config[range_val] - break - - # 3. 获取当前档位 - control = self.vehicle.get_control() - current_gear = control.gear - if control.reverse or speed_kmh < 0: # 倒车挡 - current_gear = 'R' - - gear_reward = 0.0 - # 4. 初始启动时输出一次(仅首次) - if not self.is_initial_gear_logged: - is_correct = (current_gear == recommended_gear) - if is_correct: - gear_reward = self.gear_correct_reward - if speed_kmh < 0: - print(f"初始倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") - else: - print(f"初始档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前{current_gear}挡,对应{current_speed_range}-{current_speed_range+10}km/h区间)") - self.is_initial_gear_logged = True # 标记初始日志已输出 - # 5. 跨档位区间时输出(初始之后) - elif current_speed_range != self.last_speed_range: - # 档位正确判断 - is_correct = (current_gear == recommended_gear) - if is_correct: - gear_reward = self.gear_correct_reward - # 仅档位正确时输出日志 - if speed_kmh < 0: - print(f"倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") - else: - print(f"车速进入{current_speed_range}-{current_speed_range+10}km/h区间,档位{current_gear}正确加分:{self.gear_correct_reward}") - # 更新上次车速区间 - self.last_speed_range = current_speed_range - - return gear_reward - # ========== 天气切换:输出中文名称 ========== def switch_weather(self): available_weathers = [w for w in self.preset_weathers if w != self.current_weather] @@ -396,7 +325,7 @@ def _spawn_small_npc(self): p.destroy() self.npc_pedestrian_list.clear() - vehicle_bps = self.world.get_blueprint_library().filter('vehicle.*') + vehicle_bps = self.blueprint_library.filter('vehicle.*') spawn_points = self.world.get_map().get_spawn_points() if len(spawn_points) < 50: @@ -426,7 +355,7 @@ def _spawn_small_npc(self): except: continue - pedestrian_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*') + pedestrian_bps = self.blueprint_library.filter('walker.pedestrian.*') for _ in range(5): if self.vehicle is not None: hero_loc = self.vehicle.get_transform().location @@ -446,7 +375,7 @@ def _spawn_small_npc(self): # ========== 初始化碰撞传感器(完全保留原始代码) ========== def _init_collision_sensor(self): - collision_bp = self.world.get_blueprint_library().find('sensor.other.collision') + collision_bp = self.blueprint_library.find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) self.collision_sensor = self.world.spawn_actor( collision_bp, collision_transform, attach_to=self.vehicle @@ -467,7 +396,7 @@ def _collision_callback(self, event): # ========== 初始化相机(完全保留原始代码) ========== def _init_camera(self): - camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') camera_bp.set_attribute('fov', '90') @@ -502,17 +431,12 @@ def reset(self): # ========== 新增:重置步数 ========== self.current_step = 0 - - # ========== 新增:重置档位相关状态 ========== - self.last_speed_range = 0 - self.last_gear = 1 - self.is_initial_gear_logged = False # 重置初始档位日志标记 # ========== 切换天气(会自动输出中文名称) ========== self.switch_weather() # 生成车辆 - vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] + vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] self.vehicle = self._spawn_vehicle_safely(vehicle_bp) # ========== 新增:生成导航目标点 ========== @@ -562,7 +486,7 @@ def _camera_callback(self, image): array = np.frombuffer(image.raw_data, dtype=np.uint8) self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] - # ========== 核心:奖励函数(集成导航+步数限制 + 新增档位奖励) ========== + # ========== 核心:奖励函数(集成导航+步数限制) ========== def step(self, action): if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁,请先调用reset()") @@ -584,35 +508,12 @@ def step(self, action): elif action == 3: throttle = -0.2 - # ========== 优化:根据车速自动匹配合理档位 ========== - # 计算当前车速 - velocity = self.vehicle.get_velocity() - speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) - current_speed = speed_m_s * 3.6 - - # 匹配档位 - gear = 1 - if throttle < 0: # 倒车 - gear = -1 - else: - # 根据车速匹配前进档位 - if 0 <= current_speed < 10: - gear = 1 - elif 10 <= current_speed < 20: - gear = 2 - elif 20 <= current_speed < 30: - gear = 3 - elif 30 <= current_speed < 40: - gear = 4 - else: - gear = 5 - self.vehicle.apply_control(carla.VehicleControl( throttle=throttle, steer=steer, hand_brake=False, reverse=(throttle < 0), - gear=gear, + gear=1, manual_gear_shift=True )) @@ -625,10 +526,6 @@ def step(self, action): over_speed_reward = self._check_over_speed() lane_reward = self._check_lane_offset() collision_reward = 0.0 - - # ========== 新增:计算档位奖励 ========== - gear_reward = self._calculate_gear_reward() - done = False if self.has_collision and not self.collision_penalty_applied: @@ -668,8 +565,8 @@ def step(self, action): print(f"达到最大步数{self.max_steps},终止训练(当前距离目标点:{dist_to_target:.2f}米)") done = True - # 总奖励(新增档位奖励) - total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + gear_reward + # 总奖励 + total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward next_state = self.get_observation() return next_state, total_reward, done, { @@ -679,7 +576,6 @@ def step(self, action): "lane_reward": lane_reward, "collision_reward": collision_reward, "nav_reward": nav_reward, - "gear_reward": gear_reward, # 新增:返回档位奖励 "total_reward": total_reward, "current_step": self.current_step, "dist_to_target": dist_to_target, diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py index 72e6e6d43a..b0b29ef42e 100644 --- a/src/Mujoco_manrun/main.py +++ b/src/Mujoco_manrun/main.py @@ -3,107 +3,12 @@ from mujoco import viewer import time import os -import sys -import threading - -# 新增:键盘输入监听线程(用于手动控制状态/转向/速度,不阻塞仿真循环) -class KeyboardInputHandler(threading.Thread): - def __init__(self, stabilizer): - super().__init__(daemon=True) - self.stabilizer = stabilizer - self.running = True - - def run(self): - print("\n===== 控制指令说明 =====") - print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立") - print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速") - print("========================\n") - while self.running: - try: - # 非阻塞键盘输入(兼容不同系统) - if sys.platform == "win32": - import msvcrt - if msvcrt.kbhit(): - key = msvcrt.getch().decode('utf-8').lower() - self._handle_key(key) - else: - import select - if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: - key = sys.stdin.read(1).lower() - self._handle_key(key) - time.sleep(0.01) - except: - continue - - def _handle_key(self, key): - if key == 'w': - self.stabilizer.set_state("WALK") - print(f"[指令] 切换为行走状态 | 当前速度: {self.stabilizer.walk_speed:.2f} | 转向: {self.stabilizer.turn_angle:.2f}") - elif key == 's': - self.stabilizer.set_state("STOP") - print("[指令] 切换为停止状态") - elif key == 'e': - self.stabilizer.set_state("EMERGENCY") - print("[指令] 触发紧急停止") - elif key == 'r': - self.stabilizer.set_state("STAND") - print("[指令] 恢复站立姿态") - elif key == 'a': - self.stabilizer.set_turn_angle(self.stabilizer.turn_angle + 0.05) - print(f"[指令] 左转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") - elif key == 'd': - self.stabilizer.set_turn_angle(self.stabilizer.turn_angle - 0.05) - print(f"[指令] 右转 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") - elif key == ' ': - self.stabilizer.set_turn_angle(0.2 if self.stabilizer.turn_angle <= 0 else -0.2) - print(f"[指令] 原地转向 | 当前转向角度: {self.stabilizer.turn_angle:.2f}rad") - elif key == 'z': - self.stabilizer.set_walk_speed(self.stabilizer.walk_speed - 0.1) - print(f"[指令] 减速 | 当前速度: {self.stabilizer.walk_speed:.2f}") - elif key == 'x': - self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1) - print(f"[指令] 加速 | 当前速度: {self.stabilizer.walk_speed:.2f}") - -# 新增:CPG中枢模式发生器(替代固定正弦步态,兼容变速/步长联动) -class CPGOscillator: - def __init__(self, freq=0.5, amp=0.4, phase=0.0, coupling_strength=0.2): - self.base_freq = freq # 基础频率(对应原始步态周期2s) - self.base_amp = amp # 基础振幅(对应原始步长) - self.freq = freq - self.amp = amp - self.phase = phase # 初始相位(左右腿差π) - self.coupling = coupling_strength # 腿间耦合强度 - self.state = np.array([np.sin(phase), np.cos(phase)]) # 振荡器状态(x,y) - - def update(self, dt, target_phase=0.0): - """更新CPG状态,返回关节目标偏移量""" - # 范德波尔振荡器方程(生物节律更自然,抗干扰) - mu = 1.0 # 非线性系数,控制振荡收敛性 - x, y = self.state - dx = 2 * np.pi * self.freq * y + self.coupling * np.sin(target_phase - self.phase) - dy = 2 * np.pi * self.freq * (mu * (1 - x**2) * y - x) - # 更新状态(积分) - self.state += np.array([dx, dy]) * dt - self.phase = np.arctan2(self.state[0], self.state[1]) # 更新相位 - # 返回当前输出(关节目标偏移量) - return self.amp * self.state[0] - - def reset(self): - """重置CPG状态""" - self.freq = self.base_freq - self.amp = self.base_amp - self.phase = 0.0 if self.base_phase == 0.0 else np.pi - self.state = np.array([np.sin(self.phase), np.cos(self.phase)]) - - @property - def base_phase(self): - return 0.0 if self.phase < np.pi else np.pi - -class HumanoidStabilizer: - """适配humanoid.xml模型的稳定站立与行走控制器(新增转向/变速/状态机)""" + + +class Stabilizer: def __init__(self, model_path): - # 类型检查与模型加载(原始逻辑完全保留) + # 基础初始化(保留原有核心逻辑) if not isinstance(model_path, str): raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型") @@ -113,15 +18,15 @@ def __init__(self, model_path): except Exception as e: raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性") - # 仿真核心参数(原始逻辑保留) + # 1. 仿真参数优化:固定时间步+更保守的初始稳定 self.sim_duration = 120.0 - self.dt = self.model.opt.timestep - self.init_wait_time = 4.0 + self.dt = 0.005 # 固定0.005s步长,提升仿真稳定性 + self.model.opt.timestep = self.dt + self.init_wait_time = 5.0 # 延长初始稳定时间至5s self.model.opt.gravity[2] = -9.81 - self.model.opt.iterations = 200 - self.model.opt.tolerance = 1e-8 + self.model.opt.iterations = 300 # 增加迭代次数提升接触求解精度 + self.model.opt.tolerance = 1e-9 # 降低容差,提升计算精度 - # 关节名称映射(原始逻辑完全保留) self.joint_names = [ "abdomen_z", "abdomen_y", "abdomen_x", "hip_x_right", "hip_z_right", "hip_y_right", @@ -134,182 +39,117 @@ def __init__(self, model_path): self.joint_name_to_idx = {name: i for i, name in enumerate(self.joint_names)} self.num_joints = len(self.joint_names) - # PD控制增益(原始逻辑完全保留) - self.kp_roll = 120.0 - self.kd_roll = 40.0 - self.kp_pitch = 100.0 - self.kd_pitch = 35.0 - self.kp_yaw = 30.0 - self.kd_yaw = 15.0 - self.kp_hip = 250.0 - self.kd_hip = 45.0 - self.kp_knee = 280.0 - self.kd_knee = 50.0 - self.kp_ankle = 200.0 - self.kd_ankle = 60.0 - self.kp_waist = 40.0 - self.kd_waist = 20.0 - self.kp_arm = 20.0 - self.kd_arm = 20.0 - - # 重心目标(原始逻辑保留) - self.com_target = np.array([0.05, 0.0, 0.78]) - self.kp_com = 50.0 - self.foot_contact_threshold = 1.5 - - # 状态变量(原始逻辑保留,新增状态机/CPG/转向/变速参数) + # PD增益优化:分阶段动态增益(核心) + # 基础增益(支撑腿) + self.kp_roll = 150.0 # 提高侧倾增益,增强横向稳定 + self.kd_roll = 50.0 + self.kp_pitch = 130.0 # 提高俯仰增益,抑制后仰 + self.kd_pitch = 45.0 + self.kp_yaw = 40.0 + self.kd_yaw = 20.0 + + # 腿部增益(分支撑/摆动) + self.kp_hip_support = 300.0 # 支撑腿髋关节高增益 + self.kd_hip_support = 60.0 + self.kp_hip_swing = 150.0 # 摆动腿髋关节低增益,避免过度摆动 + self.kd_hip_swing = 30.0 + + self.kp_knee_support = 320.0 # 支撑腿膝关节高增益 + self.kd_knee_support = 70.0 + self.kp_knee_swing = 180.0 # 摆动腿膝关节低增益 + self.kd_knee_swing = 40.0 + + self.kp_ankle_support = 250.0 # 支撑腿踝关节高增益(核心防踮脚) + self.kd_ankle_support = 80.0 + self.kp_ankle_swing = 120.0 # 摆动腿踝关节低增益 + self.kd_ankle_swing = 40.0 + + # 腰部/手臂增益(小幅提升) + self.kp_waist = 50.0 + self.kd_waist = 25.0 + self.kp_arm = 25.0 + self.kd_arm = 25.0 + + # 重心控制优化:动态目标+速度反馈 + self.com_base_target = np.array([0.0, 0.0, 0.85]) # 基础重心高度提升,降低跌倒风险 + # ========== 关键修复1:初始化com_target,避免属性不存在 ========== + self.com_target = self.com_base_target.copy() # 初始赋值,与基础重心一致 + self.com_gait_offset = np.array([0.08, 0.0, 0.02]) # 步态相位对应的重心偏移 + self.kp_com = 80.0 # 提高重心比例增益 + self.kd_com = 30.0 # 新增重心速度阻尼 + self.foot_contact_threshold = 2.0 # 提高接触阈值,减少误判 + + # 状态变量优化:新增滤波和平滑参数 self.joint_targets = np.zeros(self.num_joints) + self.joint_targets_prev = np.zeros(self.num_joints) # 上一帧关节目标(滤波用) self.prev_com = np.zeros(3) - self.foot_contact = np.zeros(2) + self.com_vel = np.zeros(3) # 重心速度 + self.foot_contact = np.zeros(2) # [right, left] + self.foot_contact_history = np.zeros((2, 5)) # 接触状态历史(5帧滤波) self.integral_roll = 0.0 self.integral_pitch = 0.0 - self.integral_limit = 0.15 - - # 新增:运动状态机(核心) - self.state = "STAND" # 初始状态:STAND/WALK/STOP/EMERGENCY - self.state_map = { - "STAND": self._state_stand, # 站立(初始稳定) - "WALK": self._state_walk, # 行走(CPG驱动) - "STOP": self._state_stop, # 停止(关节归零) - "EMERGENCY": self._state_emergency # 急停(力矩清零) - } - - # 新增:CPG振荡器(替代原始固定正弦步态,兼容变速) - self.right_leg_cpg = CPGOscillator(freq=0.5, amp=0.4, phase=0.0) # 右腿初始相位0 - self.left_leg_cpg = CPGOscillator(freq=0.5, amp=0.4, phase=np.pi) # 左腿初始相位π(交替) - self.gait_phase = 0.0 # 保留原始相位变量,兼容调试输出 - - # 新增:转向参数 - self.turn_angle = 0.0 # 转向角度(左正右负,范围±0.3rad) - self.turn_gain = 0.1 # 躯干偏航增益 - - # 新增:变速参数 - self.walk_speed = 0.5 # 行走速度(0.1~1.0,映射到CPG频率/振幅) - self.speed_freq_gain = 0.4 # 速度→频率增益(0.3~0.7Hz) - self.speed_amp_gain = 0.2 # 速度→振幅增益(0.3~0.5) - - # 行走功能参数(原始逻辑保留,CPG替代后不再使用,仅做兼容) - self.gait_cycle = 2.0 - self.step_offset_hip = 0.4 - self.step_offset_knee = 0.6 - self.step_offset_ankle = 0.3 + self.integral_limit = 0.1 # 进一步收紧积分限幅 + self.integral_deadband = 0.01 # 积分死区,小误差不积分 + + # 步态规划优化:平滑插值+慢周期 + self.gait_phase = 0.0 + self.gait_cycle = 2.5 # 延长步态周期至2.5s,动作更平缓 + self.step_offset_hip = 0.35 # 降低髋关节偏移,减少重心波动 + self.step_offset_knee = 0.5 # 降低膝关节偏移 + self.step_offset_ankle = 0.25 self.walk_start_time = None + self.filter_alpha = 0.2 # 关节目标低通滤波系数 - # 初始化稳定姿态(原始逻辑保留) + # 力矩输出滤波:减少力矩突变 + self.torque_prev = np.zeros(self.num_joints) + self.torque_filter_alpha = 0.15 + + # 初始化稳定姿态 self._init_stable_pose() def _init_stable_pose(self): - """初始化稳定姿态(原始逻辑完全保留)""" + """优化初始姿态:更贴近自然站立,降低初始冲击""" mujoco.mj_resetData(self.model, self.data) - self.data.qpos[2] = 1.282 + + # 躯干初始位置:略高+零偏航,减少初始调整 + self.data.qpos[2] = 1.3 # 初始高度略高,落地更平稳 self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] self.data.qvel[:] = 0.0 self.data.xfrc_applied[:] = 0.0 - # 腰部关节 + # 腰部关节:微前倾,匹配自然站立 self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.05 # 微前倾,防止后仰 self.joint_targets[self.joint_name_to_idx["abdomen_x"]] = 0.0 - # 右腿关节 + # 右腿关节:微弯,增加缓冲 self.joint_targets[self.joint_name_to_idx["hip_x_right"]] = 0.0 self.joint_targets[self.joint_name_to_idx["hip_z_right"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.4 + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 + self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 # 膝盖微弯,减少落地冲击 self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 self.joint_targets[self.joint_name_to_idx["ankle_x_right"]] = 0.0 - # 左腿关节 + # 左腿关节:镜像右腿,保证对称 self.joint_targets[self.joint_name_to_idx["hip_x_left"]] = 0.0 self.joint_targets[self.joint_name_to_idx["hip_z_left"]] = 0.0 - self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.4 + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 + self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0 self.joint_targets[self.joint_name_to_idx["ankle_x_left"]] = 0.0 - # 手臂关节 - self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.5 - self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.1 - self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.5 + # 手臂关节:自然下垂,减少干扰 + self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.2 + self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.6 + self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.0 + self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.2 + self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.6 mujoco.mj_forward(self.model, self.data) - # 新增:状态机核心方法 - def _state_stand(self): - """站立状态:维持初始稳定姿态,CPG重置""" - self.right_leg_cpg.reset() - self.left_leg_cpg.reset() - self._init_stable_pose() # 恢复站立姿态 - - def _state_walk(self): - """行走状态:CPG驱动+转向+变速""" - if self.walk_start_time is None: - self.walk_start_time = self.data.time - # 1. 变速联动:速度→CPG频率+振幅 - self.right_leg_cpg.freq = 0.3 + self.walk_speed * self.speed_freq_gain - self.left_leg_cpg.freq = 0.3 + self.walk_speed * self.speed_freq_gain - self.right_leg_cpg.amp = 0.3 + self.walk_speed * self.speed_amp_gain - self.left_leg_cpg.amp = 0.3 + self.walk_speed * self.speed_amp_gain - - # 2. 转向联动:躯干偏航 + 左右腿步长差 - # 躯干偏航(abdomen_z)由转向角度控制 - self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = self.turn_angle * self.turn_gain - # 左右腿步长差(CPG振幅差):左转→左腿振幅减小/右腿增大,右转反之 - if self.turn_angle > 0: # 左转 - self.right_leg_cpg.amp *= 1.1 - self.left_leg_cpg.amp *= 0.9 - elif self.turn_angle < 0: # 右转 - self.right_leg_cpg.amp *= 0.9 - self.left_leg_cpg.amp *= 1.1 - - # 3. CPG更新(替代原始固定正弦步态) - right_hip_offset = self.right_leg_cpg.update(self.dt, target_phase=self.left_leg_cpg.phase) - left_hip_offset = self.left_leg_cpg.update(self.dt, target_phase=self.right_leg_cpg.phase) - - # 4. 更新关节目标(兼容原始关节映射) - self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.1 + right_hip_offset - self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.4 - right_hip_offset * 1.2 # 膝髋联动 - self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0 + right_hip_offset * 0.5 # 踝髋联动 - - self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.1 + left_hip_offset - self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.4 - left_hip_offset * 1.2 - self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0 + left_hip_offset * 0.5 - - # 更新原始步态相位(兼容调试输出) - self.gait_phase = self.right_leg_cpg.phase / (2 * np.pi) % 1.0 - - def _state_stop(self): - """停止状态:关节目标归零,缓慢减速""" - self.joint_targets *= 0.95 # 渐进归零,避免突变 - self.data.qvel[:] *= 0.9 # 速度阻尼 - - def _state_emergency(self): - """急停状态:力矩清零,速度归零""" - self.data.ctrl[:] = 0.0 - self.data.qvel[:] = 0.0 - self.joint_targets[:] = 0.0 - - # 新增:外部控制接口(设置状态/转向/速度) - def set_state(self, state): - if state in self.state_map.keys(): - self.state = state - # 状态切换时重置行走开始时间 - if state == "WALK": - self.walk_start_time = None - elif state == "STAND": - self._init_stable_pose() - - def set_turn_angle(self, angle): - self.turn_angle = np.clip(angle, -0.3, 0.3) # 限制转向角度,避免失衡 - - def set_walk_speed(self, speed): - self.walk_speed = np.clip(speed, 0.1, 1.0) # 限制速度范围 - - # 原始方法:四元数转欧拉角(完全保留) def _quat_to_euler_xyz(self, quat): + """四元数转欧拉角(保留)""" w, x, y, z = quat sinr_cosp = 2 * (w * x + y * z) cosr_cosp = 1 - 2 * (x * x + y * y) @@ -321,19 +161,20 @@ def _quat_to_euler_xyz(self, quat): yaw = np.arctan2(siny_cosp, cosy_cosp) return np.array([roll, pitch, yaw]) - # 原始方法:提取躯干欧拉角(完全保留) def _get_root_euler(self): + """提取躯干欧拉角(保留,增加限幅)""" quat = self.data.qpos[3:7].astype(np.float64).copy() euler = self._quat_to_euler_xyz(quat) euler = np.mod(euler + np.pi, 2 * np.pi) - np.pi - return np.clip(euler, -0.3, 0.3) + return np.clip(euler, -0.2, 0.2) # 更严格的倾角限制,防止过度倾斜 - # 原始方法:检测脚部接触(完全保留) def _detect_foot_contact(self): + """优化接触检测:5帧历史滤波+迟滞,避免频繁切换""" try: left_foot_geoms = ["foot1_left", "foot2_left"] right_foot_geoms = ["foot1_right", "foot2_right"] + # 计算当前接触力 left_force = 0.0 for geom_name in left_foot_geoms: geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) @@ -348,63 +189,139 @@ def _detect_foot_contact(self): mujoco.mj_contactForce(self.model, self.data, geom_id, force) right_force += np.linalg.norm(force[:3]) - self.foot_contact[1] = 1 if left_force > self.foot_contact_threshold else 0 - self.foot_contact[0] = 1 if right_force > self.foot_contact_threshold else 0 + # 1. 更新接触历史(滑动窗口) + self.foot_contact_history[0] = np.roll(self.foot_contact_history[0], -1) + self.foot_contact_history[1] = np.roll(self.foot_contact_history[1], -1) + self.foot_contact_history[0, -1] = 1 if right_force > self.foot_contact_threshold else 0 + self.foot_contact_history[1, -1] = 1 if left_force > self.foot_contact_threshold else 0 + + # 2. 多数表决:5帧中≥3帧接触则判定为支撑 + self.foot_contact[0] = 1 if np.sum(self.foot_contact_history[0]) >= 3 else 0 + self.foot_contact[1] = 1 if np.sum(self.foot_contact_history[1]) >= 3 else 0 except Exception as e: print(f"接触检测警告: {e}") self.foot_contact = np.ones(2) + self.foot_contact_history = np.ones((2, 5)) - # 原始方法:计算稳定力矩(修改:加入状态机逻辑,保留所有原始PD/补偿逻辑) - def _calculate_stabilizing_torques(self): - # 状态机驱动:根据当前状态更新关节目标 - self.state_map[self.state]() + def _update_gait_phase(self): + """步态相位更新(保留)""" + if self.walk_start_time is None: + self.walk_start_time = self.data.time + self.gait_phase = (self.data.time - self.walk_start_time) % self.gait_cycle / self.gait_cycle + + def _update_walk_joint_targets(self): + """优化步态规划:余弦插值+动态重心,动作更平滑""" + if self.data.time < self.init_wait_time: + # 初始稳定阶段:渐进式提升目标,避免突变 + alpha = min(1.0, (self.data.time) / self.init_wait_time) + self.joint_targets = self.joint_targets * alpha + self.joint_targets_prev * (1 - alpha) + # ========== 关键修复2:初始阶段保持com_target为基础值 ========== + self.com_target = self.com_base_target.copy() + return + + self._update_gait_phase() + phase = self.gait_phase + + # 核心优化:使用余弦插值代替正弦,加速度更平滑(减少冲击) + phase_rad = phase * 2 * np.pi + cos_phase = (1 - np.cos(phase_rad)) / 2 # 0→1→0,无加速度突变 + + # 动态重心目标:随步态相位前移/后移,匹配迈步节奏 + if phase < 0.5: + # 右腿摆动:重心小幅右移+前移 + self.com_target = self.com_base_target + np.array([0.05, 0.03, 0.0]) + # 右腿关节目标:余弦插值,动作更平滑 + right_hip_yaw = 0.05 + self.step_offset_hip * cos_phase + right_knee = -0.35 - self.step_offset_knee * cos_phase + right_ankle = 0.0 + self.step_offset_ankle * cos_phase + + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = right_hip_yaw + self.joint_targets[self.joint_name_to_idx["knee_right"]] = right_knee + self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = right_ankle + + # 左腿支撑:微调整,保持平衡 + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 - self.step_offset_hip * 0.1 + self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 + self.step_offset_knee * 0.05 + else: + # 左腿摆动:重心小幅左移+前移 + self.com_target = self.com_base_target + np.array([0.05, -0.03, 0.0]) + # 左腿关节目标:余弦插值 + left_phase_rad = (phase - 0.5) * 2 * np.pi + left_cos_phase = (1 - np.cos(left_phase_rad)) / 2 + left_hip_yaw = 0.05 + self.step_offset_hip * left_cos_phase + left_knee = -0.35 - self.step_offset_knee * left_cos_phase + left_ankle = 0.0 + self.step_offset_ankle * left_cos_phase + + self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = left_hip_yaw + self.joint_targets[self.joint_name_to_idx["knee_left"]] = left_knee + self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = left_ankle + + # 右腿支撑:微调整 + self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 - self.step_offset_hip * 0.1 + self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 + self.step_offset_knee * 0.05 + + # 关节目标低通滤波:平滑目标值,减少抖动 + self.joint_targets = self.filter_alpha * self.joint_targets + (1 - self.filter_alpha) * self.joint_targets_prev + self.joint_targets_prev = self.joint_targets.copy() - # 原始力矩计算逻辑(完全保留) + def _calculate_stabilizing_torques(self): + """优化力矩计算:动态增益+积分分离+速度反馈""" + self._update_walk_joint_targets() torques = np.zeros(self.num_joints, dtype=np.float64) - # 躯干姿态控制 + # 躯干姿态控制:积分分离(核心防积分饱和) root_euler = self._get_root_euler() root_vel = self.data.qvel[3:6].astype(np.float64).copy() - root_vel = np.clip(root_vel, -3.0, 3.0) + root_vel = np.clip(root_vel, -2.0, 2.0) # 更严格的速度限制 + # 积分分离:误差超过死区时不积分 roll_error = -root_euler[0] - self.integral_roll += roll_error * self.dt + if abs(roll_error) > self.integral_deadband: + self.integral_roll = 0.0 + else: + self.integral_roll += roll_error * self.dt self.integral_roll = np.clip(self.integral_roll, -self.integral_limit, self.integral_limit) - roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 5.0 * self.integral_roll + roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 3.0 * self.integral_roll pitch_error = -root_euler[1] - self.integral_pitch += pitch_error * self.dt + if abs(pitch_error) > self.integral_deadband: + self.integral_pitch = 0.0 + else: + self.integral_pitch += pitch_error * self.dt self.integral_pitch = np.clip(self.integral_pitch, -self.integral_limit, self.integral_limit) - pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 4.0 * self.integral_pitch + pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 2.0 * self.integral_pitch yaw_error = -root_euler[2] yaw_torque = self.kp_yaw * yaw_error + self.kd_yaw * (-root_vel[2]) torso_torque = np.array([roll_torque, pitch_torque, yaw_torque]) - torso_torque = np.clip(torso_torque, -30.0, 30.0) + torso_torque = np.clip(torso_torque, -25.0, 25.0) # 降低躯干力矩上限,减少抖动 - # 重心补偿 + # 重心补偿:新增速度反馈,抑制重心波动 com = self.data.subtree_com[0].astype(np.float64).copy() + self.com_vel = (com - self.prev_com) / self.dt # 计算重心速度 + self.com_vel = np.clip(self.com_vel, -0.5, 0.5) # 速度限幅 com_error = self.com_target - com - com_error = np.clip(com_error, -0.03, 0.03) - com_compensation = self.kp_com * com_error + com_error = np.clip(com_error, -0.05, 0.05) + # 比例+微分补偿,减少重心震荡 + com_compensation = self.kp_com * com_error + self.kd_com * (-self.com_vel) - # 关节控制 + # 关节控制:动态增益(支撑/摆动腿区分) self._detect_foot_contact() current_joints = self.data.qpos[7:7 + self.num_joints].astype(np.float64) current_vel = self.data.qvel[6:6 + self.num_joints].astype(np.float64) - current_vel = np.clip(current_vel, -8.0, 8.0) + current_vel = np.clip(current_vel, -5.0, 5.0) # 降低关节速度上限 - # 腰部关节控制 + # 腰部关节控制(小幅调整) waist_joints = ["abdomen_z", "abdomen_y", "abdomen_x"] for joint_name in waist_joints: idx = self.joint_name_to_idx[joint_name] joint_error = self.joint_targets[idx] - current_joints[idx] - joint_error = np.clip(joint_error, -0.3, 0.3) + joint_error = np.clip(joint_error, -0.2, 0.2) torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx] - # 腿部关节控制 + # 腿部关节控制:动态增益(核心优化) leg_joints = [ "hip_x_right", "hip_z_right", "hip_y_right", "knee_right", "ankle_y_right", "ankle_x_right", @@ -415,36 +332,34 @@ def _calculate_stabilizing_torques(self): for joint_name in leg_joints: idx = self.joint_name_to_idx[joint_name] joint_error = self.joint_targets[idx] - current_joints[idx] - joint_error = np.clip(joint_error, -0.3, 0.3) + joint_error = np.clip(joint_error, -0.2, 0.2) + # 判断是支撑腿还是摆动腿,分配不同增益 + is_support = False + if "right" in joint_name and self.foot_contact[0] == 1: + is_support = True + elif "left" in joint_name and self.foot_contact[1] == 1: + is_support = True + + # 动态分配增益 if "hip" in joint_name: - kp = self.kp_hip - kd = self.kd_hip + kp = self.kp_hip_support if is_support else self.kp_hip_swing + kd = self.kd_hip_support if is_support else self.kd_hip_swing if "y" in joint_name: - if "right" in joint_name: - joint_error += torso_torque[1] * 0.02 - else: - joint_error += torso_torque[1] * 0.02 - + joint_error += torso_torque[1] * 0.01 # 降低姿态补偿,减少后仰 elif "knee" in joint_name: - kp = self.kp_knee - kd = self.kd_knee - joint_error += com_compensation[2] * 0.05 - + kp = self.kp_knee_support if is_support else self.kp_knee_swing + kd = self.kd_knee_support if is_support else self.kd_knee_swing + joint_error += com_compensation[2] * 0.03 # 降低重心补偿,减少抖动 elif "ankle" in joint_name: - kp = self.kp_ankle - kd = self.kd_ankle + kp = self.kp_ankle_support if is_support else self.kp_ankle_swing + kd = self.kd_ankle_support if is_support else self.kd_ankle_swing if "y" in joint_name: - joint_error += torso_torque[1] * 0.01 - - if ("left" in joint_name and self.foot_contact[1] == 0) or \ - ("right" in joint_name and self.foot_contact[0] == 0): - kp *= 0.8 - kd *= 0.9 + joint_error += torso_torque[1] * 0.005 # 极小的姿态补偿,防踮脚 torques[idx] = kp * joint_error - kd * current_vel[idx] - # 手臂关节控制 + # 手臂关节控制(保留,小幅调整) arm_joints = [ "shoulder1_right", "shoulder2_right", "elbow_right", "shoulder1_left", "shoulder2_left", "elbow_left" @@ -454,87 +369,93 @@ def _calculate_stabilizing_torques(self): joint_error = self.joint_targets[idx] - current_joints[idx] torques[idx] = self.kp_arm * joint_error - self.kd_arm * current_vel[idx] - # 力矩限幅 + # 力矩限幅:根据支撑/摆动调整,支撑腿更高 torque_limits = { - "abdomen_z": 50, "abdomen_y": 50, "abdomen_x": 50, - "hip_x_right": 150, "hip_z_right": 150, "hip_y_right": 150, - "knee_right": 200, "ankle_y_right": 120, "ankle_x_right": 100, - "hip_x_left": 150, "hip_z_left": 150, "hip_y_left": 150, - "knee_left": 200, "ankle_y_left": 120, "ankle_x_left": 100, - "shoulder1_right": 20, "shoulder2_right": 20, "elbow_right": 20, - "shoulder1_left": 20, "shoulder2_left": 20, "elbow_left": 20 + "abdomen_z": 60, "abdomen_y": 60, "abdomen_x": 60, + "hip_x_right": 180 if self.foot_contact[0] == 1 else 100, + "hip_z_right": 180 if self.foot_contact[0] == 1 else 100, + "hip_y_right": 180 if self.foot_contact[0] == 1 else 100, + "knee_right": 220 if self.foot_contact[0] == 1 else 120, + "ankle_y_right": 150 if self.foot_contact[0] == 1 else 80, + "ankle_x_right": 120 if self.foot_contact[0] == 1 else 70, + "hip_x_left": 180 if self.foot_contact[1] == 1 else 100, + "hip_z_left": 180 if self.foot_contact[1] == 1 else 100, + "hip_y_left": 180 if self.foot_contact[1] == 1 else 100, + "knee_left": 220 if self.foot_contact[1] == 1 else 120, + "ankle_y_left": 150 if self.foot_contact[1] == 1 else 80, + "ankle_x_left": 120 if self.foot_contact[1] == 1 else 70, + "shoulder1_right": 25, "shoulder2_right": 25, "elbow_right": 25, + "shoulder1_left": 25, "shoulder2_left": 25, "elbow_left": 25 } for joint_name, limit in torque_limits.items(): idx = self.joint_name_to_idx[joint_name] torques[idx] = np.clip(torques[idx], -limit, limit) - # 调试输出(保留原始逻辑,新增状态/速度/转向输出) - if self.data.time % 1 < 0.1 and self.data.time > self.init_wait_time: + # 力矩输出低通滤波:减少力矩突变,提升平滑性 + torques = self.torque_filter_alpha * torques + (1 - self.torque_filter_alpha) * self.torque_prev + self.torque_prev = torques.copy() + + # 调试输出(保留) + if self.data.time % 2 < 0.1 and self.data.time > self.init_wait_time: print(f"=== 行走调试 ===") - print(f"状态: {self.state} | 步态相位: {self.gait_phase:.2f} | 速度: {self.walk_speed:.2f} | 转向: {self.turn_angle:.2f}") - print(f"右腿髋目标: {self.joint_targets[self.joint_name_to_idx['hip_y_right']]:.2f}") - print(f"左腿髋目标: {self.joint_targets[self.joint_name_to_idx['hip_y_left']]:.2f}") + print(f"步态相位: {self.gait_phase:.2f} | 重心: {com[0]:.3f}/{com[2]:.3f}m") print(f"右脚接触: {self.foot_contact[0]}, 左脚接触: {self.foot_contact[1]}") + print(f"躯干倾角: roll={root_euler[0]:.3f}, pitch={root_euler[1]:.3f}") self.prev_com = com return torques - # 原始方法:仿真循环(修改:加入键盘监听线程,保留所有原始逻辑) def simulate_stable_standing(self): - # 新增:启动键盘监听线程(控制状态/转向/速度) - keyboard_handler = KeyboardInputHandler(self) - keyboard_handler.start() - + """优化仿真循环:渐进式启动+更严格的跌倒判定""" with viewer.launch_passive(self.model, self.data) as v: - # 优化相机视角(原始逻辑保留) - v.cam.distance = 3.0 - v.cam.azimuth = 90 - v.cam.elevation = -25 - v.cam.lookat = [0, 0, 0.6] + # 优化相机视角:更清晰观察行走 + v.cam.distance = 4.0 + v.cam.azimuth = 75 # 调整视角,避免遮挡 + v.cam.elevation = -20 + v.cam.lookat = [0, 0, 0.8] - print("人形机器人稳定站立+行走仿真启动...") - print(f"初始稳定{self.init_wait_time}秒后,按W开始行走 | 支持转向/变速/启停/急停") + print("人形机器人高稳定性行走仿真启动...") + print(f"初始稳定{self.init_wait_time}秒后自动开始行走") - # 初始落地阶段(原始逻辑保留) + # 初始稳定阶段:渐进式提升控制力度,避免冲击 start_time = time.time() while time.time() - start_time < self.init_wait_time: alpha = min(1.0, (time.time() - start_time) / self.init_wait_time) torques = self._calculate_stabilizing_torques() * alpha self.data.ctrl[:] = torques mujoco.mj_step(self.model, self.data) - self.data.qvel[:] *= 0.97 + self.data.qvel[:] *= 0.98 # 稍弱的速度衰减,避免过度制动 v.sync() - time.sleep(self.dt) + time.sleep(self.dt * 0.8) # 稍快的仿真,提升流畅度 - # 主仿真循环(原始逻辑保留,状态机驱动) - print("=== 初始稳定完成,可输入控制指令 ===") + # 主仿真循环:更严格的跌倒判定 + print("=== 开始稳定行走 ===") while self.data.time < self.sim_duration: torques = self._calculate_stabilizing_torques() self.data.ctrl[:] = torques mujoco.mj_step(self.model, self.data) - # 状态监测(原始逻辑保留) + # 状态监测:增加重心速度输出 if self.data.time % 2 < 0.1: com = self.data.subtree_com[0] euler = self._get_root_euler() print( - f"时间:{self.data.time:.1f}s | 重心(x/z):{com[0]:.3f}/{com[2]:.3f}m | " + f"时间:{self.data.time:.1f}s | 重心(x/z/vel):{com[0]:.3f}/{com[2]:.3f}/{self.com_vel[0]:.3f}m/s | " f"姿态(roll/pitch):{euler[0]:.3f}/{euler[1]:.3f}rad | 脚接触:{self.foot_contact}" ) v.sync() - time.sleep(self.dt * 0.5) + time.sleep(self.dt * 0.6) - # 跌倒判定(原始逻辑保留) + # 优化跌倒判定:更严格的阈值,提前预警 com = self.data.subtree_com[0] euler = self._get_root_euler() - if com[2] < 0.4 or abs(euler[0]) > 0.6 or abs(euler[1]) > 0.6: + if com[2] < 0.6 or abs(euler[0]) > 0.4 or abs(euler[1]) > 0.4: print( - f"跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " + f"⚠️ 跌倒预警/跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | " f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad" ) - self.set_state("STAND") # 跌倒后自动恢复站立 - # break # 注释掉break,跌倒后可继续控制 + break print("仿真完成!") @@ -548,7 +469,7 @@ def simulate_stable_standing(self): raise FileNotFoundError(f"模型文件不存在:{model_file_path}") try: - stabilizer = HumanoidStabilizer(model_file_path) + stabilizer = Stabilizer(model_file_path) stabilizer.simulate_stable_standing() except Exception as e: print(f"错误:{e}") diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 919f3f96c9..97cba38106 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1178,7 +1178,7 @@ def _pid_control(self, target_speed, current_speed): return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) -class PIDLateralController: +class PIDLateralController(): """ PIDLateralController implements lateral control using a PID. """ @@ -1187,98 +1187,64 @@ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): """ Constructor method. - Args: - vehicle: Actor to apply local planner logic onto - K_P: Proportional term coefficient (default: 1.0) - K_D: Differential term coefficient (default: 0.0) - K_I: Integral term coefficient (default: 0.0) - dt: Time differential in seconds (default: 0.03) + :param vehicle: actor to apply to local planner logic onto + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + :param dt: time differential in seconds """ self._vehicle = vehicle self._k_p = K_P self._k_d = K_D self._k_i = K_I self._dt = dt - self._error_buffer = deque(maxlen=10) + self._e_buffer = deque(maxlen=10) def run_step(self, waypoint): """ - Execute one step of lateral control to steer the vehicle towards a waypoint. + Execute one step of lateral control to steer + the vehicle towards a certain waypoin. - Args: - waypoint: Target waypoint - - Returns: - float: Steering control in range [-1, 1] where: - -1 = maximum steering to left - +1 = maximum steering to right + :param waypoint: target waypoint + :return: steering control in the range [-1, 1] where: + -1 maximum steering to left + +1 maximum steering to right """ return self._pid_control(waypoint, self._vehicle.get_transform()) def _pid_control(self, waypoint, vehicle_transform): """ - Estimate steering angle based on PID equations. + Estimate the steering angle of the vehicle based on the PID equations - Args: - waypoint: Target waypoint - vehicle_transform: Current transform of the vehicle - - Returns: - float: Steering control in range [-1, 1] + :param waypoint: target waypoint + :param vehicle_transform: current transform of the vehicle + :return: steering control in the range [-1, 1] """ - # Calculate vehicle direction vector v_begin = vehicle_transform.location - yaw_rad = math.radians(vehicle_transform.rotation.yaw) - v_end = v_begin + carla.Location( - x=math.cos(yaw_rad), - y=math.sin(yaw_rad) - ) + v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), + y=math.sin(math.radians(vehicle_transform.rotation.yaw))) - # Calculate vectors v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([ - waypoint.transform.location.x - v_begin.x, - waypoint.transform.location.y - v_begin.y, - 0.0 - ]) - - # Calculate angle difference - v_norm = np.linalg.norm(v_vec) - w_norm = np.linalg.norm(w_vec) - dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm) - dot_product_clipped = np.clip(dot_product, -1.0, 1.0) - angle_diff = math.acos(dot_product_clipped) - - # Determine sign based on cross product - cross_product = np.cross(v_vec, w_vec) - if cross_product[2] < 0: - angle_diff *= -1.0 - - # Update error buffer and calculate PID terms - self._error_buffer.append(angle_diff) - - # Calculate derivative and integral terms - derivative_error = self._calculate_derivative_error() - integral_error = self._calculate_integral_error() - - # PID calculation with clamping - pid_output = ( - self._k_p * angle_diff + - self._k_d * derivative_error + - self._k_i * integral_error - ) + w_vec = np.array([waypoint.transform.location.x - + v_begin.x, waypoint.transform.location.y - + v_begin.y, 0.0]) + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / + (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) - return np.clip(pid_output, -1.0, 1.0) + _cross = np.cross(v_vec, w_vec) - def _calculate_derivative_error(self): - """Calculate derivative error term.""" - if len(self._error_buffer) >= 2: - return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - return 0.0 + if _cross[2] < 0: + _dot *= -1.0 + + self._e_buffer.append(_dot) + if len(self._e_buffer) >= 2: + _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt + _ie = sum(self._e_buffer) * self._dt + else: + _de = 0.0 + _ie = 0.0 - def _calculate_integral_error(self): - """Calculate integral error term.""" - return sum(self._error_buffer) * self._dt + return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) # ============================================================================== diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md deleted file mode 100644 index d73a1916c9..0000000000 --- a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md +++ /dev/null @@ -1 +0,0 @@ -存放标签文件 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md deleted file mode 100644 index 2f64921067..0000000000 --- a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md +++ /dev/null @@ -1 +0,0 @@ -存放训练索引文件 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md deleted file mode 100644 index 58f32ea1aa..0000000000 --- a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md +++ /dev/null @@ -1 +0,0 @@ -存放图片文件 \ No newline at end of file diff --git a/src/airsim_control/train.py b/src/airsim_control/train.py index 2cf132a69d..4b0a7322ce 100644 --- a/src/airsim_control/train.py +++ b/src/airsim_control/train.py @@ -1,12 +1,8 @@ import glob import os -import torch as th from stable_baselines3 import PPO from stable_baselines3.common.callbacks import CheckpointCallback -from stable_baselines3.common.utils import set_random_seed -from stable_baselines3.common.vec_env import DummyVecEnv, VecMonitor from custom_env import AirSimMazeEnv -from typing import Callable # === 路径配置 === MODELS_DIR = r"D:\Others\MyAirsimprojects\models" @@ -16,21 +12,6 @@ os.makedirs(LOG_DIR, exist_ok=True) -# === 优化 1: 线性学习率调度器 === -def linear_schedule(initial_value: float) -> Callable[[float], float]: - """ - 线性学习率调度函数。 - :param initial_value: 初始学习率 - :return: 当前步数的学习率 - """ - - def func(progress_remaining: float) -> float: - # progress_remaining 会从 1.0 (开始) 线性减少到 0.0 (结束) - return progress_remaining * initial_value - - return func - - def get_latest_model_path(path_dir): list_of_files = glob.glob(os.path.join(path_dir, '*.zip')) if not list_of_files: @@ -39,71 +20,41 @@ def get_latest_model_path(path_dir): def main(): - set_random_seed(42) # 固定随机种子,方便复现 - - # 实例化环境 - # 使用 DummyVecEnv 包装,虽然只有一个环境,但这是标准做法,方便未来扩展 - env = DummyVecEnv([lambda: AirSimMazeEnv()]) - env = VecMonitor(env) # 添加 Monitor 以记录详细日志 - + env = AirSimMazeEnv() latest_model_path = get_latest_model_path(MODELS_DIR) - # === 优化 2: 自定义网络架构 === - # net_arch=[256, 256]: 使用两个 256 神经元的隐藏层 (比默认的 64 强大很多) - # activation_fn: 使用 Tanh 或 ReLU - policy_kwargs = dict( - activation_fn=th.nn.Tanh, - net_arch=dict(pi=[256, 256], vf=[256, 256]) - ) - if latest_model_path: print(f"--- 发现存档: {latest_model_path},继续训练 ---") - # 加载旧模型 - # 注意:加载旧模型时,learning_rate 会被覆盖为旧模型的设置 - # 如果你想在旧模型上强制使用新参数,需要手动修改 model.learning_rate model = PPO.load(latest_model_path, env=env, tensorboard_log=LOG_DIR) - - # 强制更新学习率策略 (可选,如果你想让旧模型也享受线性衰减) - # model.lr_schedule = linear_schedule(0.0003) - reset_timesteps = False else: - print(f"--- 未发现存档,开始【从头训练】(优化版) ---") + print(f"--- 未发现存档,开始【从头训练】 ---") model = PPO( "MultiInputPolicy", env, verbose=1, tensorboard_log=LOG_DIR, - - # --- 核心超参数优化 --- - learning_rate=linear_schedule(0.0003), # 动态学习率 - batch_size=256, # 大批量,更稳 - n_steps=2048, # 每次更新前的采样步数 - gamma=0.99, # 折扣因子 (长远利益) - gae_lambda=0.95, # 优势估计 - clip_range=0.2, # PPO 修剪范围 - ent_coef=0.01, # 优化 3: 熵系数,鼓励探索,防止过早死板 - policy_kwargs=policy_kwargs, # 应用更大的网络 - device="auto" # 自动使用 GPU + learning_rate=0.0003, + batch_size=256, # 大Batch加速 + n_steps=2048, + gamma=0.99 ) reset_timesteps = True checkpoint_callback = CheckpointCallback( - save_freq=20000, # 每 2万步保存一次,不用太频繁 + save_freq=10000, save_path=MODELS_DIR, - name_prefix='drone_maze_opt' # 改个名字区分一下 + name_prefix='drone_maze' ) - print("🚀 优化版训练引擎启动...") - print("配置: Linear LR, Net=[256,256], Ent=0.01") - + print("🚀 训练引擎启动...") model.learn( - total_timesteps=1000000, # 建议跑 100万步 + total_timesteps=500000, callback=checkpoint_callback, reset_num_timesteps=reset_timesteps ) - model.save(os.path.join(MODELS_DIR, "drone_maze_final_opt")) + model.save(os.path.join(MODELS_DIR, "drone_maze_final")) print("训练结束。") diff --git a/src/carla_autonomous_driving_perception/carla_model3_spawn_with_spectator.py b/src/carla_autonomous_driving_perception/carla_model3_spawn_with_spectator.py index e635e9e3e5..6ce9ec45b5 100644 --- a/src/carla_autonomous_driving_perception/carla_model3_spawn_with_spectator.py +++ b/src/carla_autonomous_driving_perception/carla_model3_spawn_with_spectator.py @@ -77,59 +77,42 @@ def on_world_tick(snapshot): if not vehicle: raise Exception("主角车辆生成失败,请重启CARLA服务器") -# 4. 初始化摄像头通用函数(复用代码,修复shutter_speed属性问题) -def init_camera(vehicle, camera_type, transform, width=1024, height=720, fov=90): - """ - 初始化摄像头(RGB/语义分割) - :param vehicle: 挂载的车辆 - :param camera_type: 摄像头类型('rgb'/'semantic') - :param transform: 摄像头位姿 - :param width/height: 图像分辨率 - :param fov: 视场角 - :return: 摄像头actor + 数据队列 - """ - if camera_type == 'rgb': - camera_bp = bp_lib.find('sensor.camera.rgb') - elif camera_type == 'semantic': - camera_bp = bp_lib.find('sensor.camera.semantic_segmentation') - else: - raise ValueError("camera_type must be 'rgb' or 'semantic'") - - # 通用属性(所有摄像头都支持) - camera_bp.set_attribute('image_size_x', str(width)) - camera_bp.set_attribute('image_size_y', str(height)) - camera_bp.set_attribute('fov', str(fov)) - - # 仅RGB摄像头设置shutter_speed(语义分割摄像头不支持) - if camera_type == 'rgb': - camera_bp.set_attribute('shutter_speed', '100') - - camera = world.spawn_actor(camera_bp, transform, attach_to=vehicle) +# 4. 初始化RGB摄像头 +def init_rgb_camera(vehicle): + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '1024') + camera_bp.set_attribute('image_size_y', '720') + camera_bp.set_attribute('fov', '90') + camera_bp.set_attribute('shutter_speed', '100') + camera_transform = carla.Transform( + carla.Location(x=2.0, z=1.5), + carla.Rotation(pitch=-5) + ) + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) image_queue = queue.Queue() camera.listen(image_queue.put) - print(f"{camera_type.upper()}摄像头初始化完成({transform.location})") + print("RGB摄像头初始化完成") return camera, image_queue -# 4.1 初始化前视RGB摄像头(原有) -front_rgb_transform = carla.Transform( - carla.Location(x=2.0, z=1.5), - carla.Rotation(pitch=-5) -) -front_rgb_camera, front_rgb_queue = init_camera(vehicle, 'rgb', front_rgb_transform) - -# 4.2 初始化前视语义分割摄像头(修复shutter_speed问题) -front_sem_transform = carla.Transform( - carla.Location(x=2.0, z=1.5), - carla.Rotation(pitch=-5) -) -front_sem_camera, front_sem_queue = init_camera(vehicle, 'semantic', front_sem_transform) +# 5. 初始化语义分割摄像头 +def init_semantic_camera(vehicle): + sem_bp = bp_lib.find('sensor.camera.semantic_segmentation') + sem_bp.set_attribute('image_size_x', '1024') + sem_bp.set_attribute('image_size_y', '720') + sem_bp.set_attribute('fov', '90') + sem_transform = carla.Transform( + carla.Location(x=2.0, z=1.5), + carla.Rotation(pitch=-5) + ) + sem_camera = world.spawn_actor(sem_bp, sem_transform, attach_to=vehicle) + sem_queue = queue.Queue() + sem_camera.listen(sem_queue.put) + print("语义分割摄像头初始化完成") + return sem_camera, sem_queue -# 4.3 新增:初始化俯视RGB摄像头(鸟瞰视角) -top_rgb_transform = carla.Transform( - carla.Location(x=0.0, z=8.0), # 车辆正上方8米 - carla.Rotation(pitch=-90) # 垂直向下俯视 -) -top_rgb_camera, top_rgb_queue = init_camera(vehicle, 'rgb', top_rgb_transform, fov=120) # 广角120°覆盖更多区域 +# 初始化摄像头 +rgb_camera, rgb_queue = init_rgb_camera(vehicle) +sem_camera, sem_queue = init_semantic_camera(vehicle) # 6. 生成NPC车辆 npc_count = 100 @@ -251,13 +234,13 @@ def set_spectator_smooth(last_transform=None): spectator.set_transform(smooth_tf) return smooth_tf -# 9. 主循环(核心:多视角可视化 + 性能监控) +# 9. 主循环(核心:RGB与语义分割图像拼接显示 + 性能监控) print("\n程序运行中,按Ctrl+C或窗口按'q'退出...") -print(f"功能:前视+俯视双视角可视化 + {actual_npc_count}辆车辆 + {actual_walker_count}个行人 + 性能监控") +print(f"功能:RGB与语义分割图像拼接显示 + {actual_npc_count}辆车辆 + {actual_walker_count}个行人 + 平滑视角 + 性能监控") last_spectator_tf = None clock = pygame.time.Clock() -# ==================== 性能监控初始化 ==================== +# ==================== 新增:性能监控初始化 ==================== start_time = time.time() frame_counter = 0 current_fps = 0.0 @@ -271,8 +254,9 @@ def set_spectator_smooth(last_transform=None): world.tick() last_spectator_tf = set_spectator_smooth(last_spectator_tf) - # ==================== 实时FPS计算 ==================== + # ==================== 新增:实时FPS计算 ==================== frame_counter += 1 + # 每30帧更新一次FPS(避免频繁计算) if frame_counter % 30 == 0: elapsed_time = time.time() - start_time current_fps = 30.0 / elapsed_time if elapsed_time > 0 else 0.0 @@ -280,74 +264,61 @@ def set_spectator_smooth(last_transform=None): frame_counter = 0 # ================================================================= - # 同时获取三个摄像头数据(帧同步:前视RGB + 前视语义 + 俯视RGB) - if not front_rgb_queue.empty() and not front_sem_queue.empty() and not top_rgb_queue.empty(): - # 1. 处理前视RGB图像 - front_rgb_image = front_rgb_queue.get() - front_rgb_img = np.reshape(np.copy(front_rgb_image.raw_data), - (720, 1024, 4))[:, :, :3] + # 同时获取RGB和语义分割图像(确保帧同步) + if not rgb_queue.empty() and not sem_queue.empty(): + # 处理RGB图像(移除alpha通道,保留RGB) + rgb_image = rgb_queue.get() + rgb_img = np.reshape(np.copy(rgb_image.raw_data), + (rgb_image.height, rgb_image.width, 4))[:, :, :3] # 取前3通道(RGB) - # 2. 处理前视语义分割图像 - front_sem_image = front_sem_queue.get() - front_sem_data = np.reshape(np.copy(front_sem_image.raw_data), - (720, 1024, 4))[:, :, 2].astype(np.int32) - front_sem_rgb = np.zeros((720, 1024, 3), dtype=np.uint8) + # 处理语义分割图像(转换为彩色可视化) + sem_image = sem_queue.get() + sem_data = np.reshape(np.copy(sem_image.raw_data), + (sem_image.height, sem_image.width, 4))[:, :, 2].astype(np.int32) + sem_rgb = np.zeros((sem_image.height, sem_image.width, 3), dtype=np.uint8) for i in range(len(CITYSCAPES_PALETTE)): - front_sem_rgb[front_sem_data == i] = CITYSCAPES_PALETTE[i] - - # 3. 处理俯视RGB图像(调整分辨率匹配前视图) - top_rgb_image = top_rgb_queue.get() - top_rgb_img = np.reshape(np.copy(top_rgb_image.raw_data), - (720, 1024, 4))[:, :, :3] + sem_rgb[sem_data == i] = CITYSCAPES_PALETTE[i] - # 4. 多视角图像拼接: - # 上半部分:前视RGB + 前视语义分割 - # 下半部分:俯视RGB(居中显示,左右补黑边匹配宽度) - upper_part = cv2.hconcat([front_rgb_img, front_sem_rgb]) # 宽度2048,高度720 - # 补全俯视图像宽度到2048(和上半部分一致) - top_rgb_padded = np.zeros((720, 2048, 3), dtype=np.uint8) - top_rgb_padded[:, (2048-1024)//2 : (2048+1024)//2] = top_rgb_img # 居中 - # 上下拼接最终图像 - combined_img = cv2.vconcat([upper_part, top_rgb_padded]) # 宽度2048,高度1440 + # 横向拼接两张图像(宽度合并,高度不变) + combined_img = cv2.hconcat([rgb_img, sem_rgb]) - # 5. 添加视角标题 - # 前视RGB标题 - cv2.putText(combined_img, "Front View (RGB)", + # ==================== 修复:调整标题位置,为性能监控腾出空间 ==================== + cv2.putText(combined_img, + f"RGB Image | Semantic Segmentation (Vehicles:{actual_npc_count} Pedestrians:{actual_walker_count})", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2) - # 前视语义标题 - cv2.putText(combined_img, "Front View (Semantic)", - (1024 + 10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2) - # 俯视标题 - cv2.putText(combined_img, "Top View (RGB / Bird's Eye)", - (10, 720 + 30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2) + # ================================================================= - # 6. 绘制性能监控(左上角,标题下方) + # ==================== 修复:性能监控移到左上角标题下方,避免被裁剪 ==================== + # 性能信息列表 perf_info = [ f"FPS: {current_fps:.1f}", - f"RGB Queue: {front_rgb_queue.qsize()}", - f"Sem Queue: {front_sem_queue.qsize()}", + f"RGB Queue: {rgb_queue.qsize()}", + f"Sem Queue: {sem_queue.qsize()}", f"Sync Frame: {world.get_snapshot().frame}", - f"Vehicles: {actual_npc_count} | Pedestrians: {actual_walker_count}" + f"Fixed Delta: {settings.fixed_delta_seconds:.3f}s" ] + + # 绘制位置:左上角,标题下方(y从60开始) perf_x = 10 - perf_y = 60 + perf_y = 60 # 标题在30位置,这里从60开始 perf_line_height = 25 - perf_color = (0, 255, 255) # 黄色 + perf_color = (0, 255, 255) # 黄色字体 + for idx, info in enumerate(perf_info): y_pos = perf_y + idx * perf_line_height - # 半透明背景 + # 绘制半透明黑色背景(提升可读性) cv2.rectangle(combined_img, (perf_x - 5, y_pos - 15), - (perf_x + 300, y_pos + 5), + (perf_x + 220, y_pos + 5), (0, 0, 0), -1) - cv2.putText(combined_img, info, (perf_x, y_pos), + # 绘制性能文本 + cv2.putText(combined_img, info, + (perf_x, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.6, perf_color, 2) + # ================================================================= - # 7. 显示最终图像(自动调整窗口大小) - cv2.namedWindow('CARLA Multi-View (Front + Top) + Semantic Segmentation', cv2.WINDOW_NORMAL) - cv2.resizeWindow('CARLA Multi-View (Front + Top) + Semantic Segmentation', 1920, 1080) - cv2.imshow('CARLA Multi-View (Front + Top) + Semantic Segmentation', combined_img) - + # 显示拼接后的图像 + cv2.imshow('CARLA RGB + Semantic Segmentation (with Pedestrians & Performance)', combined_img) if cv2.waitKey(1) == ord('q'): break @@ -356,29 +327,25 @@ def set_spectator_smooth(last_transform=None): except KeyboardInterrupt: print("\n用户中断,清理资源...") finally: - # ==================== 清理所有摄像头资源 ==================== - # 前视RGB - front_rgb_camera.stop() - front_rgb_camera.destroy() - # 前视语义 - front_sem_camera.stop() - front_sem_camera.destroy() - # 俯视RGB - top_rgb_camera.stop() - top_rgb_camera.destroy() - # ================================================================= - # ==================== 清理行人资源 ==================== + # 停止并销毁行人控制器 for controller in walker_controllers: if controller.is_alive: controller.stop() controller.destroy() + # 销毁行人 for walker in walkers: if walker.is_alive: walker.destroy() print(f"已销毁{len(walker_controllers)}个行人控制器 + {len(walkers)}个行人") # ================================================================= + # 清理传感器 + rgb_camera.stop() + rgb_camera.destroy() + sem_camera.stop() + sem_camera.destroy() + # 恢复CARLA设置 settings.synchronous_mode = False tm.set_synchronous_mode(False) diff --git a/src/driverless_TG/prediction direction.py b/src/driverless_TG/prediction direction.py deleted file mode 100644 index 8596152b31..0000000000 --- a/src/driverless_TG/prediction direction.py +++ /dev/null @@ -1,309 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -from typing import Tuple, Dict, List -from sklearn.model_selection import train_test_split -from sklearn.ensemble import RandomForestClassifier -from sklearn.preprocessing import StandardScaler -from sklearn.metrics import ( - accuracy_score, classification_report, confusion_matrix -) -from sklearn.linear_model import LogisticRegression -import warnings - -warnings.filterwarnings("ignore") - - -# ===================== 1. 数据生成(纯numpy+内置库) ===================== -def generate_vehicle_motion_data( - sample_num: int = 1000, # 样本数量 - noise_level: float = 0.1 # 传感器噪声 -) -> Tuple[np.ndarray, np.ndarray]: - """ - 生成无人车运动数据(纯numpy实现) - 特征:速度(km/h)、加速度(km/h/s)、方向盘角度(°)、偏航角(°)、角速度(°/s) - 标签:运动方向(0=直行,1=左转,2=右转) - """ - # 初始化特征矩阵(sample_num行 × 5列)和标签数组 - features = np.zeros((sample_num, 5), dtype=np.float32) - labels = np.zeros(sample_num, dtype=np.int32) - - # 随机生成数据 - for i in range(sample_num): - # 基础特征(速度、加速度) - speed = np.random.uniform(0, 40) # 0-40km/h - acceleration = np.random.uniform(-2, 2) # -2~2 km/h/s - - # 随机选择运动方向(直行60%、左转20%、右转20%) - direction = np.random.choice([0, 1, 2], p=[0.6, 0.2, 0.2]) - - # 根据方向生成核心特征 - if direction == 0: # 直行 - steering_angle = np.random.normal(0, 1) # 方向盘角度接近0° - yaw_angle = np.random.normal(0, 0.5) # 偏航角接近0° - angular_velocity = np.random.normal(0, 0.3) # 角速度接近0°/s - elif direction == 1: # 左转 - steering_angle = np.random.uniform(10, 30) # 方向盘左偏10-30° - yaw_angle = np.random.uniform(5, 15) # 偏航角左偏5-15° - angular_velocity = np.random.uniform(1, 3) # 左转角速度1-3°/s - else: # 右转 - steering_angle = np.random.uniform(-30, -10) # 方向盘右偏10-30° - yaw_angle = np.random.uniform(-15, -5) # 偏航角右偏5-15° - angular_velocity = np.random.uniform(-3, -1) # 右转角速度1-3°/s - - # 加入传感器噪声 - steering_angle += np.random.normal(0, noise_level) - yaw_angle += np.random.normal(0, noise_level) - angular_velocity += np.random.normal(0, noise_level) - - # 赋值到特征矩阵(保留2位小数) - features[i] = [ - round(speed, 2), - round(acceleration, 2), - round(steering_angle, 2), - round(yaw_angle, 2), - round(angular_velocity, 2) - ] - labels[i] = direction - - return features, labels - - -# ===================== 2. 数据预处理 ===================== -def preprocess_data(features: np.ndarray, labels: np.ndarray) -> Tuple[ - np.ndarray, np.ndarray, np.ndarray, np.ndarray, StandardScaler]: - """ - 数据预处理:标准化 + 划分训练/测试集 - """ - # 划分训练集(80%)和测试集(20%),保证标签分布均匀 - X_train, X_test, y_train, y_test = train_test_split( - features, labels, test_size=0.2, random_state=42, stratify=labels - ) - - # 特征标准化(提升模型性能) - scaler = StandardScaler() - X_train_scaled = scaler.fit_transform(X_train) - X_test_scaled = scaler.transform(X_test) - - return X_train_scaled, X_test_scaled, y_train, y_test, scaler - - -# ===================== 3. 模型训练 ===================== -def train_direction_model( - X_train: np.ndarray, - y_train: np.ndarray, - model_type: str = "random_forest" # 可选:random_forest / logistic_regression -) -> object: - """ - 训练运动方向预测模型 - """ - if model_type == "random_forest": - # 随机森林(核心模型,鲁棒性强) - model = RandomForestClassifier( - n_estimators=100, # 决策树数量 - max_depth=8, # 防止过拟合 - random_state=42 - ) - elif model_type == "logistic_regression": - # 逻辑回归(基准模型,易解释) - model = LogisticRegression(max_iter=200, random_state=42) - else: - raise ValueError("仅支持 random_forest / logistic_regression 模型") - - # 训练模型 - model.fit(X_train, y_train) - return model - - -# ===================== 4. 模型评估(无pandas可视化) ===================== -def evaluate_model( - model: object, - X_test: np.ndarray, - y_test: np.ndarray, - feature_names: List[str] -) -> Dict[str, float]: - """ - 评估模型性能,输出指标并绘制混淆矩阵(纯matplotlib实现) - """ - # 预测测试集 - y_pred = model.predict(X_test) - - # 计算核心指标 - accuracy = accuracy_score(y_test, y_pred) - - # 输出分类报告 - print("\n===== 分类详细报告 =====") - print(classification_report( - y_test, y_pred, - target_names=["直行", "左转", "右转"] - )) - - # 输出特征重要性(仅随机森林支持) - if hasattr(model, "feature_importances_"): - print("\n===== 特征重要性 =====") - importance = model.feature_importances_ - for name, imp in zip(feature_names, importance): - print(f"{name}: {imp:.4f}") - - # 绘制混淆矩阵(纯matplotlib,无pandas依赖) - plt.rcParams["font.sans-serif"] = ["SimHei"] - plt.rcParams["axes.unicode_minus"] = False - - cm = confusion_matrix(y_test, y_pred) - fig, ax = plt.subplots(figsize=(8, 6)) - im = ax.imshow(cm, interpolation='nearest', cmap=plt.cm.Blues) - ax.figure.colorbar(im, ax=ax) - - # 设置坐标轴标签 - ax.set(xticks=np.arange(cm.shape[1]), - yticks=np.arange(cm.shape[0]), - xticklabels=["直行", "左转", "右转"], - yticklabels=["直行", "左转", "右转"], - title="混淆矩阵(运动方向预测)", - ylabel="真实标签", - xlabel="预测标签") - - # 标注数值 - thresh = cm.max() / 2. - for i in range(cm.shape[0]): - for j in range(cm.shape[1]): - ax.text(j, i, format(cm[i, j], 'd'), - ha="center", va="center", - color="white" if cm[i, j] > thresh else "black") - - fig.tight_layout() - plt.show() - - return {"accuracy": accuracy} - - -# ===================== 5. 特征关系可视化(纯matplotlib) ===================== -def visualize_feature_relationship(features: np.ndarray, labels: np.ndarray): - """ - 可视化核心特征与运动方向的关系(无pandas依赖) - """ - plt.rcParams["font.sans-serif"] = ["SimHei"] - plt.rcParams["axes.unicode_minus"] = False - - # 特征名称映射 - feature_idx = { - "speed": 0, - "steering_angle": 2, - "yaw_angle": 3, - "angular_velocity": 4 - } - - # 创建2x1子图 - fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8)) - - # 子图1:方向盘角度 vs 速度(散点图) - # 直行(绿)、左转(红)、右转(蓝) - ax1.scatter( - features[labels == 0, feature_idx["steering_angle"]], - features[labels == 0, feature_idx["speed"]], - label="直行", alpha=0.6, s=30, color="green" - ) - ax1.scatter( - features[labels == 1, feature_idx["steering_angle"]], - features[labels == 1, feature_idx["speed"]], - label="左转", alpha=0.6, s=30, color="red" - ) - ax1.scatter( - features[labels == 2, feature_idx["steering_angle"]], - features[labels == 2, feature_idx["speed"]], - label="右转", alpha=0.6, s=30, color="blue" - ) - ax1.set_xlabel("方向盘角度 (°)") - ax1.set_ylabel("速度 (km/h)") - ax1.set_title("方向盘角度 vs 速度 vs 运动方向") - ax1.legend() - ax1.grid(alpha=0.3) - - # 子图2:角速度分布(直方图) - ax2.hist( - [features[labels == 0, feature_idx["angular_velocity"]], - features[labels == 1, feature_idx["angular_velocity"]], - features[labels == 2, feature_idx["angular_velocity"]]], - bins=20, label=["直行", "左转", "右转"], - color=["green", "red", "blue"], alpha=0.6 - ) - ax2.set_xlabel("角速度 (°/s)") - ax2.set_ylabel("样本数量") - ax2.set_title("角速度分布 vs 运动方向") - ax2.legend() - ax2.grid(alpha=0.3) - - plt.tight_layout() - plt.show() - - -# ===================== 6. 单样本实时预测 ===================== -def predict_single_sample( - model: object, - scaler: object, - sample: List[float] # [speed, acceleration, steering_angle, yaw_angle, angular_velocity] -) -> Tuple[int, str]: - """ - 预测单个样本的运动方向(模拟无人车在线推理) - """ - # 特征标准化 - sample_scaled = scaler.transform([sample]) - - # 预测方向 - pred_label = model.predict(sample_scaled)[0] - - # 标签转名称 - direction_map = {0: "直行", 1: "左转", 2: "右转"} - pred_name = direction_map[pred_label] - - return pred_label, pred_name - - -# ===================== 主函数(整合全流程) ===================== -def main(): - # 1. 生成模拟数据 - print("===== 1. 生成无人车运动数据 =====") - features, labels = generate_vehicle_motion_data(sample_num=1000, noise_level=0.1) - print(f"生成样本数量:{len(features)} 条") - print("前5条特征数据:") - print(features[:5]) - print("前5条标签(0=直行,1=左转,2=右转):", labels[:5]) - - # 2. 可视化特征关系 - print("\n===== 2. 可视化特征与运动方向的关系 =====") - visualize_feature_relationship(features, labels) - - # 3. 数据预处理 - print("\n===== 3. 数据预处理 =====") - feature_names = ["speed", "acceleration", "steering_angle", "yaw_angle", "angular_velocity"] - X_train, X_test, y_train, y_test, scaler = preprocess_data(features, labels) - print(f"训练集数量:{len(X_train)} 条,测试集数量:{len(X_test)} 条") - - # 4. 训练模型 - print("\n===== 4. 训练运动方向预测模型 =====") - model = train_direction_model(X_train, y_train, model_type="random_forest") - - # 5. 评估模型 - print("\n===== 5. 模型评估 =====") - eval_metrics = evaluate_model(model, X_test, y_test, feature_names) - print(f"模型整体准确率:{eval_metrics['accuracy']:.2%}") - - # 6. 单样本实时预测 - print("\n===== 6. 单样本实时预测 =====") - # 示例1:直行样本 - sample1 = [20.0, 0.5, 0.2, 0.1, 0.05] - pred_label1, pred_name1 = predict_single_sample(model, scaler, sample1) - print(f"样本1特征:{sample1} → 预测结果:{pred_name1}(标签:{pred_label1})") - - # 示例2:左转样本 - sample2 = [15.0, 0.3, 20.0, 10.0, 2.0] - pred_label2, pred_name2 = predict_single_sample(model, scaler, sample2) - print(f"样本2特征:{sample2} → 预测结果:{pred_name2}(标签:{pred_label2})") - - # 示例3:右转样本 - sample3 = [18.0, 0.2, -18.0, -8.0, -1.5] - pred_label3, pred_name3 = predict_single_sample(model, scaler, sample3) - print(f"样本3特征:{sample3} → 预测结果:{pred_name3}(标签:{pred_label3})") - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/src/driverless_car/main_run.py b/src/driverless_car/main_run.py index de60642468..92d72c914f 100644 --- a/src/driverless_car/main_run.py +++ b/src/driverless_car/main_run.py @@ -1,185 +1,349 @@ +import tkinter as tk +from tkinter import ttk, messagebox import numpy as np +import matplotlib.pyplot as plt +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg import torch import torch.nn as nn import torch.optim as optim from torch.utils.data import Dataset, DataLoader -import matplotlib.pyplot as plt +import time import random -import os - -# -------------------------- 修复Matplotlib后端问题(核心新增代码) -------------------------- -# 方案一:强制设置Matplotlib后端,避开PyCharm的兼容问题 -plt.switch_backend('TkAgg') # 优先用TkAgg后端(需安装tkinter,大部分环境已预装) -# 如果TkAgg报错,可替换为以下后端: -# plt.switch_backend('Agg') # 无界面后端,需配合保存图片使用 -# plt.switch_backend('Qt5Agg') # 需安装PyQt5 - -# -------------------------- 1. 配置参数 -------------------------- -device = torch.device("cuda" if torch.cuda.is_available() else "cpu") -img_size = (64, 64) -batch_size = 32 -epochs = 30 -lr = 1e-3 -threshold = 0.02 - -# -------------------------- 2. 代码生成模拟无人机巡检数据 -------------------------- -def generate_normal_sample(): - """生成单张正常的无人机巡检图像(3通道,64x64)""" - img = np.ones((img_size[0], img_size[1], 3), dtype=np.float32) * 0.5 - # 绘制网格纹理 - for i in range(0, img_size[0], 4): - img[i, :, :] = 0.1 - for j in range(0, img_size[1], 4): - img[:, j, :] = 0.1 - # 添加噪声 - noise = np.random.normal(0, 0.02, img.shape).astype(np.float32) - img = np.clip(img + noise, 0, 1) - return img - -def generate_abnormal_sample(): - """生成单张异常的无人机巡检图像(3通道,64x64)""" - img = generate_normal_sample() - # 制造异常区域 - x1 = random.randint(10, 30) - y1 = random.randint(10, 30) - x2 = random.randint(30, 50) - y2 = random.randint(30, 50) - img[x1:x2, y1:y2, :] = np.random.uniform(0.7, 1.0, (x2-x1, y2-y1, 3)) - return img - -# 自定义数据集 -class DroneInspectionDataset(Dataset): - def __init__(self, is_normal, sample_num=1000): - self.sample_num = sample_num - self.is_normal = is_normal + +# 设置中文显 示 +plt.rcParams["font.family"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"] +plt.rcParams["axes.unicode_minus"] = False + + + +# 1. 数据生成与预处理(模拟无人机飞行数据) +class DroneSpeedDataset(Dataset): + """无人机速度控制数据集(模拟数据)""" + + def __init__(self, num_samples=10000, dt=0.1): + self.dt = dt # 采样时间间隔(秒) + self.data = self._generate_data(num_samples) + + def _generate_data(self, num_samples): + """生成模拟数据:输入为速度误差、误差变化率,输出为最优控制量""" + data = [] + # 模拟不同目标速度(-5~5 m/s,无人机水平速度范围) + target_speeds = np.random.uniform(-5, 5, num_samples) + # 初始速度 + current_speeds = np.random.uniform(-3, 3, num_samples) + + for v_target, v_current in zip(target_speeds, current_speeds): + # 计算误差 + error = v_target - v_current + # 误差变化率(简化:假设上一时刻误差为当前误差的0.8倍) + error_dot = (error - 0.8 * error) / self.dt + # 用PID公式生成"最优"控制量(作为标签) + # 控制量范围:0~1(归一化的电机输出) + kp, ki, kd = 0.1, 0.01, 0.05 + control = 0.5 + kp * error + kd * error_dot + control = np.clip(control, 0.0, 1.0) # 限制范围 + + # 加入噪声(模拟真实环境) + error += np.random.normal(0, 0.1) + error_dot += np.random.normal(0, 0.05) + + data.append((error, error_dot, control)) + return np.array(data, dtype=np.float32) def __len__(self): - return self.sample_num + return len(self.data) def __getitem__(self, idx): - if self.is_normal: - img = generate_normal_sample() - else: - img = generate_abnormal_sample() - # 转换为张量:(H, W, C) → (C, H, W) - img_tensor = torch.from_numpy(img).permute(2, 0, 1) - img_tensor = (img_tensor - 0.5) / 0.5 # 归一化到[-1,1] - return img_tensor - -# -------------------------- 3. 定义轻量化自编码器模型 -------------------------- -class LightweightAutoencoder(nn.Module): - def __init__(self): - super(LightweightAutoencoder, self).__init__() - # 编码器:3通道输入 - self.encoder = nn.Sequential( - nn.Conv2d(3, 16, 3, stride=2, padding=1), # 3→16,64→32 - nn.ReLU(True), - nn.Conv2d(16, 32, 3, stride=2, padding=1), # 16→32,32→16 - nn.ReLU(True), - nn.Conv2d(32, 64, 3, stride=2, padding=1), # 32→64,16→8 - nn.ReLU(True) - ) - # 解码器:3通道输出 - self.decoder = nn.Sequential( - nn.ConvTranspose2d(64, 32, 3, stride=2, padding=1, output_padding=1), # 8→16 - nn.ReLU(True), - nn.ConvTranspose2d(32, 16, 3, stride=2, padding=1, output_padding=1), # 16→32 - nn.ReLU(True), - nn.ConvTranspose2d(16, 3, 3, stride=2, padding=1, output_padding=1), # 32→64,输出3通道 - nn.Tanh() + x = self.data[idx, :2] # 输入:误差、误差变化率 + y = self.data[idx, 2:] # 输出:控制量 + return torch.tensor(x), torch.tensor(y) + + +# 2. 深度学习控制器模型 +class SpeedController(nn.Module): + """基于MLP的速度控制器""" + + def __init__(self, input_dim=2, hidden_dim=64, output_dim=1): + super(SpeedController, self).__init__() + self.network = nn.Sequential( + nn.Linear(input_dim, hidden_dim), + nn.ReLU(), + nn.Linear(hidden_dim, hidden_dim // 2), + nn.ReLU(), + nn.Linear(hidden_dim // 2, output_dim), + nn.Sigmoid() # 输出控制量(0~1) ) def forward(self, x): - # 自动处理维度:3维→4维(添加batch维度) - if len(x.shape) == 3: - x = x.unsqueeze(0) # (C,H,W) → (1,C,H,W) - encoded = self.encoder(x) - decoded = self.decoder(encoded) - return decoded - -# -------------------------- 4. 训练自编码器 -------------------------- -def train_model(): - train_dataset = DroneInspectionDataset(is_normal=True, sample_num=1000) - train_loader = DataLoader(train_dataset, batch_size=batch_size, shuffle=True) - - model = LightweightAutoencoder().to(device) + return self.network(x) + + +# 3. 模型训练函数 +def train_controller(num_epochs=50, batch_size=64): + # 生成数据集 + dataset = DroneSpeedDataset(num_samples=20000) + train_loader = DataLoader(dataset, batch_size=batch_size, shuffle=True) + + # 初始化模型、损失函数和优化器 + model = SpeedController() criterion = nn.MSELoss() - optimizer = optim.Adam(model.parameters(), lr=lr) + optimizer = optim.Adam(model.parameters(), lr=0.001) - # 训练 - model.train() - for epoch in range(epochs): + # 训练过程 + loss_history = [] + for epoch in range(num_epochs): + model.train() total_loss = 0.0 - for data in train_loader: - img = data.to(device) - output = model(img) - loss = criterion(output, img) - # 反向传播 + for x, y in train_loader: optimizer.zero_grad() + outputs = model(x) + loss = criterion(outputs, y) loss.backward() optimizer.step() - total_loss += loss.item() * data.size(0) - avg_loss = total_loss / len(train_loader.dataset) - print(f"Epoch [{epoch+1}/{epochs}], Average Loss: {avg_loss:.6f}") - print("训练完成!") - return model - -# -------------------------- 5. 异常检测测试 -------------------------- -def test_anomaly_detection(model): - model.eval() - # 生成测试样本:5正常+5异常 - test_normal = DroneInspectionDataset(is_normal=True, sample_num=5) - test_abnormal = DroneInspectionDataset(is_normal=False, sample_num=5) - # 拼接测试样本:(10,3,64,64) - test_samples = torch.stack([test_normal[i] for i in range(5)] + [test_abnormal[i] for i in range(5)]) - test_labels = [0]*5 + [1]*5 # 0=正常,1=异常 - - # 计算重构误差 - errors = [] - with torch.no_grad(): - for img in test_samples: - img = img.to(device) - output = model(img) - # 还原到[0,1]计算误差 - img_01 = (img + 1) / 2 # (3,64,64) - output_01 = (output.squeeze(0) + 1) / 2 # 去掉batch维度 - error = nn.MSELoss()(output_01, img_01).item() - errors.append(error) - - # 可视化结果(修复后) - plt.figure(figsize=(15, 8)) - for i in range(len(test_samples)): - # 处理图像用于显示:(C,H,W) → (H,W,C) - img = (test_samples[i].permute(1, 2, 0).cpu().numpy() + 1) / 2 - error = errors[i] - is_anomaly = error > threshold - # 绘制 - plt.subplot(2, 5, i+1) - plt.imshow(img) - plt.title(f"Label: {'Normal' if test_labels[i]==0 else 'Abnormal'}\nError: {error:.4f}\nDetect: {'Anomaly' if is_anomaly else 'Normal'}") - plt.axis('off') - plt.tight_layout() - - # -------------------------- 修复可视化的核心修改 -------------------------- - try: - # 尝试显示图像 - plt.show() - except Exception as e: - # 如果显示失败,保存图像到本地(方案二兜底) - print(f"显示图像失败:{e},将保存图像到本地") - # 创建保存目录(如果不存在) - if not os.path.exists("drone_anomaly_results"): - os.makedirs("drone_anomaly_results") - # 保存图像 - plt.savefig("drone_anomaly_results/anomaly_detection_result.png", dpi=300, bbox_inches='tight') - print("图像已保存到:drone_anomaly_results/anomaly_detection_result.png") - finally: - # 关闭画布,释放资源 - plt.close() - -# -------------------------- 主函数 -------------------------- + total_loss += loss.item() * x.size(0) + + avg_loss = total_loss / len(dataset) + loss_history.append(avg_loss) + if (epoch + 1) % 10 == 0: + print(f"Epoch [{epoch + 1}/{num_epochs}], 平均损失: {avg_loss:.6f}") + + # 保存模型 + torch.save(model.state_dict(), "speed_controller.pth") + return model, loss_history + + +# 4. 无人机速度控制仿真 +class DroneSimulator: + """无人机速度控制仿真器(简化物理模型)""" + + def __init__(self, dt=0.1): + self.dt = dt # 采样时间 + self.current_speed = 0.0 # 当前速度(m/s) + self.mass = 1.5 # 无人机质量(kg) + self.drag_coef = 0.2 # 阻力系数 + self.max_thrust = 5.0 # 最大推力(N) + + def update(self, control): + """根据控制量更新速度""" + # 控制量(0~1)映射到推力 + thrust = control * self.max_thrust + # 合力 = 推力 - 阻力(阻力与速度平方成正比) + drag = self.drag_coef * self.current_speed ** 2 * np.sign(self.current_speed) + force = thrust - drag + # 加速度 = 合力 / 质量 + acceleration = force / self.mass + # 更新速度(加入噪声模拟风干扰) + self.current_speed += acceleration * self.dt + np.random.normal(0, 0.05) + # 限制最大速度(-8~8 m/s) + self.current_speed = np.clip(self.current_speed, -8, 8) + return self.current_speed + + +# 5. 可视化控制界面 +class SpeedControlVisualizer: + def __init__(self, root): + self.root = root + self.root.title("无人机速度控制可视化系统") + self.root.geometry("1000x700") + + # 控制参数 + self.dt = 0.1 # 控制频率(10Hz) + self.target_speed = 2.0 # 初始目标速度(m/s) + self.running = False + self.data = { + "time": [], + "current_speed": [], + "target_speed": [], + "error": [], + "control": [] + } + self.max_points = 200 # 最大显示数据点 + + # 初始化组件 + self._init_ui() + self._init_plots() + + # 初始化模型和仿真器 + self.model = None + self.simulator = DroneSimulator(dt=self.dt) + + def _init_ui(self): + """创建UI控件""" + # 控制面板 + control_frame = ttk.LabelFrame(self.root, text="控制参数") + control_frame.pack(side=tk.TOP, fill=tk.X, padx=10, pady=5) + + # 目标速度设置 + ttk.Label(control_frame, text="目标速度 (m/s):").grid(row=0, column=0, padx=5, pady=5) + self.target_entry = ttk.Entry(control_frame, width=10) + self.target_entry.insert(0, str(self.target_speed)) + self.target_entry.grid(row=0, column=1, padx=5, pady=5) + + # 模型控制按钮 + self.train_btn = ttk.Button(control_frame, text="训练模型", command=self.train_model) + self.train_btn.grid(row=0, column=2, padx=5, pady=5) + + self.start_btn = ttk.Button(control_frame, text="开始控制", command=self.start_control, state=tk.DISABLED) + self.start_btn.grid(row=0, column=3, padx=5, pady=5) + + self.pause_btn = ttk.Button(control_frame, text="暂停", command=self.pause_control, state=tk.DISABLED) + self.pause_btn.grid(row=0, column=4, padx=5, pady=5) + + # 状态显示 + self.status_var = tk.StringVar(value="就绪 | 模型状态: 未训练") + ttk.Label(control_frame, textvariable=self.status_var).grid(row=0, column=5, padx=20, pady=5) + + def _init_plots(self): + """初始化绘图区域""" + self.fig, (self.ax1, self.ax2) = plt.subplots(2, 1, figsize=(9, 6), sharex=True) + self.canvas = FigureCanvasTkAgg(self.fig, master=self.root) + self.canvas.get_tk_widget().pack(side=tk.BOTTOM, fill=tk.BOTH, expand=True, padx=10, pady=5) + + # 速度曲线(上半部分) + self.line_current, = self.ax1.plot([], [], label="当前速度", color='blue', linewidth=2) + self.line_target, = self.ax1.plot([], [], label="目标速度", color='red', linestyle='--', linewidth=1.5) + self.ax1.set_ylabel("速度 (m/s)") + self.ax1.set_title("无人机速度跟踪曲线") + self.ax1.grid(True) + self.ax1.legend() + self.ax1.set_ylim(-6, 6) # 速度范围 + + # 误差和控制量曲线(下半部分) + self.line_error, = self.ax2.plot([], [], label="速度误差", color='green', linewidth=2) + self.ax2_twin = self.ax2.twinx() # 双Y轴 + self.line_control, = self.ax2_twin.plot([], [], label="控制量", color='orange', linewidth=2, alpha=0.7) + self.ax2.set_xlabel("时间 (秒)") + self.ax2.set_ylabel("误差 (m/s)") + self.ax2_twin.set_ylabel("控制量 (0~1)") + self.ax2.set_title("速度误差与控制量变化") + self.ax2.grid(True) + self.ax2.set_ylim(-4, 4) # 误差范围 + self.ax2_twin.set_ylim(0, 1) # 控制量范围 + # 合并图例 + lines = [self.line_error, self.line_control] + self.ax2.legend(lines, [l.get_label() for l in lines], loc='upper left') + + self.fig.tight_layout() + + def train_model(self): + """训练深度学习控制器""" + self.status_var.set("开始训练模型...") + self.root.update() + + try: + # 训练模型 + self.model, loss_history = train_controller(num_epochs=50) + self.status_var.set("模型训练完成") + self.start_btn.config(state=tk.NORMAL) + + # 显示训练损失曲线 + self._plot_loss_curve(loss_history) + except Exception as e: + messagebox.showerror("训练失败", str(e)) + self.status_var.set(f"训练失败: {str(e)}") + + def _plot_loss_curve(self, loss_history): + """显示训练损失曲线""" + # 创建新窗口显示损失曲线 + loss_window = tk.Toplevel(self.root) + loss_window.title("训练损失曲线") + loss_window.geometry("600x400") + + fig, ax = plt.subplots(figsize=(6, 4)) + ax.plot(range(1, len(loss_history) + 1), loss_history, color='purple') + ax.set_title("模型训练损失") + ax.set_xlabel("Epoch") + ax.set_ylabel("MSE损失") + ax.grid(True) + + canvas = FigureCanvasTkAgg(fig, master=loss_window) + canvas.draw() + canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True) + + def start_control(self): + """开始速度控制""" + # 更新目标速度 + try: + self.target_speed = float(self.target_entry.get()) + except ValueError: + messagebox.showerror("输入错误", "目标速度必须是数字") + return + + self.running = True + self.start_btn.config(state=tk.DISABLED) + self.pause_btn.config(state=tk.NORMAL) + self.status_var.set(f"控制中 | 目标速度: {self.target_speed} m/s") + + # 重置仿真器 + self.simulator = DroneSimulator(dt=self.dt) + # 启动控制循环 + self._control_loop() + + def pause_control(self): + """暂停控制""" + self.running = False + self.start_btn.config(state=tk.NORMAL) + self.pause_btn.config(state=tk.DISABLED) + self.status_var.set(f"已暂停 | 当前速度: {self.data['current_speed'][-1]:.2f} m/s") + + def _control_loop(self): + """控制循环(实时更新速度和控制量)""" + if not self.running: + return + + # 获取当前时间 + t = self.data["time"][-1] + self.dt if self.data["time"] else 0.0 + + # 1. 计算当前状态 + current_speed = self.simulator.current_speed + error = self.target_speed - current_speed + # 计算误差变化率 + error_dot = (error - self.data["error"][-1]) / self.dt if self.data["error"] else 0.0 + + # 2. 深度学习模型计算控制量 + model_input = torch.tensor([[error, error_dot]], dtype=torch.float32) + with torch.no_grad(): + control = self.model(model_input).item() # 输出0~1 + + # 3. 更新无人机速度 + new_speed = self.simulator.update(control) + + # 4. 保存数据 + self.data["time"].append(t) + self.data["current_speed"].append(new_speed) + self.data["target_speed"].append(self.target_speed) + self.data["error"].append(error) + self.data["control"].append(control) + + # 限制数据长度 + if len(self.data["time"]) > self.max_points: + for key in self.data: + self.data[key].pop(0) + + # 5. 更新绘图 + self._update_plots() + + # 6. 循环调用(模拟实时控制) + self.root.after(int(self.dt * 1000), self._control_loop) + + def _update_plots(self): + """更新图表""" + # 更新速度曲线 + self.line_current.set_data(self.data["time"], self.data["current_speed"]) + self.line_target.set_data(self.data["time"], self.data["target_speed"]) + + # 更新误差和控制量曲线 + self.line_error.set_data(self.data["time"], self.data["error"]) + self.line_control.set_data(self.data["time"], self.data["control"]) + + # 自动调整X轴范围 + for ax in [self.ax1, self.ax2, self.ax2_twin]: + ax.set_xlim(max(0, self.data["time"][-1] - 20), self.data["time"][-1] + 1) # 显示最近20秒数据 + + self.canvas.draw() + + if __name__ == "__main__": - # 训练模型 - model = train_model() - # 测试异常检测 - test_anomaly_detection(model) \ No newline at end of file + root = tk.Tk() + app = SpeedControlVisualizer(root) + root.mainloop() \ No newline at end of file diff --git a/src/driverless_car_yb/main.ring.py b/src/driverless_car_yb/main.ring.py deleted file mode 100644 index 24edde1166..0000000000 --- a/src/driverless_car_yb/main.ring.py +++ /dev/null @@ -1,252 +0,0 @@ -import torch -import torch.nn as nn -import numpy as np -import pandas as pd -import matplotlib.pyplot as plt -from sklearn.preprocessing import MinMaxScaler -from datetime import datetime -import time - -# -------------------------- 1. 配置参数 -------------------------- -DEVICE = torch.device("cuda" if torch.cuda.is_available() else "cpu") # 自动选择GPU/CPU -SEQ_LENGTH = 10 # 输入序列长度(用前10个时刻速度预测下1个) -PREDICT_STEP = 1 # 预测步长(预测未来1个时刻) -BATCH_SIZE = 32 -EPOCHS = 50 -LEARNING_RATE = 0.001 -SPEED_THRESHOLD_MAX = 60.0 # 最大允许速度(km/h) -SPEED_THRESHOLD_MIN = 5.0 # 最小允许速度(km/h) -ERROR_THRESHOLD = 5.0 # 预测误差阈值(超过则认为异常) - - -# -------------------------- 2. 数据生成与预处理 -------------------------- -def generate_speed_data(n_samples=1000, noise=0.5): - """生成无人车速度时序数据(含正常行驶+随机异常)""" - time_steps = np.arange(n_samples) - # 正常速度:模拟加速-匀速-减速过程 - normal_speed = np.zeros(n_samples) - normal_speed[:200] = np.linspace(0, 50, 200) # 加速 - normal_speed[200:700] = 50 + np.random.normal(0, noise, 500) # 匀速(含噪声) - normal_speed[700:] = np.linspace(50, 10, 300) # 减速 - - # 添加异常数据(超速/低速) - abnormal_indices = np.random.choice(n_samples, size=50, replace=False) # 50个异常点 - for idx in abnormal_indices: - if np.random.random() > 0.5: - normal_speed[idx] = np.random.uniform(65, 80) # 超速 - else: - normal_speed[idx] = np.random.uniform(0, 3) # 低速异常 - - # 封装为DataFrame - data = pd.DataFrame({ - "time": pd.date_range(start="2025-01-01", periods=n_samples, freq="1s"), - "speed": normal_speed - }) - return data - - -def create_sequences(data, seq_len, predict_step): - """将时序数据转换为输入序列(X)和标签(y)""" - X, y = [], [] - for i in range(len(data) - seq_len - predict_step + 1): - seq = data[i:i + seq_len] # 输入序列:前seq_len个数据 - label = data[i + seq_len:i + seq_len + predict_step] # 标签:后predict_step个数据 - X.append(seq) - y.append(label) - return np.array(X), np.array(y) - - -# 生成并预处理数据 -raw_data = generate_speed_data(n_samples=1000) -scaler = MinMaxScaler(feature_range=(0, 1)) # 归一化 -scaled_speed = scaler.fit_transform(raw_data[["speed"]].values) - -# 划分训练集(80%)和测试集(20%) -train_size = int(0.8 * len(scaled_speed)) -train_data = scaled_speed[:train_size] -test_data = scaled_speed[train_size:] - -# 创建序列数据 -X_train, y_train = create_sequences(train_data, SEQ_LENGTH, PREDICT_STEP) -X_test, y_test = create_sequences(test_data, SEQ_LENGTH, PREDICT_STEP) - -# 转换为PyTorch张量([batch_size, seq_len, input_dim]) -X_train = torch.tensor(X_train, dtype=torch.float32).to(DEVICE) -y_train = torch.tensor(y_train, dtype=torch.float32).to(DEVICE) -X_test = torch.tensor(X_test, dtype=torch.float32).to(DEVICE) -y_test = torch.tensor(y_test, dtype=torch.float32).to(DEVICE) - -# 创建DataLoader -from torch.utils.data import TensorDataset, DataLoader - -train_dataset = TensorDataset(X_train, y_train) -test_dataset = TensorDataset(X_test, y_test) -train_loader = DataLoader(train_dataset, batch_size=BATCH_SIZE, shuffle=True) -test_loader = DataLoader(test_dataset, batch_size=BATCH_SIZE, shuffle=False) - - -# -------------------------- 3. LSTM预测模型 -------------------------- -class SpeedPredictionLSTM(nn.Module): - def __init__(self, input_dim=1, hidden_dim=64, num_layers=2, output_dim=1): - super(SpeedPredictionLSTM, self).__init__() - self.hidden_dim = hidden_dim - self.num_layers = num_layers - # LSTM层 - self.lstm = nn.LSTM( - input_dim, hidden_dim, num_layers, - batch_first=True, dropout=0.2 # batch_first=True:输入格式[batch, seq, dim] - ) - # 全连接层(输出预测值) - self.fc = nn.Linear(hidden_dim, output_dim) - - def forward(self, x): - # x: [batch_size, seq_len, input_dim] - lstm_out, (hidden, cell) = self.lstm(x) # lstm_out: [batch, seq_len, hidden_dim] - # 取最后一个时间步的输出作为全连接层输入 - out = self.fc(lstm_out[:, -1, :]) # [batch_size, output_dim] - return out - - -# 初始化模型 -model = SpeedPredictionLSTM(input_dim=1, hidden_dim=64, num_layers=2, output_dim=1).to(DEVICE) -criterion = nn.MSELoss() # 均方误差损失 -optimizer = torch.optim.Adam(model.parameters(), lr=LEARNING_RATE) # Adam优化器 - - -# -------------------------- 4. 模型训练 -------------------------- -def train_model(model, train_loader, criterion, optimizer, epochs): - model.train() - train_losses = [] - for epoch in range(epochs): - epoch_loss = 0.0 - for batch_x, batch_y in train_loader: - optimizer.zero_grad() # 梯度清零 - outputs = model(batch_x) # 前向传播 - loss = criterion(outputs, batch_y.squeeze()) # 计算损失 - loss.backward() # 反向传播 - optimizer.step() # 更新参数 - epoch_loss += loss.item() * batch_x.size(0) - epoch_loss /= len(train_loader.dataset) - train_losses.append(epoch_loss) - if (epoch + 1) % 10 == 0: - print(f"Epoch [{epoch + 1}/{epochs}], Train Loss: {epoch_loss:.6f}") - return train_losses - - -# 开始训练 -print("开始训练模型...") -train_losses = train_model(model, train_loader, criterion, optimizer, EPOCHS) - -# 绘制训练损失曲线 -plt.figure(figsize=(10, 4)) -plt.plot(train_losses, label="Train Loss") -plt.xlabel("Epoch") -plt.ylabel("MSE Loss") -plt.title("Model Training Loss") -plt.legend() -plt.show() - - -# -------------------------- 5. 速度预测与异常检测 -------------------------- -def predict_and_detect(model, data, seq_len, scaler): - """预测速度并检测异常""" - model.eval() - predictions = [] - anomalies = [] # 存储异常信息:(时间索引, 实际速度, 预测速度, 异常类型) - - with torch.no_grad(): - for i in range(len(data) - seq_len): - # 构建输入序列 - seq = data[i:i + seq_len].reshape(1, seq_len, 1) # [1, seq_len, 1] - seq_tensor = torch.tensor(seq, dtype=torch.float32).to(DEVICE) - - # 预测 - pred_scaled = model(seq_tensor) - pred = scaler.inverse_transform(pred_scaled.cpu().numpy())[0][0] # 反归一化 - predictions.append(pred) - - # 获取实际速度(预测步长为1,实际速度是序列后一个时刻) - actual = scaler.inverse_transform(data[i + seq_len].reshape(1, 1))[0][0] - time_idx = i + seq_len # 实际速度对应的时间索引 - - # 计算预测误差 - error = abs(actual - pred) - - # 异常检测逻辑 - if actual > SPEED_THRESHOLD_MAX: - anomalies.append((time_idx, actual, pred, "超速")) - elif actual < SPEED_THRESHOLD_MIN: - anomalies.append((time_idx, actual, pred, "低速")) - elif error > ERROR_THRESHOLD: - anomalies.append((time_idx, actual, pred, "预测异常(误差过大)")) - - return predictions, anomalies - - -# 对测试集进行预测和异常检测 -test_predictions, test_anomalies = predict_and_detect( - model, test_data, SEQ_LENGTH, scaler -) - -# 处理测试集实际速度(对齐预测结果长度) -test_actual = scaler.inverse_transform(test_data[SEQ_LENGTH:SEQ_LENGTH + len(test_predictions)]).flatten() - - -# -------------------------- 6. 报警模块 -------------------------- -def trigger_alarm(anomaly_info): - """触发报警(控制台模拟+时间记录)""" - time_idx, actual_speed, pred_speed, anomaly_type = anomaly_info - current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S") - alarm_msg = ( - f"\n【{current_time}】⚠️ 速度异常报警!" - f"\n- 时间索引:{time_idx}" - f"\n- 实际速度:{actual_speed:.2f} km/h" - f"\n- 预测速度:{pred_speed:.2f} km/h" - f"\n- 异常类型:{anomaly_type}" - f"\n----------------------------------------" - ) - print(alarm_msg) - - # 可选:调用硬件报警(如蜂鸣器、LED灯),需根据硬件接口修改 - # import RPi.GPIO as GPIO - # GPIO.setmode(GPIO.BCM) - # GPIO.setup(18, GPIO.OUT) - # GPIO.output(18, GPIO.HIGH) # 蜂鸣器响 - # time.sleep(0.5) - # GPIO.output(18, GPIO.LOW) - -# 输出所有异常报警 -print("\n========== 异常检测结果 ==========") -if test_anomalies: - for anomaly in test_anomalies: - trigger_alarm(anomaly) -else: - print("✅ 未检测到速度异常!") - - -# -------------------------- 7. 结果可视化 -------------------------- -def plot_results(test_actual, test_predictions, test_anomalies, seq_len): - """绘制实际速度、预测速度和异常点""" - plt.figure(figsize=(15, 6)) - # 绘制实际速度和预测速度 - plt.plot(test_actual, label="实际速度", color="blue", linewidth=1.5) - plt.plot(test_predictions, label="预测速度", color="orange", linewidth=1.5, linestyle="--") - # 绘制速度阈值线 - plt.axhline(y=SPEED_THRESHOLD_MAX, color="red", linestyle=":", label=f"最大阈值 {SPEED_THRESHOLD_MAX} km/h") - plt.axhline(y=SPEED_THRESHOLD_MIN, color="green", linestyle=":", label=f"最小阈值 {SPEED_THRESHOLD_MIN} km/h") - # 标记异常点 - if test_anomalies: - anomaly_indices = [a[0] - seq_len for a in test_anomalies] # 对齐测试集索引 - anomaly_speeds = [a[1] for a in test_anomalies] - plt.scatter(anomaly_indices, anomaly_speeds, color="red", s=50, label="异常点", zorder=5) - # 图表配置 - plt.xlabel("时间步(1步=1秒)") - plt.ylabel("速度(km/h)") - plt.title("无人车速度预测与异常检测结果") - plt.legend() - plt.grid(True, alpha=0.3) - plt.show() - - -# 绘制结果 -plot_results(test_actual, test_predictions, test_anomalies, SEQ_LENGTH) \ No newline at end of file diff --git a/src/drone_perception/drone_perception_fusion.py b/src/drone_perception/drone_perception_fusion.py index b097f41d92..7629fb32b7 100644 --- a/src/drone_perception/drone_perception_fusion.py +++ b/src/drone_perception/drone_perception_fusion.py @@ -3,6 +3,7 @@ 无人机感知模块 - 多传感器融合版本 修复版本 """ + import cv2 import numpy as np from typing import Tuple, Optional, List, Dict, Any @@ -968,4 +969,4 @@ def run_improved_demo(): except Exception as e: print(f"程序运行出错: {e}") import traceback - traceback.print_exc() + traceback.print_exc() \ No newline at end of file diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py deleted file mode 100644 index 0cda60eea3..0000000000 --- a/src/drone_perception/perceptive_drone_explorer.py +++ /dev/null @@ -1,436 +0,0 @@ -""" -AirSimNH 感知驱动自主探索无人机 -核心:视觉感知 → 语义理解 → 智能决策 → 安全执行 -""" - -import airsim -import time -import numpy as np -import cv2 -import math -from collections import deque -from dataclasses import dataclass -from enum import Enum -import threading -from typing import Tuple, List, Optional - - -class FlightState(Enum): - """无人机状态枚举""" - TAKEOFF = "起飞" - HOVERING = "悬停观测" - EXPLORING = "主动探索" - AVOIDING = "避障机动" - LANDING = "降落" - EMERGENCY = "紧急状态" - - -@dataclass -class PerceptionResult: - """感知结果数据结构""" - has_obstacle: bool = False - obstacle_distance: float = 100.0 - obstacle_direction: float = 0.0 # 障碍物相对方向(弧度) - terrain_slope: float = 0.0 # 地形坡度 - open_space_score: float = 0.0 # 开阔度评分 (0-1) - recommended_height: float = -15.0 # 推荐飞行高度 - safe_directions: List[float] = None # 安全方向列表 - - def __post_init__(self): - if self.safe_directions is None: - self.safe_directions = [] - - -class PerceptiveExplorer: - """基于感知的自主探索无人机""" - - def __init__(self, drone_name=""): - print("=" * 60) - print("AirSimNH 感知驱动自主探索系统") - print("=" * 60) - - # 初始化AirSim连接 - self.client = airsim.MultirotorClient() - self.client.confirmConnection() - self.drone_name = drone_name - - # 启用API控制 - self.client.enableApiControl(True, vehicle_name=drone_name) - self.client.armDisarm(True, vehicle_name=drone_name) - - # 状态管理 - self.state = FlightState.TAKEOFF - self.state_history = deque(maxlen=20) - self.emergency_flag = False - - # 感知参数 - self.depth_threshold_near = 5.0 # 近距离警报阈值(米) - self.depth_threshold_safe = 10.0 # 安全距离阈值(米) - self.min_ground_clearance = 2.0 # 最小离地间隙(米) - self.max_pitch_angle = math.radians(15) # 最大允许俯仰角 - - # 探索参数 - self.exploration_time = 180 # 总探索时间(秒) - self.preferred_speed = 3.0 # 优选速度(m/s) - self.max_altitude = -30 # 最大海拔(米) - self.min_altitude = -8 # 最小海拔(米) - - # 记忆系统 - self.visited_positions = deque(maxlen=100) - self.obstacle_map = {} # 障碍物位置记忆 - self.traversability_map = {} # 地形可通行性记忆 - - # 性能监控 - self.perception_fps = 0 - self.decision_fps = 0 - self.start_time = time.time() - - print("✅ 系统初始化完成") - print(f" 开始时间: {time.strftime('%H:%M:%S')}") - print(f" 预计探索时长: {self.exploration_time}秒") - - def get_depth_perception(self) -> PerceptionResult: - """获取并分析深度图像,理解环境""" - result = PerceptionResult() - - try: - # 请求深度图像(使用正确的DepthPlanar类型) - responses = self.client.simGetImages([ - airsim.ImageRequest( - "0", - airsim.ImageType.DepthPlanar, - pixels_as_float=True, - compress=False - ) - ]) - - if not responses or not responses[0]: - print("⚠ 深度图像获取失败") - return result - - # 转换深度数据为numpy数组 - depth_img = responses[0] - depth_array = np.array(depth_img.image_data_float, dtype=np.float32) - depth_array = depth_array.reshape(depth_img.height, depth_img.width) - - # 分析深度图像的不同区域 - h, w = depth_array.shape - - # 1. 前方近距离区域(紧急避障) - front_near = depth_array[h // 2:, w // 3:2 * w // 3] - min_front_distance = np.min(front_near) if front_near.size > 0 else 100 - - # 2. 多方向扇形扫描 - directions = [] - scan_angles = [-45, -30, -15, 0, 15, 30, 45] # 度 - - for angle_deg in scan_angles: - angle_rad = math.radians(angle_deg) - # 计算对应图像列 - col = int(w / 2 + (w / 2) * math.tan(angle_rad) * 0.5) - col = max(0, min(w - 1, col)) - - # 分析该列的深度 - col_data = depth_array[h // 2:, col] - if col_data.size > 0: - dir_distance = np.percentile(col_data, 25) # 使用25%分位数(较保守) - directions.append((angle_rad, dir_distance)) - - if dir_distance > self.depth_threshold_safe: - result.safe_directions.append(angle_rad) - - # 3. 地形分析(通过深度梯度估计坡度) - ground_region = depth_array[3 * h // 4:, :] - if ground_region.size > 10: - row_variances = np.var(ground_region, axis=1) - result.terrain_slope = np.mean(row_variances) * 100 - - # 4. 开阔度评分(基于有效距离像素比例) - open_pixels = np.sum(depth_array[h // 2:, :] > self.depth_threshold_safe) - total_pixels = depth_array[h // 2:, :].size - result.open_space_score = open_pixels / total_pixels if total_pixels > 0 else 0 - - # 整合感知结果 - result.has_obstacle = min_front_distance < self.depth_threshold_near - result.obstacle_distance = min_front_distance - - if directions: - # 找出最近障碍物的方向 - closest_dir = min(directions, key=lambda x: x[1]) - result.obstacle_direction = closest_dir[0] - - # 根据感知动态调整推荐高度 - if result.terrain_slope > 5: - result.recommended_height = -20 # 陡峭地形飞高些 - elif result.open_space_score > 0.7: - result.recommended_height = -12 # 开阔地带可以飞低些 - else: - result.recommended_height = -15 # 默认高度 - - # 更新感知FPS - self.perception_fps = 1 / (time.time() - self.perception_start) - - except Exception as e: - print(f"❌ 深度感知异常: {e}") - - return result - - def get_visual_perception(self): - """获取视觉图像用于高级感知(可选)""" - try: - responses = self.client.simGetImages([ - airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) - ]) - - if responses and responses[0]: - # 转换为OpenCV格式 - img_data = responses[0].image_data_uint8 - img_array = np.frombuffer(img_data, dtype=np.uint8) - img = img_array.reshape(responses[0].height, responses[0].width, 3) - - # 简单颜色分析(示例:寻找绿色植被区域) - hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) - green_mask = cv2.inRange(hsv, (40, 40, 40), (80, 255, 255)) - green_ratio = np.sum(green_mask > 0) / green_mask.size - - return img, green_ratio - except: - pass - - return None, 0 - - def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: - """基于感知结果做出智能决策""" - - # 获取当前位置和状态 - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - vel = state.kinematics_estimated.linear_velocity - - # 基础决策:速度、偏航、高度 - target_vx, target_vy, target_z, target_yaw = 0.0, 0.0, perception.recommended_height, 0.0 - - # 状态机决策逻辑 - if self.state == FlightState.TAKEOFF: - target_z = -10 # 起飞到10米 - if pos.z_val < -9.5: - self.change_state(FlightState.HOVERING) - - elif self.state == FlightState.HOVERING: - # 悬停观察,缓慢旋转扫描 - target_yaw = (time.time() % 10) * 0.2 # 缓慢旋转 - - if len(perception.safe_directions) > 0: - self.change_state(FlightState.EXPLORING) - - elif self.state == FlightState.EXPLORING: - # 主动探索模式 - if perception.has_obstacle: - self.change_state(FlightState.AVOIDING) - # 紧急制动 - target_vx, target_vy = -vel.x_val, -vel.y_val - else: - # 选择最佳探索方向 - if perception.safe_directions: - # 优先选择与当前航向相差45-90度的新方向(避免来回摆动) - current_yaw = airsim.to_eularian_angles( - state.kinematics_estimated.orientation - )[2] - - # 过滤出与当前方向不同的安全方向 - diverse_dirs = [ - d for d in perception.safe_directions - if abs(d - current_yaw) > math.radians(45) - ] - - if diverse_dirs: - best_dir = diverse_dirs[0] - else: - best_dir = perception.safe_directions[0] - - # 设置前进速度 - speed_factor = min(1.0, perception.open_space_score * 1.5) - target_vx = self.preferred_speed * speed_factor * math.cos(best_dir) - target_vy = self.preferred_speed * speed_factor * math.sin(best_dir) - else: - # 没有安全方向,爬升 - target_z = pos.z_val - 5 - self.change_state(FlightState.AVOIDING) - - elif self.state == FlightState.AVOIDING: - # 避障机动 - if perception.has_obstacle: - # 根据障碍物方向决定避障策略 - if abs(perception.obstacle_direction) < math.radians(30): - # 前方障碍物:爬升 - target_z = pos.z_val - 3 - target_vx, target_vy = 0, 0 - else: - # 侧方障碍物:向反方向平移 - avoid_dir = perception.obstacle_direction + math.pi - target_vx = 1.5 * math.cos(avoid_dir) - target_vy = 1.5 * math.sin(avoid_dir) - else: - # 障碍物清除,返回探索 - self.change_state(FlightState.HOVERING) - time.sleep(1) # 避障后暂停观察 - - elif self.state == FlightState.EMERGENCY: - # 紧急状态:悬停并准备降落 - target_vx, target_vy, target_yaw = 0, 0, 0 - target_z = max(pos.z_val, -20) # 限制爬升 - - # 确保高度在安全范围内 - target_z = max(self.max_altitude, min(self.min_altitude, target_z)) - - return target_vx, target_vy, target_z, target_yaw - - def change_state(self, new_state: FlightState): - """状态转换""" - if self.state != new_state: - print(f"🔄 状态转换: {self.state.value} → {new_state.value}") - self.state = new_state - self.state_history.append((time.time(), new_state)) - - def run_perception_loop(self): - """主感知-决策-控制循环""" - print("\n" + "=" * 60) - print("启动感知-决策-控制循环") - print("=" * 60) - - # 起飞 - print("🚀 起飞中...") - self.client.takeoffAsync(vehicle_name=self.drone_name).join() - time.sleep(3) - self.change_state(FlightState.HOVERING) - - # 主循环 - loop_count = 0 - exploration_start = time.time() - - while time.time() - exploration_start < self.exploration_time and not self.emergency_flag: - loop_start = time.time() - loop_count += 1 - - # 1. 感知阶段 - self.perception_start = time.time() - perception = self.get_depth_perception() - # visual_img, green_ratio = self.get_visual_perception() # 可选 - - # 2. 决策阶段 - decision = self.make_intelligent_decision(perception) - - # 3. 控制执行阶段 - target_vx, target_vy, target_z, target_yaw = decision - - # 使用速度控制(更灵活)或位置控制(更精确) - use_velocity_control = self.state in [FlightState.EXPLORING, FlightState.AVOIDING] - - if use_velocity_control: - self.client.moveByVelocityZAsync( - target_vx, target_vy, target_z, 0.5, # 持续时间0.5秒 - vehicle_name=self.drone_name - ) - else: - self.client.moveToPositionAsync( - 0, 0, target_z, 2, # 相对当前位置移动 - vehicle_name=self.drone_name - ) - - # 记录当前位置 - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - self.visited_positions.append((pos.x_val, pos.y_val, pos.z_val)) - - # 性能监控输出 - if loop_count % 20 == 0: - elapsed = time.time() - exploration_start - print(f"\n📊 循环{loop_count} | 已运行{elapsed:.1f}s | 状态:{self.state.value}") - print(f" 感知FPS:{self.perception_fps:.1f} | 障碍:{perception.has_obstacle}") - print(f" 最近障碍:{perception.obstacle_distance:.1f}m | 开阔度:{perception.open_space_score:.2f}") - print(f" 位置:({pos.x_val:.1f}, {pos.y_val:.1f}, {-pos.z_val:.1f}m)") - print(f" 安全方向数:{len(perception.safe_directions)}") - - # 循环频率控制(10Hz) - loop_time = time.time() - loop_start - if loop_time < 0.1: - time.sleep(0.1 - loop_time) - - # 探索结束,准备降落 - print("\n" + "=" * 60) - print("探索完成,开始返航降落") - print("=" * 60) - - self.change_state(FlightState.LANDING) - self.return_to_start() - - def return_to_start(self): - """返回起始点附近并降落""" - try: - # 回到起点附近(简单实现:回到原点) - print("↩️ 返回起始区域...") - self.client.moveToPositionAsync(0, 0, -10, 5, vehicle_name=self.drone_name).join() - time.sleep(2) - - # 降落 - print("🛬 降落中...") - self.client.landAsync(vehicle_name=self.drone_name).join() - time.sleep(3) - - # 断开控制 - self.client.armDisarm(False, vehicle_name=self.drone_name) - self.client.enableApiControl(False, vehicle_name=self.drone_name) - - print("✅ 任务完成,系统安全关闭") - - except Exception as e: - print(f"❌ 降落异常: {e}") - print("⚠ 尝试紧急降落...") - try: - self.client.landAsync(vehicle_name=self.drone_name).join() - except: - pass - - def emergency_stop(self): - """紧急停止""" - print("🆘 紧急停止触发!") - self.emergency_flag = True - self.change_state(FlightState.EMERGENCY) - self.client.hoverAsync(vehicle_name=self.drone_name).join() - - -def main(): - """主程序入口""" - try: - # 创建感知探索器 - explorer = PerceptiveExplorer(drone_name="") - - # 设置键盘中断处理 - import signal - def signal_handler(sig, frame): - print("\n⚠ 用户中断,正在安全停止...") - explorer.emergency_stop() - exit(0) - - signal.signal(signal.SIGINT, signal_handler) - - # 运行主循环 - explorer.run_perception_loop() - - except Exception as e: - print(f"\n❌ 系统异常: {e}") - import traceback - traceback.print_exc() - - # 尝试安全降落 - try: - client = airsim.MultirotorClient() - client.landAsync().join() - client.armDisarm(False) - client.enableApiControl(False) - except: - pass - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/src/image_object_detection/image_detector.py b/src/image_object_detection/image_detector.py new file mode 100644 index 0000000000..03ba5c75c1 --- /dev/null +++ b/src/image_object_detection/image_detector.py @@ -0,0 +1,87 @@ +# image_detector.py +# 功能:封装 YOLOv8 模型的加载与推理逻辑,提供干净的检测接口 + +from ultralytics import YOLO +import io +import sys +import numpy as np + + +class ImageDetector: + """ + 目标检测引擎类。 + 负责加载 YOLO 模型并对输入图像帧执行推理, + 同时屏蔽模型内部的冗余打印输出(如进度条、日志等), + 使主程序输出更整洁。 + """ + + def __init__(self, model_path="yolov8n.pt", conf_threshold=0.25): + """ + 初始化检测引擎。 + + 参数: + model_path (str): YOLO 模型文件路径或名称(如 'yolov8n.pt') + conf_threshold (float): 置信度阈值,低于此值的检测结果将被过滤 + """ + self.model_path = model_path + self.conf_threshold = conf_threshold + # 加载 YOLO 模型(在初始化时完成,避免每次检测重复加载) + self.model = self._load_model() + + def _load_model(self): + """ + 私有方法:加载 YOLO 模型,并抑制其标准输出和错误输出。 + 原因:YOLO 在加载模型或首次推理时会自动打印信息(如设备、尺寸等), + 这些信息在 GUI 或自动化脚本中属于干扰。通过临时重定向 stdout/stderr 来静默加载。 + + 返回: + YOLO: 已加载的模型实例 + """ + # 保存原始的标准输出和错误流 + old_stdout = sys.stdout + old_stderr = sys.stderr + # 临时重定向到 StringIO 缓冲区(丢弃所有输出) + sys.stdout = io.StringIO() + sys.stderr = io.StringIO() + try: + # 加载模型(若为官方模型且未下载,会自动联网下载) + model = YOLO(self.model_path) + finally: + # 确保无论是否出错,都恢复原始输出流 + sys.stdout = old_stdout + sys.stderr = old_stderr + return model + + def detect(self, frame: np.ndarray): + """ + 对单帧图像执行目标检测。 + + 参数: + frame (np.ndarray): 输入图像,格式为 HWC(高度, 宽度, 通道),BGR 或 RGB 均可(YOLO 内部会处理) + + 返回: + tuple: + - annotated_frame (np.ndarray): 带有检测框、标签和置信度的可视化图像(HWC, BGR 格式) + - results (List[ultralytics.engine.results.Results]): 原始检测结果对象列表(通常长度为1) + + 注意: + - 即使没有检测到任何目标,YOLO 仍会返回一个 Results 对象; + - annotated_frame 始终是有效图像(无检测时即为原图)。 + """ + # 保存原始输出流 + old_stdout = sys.stdout + old_stderr = sys.stderr + # 临时静音 YOLO 的 verbose 输出(如 "image 1/1 ...") + sys.stdout = io.StringIO() + sys.stderr = io.StringIO() + try: + # 执行推理:传入图像、置信度阈值,并关闭详细日志(verbose=False) + results = self.model(frame, conf=self.conf_threshold, verbose=False) + finally: + # 恢复标准输出 + sys.stdout = old_stdout + sys.stderr = old_stderr + + # 使用 YOLO 内置的 plot() 方法生成带标注的图像(numpy array, HWC) + annotated_frame = results[0].plot() # results 总是包含至少一个元素 + return annotated_frame, results diff --git a/src/openpilot_model/main.py b/src/openpilot_model/main.py index ec34f0b92c..d7653f264d 100644 --- a/src/openpilot_model/main.py +++ b/src/openpilot_model/main.py @@ -1,496 +1,154 @@ #!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Lane Line Detection Program (Enhanced Version) - -核心功能: -1. 基于边缘检测+霍夫变换实现动态车道线检测,兼容固定坐标绘制降级方案; -2. 支持单视频/批量视频处理,进度条可视化处理状态; -3. 日志双端输出(终端+文件),视频编码自适应,资源自动释放; -4. 修复Matplotlib中文乱码问题,增强程序鲁棒性。 - -适用环境:Ubuntu (Python 3.10 + OpenCV + Matplotlib + tqdm) -""" - -# ===================== 模块导入 ===================== import sys import os -import time -import logging -import argparse -from pathlib import Path -from typing import List, Tuple, Optional - -import cv2 import numpy as np -import matplotlib +import cv2 import matplotlib.pyplot as plt -import matplotlib.image as mpimg -from matplotlib.font_manager import FontProperties from tqdm import tqdm +from tensorflow.keras.models import load_model + +from common.transformations.camera import transform_img, eon_intrinsics +from common.transformations.model import medmodel_intrinsics +from common.tools.lib.parser import parser + +def frames_to_tensor(frames): + if len(frames) == 0: + return np.array([]) + H = (frames.shape[1] * 2) // 3 + W = frames.shape[2] + tensor = np.zeros((frames.shape[0], 6, H//2, W//2), dtype=np.float32) + tensor[:, 0] = frames[:, 0:H:2, 0::2] + tensor[:, 1] = frames[:, 1:H:2, 0::2] + tensor[:, 2] = frames[:, 0:H:2, 1::2] + tensor[:, 3] = frames[:, 1:H:2, 1::2] + tensor[:, 4] = frames[:, H:H+H//4].reshape((-1, H//2, W//2)) + tensor[:, 5] = frames[:, H+H//4:H+H//2].reshape((-1, H//2, W//2)) + return tensor / 128.0 - 1.0 + +def preprocess_frames(imgs): + if not imgs: + return np.array([]) + processed = np.zeros((len(imgs), 384, 512), dtype=np.uint8) + for i, img in enumerate(imgs): + try: + processed[i] = transform_img(img, from_intr=eon_intrinsics, to_intr=medmodel_intrinsics, yuv=True, output_size=(512, 256)) + except: + processed[i] = np.zeros((384, 512), dtype=np.uint8) + return frames_to_tensor(processed) -# ===================== 全局配置 ===================== -# 项目根路径 -PROJECT_ROOT = Path(os.path.expanduser("~/nn")) -# 默认视频路径 -VIDEO_PATH = PROJECT_ROOT / "sample.mp4" -# 结果输出目录(自动创建) -RESULT_DIR = PROJECT_ROOT / "lane_detection_results" -RESULT_DIR.mkdir(exist_ok=True) -# 日志文件路径 -LOG_FILE = PROJECT_ROOT / "lane_detection.log" - -# 中文字体路径(微黑字体) -CHINESE_FONT_PATH = "/usr/share/fonts/truetype/wqy/wqy-microhei.ttc" - -# 视频处理参数 -DEFAULT_MAX_FRAMES = 10 # 默认最大处理帧数 -FPS = 25 # 默认帧率(自适应原视频帧率) -DEFAULT_DETECT_MODE = "dynamic" # 默认检测模式:dynamic-动态检测,fixed-固定坐标 - -# 车道线检测参数(霍夫变换+边缘检测) -CANNY_LOW_THRESH = 50 # Canny边缘检测低阈值 -CANNY_HIGH_THRESH = 150 # Canny边缘检测高阈值 -HOUGH_RHO = 1 # 霍夫变换极坐标rho步长 -HOUGH_THETA = np.pi / 180 # 霍夫变换极坐标theta步长 -HOUGH_THRESHOLD = 20 # 霍夫变换检测阈值 -HOUGH_MIN_LINE_LEN = 40 # 最小线段长度 -HOUGH_MAX_LINE_GAP = 20 # 最大线段间隙 - -# ===================== 日志初始化 ===================== -def setup_logger() -> logging.Logger: - """初始化日志处理器:同时输出到终端和文件 - - Returns: - logging.Logger: 配置完成的日志实例 - """ - logger = logging.getLogger("LaneDetection") - logger.setLevel(logging.INFO) - logger.handlers.clear() # 清除重复处理器 - - # 终端日志处理器 - stream_handler = logging.StreamHandler() - stream_formatter = logging.Formatter( - "%(asctime)s - %(levelname)s - %(message)s", - datefmt="%Y-%m-%d %H:%M:%S" - ) - stream_handler.setFormatter(stream_formatter) - - # 文件日志处理器(UTF-8编码) - file_handler = logging.FileHandler(LOG_FILE, encoding="utf-8") - file_formatter = logging.Formatter( - "%(asctime)s - %(name)s - %(levelname)s - %(message)s", - datefmt="%Y-%m-%d %H:%M:%S" - ) - file_handler.setFormatter(file_formatter) - - logger.addHandler(stream_handler) - logger.addHandler(file_handler) - return logger - -logger = setup_logger() +def read_video_with_opencv(video_path, max_frames=10): # 关键:帧数从20减到10,进一步降低压力 + cap = cv2.VideoCapture(video_path) + if not cap.isOpened(): + raise Exception(f"无法打开视频:{video_path},请安装FFmpeg(sudo apt install ffmpeg)") + imgs = [] + raw_frames = [] + for i in range(max_frames): + ret, frame = cap.read() + if not ret: + break + raw_frames.append(frame) + yuv = cv2.cvtColor(frame, cv2.COLOR_BGR2YUV_I420) + yuv_resized = cv2.resize(yuv, (512, 384), interpolation=cv2.INTER_AREA) + imgs.append(yuv_resized) + cap.release() + return imgs, raw_frames -# ===================== 环境初始化 ===================== -def init_environment() -> FontProperties: - """初始化Matplotlib环境,修复中文显示乱码 - - Returns: - FontProperties: 加载完成的中文字体实例 - - Raises: - SystemExit: 字体文件不存在或加载失败时退出程序 - """ - # 适配GUI/无GUI环境的Matplotlib后端 - if os.environ.get('DISPLAY') is None: - matplotlib.use('Agg') - logger.info("Matplotlib后端:Agg (无GUI模式)") - else: - matplotlib.use('TkAgg') - logger.info("Matplotlib后端:TkAgg (GUI模式)") +def main(): + if len(sys.argv) != 2: + print("用法: python main.py <视频文件路径>") + sys.exit(1) + video_path = sys.argv[1] + if not os.path.exists(video_path): + print(f"错误:视频文件不存在 - {video_path}") + sys.exit(1) - # 校验字体文件 - if not os.path.exists(CHINESE_FONT_PATH): - logger.error(f"中文字体文件不存在:{CHINESE_FONT_PATH}") - logger.error("安装命令:sudo apt install fonts-wqy-microhei") + model_path = "models/supercombo.h5" + if not os.path.exists(model_path): + print(f"错误:模型文件不存在 - {model_path}") sys.exit(1) - # 加载中文字体 + # 加载模型 try: - chinese_font = FontProperties(fname=CHINESE_FONT_PATH, size=12) - logger.info("中文字体加载成功") - return chinese_font + print(f"加载模型:{model_path}") + supercombo = load_model(model_path, compile=False) except Exception as e: - logger.error(f"字体加载失败:{str(e)}") + print(f"模型加载失败:{str(e)}") sys.exit(1) -# ===================== 视频读取 ===================== -def read_video( - video_path: str, - max_frames: int = DEFAULT_MAX_FRAMES -) -> Tuple[List[np.ndarray], Optional[cv2.VideoWriter], Tuple[int, int]]: - """读取视频帧并初始化视频写入器 - - Args: - video_path: 视频文件路径 - max_frames: 最大读取帧数 - - Returns: - Tuple[List[np.ndarray], Optional[cv2.VideoWriter], Tuple[int, int]]: - - 读取到的视频帧列表 - - 初始化后的视频写入器(失败则为None) - - 视频分辨率(width, height) - """ - # 校验视频文件存在性 - if not os.path.exists(video_path): - logger.error(f"视频文件不存在:{video_path}") - return [], None, (0, 0) - - # 打开视频流 - cap = cv2.VideoCapture(video_path) - if not cap.isOpened(): - logger.error(f"视频打开失败:{video_path} (请检查FFmpeg是否安装:sudo apt install ffmpeg)") - cap.release() - return [], None, (0, 0) - - # 获取视频基础信息 - width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) - height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) - fps = int(cap.get(cv2.CAP_PROP_FPS)) or FPS # 自适应原视频帧率 - logger.info(f"视频信息:{video_path} | 分辨率 {width}x{height} | 帧率 {fps}") - - # 初始化视频写入器(输出带车道线的视频) - video_name = Path(video_path).stem - result_video_path = RESULT_DIR / f"{video_name}_lane_detected.mp4" - fourcc = cv2.VideoWriter_fourcc(*'mp4v') # MP4编码格式 - writer = cv2.VideoWriter( - str(result_video_path), fourcc, fps, (width, height) - ) - - if not writer.isOpened(): - logger.error(f"视频写入器初始化失败:{result_video_path}") - cap.release() - return [], None, (width, height) - - # 读取视频帧(带进度条) - frames = [] - count = 0 - logger.info(f"开始读取视频帧(最大{max_frames}帧)...") - with tqdm(total=max_frames, desc="读取帧") as pbar: - while cap.isOpened() and count < max_frames: - ret, frame = cap.read() - if not ret: - break - frames.append(frame) - count += 1 - pbar.update(1) - - cap.release() - logger.info(f"视频帧读取完成:共读取 {len(frames)} 帧") - return frames, writer, (width, height) - -# ===================== 车道线检测 ===================== -def detect_lane_lines( - frame: np.ndarray, - mode: str = "dynamic" -) -> np.ndarray: - """检测并绘制车道线 - - Args: - frame: 单帧视频图像(BGR格式) - mode: 检测模式,可选值:dynamic/fixed - - Returns: - np.ndarray: 绘制了车道线的帧图像 - """ - frame_copy = frame.copy() - h, w = frame.shape[:2] - - # 固定坐标绘制车道线(降级方案) - if mode == "fixed": - cv2.line(frame_copy, (w//3, h), (w//3, h//2), (255, 0, 0), 5) # 左车道线(蓝) - cv2.line(frame_copy, (2*w//3, h), (2*w//3, h//2), (0, 0, 255), 5) # 右车道线(红) - cv2.line(frame_copy, (w//2, h), (w//2, h//2), (0, 255, 0), 3) # 中心线(绿) - return frame_copy - - # 动态检测车道线(Canny边缘检测 + 霍夫变换) + # 读取视频(仅10帧) try: - # 1. 灰度化 - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - # 2. 高斯模糊去噪 - blur = cv2.GaussianBlur(gray, (5, 5), 0) - # 3. 边缘检测 - edges = cv2.Canny(blur, CANNY_LOW_THRESH, CANNY_HIGH_THRESH) - # 4. 区域掩码(仅检测图像下半部分,聚焦车道区域) - mask = np.zeros_like(edges) - polygon = np.array([[ - (0, h), - (w//2, h//2), - (w, h) - ]], np.int32) - cv2.fillPoly(mask, polygon, 255) - masked_edges = cv2.bitwise_and(edges, mask) - # 5. 霍夫变换检测直线 - lines = cv2.HoughLinesP( - masked_edges, - HOUGH_RHO, - HOUGH_THETA, - HOUGH_THRESHOLD, - np.array([]), - minLineLength=HOUGH_MIN_LINE_LEN, - maxLineGap=HOUGH_MAX_LINE_GAP - ) - - # 6. 绘制检测到的车道线 - line_count = 0 - if lines is not None: - for line in lines: - x1, y1, x2, y2 = line[0] - cv2.line(frame_copy, (x1, y1), (x2, y2), (0, 255, 0), 3) # 车道线(绿) - line_count += 1 - - # 7. 标注检测到的车道线数量 - cv2.putText( - frame_copy, - f"检测到车道线:{line_count}条", - (20, 40), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (255, 255, 0), - 2 - ) - return frame_copy - + print(f"读取视频:{video_path}(仅10帧,轻量化模式)") + imgs, raw_frames = read_video_with_opencv(video_path) + if not imgs: + print("错误:未读取到帧") + sys.exit(1) except Exception as e: - logger.warning(f"动态车道线检测失败:{str(e)},自动切换为固定坐标绘制") - # 降级到固定坐标绘制 - cv2.line(frame_copy, (w//3, h), (w//3, h//2), (255, 0, 0), 5) - cv2.line(frame_copy, (2*w//3, h), (2*w//3, h//2), (0, 0, 255), 5) - return frame_copy - -# ===================== 可视化初始化 ===================== -def init_visualization( - chinese_font: FontProperties, - width: int, - height: int -) -> Tuple[plt.Figure, plt.Axes, mpimg.AxesImage]: - """初始化Matplotlib可视化窗口 - - Args: - chinese_font: 中文字体实例 - width: 视频宽度 - height: 视频高度 - - Returns: - Tuple[plt.Figure, plt.Axes, mpimg.AxesImage]: - - 可视化画布实例 - - 坐标轴实例 - - 图像显示对象 - """ - plt.ion() # 开启交互模式 - fig, ax = plt.subplots(figsize=(12, 8)) - fig.suptitle( - "车道线检测结果(叠加可视化)", - fontproperties=chinese_font, - fontweight='bold', - fontsize=16 - ) + print(f"视频读取失败:{str(e)}") + sys.exit(1) - # 标注说明文字 - ax.text( - 0.02, 0.95, - "动态检测车道线(绿色)| 固定左车道(蓝色)| 固定右车道(红色)| 按Q退出", - transform=ax.transAxes, - color='white', - bbox=dict(facecolor='black', alpha=0.8, boxstyle='round,pad=0.5'), - fontproperties=chinese_font - ) - ax.axis('off') # 隐藏坐标轴 + # 预处理帧 + print("预处理帧数据...") + frame_tensors = preprocess_frames(imgs) + if frame_tensors.size == 0: + print("错误:预处理无有效数据") + sys.exit(1) - # 初始化空图像 - dummy_frame = np.zeros((height, width, 3), dtype=np.uint8) - img_display = ax.imshow(cv2.cvtColor(dummy_frame, cv2.COLOR_BGR2RGB)) - return fig, ax, img_display + # 初始化模型状态 + state = np.zeros((1, 512)) + desire = np.zeros((1, 8)) + + # -------------------------- 轻量化可视化(仅1个窗口,只画车道线) -------------------------- + plt.ion() # 开启交互模式(简化版) + fig, ax = plt.subplots(figsize=(8, 6)) # 单个窗口,避免子图渲染压力 + ax.set_title("车道线预测(蓝=左车道,红=右车道,绿=路径)") + ax.set_ylim(0, 191) # 固定Y轴,减少重绘计算 + ax.invert_xaxis() # 匹配驾驶视角 + ax.grid(alpha=0.3) # 简单网格,不占资源 + + # 初始化三条线(提前创建,避免每次重绘新建) + lll_line, = ax.plot([], [], "b-", linewidth=3, label="左车道线") + rll_line, = ax.plot([], [], "r-", linewidth=3, label="右车道线") + path_line, = ax.plot([], [], "g-", linewidth=2, label="预测路径") + ax.legend() + # ------------------------------------------------------------------- -# ===================== 批量处理 ===================== -def batch_process( - input_dir: str, - max_frames: int = DEFAULT_MAX_FRAMES, - detect_mode: str = "dynamic" -): - """批量处理指定目录下的所有视频文件 - Args: - input_dir: 视频目录路径 - max_frames: 单视频最大处理帧数 - detect_mode: 检测模式(dynamic/fixed) - """ - if not os.path.isdir(input_dir): - logger.error(f"输入目录不存在:{input_dir}") - return - - # 筛选视频文件(支持mp4/avi/mov) - video_extensions = (".mp4", ".avi", ".mov") - video_files = [ - os.path.join(input_dir, f) - for f in os.listdir(input_dir) - if f.lower().endswith(video_extensions) - ] - - if not video_files: - logger.info(f"目录{input_dir}下未检测到视频文件(支持格式:{video_extensions})") - return - - logger.info(f"批量处理启动:共检测到 {len(video_files)} 个视频文件") - chinese_font = init_environment() - - # 逐视频处理 - for video_file in video_files: - logger.info(f"\n开始处理视频:{video_file}") + print(f"\n开始推理+可视化(共{len(frame_tensors)-1}帧,按Q键退出)...") + for i in range(len(frame_tensors) - 1): try: - # 读取视频帧 - frames, writer, (width, height) = read_video(video_file, max_frames) - if not frames: - continue - - # 初始化可视化窗口 - fig, ax, img_display = init_visualization(chinese_font, width, height) - - # 逐帧处理(带进度条) - with tqdm(total=len(frames), desc=f"处理{Path(video_file).stem}") as pbar: - for i, frame in enumerate(frames): - start_time = time.time() - # 检测并绘制车道线 - frame_with_lane = detect_lane_lines(frame, detect_mode) - # 计算单帧处理耗时 - process_time = time.time() - start_time - # 标注帧信息 - cv2.putText( - frame_with_lane, - f"帧:{i+1}/{len(frames)} | 耗时:{process_time:.3f}s", - (20, 80), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (0, 255, 255), - 2 - ) - # 更新可视化 - img_display.set_data(cv2.cvtColor(frame_with_lane, cv2.COLOR_BGR2RGB)) - fig.canvas.draw() - fig.canvas.flush_events() - # 写入视频文件 - if writer: - writer.write(frame_with_lane) - # 按Q键退出 - if cv2.waitKey(10) & 0xFF == ord('q'): - logger.info("用户按下Q键,终止处理") - break - pbar.update(1) + # 模型推理 + inputs = [np.vstack(frame_tensors[i:i+2])[None], desire, state] + outs = supercombo.predict(inputs, verbose=0) + parsed = parser(outs) + state = outs[-1] + + + lll_line.set_data(parsed["lll"][0], range(192)) # 只更新左车道线数据 + rll_line.set_data(parsed["rll"][0], range(192)) # 只更新右车道线数据 + path_line.set_data(parsed["path"][0], range(192))# 只更新路径数据 + fig.canvas.draw() # 轻量重绘(只更改造变的部分) + fig.canvas.flush_events() # 强制刷新窗口,避免卡住 + # ------------------------------------------------------------------- + + # 显示原始帧(简化版,用Matplotlib显示,避免OpenCV额外窗口) + if i < len(raw_frames): + # 新建一个小窗口显示原始帧,减少渲染压力 + cv2.imshow("原始帧", cv2.resize(raw_frames[i], (480, 270))) # 缩小尺寸 + if cv2.waitKey(100) & 0xFF == ord('q'): # 延长等待时间,给CPU喘息 + print("用户按Q键退出") + break - # 释放资源 - plt.ioff() - plt.close(fig) - if writer: - writer.release() - cv2.destroyAllWindows() - logger.info(f"视频处理完成:{video_file} → 结果保存至 {RESULT_DIR}") + print(f"✅ 帧 {i+1}/{len(frame_tensors)-1} 完成") except Exception as e: - logger.error(f"视频处理失败:{video_file} | 错误信息:{str(e)}") + print(f"⚠️ 帧 {i+1} 失败:{str(e)}") continue -# ===================== 主函数 ===================== -def main(): - """程序主入口:解析命令行参数,执行视频处理""" - # 解析命令行参数 - parser = argparse.ArgumentParser(description="车道线检测程序(增强版)") - parser.add_argument("input", type=str, help="输入路径:视频文件路径或文件夹路径(批量处理)") - parser.add_argument("--max-frames", type=int, default=DEFAULT_MAX_FRAMES, help="最大处理帧数(默认:10)") - parser.add_argument("--detect-mode", type=str, default=DEFAULT_DETECT_MODE, - choices=["dynamic", "fixed"], help="检测模式(dynamic-动态检测,fixed-固定坐标)") - parser.add_argument("--batch", action="store_true", help="批量处理模式(输入为文件夹时启用)") - args = parser.parse_args() - - # 初始化环境(加载中文字体) - chinese_font = init_environment() - - # 批量处理模式 - if args.batch or os.path.isdir(args.input): - batch_process(args.input, args.max_frames, args.detect_mode) - return - - # 单视频处理模式 - if os.path.isfile(args.input): - frames, writer, (width, height) = read_video(args.input, args.max_frames) - if not frames: - return - - # 初始化可视化窗口 - fig, ax, img_display = init_visualization(chinese_font, width, height) - - # 逐帧处理(带进度条) - with tqdm(total=len(frames), desc="处理帧") as pbar: - for i, frame in enumerate(frames): - start_time = time.time() - # 检测并绘制车道线 - frame_with_lane = detect_lane_lines(frame, args.detect_mode) - # 计算单帧处理耗时 - process_time = time.time() - start_time - # 标注帧信息 - cv2.putText( - frame_with_lane, - f"帧:{i+1}/{len(frames)} | 耗时:{process_time:.3f}s", - (20, 80), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (0, 255, 255), - 2 - ) - # 更新可视化 - img_display.set_data(cv2.cvtColor(frame_with_lane, cv2.COLOR_BGR2RGB)) - fig.canvas.draw() - fig.canvas.flush_events() - # 写入视频文件 - if writer: - writer.write(frame_with_lane) - # 按Q键退出 - if cv2.waitKey(20) & 0xFF == ord('q'): - logger.info("用户按下Q键,终止处理") - break - pbar.update(1) - - # 释放资源 - logger.info("开始释放资源...") - plt.ioff() - plt.close(fig) - if writer: - writer.release() - cv2.destroyAllWindows() - - # 输出结果信息 - result_video = RESULT_DIR / f"{Path(args.input).stem}_lane_detected.mp4" - if os.path.exists(result_video): - logger.info(f"\n处理完成!") - logger.info(f"结果视频:{result_video}") - logger.info(f"播放命令:totem {result_video}") - logger.info(f"日志文件:{LOG_FILE}") - return - # 输入路径无效 - logger.error(f"输入路径无效:{args.input}(请输入视频文件路径或文件夹路径)") + print("\n🎉 处理完成!") + plt.ioff() + plt.close() + cv2.destroyAllWindows() -# ===================== 程序入口 ===================== if __name__ == "__main__": - try: - # 校验tqdm依赖 - from tqdm import tqdm - except ImportError: - logger.error("缺少依赖库tqdm,请执行安装:pip install tqdm") - sys.exit(1) - - try: - main() - except KeyboardInterrupt: - logger.info("程序被用户中断(Ctrl+C)") - except Exception as e: - logger.error(f"程序异常终止:{str(e)}", exc_info=True) - sys.exit(1) + main()