Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
53 commits
Select commit Hold shift + click to select a range
ec2a358
完成选题
explorer1009 Sep 28, 2025
beb314c
给代码添加注释
explorer1009 Oct 20, 2025
67db6f8
优化airsim控制代码
explorer1009 Oct 20, 2025
d6d3950
优化车辆控制代码提高可读性
explorer1009 Oct 27, 2025
c4e1cf1
Update hello_car.py
explorer1009 Oct 27, 2025
bd90afa
实现在pycharm和airsim的连接
explorer1009 Oct 27, 2025
e0e9c43
删除多余文件
explorer1009 Oct 27, 2025
5fdbb2d
1
explorer1009 Oct 27, 2025
947014b
按要求修改
explorer1009 Oct 28, 2025
18da3c7
按要求修改
explorer1009 Nov 2, 2025
23c53d8
Update README.md
explorer1009 Nov 2, 2025
33f272d
Resolve conflict: keep README.md
explorer1009 Nov 2, 2025
1409c65
实现无人机定点飞行
explorer1009 Nov 3, 2025
133caff
Merge branch 'main' into drone_233
explorer1009 Nov 3, 2025
ff2f144
配置setitngs文件打开激光雷达
explorer1009 Nov 17, 2025
51ab7f5
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Nov 17, 2025
fefa493
激光雷达避障
explorer1009 Nov 24, 2025
c6240b5
更新settings文件
explorer1009 Nov 24, 2025
08ade55
Update README.md
explorer1009 Nov 24, 2025
97536c5
Merge branch 'main' into drone_233
explorer1009 Nov 24, 2025
25ad07b
规范readme文件
explorer1009 Nov 24, 2025
b0df2b9
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Nov 24, 2025
78412a9
Update Radar_obstacle_avoidance.py
explorer1009 Nov 25, 2025
c4046e1
Merge branch 'main' into drone_233
explorer1009 Nov 25, 2025
547acdc
添加3D点云生成功能
explorer1009 Dec 1, 2025
334138e
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 1, 2025
d362e48
Update 3D_point_cloud.py
explorer1009 Dec 1, 2025
78e3f1d
Merge branch 'main' into drone_233
explorer1009 Dec 1, 2025
f6c9a01
点云坐标系修正
explorer1009 Dec 2, 2025
05428b6
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 2, 2025
7b02575
Merge branch 'main' into drone_233
explorer1009 Dec 2, 2025
000c3b7
通过激光雷达实现迷宫寻路
explorer1009 Dec 8, 2025
4ff5b83
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 8, 2025
ee00400
Merge branch 'main' into drone_233
explorer1009 Dec 8, 2025
394e9de
Update maze.py
explorer1009 Dec 8, 2025
85e5948
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 8, 2025
da0e96f
Merge branch 'main' into drone_233
explorer1009 Dec 8, 2025
bd0818a
Update README.md
explorer1009 Dec 9, 2025
5926a64
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 9, 2025
d0ffb7a
改进无人机寻路策略
explorer1009 Dec 15, 2025
a6e98a0
Merge branch 'main' into drone_233
explorer1009 Dec 15, 2025
eb731ee
Update README.md
explorer1009 Dec 15, 2025
d58bb2c
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 15, 2025
379a04e
深度强化学习无人机迷宫寻路系统
explorer1009 Dec 16, 2025
c685984
更新README文件
explorer1009 Dec 16, 2025
90f333c
Merge branch 'main' into drone_233
explorer1009 Dec 16, 2025
2ac1cf8
增加迷宫终点判定
explorer1009 Dec 16, 2025
e4d985d
Merge branch 'main' into drone_233
explorer1009 Dec 16, 2025
0288af2
无人机重生逻辑优化
explorer1009 Dec 17, 2025
7416103
Merge branch 'drone_233' of https://github.com/explorer1009/nn233 int…
explorer1009 Dec 17, 2025
5874b97
Update settings.json
explorer1009 Dec 18, 2025
6119651
无人机寻路改用A*算法
explorer1009 Dec 18, 2025
6c649b7
改进无人机转圈的问题
explorer1009 Dec 18, 2025
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
214 changes: 124 additions & 90 deletions src/airsim_control/custom_env.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
return reward, done

def close(self):
self.client.enableApiControl(False)
59 changes: 39 additions & 20 deletions src/airsim_control/run_inference.py
Original file line number Diff line number Diff line change
@@ -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__":
Expand Down
Loading
Loading