From 68a2e4063b1a125c63d58c90c3911bdb0fac9000 Mon Sep 17 00:00:00 2001 From: ZKX34 <3442977470@qq.com> Date: Fri, 26 Dec 2025 16:25:08 +0800 Subject: [PATCH] =?UTF-8?q?=E8=AE=A9=E6=97=A0=E4=BA=BA=E6=9C=BA=E6=82=AC?= =?UTF-8?q?=E7=A9=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drone_simulation/main.py | 534 +++++++++++++++++++++++++++------ src/drone_simulation/model.xml | 285 ++++++++---------- 2 files changed, 566 insertions(+), 253 deletions(-) diff --git a/src/drone_simulation/main.py b/src/drone_simulation/main.py index 78f334b53d..aba239f85f 100644 --- a/src/drone_simulation/main.py +++ b/src/drone_simulation/main.py @@ -1,112 +1,468 @@ -# simple_visible_drone.py +""" +MuJoCo 四旋翼无人机仿真 - 默认设置版本 +直接运行,无需用户选择 +""" + import mujoco import mujoco.viewer import numpy as np import time -import os -import glob - -# 定义日志文件路径(可根据实际情况修改) -LOG_FILE_PATHS = [ - "mujoco.log", - "drone_simulation.log", - "./logs/*.log" -] - - -def delete_log_files(): - """删除指定的日志文件(无控制台输出)""" - deleted_count = 0 - for path in LOG_FILE_PATHS: - if "*" in path: - for file in glob.glob(path): - try: - os.remove(file) - deleted_count += 1 - except (FileNotFoundError, PermissionError): - continue - else: - if os.path.exists(path): - try: - os.remove(path) - deleted_count += 1 - except (FileNotFoundError, PermissionError): - continue +import math + + +class QuadrotorSimulation: + def __init__(self): + """初始化四旋翼无人机仿真""" + # 使用简化的XML字符串,避免纹理问题 + xml_string = self.create_minimal_quadrotor_xml() + + # 从XML字符串加载模型 + self.model = mujoco.MjModel.from_xml_string(xml_string) + print("✓ 模型加载成功") + + # 创建仿真数据 + self.data = mujoco.MjData(self.model) + + # 获取执行器数量 + self.n_actuators = self.model.nu + print(f"✓ 执行器数量: {self.n_actuators}") + + # 设置初始控制输入 + self.set_initial_control() + + def create_minimal_quadrotor_xml(self): + """创建最简单的四旋翼无人机XML配置""" + xml_string = """ + + + -# 最小化但可见的模型 -MJCF_MODEL = """ - - - - + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - -""" + + + + + + + + -def main(): - # 删除日志文件(无输出) - delete_log_files() +""" + return xml_string + + def set_initial_control(self): + """设置初始控制输入""" + # 设置初始推力 + hover_thrust = 500 # 悬停推力值 + self.data.ctrl[:] = [hover_thrust] * self.n_actuators + + def get_state(self): + """获取无人机状态""" + state = { + 'position': self.data.qpos[0:3].copy(), + 'orientation': self.data.qpos[3:7].copy(), + 'linear_velocity': self.data.qvel[0:3].copy(), + 'angular_velocity': self.data.qvel[3:6].copy(), + 'rotor_angles': self.data.qpos[7:11].copy(), + 'rotor_velocities': self.data.qvel[6:10].copy() + } + return state + + def print_state(self): + """打印无人机状态""" + state = self.get_state() + + print("\n" + "=" * 50) + print("四旋翼无人机状态:") + print("=" * 50) + print(f"位置: [{state['position'][0]:.3f}, {state['position'][1]:.3f}, {state['position'][2]:.3f}] m") + print(f"姿态四元数: [{state['orientation'][0]:.3f}, {state['orientation'][1]:.3f}, " + f"{state['orientation'][2]:.3f}, {state['orientation'][3]:.3f}]") + print(f"线速度: [{state['linear_velocity'][0]:.3f}, {state['linear_velocity'][1]:.3f}, " + f"{state['linear_velocity'][2]:.3f}] m/s") + print(f"角速度: [{state['angular_velocity'][0]:.3f}, {state['angular_velocity'][1]:.3f}, " + f"{state['angular_velocity'][2]:.3f}] rad/s") + print("=" * 50) + + def apply_control(self, ctrl_values): + """应用控制输入""" + if len(ctrl_values) != self.n_actuators: + print(f"⚠ 警告:控制值数量应为{self.n_actuators},使用默认值500") + ctrl_values = [500] * self.n_actuators + + # 应用控制值 + self.data.ctrl[:] = ctrl_values + + def altitude_controller(self, target_z=1.5): + """高度控制器""" + # PID参数 + Kp = 200.0 # 比例增益 + Kd = 50.0 # 微分增益 + + # 获取当前状态 + current_z = self.data.qpos[2] + current_vz = self.data.qvel[2] + + # 计算误差 + error_z = target_z - current_z + error_vz = 0 - current_vz + + # PID控制 + control_input = Kp * error_z + Kd * error_vz + + # 基础推力 + base_thrust = 500 + + # 计算推力 + thrust = base_thrust + control_input - # 加载模型和数据 - model = mujoco.MjModel.from_xml_string(MJCF_MODEL) - data = mujoco.MjData(model) + # 限制推力范围 + thrust = np.clip(thrust, 400, 600) - # 等待3秒(无提示) - time.sleep(3) + # 应用到所有电机 + ctrl_values = [thrust] * self.n_actuators + self.apply_control(ctrl_values) - with mujoco.viewer.launch_passive(model, data) as viewer: - # 设置相机视角 - viewer.cam.lookat[:] = [0, 0, 1] - viewer.cam.distance = 8.0 - viewer.cam.azimuth = 45 - viewer.cam.elevation = -30 + return error_z, thrust + + def position_controller(self, target_pos=[0, 0, 1.5]): + """位置控制器""" + # PID参数 + Kp_pos = np.array([100.0, 100.0, 200.0]) + Kd_pos = np.array([30.0, 30.0, 50.0]) + + # 获取当前状态 + current_pos = self.data.qpos[0:3] + current_vel = self.data.qvel[0:3] + + # 计算误差 + pos_error = np.array(target_pos) - current_pos + vel_error = -current_vel + + # 位置控制 + pos_control = Kp_pos * pos_error + Kd_pos * vel_error + + # 基础推力 + base_thrust = 500 + + # 总推力 + total_thrust = base_thrust + pos_control[2] + + # 姿态控制 + roll_control = -pos_control[1] * 0.02 + pitch_control = pos_control[0] * 0.02 + + # 四旋翼混控 + ctrl_values = [ + total_thrust - pitch_control - roll_control, # 前右 + total_thrust - pitch_control + roll_control, # 前左 + total_thrust + pitch_control + roll_control, # 后左 + total_thrust + pitch_control - roll_control # 后右 + ] + + # 限制推力范围 + ctrl_values = np.clip(ctrl_values, 400, 600) + + self.apply_control(ctrl_values) + + return pos_error, ctrl_values + + def run_simulation(self, duration=10.0, use_viewer=True, controller_type="altitude"): + """运行仿真""" + print(f"\n▶ 开始仿真,时长: {duration}秒") + print(f"▶ 控制器类型: {controller_type}") + + if use_viewer: + print("▶ 使用可视化查看器 (按ESC退出)") + else: + print("▶ 无可视化模式") + + # 记录数据 + time_history = [] + height_history = [] + thrust_history = [] + + try: + if use_viewer: + with mujoco.viewer.launch_passive(self.model, self.data) as viewer: + # 设置相机 + viewer.cam.azimuth = 180 + viewer.cam.elevation = -20 + viewer.cam.distance = 5.0 + viewer.cam.lookat[:] = [0.0, 0.0, 1.0] + + self.simulation_loop(viewer, duration, controller_type, + time_history, height_history, thrust_history) + else: + self.simulation_loop(None, duration, controller_type, + time_history, height_history, thrust_history) + + except Exception as e: + print(f"⚠ 仿真错误: {e}") + + # 分析数据 + if time_history: + self.analyze_data(time_history, height_history, thrust_history) + + def simulation_loop(self, viewer, duration, controller_type, + time_history, height_history, thrust_history): + """仿真循环""" + start_time = time.time() + last_print_time = time.time() + step_count = 0 + + while (viewer is None or (viewer and viewer.is_running())) and (time.time() - start_time) < duration: + step_start = time.time() + step_count += 1 + + # 应用控制器 + if controller_type == "position": + # 移动目标点 + t = self.data.time + target_x = 1.0 * math.sin(t * 0.5) + target_y = 1.0 * math.cos(t * 0.5) + target_z = 1.5 + 0.3 * math.sin(t * 0.3) + + pos_error, thrusts = self.position_controller([target_x, target_y, target_z]) + control_info = f"位置误差: [{pos_error[0]:.2f}, {pos_error[1]:.2f}, {pos_error[2]:.2f}] m" + else: + error_z, thrust = self.altitude_controller(1.5) + thrusts = [thrust] * 4 + control_info = f"高度误差: {error_z:.2f} m" + + # 记录数据 + current_time = self.data.time + current_height = self.data.qpos[2] + time_history.append(current_time) + height_history.append(current_height) + thrust_history.append(np.mean(thrusts)) + + # 执行仿真步 + mujoco.mj_step(self.model, self.data) + + # 更新螺旋桨旋转(视觉效果) + rotor_speed = 80.0 + for i in range(4): + self.data.qpos[7 + i] += rotor_speed * self.model.opt.timestep + + # 更新查看器 + if viewer: + viewer.sync() + + # 打印状态信息 + if time.time() - last_print_time > 1.0: + print(f"\n时间: {current_time:.1f}s | 高度: {current_height:.2f}m") + print(f"推力: {np.mean(thrusts):.0f} | {control_info}") + print(f"步数: {step_count}") + last_print_time = time.time() + + # 控制仿真速度 + elapsed = time.time() - step_start + sleep_time = self.model.opt.timestep - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + def analyze_data(self, time_data, height_data, thrust_data): + """分析仿真数据""" + print("\n" + "=" * 50) + print("📊 仿真数据分析:") + print("=" * 50) + + if not time_data: + print("无数据") + return + + time_array = np.array(time_data) + height_array = np.array(height_data) + thrust_array = np.array(thrust_data) + + print(f"总步数: {len(time_array)}") + print(f"仿真时长: {time_array[-1]:.2f} 秒") + print(f"平均高度: {np.mean(height_array):.3f} m") + print(f"高度稳定性: ±{np.std(height_array):.3f} m") + print(f"高度范围: [{np.min(height_array):.3f}, {np.max(height_array):.3f}] m") + print(f"平均推力: {np.mean(thrust_array):.0f}") + print(f"推力范围: [{np.min(thrust_array):.0f}, {np.max(thrust_array):.0f}]") + + # 询问是否绘图 + try: + plot = input("\n是否绘制图表? (y/n): ").strip().lower() + if plot == 'y': + self.plot_results(time_array, height_array, thrust_array) + except: + pass + + def plot_results(self, time_data, height_data, thrust_data): + """绘制结果图表""" + try: + import matplotlib.pyplot as plt + + fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8)) + + # 高度图 + ax1.plot(time_data, height_data, 'b-', linewidth=2, label='实际高度') + ax1.axhline(y=1.5, color='r', linestyle='--', alpha=0.7, label='目标高度') + ax1.fill_between(time_data, 1.45, 1.55, color='r', alpha=0.1) + ax1.set_xlabel('时间 (秒)') + ax1.set_ylabel('高度 (米)') + ax1.set_title('四旋翼无人机高度控制') + ax1.legend() + ax1.grid(True, alpha=0.3) + + # 推力图 + ax2.plot(time_data, thrust_data, 'g-', linewidth=2, label='平均推力') + ax2.axhline(y=500, color='orange', linestyle='--', alpha=0.7, label='悬停推力') + ax2.set_xlabel('时间 (秒)') + ax2.set_ylabel('推力') + ax2.set_title('电机推力变化') + ax2.legend() + ax2.grid(True, alpha=0.3) + + plt.tight_layout() + plt.show() + + except ImportError: + print("⚠ 需要安装matplotlib: pip install matplotlib") + except Exception as e: + print(f"⚠ 绘图错误: {e}") + + +def main(): + """主函数 - 使用默认设置""" + print("🚁 MuJoCo 四旋翼无人机仿真系统") + print("=" * 50) - t = 0 - while viewer.is_running() and t < 20: - # 简单的上下浮动 - force_z = 20 * np.sin(t * 2) + 50 + try: + # 创建仿真实例 + print("正在初始化...") + sim = QuadrotorSimulation() + print("✅ 初始化完成") - # 应用力 - data.qfrc_applied[2] = force_z + # 使用默认设置 + controller_type = "position" # 默认使用位置控制器 + duration = 15.0 # 默认仿真15秒 + use_viewer = True # 默认使用可视化 - # 缓慢旋转 - data.qfrc_applied[5] = 5 * np.sin(t * 0.5) + print(f"\n📋 默认设置:") + print(f" 控制器类型: {controller_type}") + print(f" 仿真时长: {duration}秒") + print(f" 可视化: {'是' if use_viewer else '否'}") - mujoco.mj_step(model, data) - viewer.sync() + # 运行仿真 + sim.run_simulation( + duration=duration, + use_viewer=use_viewer, + controller_type=controller_type + ) - t += 0.01 - time.sleep(0.01) + except KeyboardInterrupt: + print("\n\n⏹ 仿真被用户中断") + except Exception as e: + print(f"\n❌ 错误: {e}") + import traceback + traceback.print_exc() if __name__ == "__main__": - main() + # 直接运行,无需用户输入 + main() \ No newline at end of file diff --git a/src/drone_simulation/model.xml b/src/drone_simulation/model.xml index 717a287e45..f101668975 100644 --- a/src/drone_simulation/model.xml +++ b/src/drone_simulation/model.xml @@ -1,164 +1,121 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file +import mujoco +import mujoco.viewer as viewer +import os +import numpy as np + +class QuadrotorSimulator: + def __init__(self, model_path): + # 加载MuJoCo模型和数据 + self.model = mujoco.MjModel.from_xml_path(model_path) + self.data = mujoco.MjData(self.model) + + # 获取电机控制索引 + self.motor_ids = { + "front_right": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "motor_front_right"), + "front_left": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "motor_front_left"), + "back_left": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "motor_back_left"), + "back_right": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "motor_back_right") + } + + # 初始悬停推力 + self.base_thrust = 500.0 + + def set_motor_thrust(self, front_right=None, front_left=None, back_left=None, back_right=None): + """设置四个电机的推力""" + if front_right is not None: + self.data.ctrl[self.motor_ids["front_right"]] = front_right + if front_left is not None: + self.data.ctrl[self.motor_ids["front_left"]] = front_left + if back_left is not None: + self.data.ctrl[self.motor_ids["back_left"]] = back_left + if back_right is not None: + self.data.ctrl[self.motor_ids["back_right"]] = back_right + + def get_sensor_data(self): + """读取传感器数据""" + sensor_data = { + "position": self.data.sensor("quad_position").data.copy(), + "orientation": self.data.sensor("quad_orientation").data.copy(), + "linear_velocity": self.data.sensor("quad_linear_velocity").data.copy(), + "angular_velocity": self.data.sensor("quad_angular_velocity").data.copy(), + "linear_acceleration": self.data.sensor("quad_linear_acceleration").data.copy(), + "rotor_speeds": [ + self.data.sensor("rotor_speed_front_right").data[0], + self.data.sensor("rotor_speed_front_left").data[0], + self.data.sensor("rotor_speed_back_left").data[0], + self.data.sensor("rotor_speed_back_right").data[0] + ], + "touch": self.data.sensor("touch_sensor").data[0] + } + return sensor_data + + def run_simulation(self): + """启动仿真可视化""" + # 初始化电机推力 + self.set_motor_thrust( + front_right=self.base_thrust, + front_left=self.base_thrust, + back_left=self.base_thrust, + back_right=self.base_thrust + ) + + with viewer.launch_passive(self.model, self.data) as v: + print("仿真启动成功!") + print("操作说明:↑=上升 ↓=下降 ←=左偏航 →=右偏航 q=退出") + + step = 0 + while v.is_running(): + step_start = mujoco.mj_time(self.model) + + # 每100步打印传感器数据 + if step % 100 == 0: + sensor_data = self.get_sensor_data() + print(f"\n步骤 {step} | 位置: {np.round(sensor_data['position'], 3)}") + + # 键盘控制 + key_events = v.user_input() + thrust_delta = 5.0 + for event in key_events: + if event.key == viewer.KEY.UP and event.down: + self.base_thrust += thrust_delta + self.set_motor_thrust(self.base_thrust, self.base_thrust, self.base_thrust, self.base_thrust) + elif event.key == viewer.KEY.DOWN and event.down: + self.base_thrust = max(0, self.base_thrust - thrust_delta) + self.set_motor_thrust(self.base_thrust, self.base_thrust, self.base_thrust, self.base_thrust) + elif event.key == viewer.KEY.LEFT and event.down: + self.set_motor_thrust( + self.base_thrust+thrust_delta, self.base_thrust-thrust_delta, + self.base_thrust+thrust_delta, self.base_thrust-thrust_delta + ) + elif event.key == viewer.KEY.RIGHT and event.down: + self.set_motor_thrust( + self.base_thrust-thrust_delta, self.base_thrust+thrust_delta, + self.base_thrust-thrust_delta, self.base_thrust+thrust_delta + ) + elif event.key == viewer.KEY.Q and event.down: + v.close() + break + + # 执行仿真步 + mujoco.mj_step(self.model, self.data) + + # 控制60FPS帧率 + while mujoco.mj_time(self.model) - step_start < 1/60: + pass + + v.sync() + step += 1 + +if __name__ == "__main__": + # 拼接XML模型文件路径(和当前Python文件同目录) + current_dir = os.path.dirname(os.path.abspath(__file__)) + model_path = os.path.join(current_dir, "quadrotor.xml") + + # 检查模型文件是否存在 + if not os.path.exists(model_path): + print(f"错误:未找到模型文件 {model_path}") + print("请确保 quadrotor.xml 文件和 mainone.py 在同一目录下!") + else: + # 启动仿真 + simulator = QuadrotorSimulator(model_path) + simulator.run_simulation() \ No newline at end of file