From 24d337778577c1ad7b7bf2cce9e9cfaecb2dc5ec Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Tue, 23 Dec 2025 22:28:54 +0800 Subject: [PATCH 1/7] =?UTF-8?q?=E9=87=8D=E6=96=B0=E6=9E=84=E5=BB=BA?= =?UTF-8?q?=E5=B0=8F=E8=BD=A6=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../qwer.py | 928 +++++++++++------- 1 file changed, 552 insertions(+), 376 deletions(-) diff --git a/src/autonomous_vehicle_driving_automatic_obstacle_avoidance/qwer.py b/src/autonomous_vehicle_driving_automatic_obstacle_avoidance/qwer.py index def1597ab1..8993de5f20 100644 --- a/src/autonomous_vehicle_driving_automatic_obstacle_avoidance/qwer.py +++ b/src/autonomous_vehicle_driving_automatic_obstacle_avoidance/qwer.py @@ -1,396 +1,572 @@ -# qwer.py +""" +无人小车自主行驶与避让模拟 +基于MuJoCo和Python实现 +运行环境:PyCharm + MuJoCo +""" + import mujoco -import glfw +import mujoco.viewer import numpy as np -import csv +import glfw +import time +import math import os -from datetime import datetime -import imageio -from typing import List - -# -------------------------- 全局配置(已优化避障参数)-------------------------- -TIME_STEP = 0.01 -MAX_STEPS = 2000 -NSTEP = 5 - -# 激光雷达参数 -LIDAR_MOUNT_POS = np.array([0.5, 0, 0.2]) -LIDAR_MAX_DISTANCE = 8.0 -LIDAR_NUM_RAYS = 360 - -# 无人车参数(优化后,避免碰撞) -MAX_STEER_ANGLE = 0.6 # 增大转向角 -MAX_DRIVE_TORQUE = 15.0 # 增大驱动力矩 -SAFE_SPEED = 1.8 # 提高直行速度 -AVOID_SPEED = 1.0 # 提高避让速度 -OBSTACLE_THRESHOLD = 4.0 # 提前触发避让 -AVOID_STEER_ANGLE = 0.5 # 增大避让转向角 -TORQUE_GAIN = 8.0 # 提高加速响应 - -# 执行器索引 -DRIVE_ACTUATOR_IDS = [0, 1] -STEER_ACTUATOR_IDS = [2, 3] - -# -------------------------- MuJoCo XML(精细化车辆和障碍物模型)-------------------------- -SCENE_XML = """ - - - - - - - -""" -# -------------------------- 激光雷达(已匹配mj_ray参数)-------------------------- -class LidarSensor: - def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): - self.model = model - self.data = data - self.angles = np.linspace(0, 2 * np.pi, LIDAR_NUM_RAYS, endpoint=False) - self.geomgroup = np.zeros(6, dtype=np.uint8) - self.geomgroup[0] = 1 # 只检测第0组的geom - - def rotate_vector(self, vec: np.ndarray, quat: np.ndarray) -> np.ndarray: - qx, qy, qz, qw = quat - rot_mat = np.array([ - [1 - 2 * qy ** 2 - 2 * qz ** 2, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw], - [2 * qx * qy + 2 * qz * qw, 1 - 2 * qx ** 2 - 2 * qz ** 2, 2 * qy * qz - 2 * qx * qw], - [2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx ** 2 - 2 * qy ** 2] - ]) - return np.dot(rot_mat, vec) - - def get_distance_data(self) -> List[float]: - distances = [LIDAR_MAX_DISTANCE] * LIDAR_NUM_RAYS - vehicle_pos = self.data.qpos[:3] - vehicle_quat = self.data.qpos[3:7] - - for i, angle in enumerate(self.angles): - local_dir = np.array([np.cos(angle), np.sin(angle), 0.0], dtype=np.float64) - local_dir = local_dir / np.linalg.norm(local_dir) - world_dir = self.rotate_vector(local_dir, vehicle_quat) - world_dir = world_dir / np.linalg.norm(world_dir) - - ray_start = vehicle_pos + self.rotate_vector(LIDAR_MOUNT_POS, vehicle_quat) - ray_start = ray_start.astype(np.float64) - - # 匹配mj_ray参数格式 - pnt = ray_start.reshape(3, 1) - vec = world_dir.reshape(3, 1) - geomid = np.zeros(1, dtype=np.int32) - - detected_dist = mujoco.mj_ray( - self.model, self.data, - pnt=pnt, - vec=vec, - geomgroup=self.geomgroup, - flg_static=1, - bodyexclude=0, - geomid=geomid - ) - - if geomid[0] != -1 and detected_dist <= LIDAR_MAX_DISTANCE: - distances[i] = detected_dist - - return distances - - -# -------------------------- 可视化类 -------------------------- -class Visualizer: - def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): - self.model = model - self.data = data - if not glfw.init(): - raise RuntimeError("Failed to initialize GLFW") - self.window = glfw.create_window(1280, 720, "无人车仿真", None, None) - if not self.window: - glfw.terminate() - raise RuntimeError("Failed to create GLFW window") - glfw.make_context_current(self.window) - glfw.swap_interval(1) - - self.cam = mujoco.MjvCamera() - self.opt = mujoco.MjvOption() - mujoco.mjv_defaultCamera(self.cam) - mujoco.mjv_defaultOption(self.opt) - self.scene = mujoco.MjvScene(self.model, maxgeom=10000) - self.context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150) - - self.cam.distance = 10.0 - self.cam.azimuth = -45.0 - self.cam.elevation = -30.0 - self.cam.lookat = [5.0, 0.0, 0.5] - - def render(self): - if glfw.window_should_close(self.window): - return False - mujoco.mjv_updateScene(self.model, self.data, self.opt, None, self.cam, mujoco.mjtCatBit.mjCAT_ALL, self.scene) - viewport = mujoco.MjrRect(0, 0, 1280, 720) - mujoco.mjr_render(viewport, self.scene, self.context) - glfw.swap_buffers(self.window) - glfw.poll_events() - return True - - def close(self): - glfw.destroy_window(self.window) - glfw.terminate() - - -# -------------------------- 日志类 -------------------------- -class DataLogger: - def __init__(self): - self.log_dir = "logs" - os.makedirs(self.log_dir, exist_ok=True) - self.log_path = os.path.join(self.log_dir, f"sim_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv") - self.fields = ['step', 'time', 'x', 'y', 'speed', 'steer', 'torque', 'collision'] - with open(self.log_path, 'w', newline='') as f: - csv.writer(f).writerow(self.fields) - - def log(self, step, time, x, y, speed, steer, torque, collision): - with open(self.log_path, 'a', newline='') as f: - csv.writer(f).writerow( - [step, round(time, 3), round(x, 3), round(y, 3), round(speed, 3), round(steer, 3), round(torque, 3), - 1 if collision else 0]) - - -# -------------------------- 决策算法类 -------------------------- -class RulePolicy: - def __init__(self): - self.front_rays = slice(350, 360) - self.front_ext = slice(0, 10) - self.left_rays = slice(270, 350) - self.right_rays = slice(10, 90) - - def get_speed(self, vel): - return np.linalg.norm(vel[:2]) - - def decide(self, lidar_data, vel): - lidar = np.array(lidar_data) - current_speed = self.get_speed(vel) - - front_dist = np.min(np.concatenate([lidar[self.front_rays], lidar[self.front_ext]])) - left_dist = np.min(lidar[self.left_rays]) - right_dist = np.min(lidar[self.right_rays]) - - if front_dist >= OBSTACLE_THRESHOLD: - steer = 0.0 - target_speed = SAFE_SPEED +class AutonomousCar: + def __init__(self, model_path=None): + """初始化无人小车模拟器""" + # 如果没有提供模型文件,使用内置的XML模型 + if model_path is None: + self.xml = """ + + + """ + + # 保存XML到临时文件 + self.temp_xml_path = "temp_car_model.xml" + with open(self.temp_xml_path, 'w') as f: + f.write(self.xml) + + try: + self.model = mujoco.MjModel.from_xml_path(self.temp_xml_path) + except Exception as e: + print(f"XML解析错误: {e}") + # 尝试简化版本 + self.create_simple_model() else: - steer = AVOID_STEER_ANGLE if left_dist > right_dist else -AVOID_STEER_ANGLE - target_speed = AVOID_SPEED + self.model = mujoco.MjModel.from_xml_path(model_path) - torque = TORQUE_GAIN * (target_speed - current_speed) - torque = np.clip(torque, -MAX_DRIVE_TORQUE, MAX_DRIVE_TORQUE) - return steer, torque + self.data = mujoco.MjData(self.model) + # 控制参数 + self.target_speed = 6.0 # 目标速度 + self.max_steering_angle = 0.5 # 最大转向角度(弧度) + self.avoidance_distance = 2.5 # 避障检测距离 + self.avoidance_strength = 2.5 # 避障强度 + + # 状态变量 + self.current_speed = 0.0 + self.steering_angle = 0.0 + self.obstacle_detected = False + self.simulation_time = 0.0 + self.target_reached = False + self.path_history = [] # 路径历史 + + # PID控制器参数 + self.speed_Kp = 4.0 + self.speed_Ki = 0.1 + self.speed_Kd = 0.3 + self.speed_integral = 0.0 + self.speed_prev_error = 0.0 + + self.steering_Kp = 6.0 + self.steering_Ki = 0.05 + self.steering_Kd = 0.2 + self.steering_integral = 0.0 + self.steering_prev_error = 0.0 + + def create_simple_model(self): + """创建简化模型(如果完整模型有问题)""" + print("使用简化模型...") + simple_xml = """ + + + """ + + with open(self.temp_xml_path, 'w') as f: + f.write(simple_xml) + + self.model = mujoco.MjModel.from_xml_path(self.temp_xml_path) + + def __del__(self): + """清理临时文件""" + if hasattr(self, 'temp_xml_path') and os.path.exists(self.temp_xml_path): + try: + os.remove(self.temp_xml_path) + except: + pass + + def get_sensor_readings(self): + """获取传感器读数""" + readings = { + 'front_distance': 10.0, + 'left_distance': 10.0, + 'right_distance': 10.0, + 'front_obstacle': False, + 'left_obstacle': False, + 'right_obstacle': False + } -# -------------------------- 仿真环境类(已整合自动录制功能)-------------------------- -class VehicleEnv: - def __init__(self): - self.model = mujoco.MjModel.from_xml_string(SCENE_XML) - self.data = mujoco.MjData(self.model) - self.lidar = LidarSensor(self.model, self.data) - self.vis = Visualizer(self.model, self.data) - self.logger = DataLogger() - self.policy = RulePolicy() + # 获取小车位置和方向 + car_pos = self.data.body('car').xpos + car_orientation = self.data.body('car').xmat.reshape(3, 3) + car_forward = car_orientation @ np.array([0, 1, 0]) # 小车前进方向 + car_left = car_orientation @ np.array([1, 0, 0]) # 小车左侧方向 + + # 检查所有障碍物 + obstacle_positions = [ + np.array([3, 2, 0.5]), # obstacle1 + np.array([5, -1.5, 0.5]), # obstacle2 + np.array([2, -2, 0.5]), # obstacle3 + np.array([6, 2, 0.5]) # obstacle4 + ] + + obstacle_sizes = [ + 1.2, # obstacle1 半径+高度 + 1.4, # obstacle2 尺寸 + 1.0, # obstacle3 直径 + 1.3 # obstacle4 半径+高度 + ] + + for i, obs_pos in enumerate(obstacle_positions): + # 计算障碍物到小车的向量 + obs_vector = obs_pos - car_pos + distance = np.linalg.norm(obs_vector[:2]) # 只考虑平面距离 + + if distance < self.avoidance_distance + obstacle_sizes[i]: + # 计算障碍物相对于小车的方向 + obs_direction = obs_vector[:2] / distance if distance > 0 else np.array([0, 0]) + + # 计算与前进方向的夹角 + forward_2d = car_forward[:2] + angle = math.atan2( + obs_direction[1] * forward_2d[0] - obs_direction[0] * forward_2d[1], + obs_direction[0] * forward_2d[0] + obs_direction[1] * forward_2d[1] + ) + + angle_deg = math.degrees(angle) + + # 更新传感器读数 + if -45 < angle_deg < 45: # 前方 + readings['front_distance'] = min(readings['front_distance'], distance) + if distance < 2.0: + readings['front_obstacle'] = True + + elif 45 <= angle_deg < 135: # 左侧 + readings['left_distance'] = min(readings['left_distance'], distance) + if distance < 1.5: + readings['left_obstacle'] = True + + elif -135 < angle_deg <= -45: # 右侧 + readings['right_distance'] = min(readings['right_distance'], distance) + if distance < 1.5: + readings['right_obstacle'] = True + + return readings + + def autonomous_driving(self, dt): + """自主驾驶算法""" + if dt <= 0: + dt = 0.01 + + # 获取传感器数据 + sensor_data = self.get_sensor_readings() + + # 获取目标位置 + target_pos = np.array([8, 0, 0.5]) + car_pos = self.data.body('car').xpos + + # 计算到目标的距离和方向 + target_vector = target_pos - car_pos + target_distance = np.linalg.norm(target_vector[:2]) + + # 检查是否到达目标 + if target_distance < 0.5: + self.target_reached = True + return np.zeros(self.model.nu) + + # 计算目标方向(归一化) + if target_distance > 0: + target_direction = target_vector[:2] / target_distance + else: + target_direction = np.array([0, 1]) + + # 获取小车当前方向 + car_orientation = self.data.body('car').xmat.reshape(3, 3) + car_direction = car_orientation @ np.array([0, 1, 0]) # 前进方向 + car_direction_2d = car_direction[:2] + + if np.linalg.norm(car_direction_2d) > 0: + car_direction_2d = car_direction_2d / np.linalg.norm(car_direction_2d) + + # 计算转向误差 + steering_error = math.atan2( + target_direction[1] * car_direction_2d[0] - target_direction[0] * car_direction_2d[1], + target_direction[0] * car_direction_2d[0] + target_direction[1] * car_direction_2d[1] + ) + + # 避障逻辑 + avoidance_steering = 0.0 + self.obstacle_detected = False + + if sensor_data['front_obstacle']: + self.obstacle_detected = True + # 前方有障碍物,根据两侧距离决定转向 + if sensor_data['left_distance'] > sensor_data['right_distance']: + avoidance_steering = 0.5 # 向左转 + else: + avoidance_steering = -0.5 # 向右转 - # 初始化帧缓存用于GIF录制 - self.frames = [] - self.step_count = 0 + elif sensor_data['left_obstacle']: + avoidance_steering = -0.3 # 向右轻微转向 - def reset(self): - mujoco.mj_resetData(self.model, self.data) - self.step_count = 0 - return self.get_state() - - def get_state(self): - return { - 'pos': self.data.qpos[:3], - 'vel': self.data.qvel[:3], - 'lidar': self.lidar.get_distance_data(), - 'collision': self.data.ncon > 0 - } + elif sensor_data['right_obstacle']: + avoidance_steering = 0.3 # 向左轻微转向 - def step(self, steer, torque): - for idx in STEER_ACTUATOR_IDS: - self.data.ctrl[idx] = np.clip(steer, -MAX_STEER_ANGLE, MAX_STEER_ANGLE) - for idx in DRIVE_ACTUATOR_IDS: - self.data.ctrl[idx] = torque - - mujoco.mj_step(self.model, self.data, nstep=NSTEP) - self.step_count += 1 - return self.get_state() - - def run(self): - state = self.reset() - print("仿真启动(按ESC关闭),正在自动录制GIF...") - print("=" * 50) - - # 主循环 - 持续运行直到用户按下ESC键 - while True: - # 1. 决策:根据激光雷达和速度生成控制指令 - steer, torque = self.policy.decide(state['lidar'], state['vel']) - # 2. 执行动作:更新车辆状态 - state = self.step(steer, torque) - # 3. 记录日志 - self.logger.log(self.step_count, self.step_count * TIME_STEP, state['pos'][0], state['pos'][1], - self.policy.get_speed(state['vel']), steer, torque, state['collision']) - # 4. 渲染画面 - if not self.vis.render(): - print("\n用户按下ESC键,仿真结束") - break - - # -------------------------- 录制当前帧 -------------------------- - try: - width, height = glfw.get_framebuffer_size(self.vis.window) - if width > 0 and height > 0: - buffer = np.zeros((height, width, 3), dtype=np.uint8) - # 创建正确的 MjrRect 对象 - viewport = mujoco.MjrRect(0, 0, width, height) - mujoco.mjr_readPixels(buffer, None, viewport, self.vis.context) - buffer = np.flipud(buffer) - self.frames.append(buffer) - except Exception as e: - print(f"录制帧时出错: {e}") - # ------------------------------------------------------------------- + # 合并转向控制 + total_steering = steering_error + avoidance_steering - # 打印进度(每100步) - if self.step_count % 100 == 0: - print( - f"Step: {self.step_count:4d} | 速度: {self.policy.get_speed(state['vel']):.2f}m/s | 碰撞: {state['collision']}") + # 限制转向角度 + total_steering = np.clip(total_steering, -self.max_steering_angle, self.max_steering_angle) - # -------------------------- 结束录制并保存 -------------------------- + # 速度控制:根据障碍物距离调整速度 + min_distance = min(sensor_data['front_distance'], + sensor_data['left_distance'], + sensor_data['right_distance']) + + if min_distance < 1.0: + speed_multiplier = 0.3 + elif min_distance < 2.0: + speed_multiplier = 0.6 + else: + speed_multiplier = 1.0 + + target_speed_adjusted = self.target_speed * speed_multiplier + + # 简单的速度控制 + if self.current_speed < target_speed_adjusted: + self.current_speed += 0.5 * dt + elif self.current_speed > target_speed_adjusted: + self.current_speed -= 0.5 * dt + + self.current_speed = np.clip(self.current_speed, 0, self.target_speed) + + # 记录路径 + self.path_history.append(car_pos.copy()) + if len(self.path_history) > 1000: + self.path_history.pop(0) + + # 生成控制信号 + control = np.zeros(self.model.nu) + + if hasattr(self.model, 'nu') and self.model.nu >= 6: + # 完整模型的控制 + control[0] = self.current_speed # 前左驱动 + control[1] = self.current_speed # 前右驱动 + control[2] = self.current_speed # 后左驱动 + control[3] = self.current_speed # 后右驱动 + control[4] = total_steering # 前左转向 + control[5] = total_steering # 前右转向 + else: + # 简化模型的控制 + control[0] = self.current_speed + if len(control) > 1: + control[1] = total_steering + + return control + + def print_status(self): + """打印状态信息""" + car_pos = self.data.body('car').xpos + target_pos = np.array([8, 0, 0.5]) + distance = np.linalg.norm(target_pos[:2] - car_pos[:2]) + + print(f"\r时间: {self.simulation_time:.1f}s | " + f"位置: ({car_pos[0]:.1f}, {car_pos[1]:.1f}) | " + f"速度: {self.current_speed:.1f}m/s | " + f"转向: {math.degrees(self.steering_angle):.0f}° | " + f"距目标: {distance:.1f}m | " + f"状态: {'避障' if self.obstacle_detected else '导航'} {'到达!' if self.target_reached else ''}", + end="") + + def run_simulation(self): + """运行模拟主循环""" + print("无人小车模拟系统启动中...") + print("=" * 80) + print("控制说明:") + print(" - 按ESC键退出模拟") + print(" - 模拟会自动运行直到按下ESC或到达目标") + print(" - 绿色球体是目标点") + print(" - 红色物体是障碍物") + print(" - 小车会自动导航并避开障碍物") + print("=" * 80) + + # 设置模拟选项 + self.model.opt.gravity[2] = -9.81 + + # 重置模拟 + mujoco.mj_resetData(self.model, self.data) + + # 启动查看器 try: - if self.frames: - # 使用imageio v3 API保存GIF,调整duration参数延长播放时间 - gif_path = 'simulation.gif' - imageio.mimsave(gif_path, self.frames, duration=0.2, loop=0) - print(f"GIF动图已保存至:{gif_path}(项目根目录)") - else: - print("没有帧数据可保存为GIF") + viewer = mujoco.viewer.launch_passive(self.model, self.data) except Exception as e: - print(f"GIF保存失败: {e}") + print(f"查看器启动失败: {e}") + print("将以无界面模式运行模拟...") + viewer = None + + last_time = time.time() + frame_count = 0 + start_time = time.time() + + try: + while True: + if viewer is not None and not viewer.is_running(): + break + + # 计算时间步长 + current_time = time.time() + dt = current_time - last_time + last_time = current_time + self.simulation_time += dt + + # 限制时间步长 + if dt > 0.1: + dt = 0.01 + + # 应用自主驾驶控制 + control = self.autonomous_driving(dt) + self.data.ctrl[:] = control + + # 执行物理模拟 + mujoco.mj_step(self.model, self.data) + + # 更新查看器 + if viewer is not None: + viewer.sync() + + # 更新状态显示 + frame_count += 1 + if frame_count % 10 == 0: + self.print_status() + + # 检查是否到达目标 + if self.target_reached: + print(f"\n\n{'=' * 80}") + print("成功到达目标点!") + print(f"总时间: {self.simulation_time:.1f}秒") + print(f"平均速度: {np.mean([v for v in self.path_history if len(v) > 0]):.1f}m/s") + print(f"{'=' * 80}") + time.sleep(2) + break + + # 检查ESC键 - 修复后的代码 + if viewer is not None: + try: + # MuJoCo新版本使用不同的API来访问窗口句柄 + if hasattr(viewer, 'context'): + # 检查查看器是否仍在运行 + if not viewer.is_running(): + break + else: + # 检查查看器是否仍在运行 + if not viewer.is_running(): + break + except: + # 如果任何API检查失败,检查查看器是否仍在运行 + if viewer is not None and not viewer.is_running(): + break + + # 控制帧率 + time.sleep(0.001) + + except KeyboardInterrupt: + print("\n\n用户中断模拟...") + finally: - self.vis.close() - # ------------------------------------------------------------------- + if viewer is not None: + viewer.close() + + # 显示模拟统计 + print(f"\n\n{'=' * 80}") + print("模拟统计:") + print(f" 总模拟时间: {self.simulation_time:.1f}秒") + print(f" 总帧数: {frame_count}") + print(f" 平均帧率: {frame_count / (time.time() - start_time):.1f} FPS") + print(f" 路径点数: {len(self.path_history)}") + print(f"{'=' * 80}") + + +def main(): + """主函数""" + print("正在初始化无人小车模拟系统...") + print("=" * 80) + + try: + # 检查必要的库 + import importlib + required_libs = ['mujoco', 'numpy', 'glfw'] + missing_libs = [] + + for lib in required_libs: + try: + importlib.import_module(lib) + except ImportError: + missing_libs.append(lib) + + if missing_libs: + print(f"缺少必要的库: {missing_libs}") + print("请使用以下命令安装:") + print("pip install mujoco glfw numpy") + return + + # 创建无人小车实例 + print("正在创建无人小车模型...") + car_sim = AutonomousCar() + + print("模型创建成功!开始模拟...") + time.sleep(1) + + # 运行模拟 + car_sim.run_simulation() + + except Exception as e: + print(f"\n模拟过程中发生错误: {e}") + import traceback + traceback.print_exc() + + # 提供故障排除建议 + print(f"\n{'=' * 80}") + print("故障排除建议:") + print("1. 确保已安装正确版本的MuJoCo:") + print(" pip install mujoco") + print("2. 如果使用简化模型,可能需要安装额外依赖:") + print(" pip install glfw") + print("3. 确保有足够的权限和磁盘空间") + print("4. 尝试重启PyCharm或系统") + print(f"{'=' * 80}") if __name__ == "__main__": - env = VehicleEnv() - env.run() + main() \ No newline at end of file From c04413bc7ff8efb155302601a8afd595eebc6f70 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Thu, 25 Dec 2025 18:11:46 +0800 Subject: [PATCH 2/7] 111 --- .../1.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/1.py diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/1.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/1.py new file mode 100644 index 0000000000..e69de29bb2 From 45e9991e3b6f08c536ef11bf8fef89f329d4f663 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Thu, 25 Dec 2025 18:12:56 +0800 Subject: [PATCH 3/7] 111 --- .../1.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/1.py diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/1.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/1.py deleted file mode 100644 index e69de29bb2..0000000000 From c63a1841997ffba51d0c1256d0001bbcfe0aae10 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Thu, 25 Dec 2025 18:16:44 +0800 Subject: [PATCH 4/7] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E4=BD=BF=E5=B0=8F?= =?UTF-8?q?=E8=BD=A6=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../qwer.py | 882 +++++++++++------- 1 file changed, 559 insertions(+), 323 deletions(-) diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py index f984626fa9..4e6e8f242c 100644 --- a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py @@ -1,346 +1,582 @@ +""" +无人小车自主行驶与避让模拟 +基于MuJoCo和Python实现 +运行环境:PyCharm + MuJoCo +""" + import mujoco -import glfw +import mujoco.viewer import numpy as np -import csv +import glfw +import time +import math import os -from datetime import datetime -import imageio -from typing import List - -# -------------------------- 全局配置 -------------------------- -TIME_STEP = 0.01 -MAX_STEPS = 2000 -NSTEP = 5 - -# 激光雷达参数 -LIDAR_MOUNT_POS = np.array([0.5, 0, 0.2]) -LIDAR_MAX_DISTANCE = 8.0 -LIDAR_NUM_RAYS = 360 - -# 无人车参数 (恢复到合理数值) -MAX_STEER_ANGLE = 0.4 # 合理的转向角度 -MAX_DRIVE_TORQUE = 8.0 # 合理的驱动力矩 -SAFE_SPEED = 1.0 # 合理的直行速度 -AVOID_SPEED = 0.5 # 合理的避让速度 -OBSTACLE_THRESHOLD = 4.0 -AVOID_STEER_ANGLE = 0.3 # 合理的避让转向角度 -TORQUE_GAIN = 4.0 # 合理的加速响应 - -# 执行器索引 -DRIVE_ACTUATOR_IDS = [0, 1] -STEER_ACTUATOR_IDS = [2, 3] - -# -------------------------- MuJoCo XML 场景定义 -------------------------- -SCENE_XML = """ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -""" - -# -------------------------- 激光雷达传感器 -------------------------- -class LidarSensor: - def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): - self.model = model - self.data = data - self.angles = np.linspace(0, 2 * np.pi, LIDAR_NUM_RAYS, endpoint=False) - self.geomgroup = np.zeros(6, dtype=np.uint8) - self.geomgroup[0] = 1 # 只检测第0组的geom - - def rotate_vector(self, vec: np.ndarray, quat: np.ndarray) -> np.ndarray: - qx, qy, qz, qw = quat - rot_mat = np.array([ - [1 - 2 * qy ** 2 - 2 * qz ** 2, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw], - [2 * qx * qy + 2 * qz * qw, 1 - 2 * qx ** 2 - 2 * qz ** 2, 2 * qy * qz - 2 * qx * qw], - [2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx ** 2 - 2 * qy ** 2] - ]) - return np.dot(rot_mat, vec) - - def get_distance_data(self) -> List[float]: - distances = [LIDAR_MAX_DISTANCE] * LIDAR_NUM_RAYS - vehicle_pos = self.data.qpos[:3] - vehicle_quat = self.data.qpos[3:7] - - for i, angle in enumerate(self.angles): - local_dir = np.array([np.cos(angle), np.sin(angle), 0.0], dtype=np.float64) - local_dir = local_dir / np.linalg.norm(local_dir) - world_dir = self.rotate_vector(local_dir, vehicle_quat) - world_dir = world_dir / np.linalg.norm(world_dir) - - ray_start = vehicle_pos + self.rotate_vector(LIDAR_MOUNT_POS, vehicle_quat) - ray_start = ray_start.astype(np.float64) - - # 匹配mj_ray参数格式 - pnt = ray_start.reshape(3, 1) - vec = world_dir.reshape(3, 1) - geomid = np.zeros(1, dtype=np.int32) - - detected_dist = mujoco.mj_ray( - self.model, self.data, - pnt=pnt, - vec=vec, - geomgroup=self.geomgroup, - flg_static=1, - bodyexclude=0, - geomid=geomid - ) - - if geomid[0] != -1 and detected_dist <= LIDAR_MAX_DISTANCE: - distances[i] = detected_dist - - return distances - - -# -------------------------- 可视化类 -------------------------- -class Visualizer: - def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): - self.model = model - self.data = data - if not glfw.init(): - raise RuntimeError("Failed to initialize GLFW") - self.window = glfw.create_window(1280, 720, "Autonomous Vehicle Simulation", None, None) - if not self.window: - glfw.terminate() - raise RuntimeError("Failed to create GLFW window") - glfw.make_context_current(self.window) - glfw.swap_interval(1) - - self.cam = mujoco.MjvCamera() - self.opt = mujoco.MjvOption() - mujoco.mjv_defaultCamera(self.cam) - mujoco.mjv_defaultOption(self.opt) - self.scene = mujoco.MjvScene(self.model, maxgeom=10000) - self.context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150) - - self.cam.distance = 10.0 - self.cam.azimuth = -45.0 - self.cam.elevation = -30.0 - self.cam.lookat = [5.0, 0.0, 0.5] - - def render(self): - if glfw.window_should_close(self.window): - return True # 返回True表示窗口应关闭 - mujoco.mjv_updateScene(self.model, self.data, self.opt, None, self.cam, mujoco.mjtCatBit.mjCAT_ALL, self.scene) - viewport = mujoco.MjrRect(0, 0, 1280, 720) - mujoco.mjr_render(viewport, self.scene, self.context) - glfw.swap_buffers(self.window) - glfw.poll_events() - return False # 返回False表示窗口继续运行 - - def close(self): - glfw.destroy_window(self.window) - glfw.terminate() - - def should_close(self): - return glfw.window_should_close(self.window) - - -# -------------------------- 数据记录类 -------------------------- -class DataLogger: - def __init__(self): - self.log_dir = "logs" - os.makedirs(self.log_dir, exist_ok=True) - self.log_path = os.path.join(self.log_dir, f"sim_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv") - self.fields = ['step', 'time', 'x', 'y', 'speed', 'steer', 'torque', 'collision'] - with open(self.log_path, 'w', newline='') as f: - csv.writer(f).writerow(self.fields) - - def log(self, step, time, x, y, speed, steer, torque, collision): - with open(self.log_path, 'a', newline='') as f: - csv.writer(f).writerow( - [step, round(time, 3), round(x, 3), round(y, 3), round(speed, 3), round(steer, 3), round(torque, 3), - 1 if collision else 0]) - - -# -------------------------- 控制策略类 -------------------------- -class RulePolicy: - def __init__(self): - self.front_rays = slice(350, 360) - self.front_ext = slice(0, 10) - self.left_rays = slice(270, 350) - self.right_rays = slice(10, 90) - - def get_speed(self, vel): - return np.linalg.norm(vel[:2]) - - def decide(self, lidar_data, vel): - lidar = np.array(lidar_data) - current_speed = self.get_speed(vel) - - front_dist = np.min(np.concatenate([lidar[self.front_rays], lidar[self.front_ext]])) - left_dist = np.min(lidar[self.left_rays]) - right_dist = np.min(lidar[self.right_rays]) - - if front_dist >= OBSTACLE_THRESHOLD: - steer = 0.0 - target_speed = SAFE_SPEED - else: - steer = AVOID_STEER_ANGLE if left_dist > right_dist else -AVOID_STEER_ANGLE - target_speed = AVOID_SPEED - torque = TORQUE_GAIN * (target_speed - current_speed) - torque = np.clip(torque, -MAX_DRIVE_TORQUE, MAX_DRIVE_TORQUE) - return steer, torque +class AutonomousCar: + def __init__(self, model_path=None): + """初始化无人小车模拟器""" + # 如果没有提供模型文件,使用内置的XML模型 + if model_path is None: + self.xml = """ + + + """ + + # 保存XML到临时文件 + self.temp_xml_path = "temp_car_model.xml" + with open(self.temp_xml_path, 'w') as f: + f.write(self.xml) + try: + self.model = mujoco.MjModel.from_xml_path(self.temp_xml_path) + except Exception as e: + print(f"XML解析错误: {e}") + # 尝试简化版本 + self.create_simple_model() + else: + self.model = mujoco.MjModel.from_xml_path(model_path) -# -------------------------- 仿真环境类 -------------------------- -class VehicleEnv: - def __init__(self): - self.model = mujoco.MjModel.from_xml_string(SCENE_XML) self.data = mujoco.MjData(self.model) - self.lidar = LidarSensor(self.model, self.data) - self.vis = Visualizer(self.model, self.data) - self.logger = DataLogger() - self.policy = RulePolicy() - - # 初始化帧缓存用于GIF录制 - self.frames = [] - self.frame_count = 0 - self.max_frames = 1000 # 最大录制帧数,防止内存溢出 - def reset(self): - mujoco.mj_resetData(self.model, self.data) - return self.get_state() - - def get_state(self): - return { - 'pos': self.data.qpos[:3], - 'vel': self.data.qvel[:3], - 'lidar': self.lidar.get_distance_data(), - 'collision': self.data.ncon > 0 + # 控制参数 + self.target_speed = 6.0 # 目标速度 + self.max_steering_angle = 0.5 # 最大转向角度(弧度) + self.avoidance_distance = 2.5 # 避障检测距离 + self.avoidance_strength = 2.5 # 避障强度 + + # 状态变量 + self.current_speed = 0.0 + self.steering_angle = 0.0 + self.obstacle_detected = False + self.simulation_time = 0.0 + self.target_reached = False + self.path_history = [] # 路径历史 + + # PID控制器参数 + self.speed_Kp = 4.0 + self.speed_Ki = 0.1 + self.speed_Kd = 0.3 + self.speed_integral = 0.0 + self.speed_prev_error = 0.0 + + self.steering_Kp = 6.0 + self.steering_Ki = 0.05 + self.steering_Kd = 0.2 + self.steering_integral = 0.0 + self.steering_prev_error = 0.0 + + def create_simple_model(self): + """创建简化模型(如果完整模型有问题)""" + print("使用简化模型...") + simple_xml = """ + + + """ + + with open(self.temp_xml_path, 'w') as f: + f.write(simple_xml) + + self.model = mujoco.MjModel.from_xml_path(self.temp_xml_path) + + def __del__(self): + """清理临时文件""" + if hasattr(self, 'temp_xml_path') and os.path.exists(self.temp_xml_path): + try: + os.remove(self.temp_xml_path) + except: + pass + + def get_sensor_readings(self): + """获取传感器读数""" + readings = { + 'front_distance': 10.0, + 'left_distance': 10.0, + 'right_distance': 10.0, + 'front_obstacle': False, + 'left_obstacle': False, + 'right_obstacle': False } - def step(self, steer, torque): - for idx in STEER_ACTUATOR_IDS: - self.data.ctrl[idx] = np.clip(steer, -MAX_STEER_ANGLE, MAX_STEER_ANGLE) - for idx in DRIVE_ACTUATOR_IDS: - self.data.ctrl[idx] = torque + # 获取小车位置和方向 + car_pos = self.data.body('car').xpos + car_orientation = self.data.body('car').xmat.reshape(3, 3) + car_forward = car_orientation @ np.array([0, 1, 0]) # 小车前进方向 + car_left = car_orientation @ np.array([1, 0, 0]) # 小车左侧方向 + + # 检查所有障碍物 + obstacle_positions = [ + np.array([3, 2, 0.5]), # obstacle1 + np.array([5, -1.5, 0.5]), # obstacle2 + np.array([2, -2, 0.5]), # obstacle3 + np.array([6, 2, 0.5]) # obstacle4 + ] + + obstacle_sizes = [ + 1.2, # obstacle1 半径+高度 + 1.4, # obstacle2 尺寸 + 1.0, # obstacle3 直径 + 1.3 # obstacle4 半径+高度 + ] + + for i, obs_pos in enumerate(obstacle_positions): + # 计算障碍物到小车的向量 + obs_vector = obs_pos - car_pos + distance = np.linalg.norm(obs_vector[:2]) # 只考虑平面距离 + + if distance < self.avoidance_distance + obstacle_sizes[i]: + # 计算障碍物相对于小车的方向 + obs_direction = obs_vector[:2] / distance if distance > 0 else np.array([0, 0]) + + # 计算与前进方向的夹角 + forward_2d = car_forward[:2] + angle = math.atan2( + obs_direction[1] * forward_2d[0] - obs_direction[0] * forward_2d[1], + obs_direction[0] * forward_2d[0] + obs_direction[1] * forward_2d[1] + ) + + angle_deg = math.degrees(angle) + + # 更新传感器读数 + if -45 < angle_deg < 45: # 前方 + readings['front_distance'] = min(readings['front_distance'], distance) + if distance < 2.0: + readings['front_obstacle'] = True + + elif 45 <= angle_deg < 135: # 左侧 + readings['left_distance'] = min(readings['left_distance'], distance) + if distance < 1.5: + readings['left_obstacle'] = True + + elif -135 < angle_deg <= -45: # 右侧 + readings['right_distance'] = min(readings['right_distance'], distance) + if distance < 1.5: + readings['right_obstacle'] = True + + return readings + + def autonomous_driving(self, dt): + """自主驾驶算法""" + if dt <= 0: + dt = 0.01 + + # 获取传感器数据 + sensor_data = self.get_sensor_readings() + + # 获取目标位置 + target_pos = np.array([8, 0, 0.5]) + car_pos = self.data.body('car').xpos + + # 计算到目标的距离和方向 + target_vector = target_pos - car_pos + target_distance = np.linalg.norm(target_vector[:2]) + + # 检查是否到达目标 + if target_distance < 0.5: + self.target_reached = True + return np.zeros(self.model.nu) + + # 计算目标方向(归一化) + if target_distance > 0: + target_direction = target_vector[:2] / target_distance + else: + target_direction = np.array([0, 1]) + + # 获取小车当前方向 + car_orientation = self.data.body('car').xmat.reshape(3, 3) + car_direction = car_orientation @ np.array([0, 1, 0]) # 前进方向 + car_direction_2d = car_direction[:2] + + if np.linalg.norm(car_direction_2d) > 0: + car_direction_2d = car_direction_2d / np.linalg.norm(car_direction_2d) + + # 计算转向误差 + steering_error = math.atan2( + target_direction[1] * car_direction_2d[0] - target_direction[0] * car_direction_2d[1], + target_direction[0] * car_direction_2d[0] + target_direction[1] * car_direction_2d[1] + ) + + # 避障逻辑 + avoidance_steering = 0.0 + self.obstacle_detected = False + + if sensor_data['front_obstacle']: + self.obstacle_detected = True + # 前方有障碍物,根据两侧距离决定转向 + if sensor_data['left_distance'] > sensor_data['right_distance']: + avoidance_steering = 0.5 # 向左转 + else: + avoidance_steering = -0.5 # 向右转 + + elif sensor_data['left_obstacle']: + avoidance_steering = -0.3 # 向右轻微转向 + + elif sensor_data['right_obstacle']: + avoidance_steering = 0.3 # 向左轻微转向 + + # 合并转向控制 + total_steering = steering_error + avoidance_steering + + # 限制转向角度 + total_steering = np.clip(total_steering, -self.max_steering_angle, self.max_steering_angle) + + # 速度控制:根据障碍物距离调整速度 + min_distance = min(sensor_data['front_distance'], + sensor_data['left_distance'], + sensor_data['right_distance']) + + if min_distance < 1.0: + speed_multiplier = 0.3 + elif min_distance < 2.0: + speed_multiplier = 0.6 + else: + speed_multiplier = 1.0 + + target_speed_adjusted = self.target_speed * speed_multiplier + + # 简单的速度控制 + if self.current_speed < target_speed_adjusted: + self.current_speed += 0.5 * dt + elif self.current_speed > target_speed_adjusted: + self.current_speed -= 0.5 * dt + + self.current_speed = np.clip(self.current_speed, 0, self.target_speed) + + # 记录路径 + self.path_history.append(car_pos.copy()) + if len(self.path_history) > 1000: + self.path_history.pop(0) + + # 生成控制信号 + control = np.zeros(self.model.nu) + + if hasattr(self.model, 'nu') and self.model.nu >= 6: + # 完整模型的控制 + control[0] = self.current_speed # 前左驱动 + control[1] = self.current_speed # 前右驱动 + control[2] = self.current_speed # 后左驱动 + control[3] = self.current_speed # 后右驱动 + control[4] = total_steering # 前左转向 + control[5] = total_steering # 前右转向 + else: + # 简化模型的控制 + control[0] = self.current_speed + if len(control) > 1: + control[1] = total_steering + + return control + + def print_status(self): + """打印状态信息""" + car_pos = self.data.body('car').xpos + target_pos = np.array([8, 0, 0.5]) + distance = np.linalg.norm(target_pos[:2] - car_pos[:2]) + + print(f"\r时间: {self.simulation_time:.1f}s | " + f"位置: ({car_pos[0]:.1f}, {car_pos[1]:.1f}) | " + f"速度: {self.current_speed:.1f}m/s | " + f"转向: {math.degrees(self.steering_angle):.0f}° | " + f"距目标: {distance:.1f}m | " + f"状态: {'避障' if self.obstacle_detected else '导航'} {'到达!' if self.target_reached else ''}", + end="") + + def run_simulation(self): + """运行模拟主循环""" + print("无人小车模拟系统启动中...") + print("=" * 80) + print("控制说明:") + print(" - 按ESC键退出模拟") + print(" - 模拟会自动运行直到按下ESC或到达目标") + print(" - 绿色球体是目标点") + print(" - 红色物体是障碍物") + print(" - 小车会自动导航并避开障碍物") + print("=" * 80) + + # 设置模拟选项 + self.model.opt.gravity[2] = -9.81 + + # 重置模拟 + mujoco.mj_resetData(self.model, self.data) - mujoco.mj_step(self.model, self.data, nstep=NSTEP) - return self.get_state() + # 启动查看器 + try: + viewer = mujoco.viewer.launch_passive(self.model, self.data) + except Exception as e: + print(f"查看器启动失败: {e}") + print("将以无界面模式运行模拟...") + viewer = None - def run(self): - state = self.reset() - print("Starting simulation (Press ESC to close and save GIF)...") - print("=" * 50) + last_time = time.time() + frame_count = 0 + start_time = time.time() - step = 0 try: - while not self.vis.should_close(): # 持续运行直到ESC被按下 - # 1. 决策:根据激光雷达和速度生成控制指令 - steer, torque = self.policy.decide(state['lidar'], state['vel']) - # 2. 执行动作:更新车辆状态 - state = self.step(steer, torque) - # 3. 记录日志(每10步记录一次,减少日志文件大小) - if step % 10 == 0: - self.logger.log(step, step * TIME_STEP, state['pos'][0], state['pos'][1], - self.policy.get_speed(state['vel']), steer, torque, state['collision']) - # 4. 渲染画面 - if self.vis.render(): - break # 如果渲染返回True,表示窗口应关闭 - - # -------------------------- 录制当前帧(控制录制频率) -------------------------- - self.frame_count += 1 - if self.frame_count % 5 == 0 and len(self.frames) < self.max_frames: # 每5步录制一帧 - try: - width, height = glfw.get_framebuffer_size(self.vis.window) - if width > 0 and height > 0: - buffer = np.zeros((height, width, 3), dtype=np.uint8) - # 创建正确的 MjrRect 对象 - viewport = mujoco.MjrRect(0, 0, width, height) - mujoco.mjr_readPixels(buffer, None, viewport, self.vis.context) - buffer = np.flipud(buffer) - self.frames.append(buffer) - except Exception as e: - print(f"Frame recording error: {e}") - # ------------------------------------------------------------------- - - # 打印进度(每100步) - if step % 100 == 0: - print( - f"Step: {step:4d} | Speed: {self.policy.get_speed(state['vel']):.2f}m/s | Collision: {state['collision']}") - - step += 1 - - # 防止无限循环的安全机制 - if step >= 10000: - print("Reached maximum steps limit (10000), stopping simulation...") + while True: + if viewer is not None and not viewer.is_running(): break + # 计算时间步长 + current_time = time.time() + dt = current_time - last_time + last_time = current_time + self.simulation_time += dt + + # 限制时间步长 + if dt > 0.1: + dt = 0.01 + + # 应用自主驾驶控制 + control = self.autonomous_driving(dt) + self.data.ctrl[:] = control + + # 执行物理模拟 + mujoco.mj_step(self.model, self.data) + + # 更新查看器 + if viewer is not None: + viewer.sync() + + # 更新状态显示 + frame_count += 1 + if frame_count % 10 == 0: + self.print_status() + + # 检查是否到达目标 + if self.target_reached: + print(f"\n\n{'=' * 80}") + print("成功到达目标点!") + print(f"总时间: {self.simulation_time:.1f}秒") + print(f"平均速度: {np.mean([v for v in self.path_history if len(v) > 0]):.1f}m/s") + print(f"{'=' * 80}") + time.sleep(2) + break + + # 检查ESC键 - 修复后的代码 + if viewer is not None: + try: + # MuJoCo新版本使用不同的API来访问窗口句柄 + if hasattr(viewer, 'context'): + # 检查查看器是否仍在运行 + if not viewer.is_running(): + break + else: + # 检查查看器是否仍在运行 + if not viewer.is_running(): + break + except: + # 如果任何API检查失败,检查查看器是否仍在运行 + if viewer is not None and not viewer.is_running(): + break + + # 控制帧率 + time.sleep(0.001) + + except KeyboardInterrupt: + print("\n\n用户中断模拟...") + finally: - # -------------------------- 结束录制并保存 -------------------------- - try: - if self.frames: - print(f"\nRecording completed. Saving GIF with {len(self.frames)} frames...") - # 使用适中的 duration 值来保证合理的播放速度 - imageio.v3.imwrite('autonomous_vehicle_simulation.gif', self.frames, duration=200, loop=0) - print("GIF animation saved to: autonomous_vehicle_simulation.gif") - else: - print("No frame data available for GIF") - except Exception as e: - print(f"GIF saving failed: {e}") - # ------------------------------------------------------------------- + if viewer is not None: + viewer.close() - # 关闭可视化窗口 - self.vis.close() - # 打印保存路径 - print(f"Log saved to: {self.logger.log_path}") - print(f"Total simulation steps: {step}") + # 显示模拟统计 + print(f"\n\n{'=' * 80}") + print("模拟统计:") + print(f" 总模拟时间: {self.simulation_time:.1f}秒") + print(f" 总帧数: {frame_count}") + print(f" 平均帧率: {frame_count / (time.time() - start_time):.1f} FPS") + print(f" 路径点数: {len(self.path_history)}") + print(f"{'=' * 80}") -# -------------------------- 主函数 -------------------------- -if __name__ == "__main__": +def main(): + """主函数""" + print("正在初始化无人小车模拟系统...") + print("=" * 80) + try: - env = VehicleEnv() - env.run() + # 检查必要的库 + import importlib + required_libs = ['mujoco', 'numpy', 'glfw'] + missing_libs = [] + + for lib in required_libs: + try: + importlib.import_module(lib) + except ImportError: + missing_libs.append(lib) + + if missing_libs: + print(f"缺少必要的库: {missing_libs}") + print("请使用以下命令安装:") + print("pip install mujoco glfw numpy") + return + + # 创建无人小车实例 + print("正在创建无人小车模型...") + car_sim = AutonomousCar() + + print("模型创建成功!开始模拟...") + time.sleep(1) + + # 运行模拟 + car_sim.run_simulation() + except Exception as e: - print(f"Error: {str(e)}") - try: - # 异常时也关闭录制器和窗口,避免文件损坏 - env.vis.close() - except: - pass + print(f"\n模拟过程中发生错误: {e}") + import traceback + traceback.print_exc() + + # 提供故障排除建议 + print(f"\n{'=' * 80}") + print("故障排除建议:") + print("1. 确保已安装正确版本的MuJoCo:") + print(" pip install mujoco") + print("2. 如果使用简化模型,可能需要安装额外依赖:") + print(" pip install glfw") + print("3. 确保有足够的权限和磁盘空间") + print("4. 尝试重启PyCharm或系统") + print(f"{'=' * 80}") + + +if __name__ == "__main__": + main() \ No newline at end of file From c60104347c52380dc55e9c92166c1a87ff66c80a Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Thu, 25 Dec 2025 20:55:23 +0800 Subject: [PATCH 5/7] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E4=BD=BF=E5=B0=8F?= =?UTF-8?q?=E8=BD=A6=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../qwer1.py | 582 ++++++++++++++++++ 1 file changed, 582 insertions(+) create mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py new file mode 100644 index 0000000000..4e6e8f242c --- /dev/null +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py @@ -0,0 +1,582 @@ +""" +无人小车自主行驶与避让模拟 +基于MuJoCo和Python实现 +运行环境:PyCharm + MuJoCo +""" + +import mujoco +import mujoco.viewer +import numpy as np +import glfw +import time +import math +import os + + +class AutonomousCar: + def __init__(self, model_path=None): + """初始化无人小车模拟器""" + # 如果没有提供模型文件,使用内置的XML模型 + if model_path is None: + self.xml = """ + + + """ + + # 保存XML到临时文件 + self.temp_xml_path = "temp_car_model.xml" + with open(self.temp_xml_path, 'w') as f: + f.write(self.xml) + + try: + self.model = mujoco.MjModel.from_xml_path(self.temp_xml_path) + except Exception as e: + print(f"XML解析错误: {e}") + # 尝试简化版本 + self.create_simple_model() + else: + self.model = mujoco.MjModel.from_xml_path(model_path) + + self.data = mujoco.MjData(self.model) + + # 控制参数 + self.target_speed = 6.0 # 目标速度 + self.max_steering_angle = 0.5 # 最大转向角度(弧度) + self.avoidance_distance = 2.5 # 避障检测距离 + self.avoidance_strength = 2.5 # 避障强度 + + # 状态变量 + self.current_speed = 0.0 + self.steering_angle = 0.0 + self.obstacle_detected = False + self.simulation_time = 0.0 + self.target_reached = False + self.path_history = [] # 路径历史 + + # PID控制器参数 + self.speed_Kp = 4.0 + self.speed_Ki = 0.1 + self.speed_Kd = 0.3 + self.speed_integral = 0.0 + self.speed_prev_error = 0.0 + + self.steering_Kp = 6.0 + self.steering_Ki = 0.05 + self.steering_Kd = 0.2 + self.steering_integral = 0.0 + self.steering_prev_error = 0.0 + + def create_simple_model(self): + """创建简化模型(如果完整模型有问题)""" + print("使用简化模型...") + simple_xml = """ + + + """ + + with open(self.temp_xml_path, 'w') as f: + f.write(simple_xml) + + self.model = mujoco.MjModel.from_xml_path(self.temp_xml_path) + + def __del__(self): + """清理临时文件""" + if hasattr(self, 'temp_xml_path') and os.path.exists(self.temp_xml_path): + try: + os.remove(self.temp_xml_path) + except: + pass + + def get_sensor_readings(self): + """获取传感器读数""" + readings = { + 'front_distance': 10.0, + 'left_distance': 10.0, + 'right_distance': 10.0, + 'front_obstacle': False, + 'left_obstacle': False, + 'right_obstacle': False + } + + # 获取小车位置和方向 + car_pos = self.data.body('car').xpos + car_orientation = self.data.body('car').xmat.reshape(3, 3) + car_forward = car_orientation @ np.array([0, 1, 0]) # 小车前进方向 + car_left = car_orientation @ np.array([1, 0, 0]) # 小车左侧方向 + + # 检查所有障碍物 + obstacle_positions = [ + np.array([3, 2, 0.5]), # obstacle1 + np.array([5, -1.5, 0.5]), # obstacle2 + np.array([2, -2, 0.5]), # obstacle3 + np.array([6, 2, 0.5]) # obstacle4 + ] + + obstacle_sizes = [ + 1.2, # obstacle1 半径+高度 + 1.4, # obstacle2 尺寸 + 1.0, # obstacle3 直径 + 1.3 # obstacle4 半径+高度 + ] + + for i, obs_pos in enumerate(obstacle_positions): + # 计算障碍物到小车的向量 + obs_vector = obs_pos - car_pos + distance = np.linalg.norm(obs_vector[:2]) # 只考虑平面距离 + + if distance < self.avoidance_distance + obstacle_sizes[i]: + # 计算障碍物相对于小车的方向 + obs_direction = obs_vector[:2] / distance if distance > 0 else np.array([0, 0]) + + # 计算与前进方向的夹角 + forward_2d = car_forward[:2] + angle = math.atan2( + obs_direction[1] * forward_2d[0] - obs_direction[0] * forward_2d[1], + obs_direction[0] * forward_2d[0] + obs_direction[1] * forward_2d[1] + ) + + angle_deg = math.degrees(angle) + + # 更新传感器读数 + if -45 < angle_deg < 45: # 前方 + readings['front_distance'] = min(readings['front_distance'], distance) + if distance < 2.0: + readings['front_obstacle'] = True + + elif 45 <= angle_deg < 135: # 左侧 + readings['left_distance'] = min(readings['left_distance'], distance) + if distance < 1.5: + readings['left_obstacle'] = True + + elif -135 < angle_deg <= -45: # 右侧 + readings['right_distance'] = min(readings['right_distance'], distance) + if distance < 1.5: + readings['right_obstacle'] = True + + return readings + + def autonomous_driving(self, dt): + """自主驾驶算法""" + if dt <= 0: + dt = 0.01 + + # 获取传感器数据 + sensor_data = self.get_sensor_readings() + + # 获取目标位置 + target_pos = np.array([8, 0, 0.5]) + car_pos = self.data.body('car').xpos + + # 计算到目标的距离和方向 + target_vector = target_pos - car_pos + target_distance = np.linalg.norm(target_vector[:2]) + + # 检查是否到达目标 + if target_distance < 0.5: + self.target_reached = True + return np.zeros(self.model.nu) + + # 计算目标方向(归一化) + if target_distance > 0: + target_direction = target_vector[:2] / target_distance + else: + target_direction = np.array([0, 1]) + + # 获取小车当前方向 + car_orientation = self.data.body('car').xmat.reshape(3, 3) + car_direction = car_orientation @ np.array([0, 1, 0]) # 前进方向 + car_direction_2d = car_direction[:2] + + if np.linalg.norm(car_direction_2d) > 0: + car_direction_2d = car_direction_2d / np.linalg.norm(car_direction_2d) + + # 计算转向误差 + steering_error = math.atan2( + target_direction[1] * car_direction_2d[0] - target_direction[0] * car_direction_2d[1], + target_direction[0] * car_direction_2d[0] + target_direction[1] * car_direction_2d[1] + ) + + # 避障逻辑 + avoidance_steering = 0.0 + self.obstacle_detected = False + + if sensor_data['front_obstacle']: + self.obstacle_detected = True + # 前方有障碍物,根据两侧距离决定转向 + if sensor_data['left_distance'] > sensor_data['right_distance']: + avoidance_steering = 0.5 # 向左转 + else: + avoidance_steering = -0.5 # 向右转 + + elif sensor_data['left_obstacle']: + avoidance_steering = -0.3 # 向右轻微转向 + + elif sensor_data['right_obstacle']: + avoidance_steering = 0.3 # 向左轻微转向 + + # 合并转向控制 + total_steering = steering_error + avoidance_steering + + # 限制转向角度 + total_steering = np.clip(total_steering, -self.max_steering_angle, self.max_steering_angle) + + # 速度控制:根据障碍物距离调整速度 + min_distance = min(sensor_data['front_distance'], + sensor_data['left_distance'], + sensor_data['right_distance']) + + if min_distance < 1.0: + speed_multiplier = 0.3 + elif min_distance < 2.0: + speed_multiplier = 0.6 + else: + speed_multiplier = 1.0 + + target_speed_adjusted = self.target_speed * speed_multiplier + + # 简单的速度控制 + if self.current_speed < target_speed_adjusted: + self.current_speed += 0.5 * dt + elif self.current_speed > target_speed_adjusted: + self.current_speed -= 0.5 * dt + + self.current_speed = np.clip(self.current_speed, 0, self.target_speed) + + # 记录路径 + self.path_history.append(car_pos.copy()) + if len(self.path_history) > 1000: + self.path_history.pop(0) + + # 生成控制信号 + control = np.zeros(self.model.nu) + + if hasattr(self.model, 'nu') and self.model.nu >= 6: + # 完整模型的控制 + control[0] = self.current_speed # 前左驱动 + control[1] = self.current_speed # 前右驱动 + control[2] = self.current_speed # 后左驱动 + control[3] = self.current_speed # 后右驱动 + control[4] = total_steering # 前左转向 + control[5] = total_steering # 前右转向 + else: + # 简化模型的控制 + control[0] = self.current_speed + if len(control) > 1: + control[1] = total_steering + + return control + + def print_status(self): + """打印状态信息""" + car_pos = self.data.body('car').xpos + target_pos = np.array([8, 0, 0.5]) + distance = np.linalg.norm(target_pos[:2] - car_pos[:2]) + + print(f"\r时间: {self.simulation_time:.1f}s | " + f"位置: ({car_pos[0]:.1f}, {car_pos[1]:.1f}) | " + f"速度: {self.current_speed:.1f}m/s | " + f"转向: {math.degrees(self.steering_angle):.0f}° | " + f"距目标: {distance:.1f}m | " + f"状态: {'避障' if self.obstacle_detected else '导航'} {'到达!' if self.target_reached else ''}", + end="") + + def run_simulation(self): + """运行模拟主循环""" + print("无人小车模拟系统启动中...") + print("=" * 80) + print("控制说明:") + print(" - 按ESC键退出模拟") + print(" - 模拟会自动运行直到按下ESC或到达目标") + print(" - 绿色球体是目标点") + print(" - 红色物体是障碍物") + print(" - 小车会自动导航并避开障碍物") + print("=" * 80) + + # 设置模拟选项 + self.model.opt.gravity[2] = -9.81 + + # 重置模拟 + mujoco.mj_resetData(self.model, self.data) + + # 启动查看器 + try: + viewer = mujoco.viewer.launch_passive(self.model, self.data) + except Exception as e: + print(f"查看器启动失败: {e}") + print("将以无界面模式运行模拟...") + viewer = None + + last_time = time.time() + frame_count = 0 + start_time = time.time() + + try: + while True: + if viewer is not None and not viewer.is_running(): + break + + # 计算时间步长 + current_time = time.time() + dt = current_time - last_time + last_time = current_time + self.simulation_time += dt + + # 限制时间步长 + if dt > 0.1: + dt = 0.01 + + # 应用自主驾驶控制 + control = self.autonomous_driving(dt) + self.data.ctrl[:] = control + + # 执行物理模拟 + mujoco.mj_step(self.model, self.data) + + # 更新查看器 + if viewer is not None: + viewer.sync() + + # 更新状态显示 + frame_count += 1 + if frame_count % 10 == 0: + self.print_status() + + # 检查是否到达目标 + if self.target_reached: + print(f"\n\n{'=' * 80}") + print("成功到达目标点!") + print(f"总时间: {self.simulation_time:.1f}秒") + print(f"平均速度: {np.mean([v for v in self.path_history if len(v) > 0]):.1f}m/s") + print(f"{'=' * 80}") + time.sleep(2) + break + + # 检查ESC键 - 修复后的代码 + if viewer is not None: + try: + # MuJoCo新版本使用不同的API来访问窗口句柄 + if hasattr(viewer, 'context'): + # 检查查看器是否仍在运行 + if not viewer.is_running(): + break + else: + # 检查查看器是否仍在运行 + if not viewer.is_running(): + break + except: + # 如果任何API检查失败,检查查看器是否仍在运行 + if viewer is not None and not viewer.is_running(): + break + + # 控制帧率 + time.sleep(0.001) + + except KeyboardInterrupt: + print("\n\n用户中断模拟...") + + finally: + if viewer is not None: + viewer.close() + + # 显示模拟统计 + print(f"\n\n{'=' * 80}") + print("模拟统计:") + print(f" 总模拟时间: {self.simulation_time:.1f}秒") + print(f" 总帧数: {frame_count}") + print(f" 平均帧率: {frame_count / (time.time() - start_time):.1f} FPS") + print(f" 路径点数: {len(self.path_history)}") + print(f"{'=' * 80}") + + +def main(): + """主函数""" + print("正在初始化无人小车模拟系统...") + print("=" * 80) + + try: + # 检查必要的库 + import importlib + required_libs = ['mujoco', 'numpy', 'glfw'] + missing_libs = [] + + for lib in required_libs: + try: + importlib.import_module(lib) + except ImportError: + missing_libs.append(lib) + + if missing_libs: + print(f"缺少必要的库: {missing_libs}") + print("请使用以下命令安装:") + print("pip install mujoco glfw numpy") + return + + # 创建无人小车实例 + print("正在创建无人小车模型...") + car_sim = AutonomousCar() + + print("模型创建成功!开始模拟...") + time.sleep(1) + + # 运行模拟 + car_sim.run_simulation() + + except Exception as e: + print(f"\n模拟过程中发生错误: {e}") + import traceback + traceback.print_exc() + + # 提供故障排除建议 + print(f"\n{'=' * 80}") + print("故障排除建议:") + print("1. 确保已安装正确版本的MuJoCo:") + print(" pip install mujoco") + print("2. 如果使用简化模型,可能需要安装额外依赖:") + print(" pip install glfw") + print("3. 确保有足够的权限和磁盘空间") + print("4. 尝试重启PyCharm或系统") + print(f"{'=' * 80}") + + +if __name__ == "__main__": + main() \ No newline at end of file From c0c90ad6786d98f45c1ae0da5466c0f445aa5460 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Fri, 26 Dec 2025 00:12:55 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E4=BD=BF=E5=B0=8F=E8=BD=A6=E5=88=9D?= =?UTF-8?q?=E6=AD=A5=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../qwer1.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py index 4e6e8f242c..4490a9c228 100644 --- a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py @@ -16,7 +16,7 @@ class AutonomousCar: def __init__(self, model_path=None): """初始化无人小车模拟器""" - # 如果没有提供模型文件,使用内置的XML模型 + # 如果没有提供模型文件,使 用内置的XML模型 if model_path is None: self.xml = """ From 021a1ed06bae1447ad6b677836686a7130c22772 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Fri, 26 Dec 2025 16:37:01 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E4=BD=BF=E5=B0=8F=E8=BD=A6=E5=88=9D?= =?UTF-8?q?=E6=AD=A5=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../qwer1.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py index 4490a9c228..c103a7e6f8 100644 --- a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer1.py @@ -16,7 +16,7 @@ class AutonomousCar: def __init__(self, model_path=None): """初始化无人小车模拟器""" - # 如果没有提供模型文件,使 用内置的XML模型 + # 如果没有提供模型文件,使 用 内置的XML模型 if model_path is None: self.xml = """