Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
221 changes: 221 additions & 0 deletions src/airsim_control/基于airsim的迷宫寻路/run_inference.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,221 @@
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"

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

不能使用绝对路径

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_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)

# 这里需要注意: 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("==================================================")

# 1. 模型选择
models = get_all_models(MODELS_DIR)
if not models:
print("错误: 未找到任何模型文件!请先运行 train.py。")
return

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)

# 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")

try:
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:
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("\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__":
main()
111 changes: 111 additions & 0 deletions src/airsim_control/基于airsim的迷宫寻路/train.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,111 @@
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"
LOG_DIR = r"D:\Others\MyAirsimprojects\airsim_logs"

os.makedirs(MODELS_DIR, exist_ok=True)
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:
return None
return max(list_of_files, key=os.path.getctime)


def main():
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"--- 未发现存档,开始【从头训练】(优化版) ---")
model = PPO(
"MultiInputPolicy",
env,
verbose=1,
tensorboard_log=LOG_DIR,

# --- 核心超参数优化 ---
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=20000, # 每 2万步保存一次,不用太频繁
save_path=MODELS_DIR,
name_prefix='drone_maze_opt' # 改个名字区分一下
)

print("🚀 优化版训练引擎启动...")
print("配置: Linear LR, Net=[256,256], Ent=0.01")

model.learn(
total_timesteps=1000000, # 建议跑 100万步
callback=checkpoint_callback,
reset_num_timesteps=reset_timesteps
)

model.save(os.path.join(MODELS_DIR, "drone_maze_final_opt"))
print("训练结束。")


if __name__ == "__main__":
main()
Loading
Loading