diff --git a/src/airsim_control/custom_env.py b/src/airsim_control/custom_env.py index a098499eb6..436b203c95 100644 --- a/src/airsim_control/custom_env.py +++ b/src/airsim_control/custom_env.py @@ -4,161 +4,195 @@ import numpy as np import cv2 import math +import time -# 目标出口位置 (注意:这里要填换算成“米”后的单位) -# UE坐标: X=790cm, Y=3360cm -> AirSim: X=7.9m, Y=33.6m -EXIT_POS = np.array([7.9, 33.6]) +# ========================================================= +# 1. 坐标系精准换算 (UE -> AirSim) +# ========================================================= +UE_START = np.array([1180.0, 610.0, 28.0]) # 出生点 +UE_GOAL = np.array([790.0, 3360.0, -50.0]) # 正方体位置 -# 允许的误差半径 (到达终点附近 2 米就算成功) -GOAL_THRESHOLD = 2.0 +# 计算相对向量 (单位: 米) +TARGET_POS_AIRSIM = (UE_GOAL - UE_START) / 100.0 + +print(f"========================================") +print(f"🚀 重启训练: 全新配置") +print(f"1. 目标相对坐标: {TARGET_POS_AIRSIM}") +print(f"2. 速度限制: 5.0 m/s (已加速)") +print(f"3. 判定半径: 5 米") +print(f"4. 防转圈机制: 已启用 Lidar 地面过滤") +print(f"========================================") class AirSimMazeEnv(gym.Env): def __init__(self): super(AirSimMazeEnv, self).__init__() - # 连接 AirSim self.client = airsim.MultirotorClient() self.client.confirmConnection() # 动作空间: [前进速度(0~1), 转向速度(-1~1)] - # 我们不控制后退,只控制前进和左右转,这样更容易走出迷宫 - self.action_space = spaces.Box(low=np.array([0, -1]), high=np.array([1, 1]), dtype=np.float32) + self.action_space = spaces.Box( + low=np.array([0, -1]), + high=np.array([1, 1]), + dtype=np.float32 + ) - # 观测空间 (多模态): - # 1. image: 84x84 的深度图 - # 2. lidar: 180 个维度的距离向量 (模拟前方180度扇区) + # 观测空间 (Lidar 50m) self.observation_space = spaces.Dict({ "image": spaces.Box(low=0, high=255, shape=(84, 84, 1), dtype=np.uint8), "lidar": spaces.Box(low=0, high=50, shape=(180,), dtype=np.float32) }) - self.state = None self.last_dist = None + self.prev_action = np.zeros(2) def step(self, action): - # 1. 执行动作 - # 将动作映射到实际物理参数 - fwd_vel = float(action[0]) * 3.0 # 最大速度 3 m/s - yaw_rate = float(action[1]) * 45 # 最大转向角速度 45度/s - + # --- 1. 执行动作 (提速版) --- + # 之前可能设低了,现在强制设为 5.0 m/s,保证飞得快 + fwd_vel = float(action[0]) * 5.0 + yaw_rate = float(action[1]) * 60 - - # 新的代码 (锁定高度在 -1.5 米): - # 注意: NED坐标系中,负数是空中。-1.5 表示离地 1.5 米。 + # 锁定高度 Z=-1.5 self.client.moveByVelocityZBodyFrameAsync( vx=fwd_vel, vy=0, - z=-1.5, # <--- 强制固定在这个高度 + z=-1.5, duration=0.1, yaw_mode=airsim.YawMode(True, yaw_rate) ).join() - # ------------------- 修改结束 ------------------- - # 2. 获取观测 + # --- 2. 获取观测 --- obs = self._get_obs() - # 3. 计算奖励和结束条件 - reward, done = self._compute_reward_and_done() + # --- 3. 计算奖励 --- + reward, done = self._compute_reward_and_done(obs, action) - truncated = False # 对于 Gym 新版本 + truncated = False return obs, reward, done, truncated, {} def reset(self, seed=None, options=None): super().reset(seed=seed) + + # 瞬移重置 (极速) self.client.reset() self.client.enableApiControl(True) self.client.armDisarm(True) - # 起飞到固定高度 - self.client.takeoffAsync().join() - self.client.moveToZAsync(-1.5, 1).join() # 保持在 1.5米高度 - # 初始化距离用于计算奖励 - pos = self.client.getMultirotorState().kinematics_estimated.position - self.last_dist = np.linalg.norm(np.array([pos.x_val, pos.y_val]) - EXIT_POS) + pose = airsim.Pose(airsim.Vector3r(0, 0, -1.5), airsim.Quaternionr(0, 0, 0, 1)) + self.client.simSetVehiclePose(pose, True) + + self.prev_action = np.zeros(2) + curr_pos = np.array([0.0, 0.0, -1.5]) + self.last_dist = np.linalg.norm(curr_pos - TARGET_POS_AIRSIM) return self._get_obs(), {} def _get_obs(self): - # --- 获取摄像头数据 (深度图) --- + # === 图像处理 === + img_obs = np.zeros((84, 84, 1), dtype=np.uint8) responses = self.client.simGetImages([ airsim.ImageRequest("front_center_custom", airsim.ImageType.DepthPlanar, True) ]) - if responses: - img1d = np.array(responses[0].image_data_float, dtype=np.float32) - img1d = np.clip(img1d, 0, 20) # 限制最大深度为20米,归一化效果更好 - img2d = img1d.reshape(responses[0].height, responses[0].width) - # 缩放到 84x84 - img_resize = cv2.resize(img2d, (84, 84)) - # 转换为 0-255 uint8 供 CNN 使用 - img_uint8 = (img_resize / 20.0 * 255).astype(np.uint8) - img_obs = np.expand_dims(img_uint8, axis=-1) - else: - img_obs = np.zeros((84, 84, 1), dtype=np.uint8) - - # --- 获取 Lidar 数据 (并处理为 1D 向量) --- - lidar_data = self.client.getLidarData("lidar_1") - points = np.array(lidar_data.point_cloud, dtype=np.float32) - - # 初始化 180 个 bins (代表前方 -90度 到 +90度) - lidar_scan = np.ones(180) * 20.0 # 默认最远距离 20米 - - if len(points) > 3: - points = np.reshape(points, (-1, 3)) - # 我们只关心局部坐标系下的 X(前) 和 Y(右) - x = points[:, 0] - y = points[:, 1] - - # 转换为极坐标: 角度和距离 - angles = np.arctan2(y, x) * 180 / np.pi # -180 到 180 - dists = np.linalg.norm(points[:, :2], axis=1) - - # 筛选前方 -90 到 90 度的点 - valid_mask = (angles >= -90) & (angles < 90) - valid_angles = angles[valid_mask] - valid_dists = dists[valid_mask] - - # 将角度映射到 0-179 的索引 - indices = ((valid_angles + 90).astype(int)) - indices = np.clip(indices, 0, 179) - - # 更新每个角度扇区的最小距离 (避障需要知道最近的障碍物) - for i, d in zip(indices, valid_dists): - if d < lidar_scan[i]: - lidar_scan[i] = d + response = responses[0] + if response.width > 0: + try: + img1d = np.array(response.image_data_float, dtype=np.float32) + img1d = np.clip(img1d, 0, 20) + img2d = img1d.reshape(response.height, response.width) + img_resize = cv2.resize(img2d, (84, 84)) + img_uint8 = (img_resize / 20.0 * 255).astype(np.uint8) + img_obs = np.expand_dims(img_uint8, axis=-1) + except: + pass + + # === Lidar 处理 (关键修复:地面过滤) === + lidar_scan = np.ones(180) * 20.0 + try: + lidar_data = self.client.getLidarData("lidar_1") + points = np.array(lidar_data.point_cloud, dtype=np.float32) + + if len(points) > 3: + points = np.reshape(points, (-1, 3)) + + # 【防止转圈的核心】 + # 你的 settings.json 是 -10 度,如果不加这个,它一定会把地板当墙 + # 我们只保留高度在 -1.0 到 0.5 之间的点 (水平视线附近的障碍物) + z_mask = (points[:, 2] > -1.0) & (points[:, 2] < 0.5) + points = points[z_mask] + + if len(points) > 0: + x = points[:, 0] + y = points[:, 1] + angles = np.arctan2(y, x) * 180 / np.pi + dists = np.linalg.norm(points[:, :2], axis=1) + + valid_mask = (angles >= -90) & (angles < 90) + valid_angles = angles[valid_mask] + valid_dists = dists[valid_mask] + + indices = ((valid_angles + 90).astype(int)) + indices = np.clip(indices, 0, 179) + for i, d in zip(indices, valid_dists): + if d < lidar_scan[i]: + lidar_scan[i] = d + except: + pass return {"image": img_obs, "lidar": lidar_scan} - def _compute_reward_and_done(self): - # 获取状态 + def _compute_reward_and_done(self, obs, action): collision = self.client.simGetCollisionInfo().has_collided - pos = self.client.getMultirotorState().kinematics_estimated.position - curr_dist = np.linalg.norm(np.array([pos.x_val, pos.y_val]) - EXIT_POS) + state = self.client.getMultirotorState().kinematics_estimated.position + curr_pos = np.array([state.x_val, state.y_val, state.z_val]) + + dist_to_goal = np.linalg.norm(curr_pos - TARGET_POS_AIRSIM) + dist_from_start = np.linalg.norm(curr_pos) reward = 0 done = False - # 1. 碰撞惩罚 (大惩罚并结束) + # 1. 撞墙 if collision: reward = -50.0 done = True + print(f"❌ 撞墙!") return reward, done - # 2. 到达终点奖励 - if curr_dist < GOAL_THRESHOLD: + # 2. 成功 (5米内) + if dist_to_goal < 5.0: reward = 100.0 done = True - print(">>> REACHED EXIT! <<<") + print(f"✅ 任务完成! (距离: {dist_to_goal:.2f}m)") return reward, done - # 3. 引导奖励 (Progress Reward) - # 如果比上一步离终点近,给正奖励;远了给负奖励 - dist_improvement = self.last_dist - curr_dist - reward += dist_improvement * 10.0 - self.last_dist = curr_dist + # 3. 越界保护 + limit_dist = np.linalg.norm(TARGET_POS_AIRSIM) + 20.0 + if dist_from_start > limit_dist: + reward = -20.0 + done = True + print(f"⚠️ 飞出边界,重置") + return reward, done + + # 4. 引导奖励 + if self.last_dist is not None: + reward += (self.last_dist - dist_to_goal) * 10.0 + self.last_dist = dist_to_goal - # 4. 时间步惩罚 (迫使它快速移动) + # 5. 避障惩罚 (防止死路) + min_obs_dist = np.min(obs['lidar']) + if min_obs_dist < 1.5: + reward -= (1.5 - min_obs_dist) * 0.5 + + # 6. 动作平滑 (防止抖动) + reward -= np.linalg.norm(action - self.prev_action) * 0.1 + self.prev_action = action.copy() + + # 7. 步数惩罚 reward -= 0.05 - return reward, done \ No newline at end of file + return reward, done + + def close(self): + self.client.enableApiControl(False) \ No newline at end of file diff --git a/src/airsim_control/run_inference.py b/src/airsim_control/run_inference.py index 2661a224fa..b672b66703 100644 --- a/src/airsim_control/run_inference.py +++ b/src/airsim_control/run_inference.py @@ -1,36 +1,55 @@ +import glob +import os +import time from stable_baselines3 import PPO from custom_env import AirSimMazeEnv -import time + +# === 配置路径 === +MODELS_DIR = r"D:\Others\MyAirsimprojects\models" + + +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() - # 加载已训练的模型 (请确保文件名对应) - model_path = "models/drone_maze_final" # 或者某个检查点文件 - try: - model = PPO.load(model_path) - print(f"成功加载模型: {model_path}") - except FileNotFoundError: - print("未找到模型文件,请先运行 train.py") + # 寻找最新的模型 + model_path = get_latest_model_path(MODELS_DIR) + + if not model_path: + print("错误:没有找到任何训练好的模型!请先运行 train.py。") return + print(f"正在加载模型进行测试: {model_path}") + model = PPO.load(model_path) + + # 开始测试循环 obs, _ = env.reset() - print("开始自动寻路测试...") - done = False - while True: - # 模型预测动作 (deterministic=True 表示不使用随机性,完全确定的策略) - action, _states = model.predict(obs, deterministic=True) + print("开始推理 (按 Ctrl+C 停止)...") + try: + while True: + # 预测动作 (deterministic=True 表示不使用随机探索,只用这一刻认为最好的动作) + action, _states = model.predict(obs, deterministic=True) + + # 执行动作 + obs, reward, done, truncated, info = env.step(action) - # 环境执行动作 - obs, reward, done, truncated, info = env.step(action) + # 如果结束了 (撞墙或到达),自动重置 + if done: + print("--- 回合结束,重置环境 ---") + obs, _ = env.reset() - if done: - print("回合结束 (碰撞或到达终点)") - time.sleep(1) - obs, _ = env.reset() + except KeyboardInterrupt: + print("停止测试") + env.close() if __name__ == "__main__": diff --git a/src/airsim_control/train.py b/src/airsim_control/train.py index 3314fd4b48..4b0a7322ce 100644 --- a/src/airsim_control/train.py +++ b/src/airsim_control/train.py @@ -1,46 +1,61 @@ +import glob +import os from stable_baselines3 import PPO from stable_baselines3.common.callbacks import CheckpointCallback from custom_env import AirSimMazeEnv -import os -# 创建日志目录 -log_dir = "./airsim_logs/" -os.makedirs(log_dir, exist_ok=True) +# === 路径配置 === +MODELS_DIR = r"D:\Others\MyAirsimprojects\models" +LOG_DIR = r"D:\Others\MyAirsimprojects\airsim_logs" + +os.makedirs(MODELS_DIR, exist_ok=True) +os.makedirs(LOG_DIR, exist_ok=True) + + +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 + return max(list_of_files, key=os.path.getctime) def main(): - # 实例化环境 env = AirSimMazeEnv() + latest_model_path = get_latest_model_path(MODELS_DIR) + + if latest_model_path: + print(f"--- 发现存档: {latest_model_path},继续训练 ---") + model = PPO.load(latest_model_path, env=env, tensorboard_log=LOG_DIR) + reset_timesteps = False + else: + 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 + ) + reset_timesteps = True - # 定义模型 - # MultiInputPolicy 会自动检测 Dict 输入 - # learning_rate: 学习率,通常 3e-4 是个不错的起点 - # n_steps: 每次更新收集的步数 - model = PPO( - "MultiInputPolicy", - env, - verbose=1, - tensorboard_log=log_dir, - learning_rate=0.0003, - batch_size=64, - n_steps=2048, - gamma=0.99 - ) - - # 保存检查点的回调函数 (每 10000 步保存一次) checkpoint_callback = CheckpointCallback( save_freq=10000, - save_path='./models/', + save_path=MODELS_DIR, name_prefix='drone_maze' ) - print("开始训练...") - # 训练步数建议:简单的迷宫可能需要 20万-50万步,复杂的需要更多 - model.learn(total_timesteps=500000, callback=checkpoint_callback) + print("🚀 训练引擎启动...") + model.learn( + total_timesteps=500000, + callback=checkpoint_callback, + reset_num_timesteps=reset_timesteps + ) - # 保存最终模型 - model.save("drone_maze_final") - print("训练完成,模型已保存。") + model.save(os.path.join(MODELS_DIR, "drone_maze_final")) + print("训练结束。") if __name__ == "__main__":