From 5a9bcfe203e791cd34130e4e65b1d9250037351e Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Sat, 20 Dec 2025 15:37:30 +0800 Subject: [PATCH 1/6] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README/main | 188 ++++++++++++++++++ .../README/md | 0 2 files changed, 188 insertions(+) create mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/main create mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/md diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/main b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/main new file mode 100644 index 0000000000..2624b32461 --- /dev/null +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/main @@ -0,0 +1,188 @@ +无人车自主行驶与自动避让系统(MuJoCo仿真)README + +一、项目简介 + +本项目基于MuJoCo(Multi-Joint dynamics with Contact)物理仿真引擎,构建了高保真的无人车自主行驶与自动避让仿真系统。系统核心目标是在虚拟仿真场景中,实现无人车沿预设路径(或自由探索)行驶,并能实时检测周围障碍物,通过决策算法完成安全避让动作。项目无需依赖真实硬件,可快速迭代验证避障算法、路径规划策略的有效性,适用于自动驾驶算法研发、强化学习训练、机器人运动控制等相关领域的学习与研究。 + +本系统具备场景可配置、障碍物类型多样、算法可替换等特性,为无人车自主导航相关算法的快速验证提供了高效的仿真平台。 + +二、功能特性 + +- 高保真物理仿真:基于MuJoCo引擎精准模拟无人车的动力学特性(如车轮摩擦、驱动力矩、惯性)和环境交互(如碰撞检测、地形附着力),还原真实行驶场景。 + +- 实时障碍物检测:集成虚拟激光雷达/深度相机传感器,实时采集周围环境数据,精准识别障碍物的位置、距离和尺寸信息。 + +- 多模式自主行驶:支持预设路径跟踪行驶和自由探索行驶两种模式,可通过配置文件灵活切换。 + +- 智能自动避让:实现基于规则/强化学习的避障决策算法,支持障碍物绕行、减速避让、紧急停车等多种避让策略。 + +- 场景可定制化:支持自定义场景地图(如直道、弯道、交叉路口)、障碍物类型(如静态障碍物、动态障碍物)和数量,可配置场景复杂度。 + +- 数据可视化与日志记录:实时可视化无人车行驶状态、传感器数据和避障决策过程,支持记录行驶轨迹、障碍物信息等数据用于后续分析。 + +- 算法模块化设计:感知、决策、控制模块解耦,支持快速替换不同的避障算法(如A*、DWA、强化学习模型)进行对比验证。 + +三、环境要求 + +3.1 软件环境 + +- 操作系统:Windows 10/11 64位、Ubuntu 20.04/22.04 64位 + +- MuJoCo版本:2.3.7 及以上(需获取官方许可证,学生可申请免费许可证) + +- 编程语言:Python 3.8 - 3.11 + +- + +核心依赖库:mujoco(MuJoCo仿真引擎Python绑定)、numpy(数值计算与数据处理)、matplotlib(数据可视化与结果绘图)、pyyaml(配置文件解析)、opencv-python(图像数据处理,用于传感器数据可视化)、torch(可选,用于强化学习模型训练与推理) + +3.2 硬件环境 + +- CPU:Intel Core i5 及以上(推荐多核处理器,提升仿真效率) + +- GPU:NVIDIA GeForce GTX 1050 及以上(可选,支持GPU加速仿真,提升复杂场景运行速度) + +- 内存:8GB 及以上(复杂场景建议16GB) + +- 磁盘空间:10GB 及以上(用于存储仿真模型、场景文件、日志数据) + +3.3 环境搭建步骤 + +1. 安装Python 3.8 - 3.11,配置Python环境变量。 + +2. 获取MuJoCo许可证,下载对应版本的MuJoCo引擎,解压至指定目录(如Windows:C:\Users\用户名\.mujoco,Ubuntu:~/.mujoco)。 + +3. + +设置MuJoCo环境变量:Windows系统需添加系统变量指定MuJoCo解压目录路径和许可证文件路径;Ubuntu系统需在终端执行相关命令配置环境变量,可写入.bashrc文件实现永久生效 + +安装依赖库:通过Python包管理工具安装所需核心依赖库,若需使用强化学习功能,额外安装对应的强化学习相关依赖库 + +验证环境:运行简单的Python命令导入mujoco库,无报错则说明环境搭建完成 + +四、项目结构 + +五、核心模块说明 + +5.1 核心模块功能说明 + +5.1.1 仿真环境模块(env/vehicle_env.py) + +仿真环境模块:核心功能为封装MuJoCo仿真引擎的核心交互逻辑,包括场景模型加载、无人车初始化、物理状态更新等。模块会根据配置文件加载指定的仿真场景,初始化无人车的转向、驱动等关键关节参数,并提供环境重置、动作执行等接口,实现仿真过程的循环推进。 + +5.1.2 传感器模块(sensor/lidar.py) + +传感器模块:核心功能为模拟实现障碍物检测所需的传感设备,以虚拟激光雷达为核心。模块会根据配置的安装位置、探测范围等参数,实时采集周围环境数据,通过射线检测原理计算障碍物距离,并将处理后的传感数据提供给决策模块使用,为避障判断提供数据支撑。 + +5.1.3 决策模块(policy/rule_based_policy.py) + +决策模块:核心功能为根据无人车状态和传感器数据生成行驶控制指令。以基于规则的决策逻辑为例,模块会划分激光雷达探测区域,判断前向及两侧是否存在障碍物,结合预设的距离阈值、安全速度、转向角度等参数,决策生成直行、转向避让、减速等控制指令,保障行驶安全。 + +5.1.4 项目入口(main.py) + +项目入口模块:核心功能为调度各模块协同运行,启动仿真流程。模块会加载各类配置文件,初始化仿真环境、决策策略和数据日志工具,通过循环调用环境重置、决策指令生成、动作执行、数据记录等接口,推动仿真过程持续进行,直至达到最大仿真步数或触发结束条件(碰撞、到达终点)。 + +5.2 配置文件说明 + +配置文件说明:系统参数可通过配置目录下的YAML文件灵活配置,无需修改核心逻辑。配置内容涵盖仿真场景路径、仿真步长、最大仿真步数等环境参数,激光雷达安装位置、探测范围等传感器参数,无人车关节ID等车辆参数,以及避障阈值、安全速度等决策参数,方便用户快速调整仿真条件。 + +六、模型说明 + +6.1 MuJoCo仿真模型(XML) + +无人车和场景的物理模型通过MuJoCo的XML文件定义,包含几何体(geom)、关节(joint)、传感器(sensor)等元素。XML文件位于env/scenes/目录,可通过utils/xml_builder.py工具生成自定义场景,或手动编写。 + +核心模型元素说明: + +- 无人车模型:由车身(geom类型为box)、车轮(geom类型为cylinder)组成,通过关节连接(转向关节为hinge,驱动关节为motor),定义了质量、惯性、摩擦系数等物理参数。 + +- 场景模型:包含地面(geom类型为plane)、障碍物(geom类型为box/cylinder,可设置静态/动态)、目标点(site类型,用于路径跟踪)。 + +- 传感器定义:可在XML中直接定义MuJoCo原生传感器(如force、torque传感器),与自定义的激光雷达传感器配合使用。 + +6.2 决策模型 + +本系统支持两种决策模型,可通过修改main.py中的政策初始化代码切换: + +- 基于规则的模型(默认):无需训练,通过预设规则(如前向障碍物距离阈值、左右避让空间判断)生成决策,适用于快速验证避障逻辑,代码位于policy/rule_based_policy.py。 + +- 强化学习模型(可选):基于PyTorch实现,以无人车的状态(位置、速度、激光雷达数据)为输入,输出转向角和驱动力矩,通过强化学习训练优化避障策略。训练代码可参考policy/rl_policy.py,训练数据和模型参数保存在models/目录。 + +6.3 模型训练(强化学习可选) + +若使用强化学习决策模型,需先进行训练: + +1. 修改main.py,初始化RLPolicy而非RuleBasedPolicy。 + +2. 配置policy_config.yaml中的训练参数(学习率、折扣因子、训练轮数等)。 + +3. 运行训练脚本:python policy/train_rl.py。 + +4. 训练完成后,模型参数保存在models/rl_policy.pth,可直接用于仿真推理。 + +七、数据输出 + +仿真过程中会记录关键数据,用于后续分析和算法优化: + +7.1 输出数据类型 + +- 无人车状态数据:位置(x,y,z)、速度、姿态(四元数)、转向角、驱动力矩。 + +- 传感器数据:激光雷达距离数据(数组)、相机图像数据(可选)。 + +- 决策数据:每步的动作指令、避障决策标签(如“直行”“左转避让”“紧急停车”)。 + +- 仿真状态数据:步数、时间、是否碰撞、是否到达终点。 + +7.2 输出格式与存储 + +- 实时可视化:通过utils/visualization.py实时显示无人车行驶轨迹、激光雷达探测范围、障碍物位置。 + +- 日志文件:仿真结束后,数据以CSV格式保存至logs/目录,文件名为simulation_YYYYMMDD_HHMMSS.csv。 + +- 图像输出:可选保存激光雷达点云图、行驶轨迹图至logs/figures/目录(需在配置文件中开启)。 + +7.3 数据可视化方法 + +数据可视化方法:可通过系统内置的可视化工具实现日志数据的可视化展示,支持绘制无人车行驶轨迹、激光雷达探测范围等关键信息,帮助用户直观分析仿真过程和系统性能,可视化结果可按需保存为图像文件。 + +八、扩展建议 + +本项目架构模块化,支持多种扩展方向,可根据需求进一步优化: + +- 多智能体交互:扩展env/vehicle_env.py,支持多个无人车同时行驶,模拟交通流场景,验证多车协同避障算法。 + +- 复杂场景建模:添加雨天、雪地、崎岖地形等场景的物理参数(如地面摩擦系数调整),通过utils/xml_builder.py生成复杂城市道路场景(含交通灯、斑马线)。 + +- 高级算法集成:集成路径规划算法(如A*、DWA、RRT*)到决策模块,实现“全局路径规划+局部避障”的双层控制;或集成深度学习模型(如PointPillars)用于障碍物检测。 + +- 传感器融合:添加虚拟相机、毫米波雷达等传感器,通过卡尔曼滤波、贝叶斯估计等算法融合多传感器数据,提升障碍物检测的准确性和鲁棒性。 + +- 真实硬件迁移:将仿真中验证有效的算法适配到真实无人车硬件(如STM32、ROS-based平台),实现“仿真验证-硬件部署”的快速迭代。 + +- 性能优化:通过MuJoCo的GPU加速功能(需配置CUDA)提升复杂场景的仿真速度;优化决策算法的计算效率,降低延迟。 + +九、故障排除 + +仿真过程中常见问题及解决方法: + +问题1:MuJoCo环境加载失败,报错“Could not find MuJoCo library” +解决方法:检查MuJoCo环境变量配置的准确性;确认MuJoCo许可证文件路径正确;重新安装MuJoCo的Python绑定库。 + +问题2:无人车无法正常行驶,出现抖动或漂移 +解决方法:调整环境配置中的仿真子步数提升稳定性;检查无人车模型关节阻尼系数,可适当增大以减少抖动;调整驱动力矩相关参数,避免速度过冲。 + +问题3:激光雷达无法检测到障碍物 +解决方法:检查激光雷达安装位置配置,确保探测范围可覆盖障碍物;确认仿真模型中障碍物的碰撞属性参数设置正确,与无人车、激光雷达的碰撞属性匹配;适当增大激光雷达最大探测距离配置。 + +- 问题4:强化学习模型训练收敛慢或不收敛 +解决方法:调整奖励函数设计(增加碰撞惩罚、到达目标奖励);优化状态空间和动作空间的归一化;调整学习率、折扣因子等超参数;增加训练轮数或提升训练数据量。 + + +问题5:仿真过程中出现碰撞但未检测到 +解决方法:检查仿真环境模块中的碰撞检测逻辑,确保正确获取MuJoCo的接触数据;在仿真模型中为无人车和障碍物设置明确的碰撞类型参数,避免碰撞过滤。 + +- 问题6:Python版本兼容问题 +解决方法:确保使用Python 3.8 - 3.11版本(mujoco库对Python 3.12及以上版本可能不兼容);通过虚拟环境(venv/conda)隔离项目依赖。 + + diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/md b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/md new file mode 100644 index 0000000000..e69de29bb2 From 4f5576c949a6be907c73d0c05b8c0e7d66a167c0 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Sat, 20 Dec 2025 20:48:34 +0800 Subject: [PATCH 2/6] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../{README/md => README.md} | 0 .../{README/main => main.py} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/{README/md => README.md} (100%) rename src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/{README/main => main.py} (100%) diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/md b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README.md similarity index 100% rename from src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/md rename to src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README.md diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/main b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/main.py similarity index 100% rename from src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README/main rename to src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/main.py From b446c9afb6887a9c6ae925a5a8c9a9d2feaba88c Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Sat, 20 Dec 2025 20:56:16 +0800 Subject: [PATCH 3/6] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README.md b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README.md index e69de29bb2..95403b831f 100644 --- a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README.md +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/README.md @@ -0,0 +1,25 @@ +无人车自主行驶与自动避让系统 + +本项目仅使用MuJoCo(Multi-Joint dynamics with Contact)物理仿真引擎与Python编程语言,构建高保真的无人车自主行驶与自动避让仿真系统。系统核心目标是在虚拟仿真场景中,实现无人车沿预设路径(或自由探索)行驶,并能实时检测周围障碍物,通过Python实现的决策算法完成安全避让动作。项目无需依赖真实硬件,可快速迭代验证避障算法、路径规划策略的有效性,适用于自动驾驶算法研发、强化学习训练、机器人运动控制等相关领域的学习与研究。 + +本系统具备场景可配置、障碍物类型多样、算法可替换等特性,为无人车自主导航相关算法的快速验证提供了高效的仿真平台。 + +环境要求 +- 操作系统:Windows 10/11 64位,Ubuntu 20.04/22.04 64位 + +- MuJoCo版本:2.3.7 及以上(需获取官方许可证,学生可申请免费许可证) + +- 编程语言:Python 3.8 - 3.11 + +核心依赖库:mujoco(MuJoCo仿真引擎Python绑定)、numpy(数值计算与数据处理)、matplotlib(数据可视化与结果绘图)、pyyaml(配置文件解析)、opencv-python(图像数据处理,用于传感器数据可视化) + +环境搭建步骤 +1. 安装Python 3.8 - 3.11,配置Python环境变量。 + +2. 获取MuJoCo许可证,下载对应版本的MuJoCo引擎,解压至指定目录(如Windows:C:\Users\用户名\.mujoco,Ubuntu:~/.mujoco)。 + +3. 设置MuJoCo环境变量:Windows系统需添加系统变量指定MuJoCo解压目录路径和许可证文件路径;Ubuntu系统需在终端执行相关命令配置环境变量,可写入.bashrc文件实现永久生效。 + +4. 安装依赖库:通过Python包管理工具安装所需核心依赖库,若需使用强化学习功能,额外安装对应的强化学习相关依赖库。 + +5. 验证环境:运行简单的Python命令导入mujoco库,无报错则说明环境搭建完成。 \ No newline at end of file From f105ce988aa18b83edcb2fe11cb7406b642a65c7 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Sat, 20 Dec 2025 20:58:51 +0800 Subject: [PATCH 4/6] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main.py | 188 ------------------ 1 file changed, 188 deletions(-) delete mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/main.py diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/main.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/main.py deleted file mode 100644 index 2624b32461..0000000000 --- a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/main.py +++ /dev/null @@ -1,188 +0,0 @@ -无人车自主行驶与自动避让系统(MuJoCo仿真)README - -一、项目简介 - -本项目基于MuJoCo(Multi-Joint dynamics with Contact)物理仿真引擎,构建了高保真的无人车自主行驶与自动避让仿真系统。系统核心目标是在虚拟仿真场景中,实现无人车沿预设路径(或自由探索)行驶,并能实时检测周围障碍物,通过决策算法完成安全避让动作。项目无需依赖真实硬件,可快速迭代验证避障算法、路径规划策略的有效性,适用于自动驾驶算法研发、强化学习训练、机器人运动控制等相关领域的学习与研究。 - -本系统具备场景可配置、障碍物类型多样、算法可替换等特性,为无人车自主导航相关算法的快速验证提供了高效的仿真平台。 - -二、功能特性 - -- 高保真物理仿真:基于MuJoCo引擎精准模拟无人车的动力学特性(如车轮摩擦、驱动力矩、惯性)和环境交互(如碰撞检测、地形附着力),还原真实行驶场景。 - -- 实时障碍物检测:集成虚拟激光雷达/深度相机传感器,实时采集周围环境数据,精准识别障碍物的位置、距离和尺寸信息。 - -- 多模式自主行驶:支持预设路径跟踪行驶和自由探索行驶两种模式,可通过配置文件灵活切换。 - -- 智能自动避让:实现基于规则/强化学习的避障决策算法,支持障碍物绕行、减速避让、紧急停车等多种避让策略。 - -- 场景可定制化:支持自定义场景地图(如直道、弯道、交叉路口)、障碍物类型(如静态障碍物、动态障碍物)和数量,可配置场景复杂度。 - -- 数据可视化与日志记录:实时可视化无人车行驶状态、传感器数据和避障决策过程,支持记录行驶轨迹、障碍物信息等数据用于后续分析。 - -- 算法模块化设计:感知、决策、控制模块解耦,支持快速替换不同的避障算法(如A*、DWA、强化学习模型)进行对比验证。 - -三、环境要求 - -3.1 软件环境 - -- 操作系统:Windows 10/11 64位、Ubuntu 20.04/22.04 64位 - -- MuJoCo版本:2.3.7 及以上(需获取官方许可证,学生可申请免费许可证) - -- 编程语言:Python 3.8 - 3.11 - -- - -核心依赖库:mujoco(MuJoCo仿真引擎Python绑定)、numpy(数值计算与数据处理)、matplotlib(数据可视化与结果绘图)、pyyaml(配置文件解析)、opencv-python(图像数据处理,用于传感器数据可视化)、torch(可选,用于强化学习模型训练与推理) - -3.2 硬件环境 - -- CPU:Intel Core i5 及以上(推荐多核处理器,提升仿真效率) - -- GPU:NVIDIA GeForce GTX 1050 及以上(可选,支持GPU加速仿真,提升复杂场景运行速度) - -- 内存:8GB 及以上(复杂场景建议16GB) - -- 磁盘空间:10GB 及以上(用于存储仿真模型、场景文件、日志数据) - -3.3 环境搭建步骤 - -1. 安装Python 3.8 - 3.11,配置Python环境变量。 - -2. 获取MuJoCo许可证,下载对应版本的MuJoCo引擎,解压至指定目录(如Windows:C:\Users\用户名\.mujoco,Ubuntu:~/.mujoco)。 - -3. - -设置MuJoCo环境变量:Windows系统需添加系统变量指定MuJoCo解压目录路径和许可证文件路径;Ubuntu系统需在终端执行相关命令配置环境变量,可写入.bashrc文件实现永久生效 - -安装依赖库:通过Python包管理工具安装所需核心依赖库,若需使用强化学习功能,额外安装对应的强化学习相关依赖库 - -验证环境:运行简单的Python命令导入mujoco库,无报错则说明环境搭建完成 - -四、项目结构 - -五、核心模块说明 - -5.1 核心模块功能说明 - -5.1.1 仿真环境模块(env/vehicle_env.py) - -仿真环境模块:核心功能为封装MuJoCo仿真引擎的核心交互逻辑,包括场景模型加载、无人车初始化、物理状态更新等。模块会根据配置文件加载指定的仿真场景,初始化无人车的转向、驱动等关键关节参数,并提供环境重置、动作执行等接口,实现仿真过程的循环推进。 - -5.1.2 传感器模块(sensor/lidar.py) - -传感器模块:核心功能为模拟实现障碍物检测所需的传感设备,以虚拟激光雷达为核心。模块会根据配置的安装位置、探测范围等参数,实时采集周围环境数据,通过射线检测原理计算障碍物距离,并将处理后的传感数据提供给决策模块使用,为避障判断提供数据支撑。 - -5.1.3 决策模块(policy/rule_based_policy.py) - -决策模块:核心功能为根据无人车状态和传感器数据生成行驶控制指令。以基于规则的决策逻辑为例,模块会划分激光雷达探测区域,判断前向及两侧是否存在障碍物,结合预设的距离阈值、安全速度、转向角度等参数,决策生成直行、转向避让、减速等控制指令,保障行驶安全。 - -5.1.4 项目入口(main.py) - -项目入口模块:核心功能为调度各模块协同运行,启动仿真流程。模块会加载各类配置文件,初始化仿真环境、决策策略和数据日志工具,通过循环调用环境重置、决策指令生成、动作执行、数据记录等接口,推动仿真过程持续进行,直至达到最大仿真步数或触发结束条件(碰撞、到达终点)。 - -5.2 配置文件说明 - -配置文件说明:系统参数可通过配置目录下的YAML文件灵活配置,无需修改核心逻辑。配置内容涵盖仿真场景路径、仿真步长、最大仿真步数等环境参数,激光雷达安装位置、探测范围等传感器参数,无人车关节ID等车辆参数,以及避障阈值、安全速度等决策参数,方便用户快速调整仿真条件。 - -六、模型说明 - -6.1 MuJoCo仿真模型(XML) - -无人车和场景的物理模型通过MuJoCo的XML文件定义,包含几何体(geom)、关节(joint)、传感器(sensor)等元素。XML文件位于env/scenes/目录,可通过utils/xml_builder.py工具生成自定义场景,或手动编写。 - -核心模型元素说明: - -- 无人车模型:由车身(geom类型为box)、车轮(geom类型为cylinder)组成,通过关节连接(转向关节为hinge,驱动关节为motor),定义了质量、惯性、摩擦系数等物理参数。 - -- 场景模型:包含地面(geom类型为plane)、障碍物(geom类型为box/cylinder,可设置静态/动态)、目标点(site类型,用于路径跟踪)。 - -- 传感器定义:可在XML中直接定义MuJoCo原生传感器(如force、torque传感器),与自定义的激光雷达传感器配合使用。 - -6.2 决策模型 - -本系统支持两种决策模型,可通过修改main.py中的政策初始化代码切换: - -- 基于规则的模型(默认):无需训练,通过预设规则(如前向障碍物距离阈值、左右避让空间判断)生成决策,适用于快速验证避障逻辑,代码位于policy/rule_based_policy.py。 - -- 强化学习模型(可选):基于PyTorch实现,以无人车的状态(位置、速度、激光雷达数据)为输入,输出转向角和驱动力矩,通过强化学习训练优化避障策略。训练代码可参考policy/rl_policy.py,训练数据和模型参数保存在models/目录。 - -6.3 模型训练(强化学习可选) - -若使用强化学习决策模型,需先进行训练: - -1. 修改main.py,初始化RLPolicy而非RuleBasedPolicy。 - -2. 配置policy_config.yaml中的训练参数(学习率、折扣因子、训练轮数等)。 - -3. 运行训练脚本:python policy/train_rl.py。 - -4. 训练完成后,模型参数保存在models/rl_policy.pth,可直接用于仿真推理。 - -七、数据输出 - -仿真过程中会记录关键数据,用于后续分析和算法优化: - -7.1 输出数据类型 - -- 无人车状态数据:位置(x,y,z)、速度、姿态(四元数)、转向角、驱动力矩。 - -- 传感器数据:激光雷达距离数据(数组)、相机图像数据(可选)。 - -- 决策数据:每步的动作指令、避障决策标签(如“直行”“左转避让”“紧急停车”)。 - -- 仿真状态数据:步数、时间、是否碰撞、是否到达终点。 - -7.2 输出格式与存储 - -- 实时可视化:通过utils/visualization.py实时显示无人车行驶轨迹、激光雷达探测范围、障碍物位置。 - -- 日志文件:仿真结束后,数据以CSV格式保存至logs/目录,文件名为simulation_YYYYMMDD_HHMMSS.csv。 - -- 图像输出:可选保存激光雷达点云图、行驶轨迹图至logs/figures/目录(需在配置文件中开启)。 - -7.3 数据可视化方法 - -数据可视化方法:可通过系统内置的可视化工具实现日志数据的可视化展示,支持绘制无人车行驶轨迹、激光雷达探测范围等关键信息,帮助用户直观分析仿真过程和系统性能,可视化结果可按需保存为图像文件。 - -八、扩展建议 - -本项目架构模块化,支持多种扩展方向,可根据需求进一步优化: - -- 多智能体交互:扩展env/vehicle_env.py,支持多个无人车同时行驶,模拟交通流场景,验证多车协同避障算法。 - -- 复杂场景建模:添加雨天、雪地、崎岖地形等场景的物理参数(如地面摩擦系数调整),通过utils/xml_builder.py生成复杂城市道路场景(含交通灯、斑马线)。 - -- 高级算法集成:集成路径规划算法(如A*、DWA、RRT*)到决策模块,实现“全局路径规划+局部避障”的双层控制;或集成深度学习模型(如PointPillars)用于障碍物检测。 - -- 传感器融合:添加虚拟相机、毫米波雷达等传感器,通过卡尔曼滤波、贝叶斯估计等算法融合多传感器数据,提升障碍物检测的准确性和鲁棒性。 - -- 真实硬件迁移:将仿真中验证有效的算法适配到真实无人车硬件(如STM32、ROS-based平台),实现“仿真验证-硬件部署”的快速迭代。 - -- 性能优化:通过MuJoCo的GPU加速功能(需配置CUDA)提升复杂场景的仿真速度;优化决策算法的计算效率,降低延迟。 - -九、故障排除 - -仿真过程中常见问题及解决方法: - -问题1:MuJoCo环境加载失败,报错“Could not find MuJoCo library” -解决方法:检查MuJoCo环境变量配置的准确性;确认MuJoCo许可证文件路径正确;重新安装MuJoCo的Python绑定库。 - -问题2:无人车无法正常行驶,出现抖动或漂移 -解决方法:调整环境配置中的仿真子步数提升稳定性;检查无人车模型关节阻尼系数,可适当增大以减少抖动;调整驱动力矩相关参数,避免速度过冲。 - -问题3:激光雷达无法检测到障碍物 -解决方法:检查激光雷达安装位置配置,确保探测范围可覆盖障碍物;确认仿真模型中障碍物的碰撞属性参数设置正确,与无人车、激光雷达的碰撞属性匹配;适当增大激光雷达最大探测距离配置。 - -- 问题4:强化学习模型训练收敛慢或不收敛 -解决方法:调整奖励函数设计(增加碰撞惩罚、到达目标奖励);优化状态空间和动作空间的归一化;调整学习率、折扣因子等超参数;增加训练轮数或提升训练数据量。 - - -问题5:仿真过程中出现碰撞但未检测到 -解决方法:检查仿真环境模块中的碰撞检测逻辑,确保正确获取MuJoCo的接触数据;在仿真模型中为无人车和障碍物设置明确的碰撞类型参数,避免碰撞过滤。 - -- 问题6:Python版本兼容问题 -解决方法:确保使用Python 3.8 - 3.11版本(mujoco库对Python 3.12及以上版本可能不兼容);通过虚拟环境(venv/conda)隔离项目依赖。 - - From 8b8856d324105fd04839d7ec832960da6752b3b1 Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Sun, 21 Dec 2025 14:15:20 +0800 Subject: [PATCH 5/6] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E5=BB=BA=E7=AB=8B?= =?UTF-8?q?=E4=BA=86=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 | 332 ++++++++++++++++++ 1 file changed, 332 insertions(+) create mode 100644 src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py new file mode 100644 index 0000000000..9430fb200f --- /dev/null +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py @@ -0,0 +1,332 @@ + +import mujoco +import glfw +import numpy as np +import csv +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): + self.close() + return + 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() + + 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 + 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 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 = [] + + 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 + } + + 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) + return self.get_state() + + def run(self): + state = self.reset() + print("仿真启动(按ESC关闭),正在自动录制GIF...") + print("=" * 50) + + for step in range(MAX_STEPS): + # 1. 决策:根据激光雷达和速度生成控制指令 + steer, torque = self.policy.decide(state['lidar'], state['vel']) + # 2. 执行动作:更新车辆状态 + state = self.step(steer, torque) + # 3. 记录日志 + self.logger.log(step, step * TIME_STEP, state['pos'][0], state['pos'][1], + self.policy.get_speed(state['vel']), steer, torque, state['collision']) + # 4. 渲染画面 + self.vis.render() + + # -------------------------- 录制当前帧 -------------------------- + 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}") + # ------------------------------------------------------------------- + + # 打印进度(每100步) + if step % 100 == 0: + print( + f"Step: {step:4d} | 速度: {self.policy.get_speed(state['vel']):.2f}m/s | 碰撞: {state['collision']}") + + # 仿真结束条件 + if state['collision'] or state['pos'][0] >= 15.0 or step >= MAX_STEPS - 1: + end_msg = "碰撞" if state['collision'] else "到达目标" if state['pos'][0] >= 15 else "超时" + print(f"\n仿真结束:{end_msg}(Step: {step})") + break + + # -------------------------- 结束录制并保存 -------------------------- + try: + if self.frames: + # 使用imageio v3 API保存GIF,调整duration参数延长播放时间 + imageio.v3.imwrite('simulation.gif', self.frames, duration=200, loop=0) + print("GIF动图已保存至:simulation.gif(项目根目录)") + else: + print("没有帧数据可保存为GIF") + except Exception as e: + print(f"GIF保存失败: {e}") + # ------------------------------------------------------------------- + + # 关闭可视化窗口 + self.vis.close() + # 打印保存路径 + print(f"日志已保存至:{self.logger.log_path}") + + +# -------------------------- 主函数 -------------------------- +if __name__ == "__main__": + try: + env = VehicleEnv() + env.run() + except Exception as e: + print(f"错误:{str(e)}") + try: + # 异常时也关闭录制器和窗口,避免文件损坏 + env.vis.close() + except: + pass \ No newline at end of file From adca0f4d5e3ffe9d5dec183ea1124c071eea89aa Mon Sep 17 00:00:00 2001 From: qeo <3786759407@qq.com> Date: Sun, 21 Dec 2025 14:58:49 +0800 Subject: [PATCH 6/6] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E5=BB=BA=E7=AB=8B?= =?UTF-8?q?=E4=BA=86=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 | 154 ++++++++++-------- 1 file changed, 84 insertions(+), 70 deletions(-) diff --git a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py index 9430fb200f..f984626fa9 100644 --- a/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py +++ b/src/autonomousvehicle_movement_and_automatic_obstacle_avoidance/qwer.py @@ -1,4 +1,3 @@ - import mujoco import glfw import numpy as np @@ -8,7 +7,7 @@ import imageio from typing import List -# -------------------------- 全局配置(已优化避障参数)-------------------------- +# -------------------------- 全局配置 -------------------------- TIME_STEP = 0.01 MAX_STEPS = 2000 NSTEP = 5 @@ -18,20 +17,20 @@ 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 # 提高加速响应 +# 无人车参数 (恢复到合理数值) +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(障碍物后移,增加反应时间)-------------------------- +# -------------------------- MuJoCo XML 场景定义 -------------------------- SCENE_XML = """ @@ -74,7 +73,7 @@ - + @@ -83,7 +82,7 @@ """ -# -------------------------- 激光雷达(已匹配mj_ray参数)-------------------------- +# -------------------------- 激光雷达传感器 -------------------------- class LidarSensor: def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): self.model = model @@ -143,7 +142,7 @@ def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): self.data = data if not glfw.init(): raise RuntimeError("Failed to initialize GLFW") - self.window = glfw.create_window(1280, 720, "无人车仿真", None, None) + 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") @@ -164,20 +163,23 @@ def __init__(self, model: mujoco.MjModel, data: mujoco.MjData): def render(self): if glfw.window_should_close(self.window): - self.close() - return + 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" @@ -194,7 +196,7 @@ def log(self, step, time, x, y, speed, steer, torque, collision): 1 if collision else 0]) -# -------------------------- 决策算法类 -------------------------- +# -------------------------- 控制策略类 -------------------------- class RulePolicy: def __init__(self): self.front_rays = slice(350, 360) @@ -225,7 +227,7 @@ def decide(self, lidar_data, vel): return steer, torque -# -------------------------- 仿真环境类(已整合自动录制功能)-------------------------- +# -------------------------- 仿真环境类 -------------------------- class VehicleEnv: def __init__(self): self.model = mujoco.MjModel.from_xml_string(SCENE_XML) @@ -237,6 +239,8 @@ def __init__(self): # 初始化帧缓存用于GIF录制 self.frames = [] + self.frame_count = 0 + self.max_frames = 1000 # 最大录制帧数,防止内存溢出 def reset(self): mujoco.mj_resetData(self.model, self.data) @@ -261,61 +265,71 @@ def step(self, steer, torque): def run(self): state = self.reset() - print("仿真启动(按ESC关闭),正在自动录制GIF...") + print("Starting simulation (Press ESC to close and save GIF)...") print("=" * 50) - for step in range(MAX_STEPS): - # 1. 决策:根据激光雷达和速度生成控制指令 - steer, torque = self.policy.decide(state['lidar'], state['vel']) - # 2. 执行动作:更新车辆状态 - state = self.step(steer, torque) - # 3. 记录日志 - self.logger.log(step, step * TIME_STEP, state['pos'][0], state['pos'][1], - self.policy.get_speed(state['vel']), steer, torque, state['collision']) - # 4. 渲染画面 - self.vis.render() - - # -------------------------- 录制当前帧 -------------------------- + 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...") + break + + finally: + # -------------------------- 结束录制并保存 -------------------------- 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) + 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"录制帧时出错: {e}") + print(f"GIF saving failed: {e}") # ------------------------------------------------------------------- - # 打印进度(每100步) - if step % 100 == 0: - print( - f"Step: {step:4d} | 速度: {self.policy.get_speed(state['vel']):.2f}m/s | 碰撞: {state['collision']}") - - # 仿真结束条件 - if state['collision'] or state['pos'][0] >= 15.0 or step >= MAX_STEPS - 1: - end_msg = "碰撞" if state['collision'] else "到达目标" if state['pos'][0] >= 15 else "超时" - print(f"\n仿真结束:{end_msg}(Step: {step})") - break - - # -------------------------- 结束录制并保存 -------------------------- - try: - if self.frames: - # 使用imageio v3 API保存GIF,调整duration参数延长播放时间 - imageio.v3.imwrite('simulation.gif', self.frames, duration=200, loop=0) - print("GIF动图已保存至:simulation.gif(项目根目录)") - else: - print("没有帧数据可保存为GIF") - except Exception as e: - print(f"GIF保存失败: {e}") - # ------------------------------------------------------------------- - - # 关闭可视化窗口 - self.vis.close() - # 打印保存路径 - print(f"日志已保存至:{self.logger.log_path}") + # 关闭可视化窗口 + self.vis.close() + # 打印保存路径 + print(f"Log saved to: {self.logger.log_path}") + print(f"Total simulation steps: {step}") # -------------------------- 主函数 -------------------------- @@ -324,9 +338,9 @@ def run(self): env = VehicleEnv() env.run() except Exception as e: - print(f"错误:{str(e)}") + print(f"Error: {str(e)}") try: # 异常时也关闭录制器和窗口,避免文件损坏 env.vis.close() except: - pass \ No newline at end of file + pass