From 3acd26cce6c10a080c4eda544131490bcc56f970 Mon Sep 17 00:00:00 2001 From: explorer1009 <2350619772@qq.com> Date: Thu, 18 Dec 2025 19:56:26 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E6=94=B9=E8=BF=9B=E8=AE=AD=E7=BB=83?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/airsim_control/train.py | 71 +++++++++++++++++++++++++++++++------ 1 file changed, 60 insertions(+), 11 deletions(-) diff --git a/src/airsim_control/train.py b/src/airsim_control/train.py index 4b0a7322ce..2cf132a69d 100644 --- a/src/airsim_control/train.py +++ b/src/airsim_control/train.py @@ -1,8 +1,12 @@ import glob import os +import torch as th from stable_baselines3 import PPO from stable_baselines3.common.callbacks import CheckpointCallback +from stable_baselines3.common.utils import set_random_seed +from stable_baselines3.common.vec_env import DummyVecEnv, VecMonitor from custom_env import AirSimMazeEnv +from typing import Callable # === 路径配置 === MODELS_DIR = r"D:\Others\MyAirsimprojects\models" @@ -12,6 +16,21 @@ os.makedirs(LOG_DIR, exist_ok=True) +# === 优化 1: 线性学习率调度器 === +def linear_schedule(initial_value: float) -> Callable[[float], float]: + """ + 线性学习率调度函数。 + :param initial_value: 初始学习率 + :return: 当前步数的学习率 + """ + + def func(progress_remaining: float) -> float: + # progress_remaining 会从 1.0 (开始) 线性减少到 0.0 (结束) + return progress_remaining * initial_value + + return func + + def get_latest_model_path(path_dir): list_of_files = glob.glob(os.path.join(path_dir, '*.zip')) if not list_of_files: @@ -20,41 +39,71 @@ def get_latest_model_path(path_dir): def main(): - env = AirSimMazeEnv() + set_random_seed(42) # 固定随机种子,方便复现 + + # 实例化环境 + # 使用 DummyVecEnv 包装,虽然只有一个环境,但这是标准做法,方便未来扩展 + env = DummyVecEnv([lambda: AirSimMazeEnv()]) + env = VecMonitor(env) # 添加 Monitor 以记录详细日志 + latest_model_path = get_latest_model_path(MODELS_DIR) + # === 优化 2: 自定义网络架构 === + # net_arch=[256, 256]: 使用两个 256 神经元的隐藏层 (比默认的 64 强大很多) + # activation_fn: 使用 Tanh 或 ReLU + policy_kwargs = dict( + activation_fn=th.nn.Tanh, + net_arch=dict(pi=[256, 256], vf=[256, 256]) + ) + if latest_model_path: print(f"--- 发现存档: {latest_model_path},继续训练 ---") + # 加载旧模型 + # 注意:加载旧模型时,learning_rate 会被覆盖为旧模型的设置 + # 如果你想在旧模型上强制使用新参数,需要手动修改 model.learning_rate model = PPO.load(latest_model_path, env=env, tensorboard_log=LOG_DIR) + + # 强制更新学习率策略 (可选,如果你想让旧模型也享受线性衰减) + # model.lr_schedule = linear_schedule(0.0003) + reset_timesteps = False else: - print(f"--- 未发现存档,开始【从头训练】 ---") + print(f"--- 未发现存档,开始【从头训练】(优化版) ---") model = PPO( "MultiInputPolicy", env, verbose=1, tensorboard_log=LOG_DIR, - learning_rate=0.0003, - batch_size=256, # 大Batch加速 - n_steps=2048, - gamma=0.99 + + # --- 核心超参数优化 --- + learning_rate=linear_schedule(0.0003), # 动态学习率 + batch_size=256, # 大批量,更稳 + n_steps=2048, # 每次更新前的采样步数 + gamma=0.99, # 折扣因子 (长远利益) + gae_lambda=0.95, # 优势估计 + clip_range=0.2, # PPO 修剪范围 + ent_coef=0.01, # 优化 3: 熵系数,鼓励探索,防止过早死板 + policy_kwargs=policy_kwargs, # 应用更大的网络 + device="auto" # 自动使用 GPU ) reset_timesteps = True checkpoint_callback = CheckpointCallback( - save_freq=10000, + save_freq=20000, # 每 2万步保存一次,不用太频繁 save_path=MODELS_DIR, - name_prefix='drone_maze' + name_prefix='drone_maze_opt' # 改个名字区分一下 ) - print("🚀 训练引擎启动...") + print("🚀 优化版训练引擎启动...") + print("配置: Linear LR, Net=[256,256], Ent=0.01") + model.learn( - total_timesteps=500000, + total_timesteps=1000000, # 建议跑 100万步 callback=checkpoint_callback, reset_num_timesteps=reset_timesteps ) - model.save(os.path.join(MODELS_DIR, "drone_maze_final")) + model.save(os.path.join(MODELS_DIR, "drone_maze_final_opt")) print("训练结束。") From 7e9870bea296498c82a94ddb9f7d23eede289231 Mon Sep 17 00:00:00 2001 From: explorer1009 <2350619772@qq.com> Date: Thu, 18 Dec 2025 23:25:34 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E7=BB=99=E6=97=A0=E4=BA=BA=E6=9C=BA?= =?UTF-8?q?=E6=B5=8B=E8=AF=95=E8=84=9A=E6=9C=AC=E6=B7=BB=E5=8A=A0=E5=8F=AF?= =?UTF-8?q?=E8=A7=86=E5=8C=96=E7=AA=97=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/airsim_control/run_inference.py | 215 ++++++++++++++++++++++++---- 1 file changed, 190 insertions(+), 25 deletions(-) diff --git a/src/airsim_control/run_inference.py b/src/airsim_control/run_inference.py index b672b66703..9e45021eb3 100644 --- a/src/airsim_control/run_inference.py +++ b/src/airsim_control/run_inference.py @@ -1,55 +1,220 @@ import glob import os import time +import numpy as np +import cv2 +import csv +from datetime import datetime from stable_baselines3 import PPO from custom_env import AirSimMazeEnv +import math -# === 配置路径 === +# ============================================================================== +# 配置区域 +# ============================================================================== MODELS_DIR = r"D:\Others\MyAirsimprojects\models" +LOG_PATH = r"D:\Others\MyAirsimprojects\inference_logs" # 轨迹保存路径 +os.makedirs(LOG_PATH, exist_ok=True) +# 可视化配置 +SHOW_DASHBOARD = True # 是否显示 OpenCV 仪表盘 +DASHBOARD_SIZE = (800, 400) # 宽, 高 -def get_latest_model_path(path_dir): - list_of_files = glob.glob(os.path.join(path_dir, '*.zip')) - if not list_of_files: - return None - latest_file = max(list_of_files, key=os.path.getctime) - return latest_file +# ============================================================================== +# 辅助函数 +# ============================================================================== -def main(): - # 实例化环境 - env = AirSimMazeEnv() +def get_all_models(path_dir): + """获取所有模型并按时间排序""" + files = glob.glob(os.path.join(path_dir, '*.zip')) + # 按修改时间倒序排列 (最新的在前) + files.sort(key=os.path.getctime, reverse=True) + return files + + +def draw_dashboard(obs, action, reward, step_count, last_info): + """绘制实时仪表盘""" + # 1. 创建黑色背景画布 + canvas = np.zeros((DASHBOARD_SIZE[1], DASHBOARD_SIZE[0], 3), dtype=np.uint8) + + # --- 左侧: 深度摄像头画面 --- + if 'image' in obs: + depth_img = obs['image'] + depth_img = cv2.cvtColor(depth_img, cv2.COLOR_GRAY2BGR) + depth_img = cv2.resize(depth_img, (350, 350), interpolation=cv2.INTER_NEAREST) + h, w, _ = depth_img.shape + canvas[25:25 + h, 25:25 + w] = depth_img + cv2.putText(canvas, "Depth Camera", (25, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + + # --- 右侧: 数据与 Lidar --- + x_start = 400 + y_start = 50 + line_height = 30 + + infos = [ + f"Step: {step_count}", + f"Fwd Vel: {action[0]:.2f} (x5.0 m/s)", + f"Yaw Rate: {action[1]:.2f} (x60 deg/s)", + f"Reward: {reward:.3f}", + f"Status: {last_info}" + ] + + for i, text in enumerate(infos): + color = (0, 255, 0) + if "Reward" in text and reward < 0: + color = (0, 0, 255) + if "Status" in text and "撞墙" in text: + color = (0, 0, 255) - # 寻找最新的模型 - model_path = get_latest_model_path(MODELS_DIR) + # 这里需要注意: OpenCV putText 不支持 emoji,如果 last_info 包含 emoji 可能会显示乱码或问号 + # 但不会报错崩溃。崩溃是在写入 CSV 时发生的。 + cv2.putText(canvas, text, (x_start, y_start + i * line_height), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2) + + # --- 右下角: Lidar 雷达图 --- + lidar_center = (600, 300) + lidar_radius = 80 + cv2.circle(canvas, lidar_center, 2, (0, 255, 255), -1) + cv2.circle(canvas, lidar_center, lidar_radius, (50, 50, 50), 1) + + if 'lidar' in obs: + lidar_data = obs['lidar'] + for i in range(0, 180, 2): + dist = lidar_data[i] + if dist < 20: + angle_deg = -90 + i + angle_rad = math.radians(angle_deg - 90) + r_pixel = (dist / 20.0) * lidar_radius + pt_x = int(lidar_center[0] + r_pixel * math.cos(angle_rad)) + pt_y = int(lidar_center[1] + r_pixel * math.sin(angle_rad)) + pt_color = (0, 255, 0) if dist > 5 else (0, 0, 255) + cv2.circle(canvas, (pt_x, pt_y), 2, pt_color, -1) + + return canvas + + +# ============================================================================== +# 主逻辑 +# ============================================================================== + +def main(): + print("==================================================") + print(" AirSim 无人机智能导航 - 推理与评估系统 ") + print("==================================================") - if not model_path: - print("错误:没有找到任何训练好的模型!请先运行 train.py。") + # 1. 模型选择 + models = get_all_models(MODELS_DIR) + if not models: + print("错误: 未找到任何模型文件!请先运行 train.py。") return - print(f"正在加载模型进行测试: {model_path}") + print(f"发现 {len(models)} 个模型存档。") + print(f"默认加载最新的: {os.path.basename(models[0])}") + model_path = models[0] + + # 2. 加载环境与模型 + print("正在初始化环境...") + env = AirSimMazeEnv() + + print(f"正在加载神经网络: {model_path} ...") model = PPO.load(model_path) - # 开始测试循环 - obs, _ = env.reset() + # 3. 初始化统计数据 + stats = { + "episodes": 0, + "success": 0, + "collision": 0, + "out_of_bounds": 0, + "timeout": 0 + } + + # 4. 创建轨迹日志文件 + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + csv_filename = os.path.join(LOG_PATH, f"trajectory_{timestamp}.csv") + + # 【修复点 1】增加 encoding='utf-8-sig' + with open(csv_filename, 'w', newline='', encoding='utf-8-sig') as f: + writer = csv.writer(f) + writer.writerow(["Episode", "Step", "X", "Y", "Z", "Reward", "DoneType"]) + + print("\n>>> 开始推理 (按 'q' 退出 OpenCV 窗口或 Ctrl+C 停止) <<<\n") - print("开始推理 (按 Ctrl+C 停止)...") try: - while True: - # 预测动作 (deterministic=True 表示不使用随机探索,只用这一刻认为最好的动作) - action, _states = model.predict(obs, deterministic=True) + obs, _ = env.reset() + episode_reward = 0 + step_count = 0 + current_traj = [] + done_reason = "Running" - # 执行动作 + while True: + action, _ = model.predict(obs, deterministic=True) obs, reward, done, truncated, info = env.step(action) + episode_reward += reward + step_count += 1 + + pos = env.client.getMultirotorState().kinematics_estimated.position + current_traj.append([stats['episodes'], step_count, pos.x_val, pos.y_val, pos.z_val, reward]) - # 如果结束了 (撞墙或到达),自动重置 if done: - print("--- 回合结束,重置环境 ---") + stats['episodes'] += 1 + if reward >= 50: + stats['success'] += 1 + done_reason = "✅ SUCCESS" + elif reward <= -50: + stats['collision'] += 1 + done_reason = "❌ COLLISION" + elif reward == -20: + stats['out_of_bounds'] += 1 + done_reason = "⚠️ OUT OF BOUNDS" + else: + stats['timeout'] += 1 + done_reason = "⏳ TIMEOUT/OTHER" + + print( + f"Episode {stats['episodes']} 结束 | 原因: {done_reason} | 总分: {episode_reward:.2f} | 步数: {step_count}") + + # 【修复点 2】增加 encoding='utf-8-sig' + with open(csv_filename, 'a', newline='', encoding='utf-8-sig') as f: + writer = csv.writer(f) + for row in current_traj: + writer.writerow(row + [done_reason]) + obs, _ = env.reset() + episode_reward = 0 + step_count = 0 + current_traj = [] + if SHOW_DASHBOARD: + time.sleep(0.5) + else: + done_reason = "Flying..." + + if SHOW_DASHBOARD: + dashboard = draw_dashboard(obs, action, reward, step_count, done_reason) + cv2.imshow("AirSim AI Dashboard", dashboard) + if cv2.waitKey(1) & 0xFF == ord('q'): + print("用户手动停止测试。") + break except KeyboardInterrupt: - print("停止测试") + print("\n检测到键盘中断,停止测试。") + finally: + print("\n" + "=" * 50) + print(" 测试总结报告 ") + print("=" * 50) + total = stats['episodes'] + if total > 0: + print(f"总回合数: {total}") + print(f"成功次数: {stats['success']} ({stats['success'] / total * 100:.1f}%)") + print(f"撞墙次数: {stats['collision']} ({stats['collision'] / total * 100:.1f}%)") + print(f"越界次数: {stats['out_of_bounds']} ({stats['out_of_bounds'] / total * 100:.1f}%)") + print(f"轨迹数据已保存至: {csv_filename}") + else: + print("未完成任何完整回合。") + print("=" * 50) + env.close() + cv2.destroyAllWindows() if __name__ == "__main__": From 578fdd639351e2f6bde345816a28de04ccd7dd4a Mon Sep 17 00:00:00 2001 From: explorer1009 <2350619772@qq.com> Date: Sun, 21 Dec 2025 20:50:21 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E5=AE=8C=E6=88=90ros=E5=B0=81=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../3D_point_cloud.py" | 0 .../README.md" | 0 .../Radar_obstacle_avoidance.py" | 0 .../custom_env.py" | 0 .../drone_lidar_test.py" | 0 .../maze.py" | 0 .../pycharm_conection.py" | 0 .../run_inference.py" | 0 .../settings.json" | 0 .../test_multirotor.py" | 0 .../train.py" | 0 .../README.md" | 104 ++++++++++++ .../custom_env.py" | 154 ++++++++++++++++++ .../train.py" | 62 +++++++ 14 files changed, 320 insertions(+) rename src/airsim_control/3D_point_cloud.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/3D_point_cloud.py" (100%) rename src/airsim_control/README.md => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/README.md" (100%) rename src/airsim_control/Radar_obstacle_avoidance.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/Radar_obstacle_avoidance.py" (100%) rename src/airsim_control/custom_env.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/custom_env.py" (100%) rename src/airsim_control/drone_lidar_test.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/drone_lidar_test.py" (100%) rename src/airsim_control/maze.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/maze.py" (100%) rename src/airsim_control/pycharm_conection.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/pycharm_conection.py" (100%) rename src/airsim_control/run_inference.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/run_inference.py" (100%) rename src/airsim_control/settings.json => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/settings.json" (100%) rename src/airsim_control/test_multirotor.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/test_multirotor.py" (100%) rename src/airsim_control/train.py => "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/train.py" (100%) create mode 100644 "src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/README.md" create mode 100644 "src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/custom_env.py" create mode 100644 "src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/train.py" diff --git a/src/airsim_control/3D_point_cloud.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/3D_point_cloud.py" similarity index 100% rename from src/airsim_control/3D_point_cloud.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/3D_point_cloud.py" diff --git a/src/airsim_control/README.md "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/README.md" similarity index 100% rename from src/airsim_control/README.md rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/README.md" diff --git a/src/airsim_control/Radar_obstacle_avoidance.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/Radar_obstacle_avoidance.py" similarity index 100% rename from src/airsim_control/Radar_obstacle_avoidance.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/Radar_obstacle_avoidance.py" diff --git a/src/airsim_control/custom_env.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/custom_env.py" similarity index 100% rename from src/airsim_control/custom_env.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/custom_env.py" diff --git a/src/airsim_control/drone_lidar_test.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/drone_lidar_test.py" similarity index 100% rename from src/airsim_control/drone_lidar_test.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/drone_lidar_test.py" diff --git a/src/airsim_control/maze.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/maze.py" similarity index 100% rename from src/airsim_control/maze.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/maze.py" diff --git a/src/airsim_control/pycharm_conection.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/pycharm_conection.py" similarity index 100% rename from src/airsim_control/pycharm_conection.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/pycharm_conection.py" diff --git a/src/airsim_control/run_inference.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/run_inference.py" similarity index 100% rename from src/airsim_control/run_inference.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/run_inference.py" diff --git a/src/airsim_control/settings.json "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/settings.json" similarity index 100% rename from src/airsim_control/settings.json rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/settings.json" diff --git a/src/airsim_control/test_multirotor.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/test_multirotor.py" similarity index 100% rename from src/airsim_control/test_multirotor.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/test_multirotor.py" diff --git a/src/airsim_control/train.py "b/src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/train.py" similarity index 100% rename from src/airsim_control/train.py rename to "src/airsim_control/\345\237\272\344\272\216airsim\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257/train.py" diff --git "a/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/README.md" "b/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/README.md" new file mode 100644 index 0000000000..6e9560d63b --- /dev/null +++ "b/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/README.md" @@ -0,0 +1,104 @@ +ROS 2 PX4 深度强化学习无人机迷宫导航系统 + + + +\[项目简介] + +本项目基于 ROS 2 (Humble) 与 PX4-Autopilot 仿真平台,使用 Stable Baselines3 框架训练无人机在 Gazebo 迷宫环境中自动寻路。 + +系统摒弃了传统的视觉处理,采用轻量化的 2D 激光雷达 (LaserScan) 数据作为输入,通过 PPO (Proximal Policy Optimization) 算法训练神经网络,实现从起点到设定终点的无碰撞自主飞行。针对虚拟机环境进行了专门的算力优化。 + + + +\[核心功能] + + + +1\. ROS 2 环境封装 + +  通过 rclpy 构建自定义 Gym 环境,自动处理 ROS 节点通信。实现了无人机的自动解锁 (Arm)、模式切换 (Offboard) 以及起飞逻辑,无需人工干预仿真流程。 + + + +2\. 基于雷达的感知决策 + +  使用 180 点激光雷达数据作为观测空间 (Observation Space),训练 MLP (多层感知机) 策略网络。相比图像输入,该方法对 CPU 算力要求极低,适合在虚拟机环境中快速训练。 + + + +3\. 安全与奖励机制 + +  防撞逻辑:当雷达检测到障碍物距离小于 0.3米 时给予高额惩罚并结束回合。 + +  引导奖励:根据无人机与目标点 (Target) 的欧氏距离实时计算奖励,引导无人机向终点靠近。 + +  高度锁定:通过 PID 逻辑强制锁定飞行高度为 2米,简化动作空间为二维平面的速度控制。 + + + +4\. 低配环境适配 (Headless Mode) + +  支持无图形界面 (Headless) 运行 Gazebo,大幅降低显卡和 CPU 负载,解决虚拟机中 "Real Time Factor" 过低导致的仿真崩溃问题。 + + + +\[环境依赖] + +运行本项目前,请确保已配置好 ROS 2 Humble、MAVROS 及 PX4-Autopilot 环境,并安装以下 Python 库: + +pip3 install gymnasium stable-baselines3 shimmy numpy + +pip3 install torch --index-url https://download.pytorch.org/whl/cpu + + + +\[项目结构] + +custom\_env.py : 自定义 Gym 环境核心代码。负责订阅 /scan (雷达) 和 /mavros/local\_position/pose (位置),发布 /mavros/setpoint\_velocity/cmd\_vel\_unstamped (速度指令),并处理服务调用。 + +train.py : 训练脚本。配置 PPO 算法超参数,实例化环境并启动训练循环,支持模型自动保存。 + +1maze.world : Gazebo 仿真使用的迷宫地图文件。 + + + +\[运行方式] + +为了保证仿真同步性,请严格按照以下顺序在不同终端启动: + + + +1\. 启动 Gazebo 仿真 (无头模式推荐) + +  为了防止虚拟机卡顿导致飞控上锁,建议使用 Headless 模式: + +  cd ~/桌面/prometheus\_px4 + +  HEADLESS=1 PX4\_SITL\_WORLD=/home/hex/桌面/maze/1maze.world make px4\_sitl gazebo + + + +2\. 启动 MAVROS 通信桥梁 + +  等待 Gazebo 启动完毕后运行: + +  ros2 launch mavros px4.launch fcu\_url:=udp://:14540@127.0.0.1:14580 + + + +3\. 启动 AI 训练 + +  确保终端位于代码所在目录 (如 .venv): + +  python3 train.py + + + +\[注意事项] + +\- 目标点设置:请根据 1maze.world 的实际出口位置,修改 custom\_env.py 中的 TARGET\_POS 坐标。 + +\- 仿真速率:如果遇到 "Time jump detected" 报错,可在启动 Gazebo 时添加 PX4\_SIM\_SPEED\_FACTOR=0.5 降低仿真速率。 + +\- 解释器选择:由于 ROS 2 库位于系统路径,运行 Python 脚本时请务必使用系统解释器 (/usr/bin/python3),避免使用隔离的虚拟环境。 + diff --git "a/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/custom_env.py" "b/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/custom_env.py" new file mode 100644 index 0000000000..e064665c8c --- /dev/null +++ "b/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/custom_env.py" @@ -0,0 +1,154 @@ +import gymnasium as gym +from gymnasium import spaces +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from geometry_msgs.msg import Twist, PoseStamped +from sensor_msgs.msg import LaserScan +from mavros_msgs.srv import CommandBool, SetMode +import math +import time +import threading + +# ========================================== +# 🎯 迷宫目标配置 +# ========================================== +TARGET_POS = np.array([10.0, 2.0, 2.0]) +SUCCESS_DIST = 1.0 + + +class AirSimMazeEnv(gym.Env): + def __init__(self): + super(AirSimMazeEnv, self).__init__() + + # --- 1. ROS 2 节点初始化 --- + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node('gym_env_node') + + qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE) + + # 发布者: 控制速度 + self.vel_pub = self.node.create_publisher(Twist, '/mavros/setpoint_velocity/cmd_vel_unstamped', 10) + + # 订阅者: 雷达和位置 + self.node.create_subscription(LaserScan, '/scan', self.scan_callback, qos) + self.node.create_subscription(PoseStamped, '/mavros/local_position/pose', self.pose_callback, qos) + + # === 关键新增:服务客户端 (用于解锁和切模式) === + self.arming_client = self.node.create_client(CommandBool, '/mavros/cmd/arming') + self.set_mode_client = self.node.create_client(SetMode, '/mavros/cmd/set_mode') + + # 启动后台线程 + self.executor = rclpy.executors.MultiThreadedExecutor() + self.executor.add_node(self.node) + self.spin_thread = threading.Thread(target=self.executor.spin, daemon=True) + self.spin_thread.start() + + self.latest_scan = None + self.current_pose = None + + self.action_space = spaces.Box(low=np.array([0, -1]), high=np.array([1, 1]), dtype=np.float32) + self.observation_space = spaces.Box(low=0, high=30, shape=(180,), dtype=np.float32) + + # === 回调函数 === + def scan_callback(self, msg): + raw = np.array(msg.ranges) + raw[raw == float('inf')] = 30.0 + raw = np.nan_to_num(raw, nan=30.0) + target_len = 180 + if len(raw) >= target_len: + step = len(raw) // target_len + self.latest_scan = raw[::step][:target_len] + else: + self.latest_scan = np.pad(raw, (0, target_len - len(raw)), constant_values=30.0) + + def pose_callback(self, msg): + self.current_pose = msg.pose + + # === 辅助函数:自动起飞逻辑 === + def _arm_and_offboard(self): + # 1. 发送心跳包 + for _ in range(10): + self.vel_pub.publish(Twist()) + time.sleep(0.05) + + # 2. 切换 OFFBOARD 模式 + req_mode = SetMode.Request() + req_mode.custom_mode = 'OFFBOARD' + if self.set_mode_client.service_is_ready(): + self.set_mode_client.call_async(req_mode) + + # 3. 解锁 (Arm) + req_arm = CommandBool.Request() + req_arm.value = True + if self.arming_client.service_is_ready(): + self.arming_client.call_async(req_arm) + + # === Step 函数 === + def step(self, action): + fwd_speed = float(action[0]) * 2.0 + yaw_rate = float(action[1]) * 1.0 + + vel_x, vel_y = 0.0, 0.0 + if self.current_pose: + q = self.current_pose.orientation + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) + current_yaw = math.atan2(siny_cosp, cosy_cosp) + vel_x = fwd_speed * math.cos(current_yaw) + vel_y = fwd_speed * math.sin(current_yaw) + + current_z = self.current_pose.position.z if self.current_pose else 0.0 + vel_z = (2.0 - current_z) * 1.0 # 保持2米高度 + + cmd = Twist() + cmd.linear.x = vel_x + cmd.linear.y = vel_y + cmd.linear.z = vel_z + cmd.angular.z = yaw_rate + self.vel_pub.publish(cmd) + + time.sleep(0.1) + + obs = self._get_obs() + reward, done = self._compute_reward(obs) + return obs, reward, done, False, {} + + def _get_obs(self): + if self.latest_scan is None: + return np.ones(180, dtype=np.float32) * 30.0 + return self.latest_scan.astype(np.float32) + + def _compute_reward(self, obs): + if self.current_pose is None: + return 0.0, False + pos = np.array([self.current_pose.position.x, self.current_pose.position.y, self.current_pose.position.z]) + dist = np.linalg.norm(pos - TARGET_POS) + + reward = -0.05 + done = False + + if np.min(obs) < 0.3: + reward = -50.0 + done = True + print("❌ 撞墙!") + elif dist < SUCCESS_DIST: + reward = 100.0 + done = True + print("✅ 成功!") + else: + reward += (30.0 - dist) * 0.1 + return reward, done + + def reset(self, seed=None, options=None): + super().reset(seed=seed) + print(">>> 新回合: 正在自动解锁起飞... <<<") + self._arm_and_offboard() + time.sleep(2.0) # 等它飞起来 + return self._get_obs(), {} + + def close(self): + self.node.destroy_node() + rclpy.shutdown() \ No newline at end of file diff --git "a/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/train.py" "b/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/train.py" new file mode 100644 index 0000000000..b8f50fdbb9 --- /dev/null +++ "b/src/airsim_control/\345\237\272\344\272\216gazebo\347\232\204\350\277\267\345\256\253\345\257\273\350\267\257\345\222\214ros\345\260\201\350\243\205/train.py" @@ -0,0 +1,62 @@ +import os +import torch as th +from stable_baselines3 import PPO +from stable_baselines3.common.callbacks import CheckpointCallback +from stable_baselines3.common.vec_env import DummyVecEnv +from custom_env import AirSimMazeEnv # 导入刚才那个文件 + +# === 路径配置 === +MODELS_DIR = os.path.expanduser("~/桌面/models") +LOG_DIR = os.path.expanduser("~/桌面/logs") + +os.makedirs(MODELS_DIR, exist_ok=True) +os.makedirs(LOG_DIR, exist_ok=True) + + +def main(): + # 实例化环境 + env = DummyVecEnv([lambda: AirSimMazeEnv()]) + + # === 网络架构配置 === + # 因为输入是雷达数据(一维数值),所以使用 MlpPolicy (多层感知机) + policy_kwargs = dict( + activation_fn=th.nn.Tanh, + net_arch=dict(pi=[256, 256], vf=[256, 256]) + ) + + print("🚀 开始训练 (ROS 2 版)...") + + # 初始化 PPO 模型 + model = PPO( + "MlpPolicy", # 关键点:雷达数据必须用 MlpPolicy + env, + verbose=1, + tensorboard_log=LOG_DIR, + learning_rate=0.0003, + batch_size=256, + n_steps=2048, + gamma=0.99, + policy_kwargs=policy_kwargs, + device="auto" + ) + + # 自动保存回调 (每 10000 步保存一次) + checkpoint_callback = CheckpointCallback( + save_freq=10000, + save_path=MODELS_DIR, + name_prefix='ros_drone' + ) + + # 开始学习 (训练 10万步试试) + model.learn( + total_timesteps=100000, + callback=checkpoint_callback + ) + + # 保存最终模型 + model.save(os.path.join(MODELS_DIR, "ros_drone_final")) + print("训练结束。") + + +if __name__ == "__main__": + main() \ No newline at end of file