diff --git a/src/airsim_control/README.md b/src/airsim_control/README.md index 049742e882..1bb913fce5 100644 --- a/src/airsim_control/README.md +++ b/src/airsim_control/README.md @@ -6,20 +6,21 @@ AirSim 无人机激光雷达系统：迷宫自主寻路与避障 [核心功能] -1. 智能迷宫寻路 - - 直行优先策略：在多条路径可选时，优先保持直行，避免在老路中左右摇摆。 - - 死胡同记忆与封锁：利用 DFS (深度优先搜索) 思想，当检测到死路时，自动掉头并生成“虚拟墙（禁区）”，防止再次进入。 - - 出口诱导机制：当雷达检测到极远距离（>15米）的开阔地时，判定为出口并全速冲刺。 - - 机身坐标系控制：使用 Body Frame 控制飞行，彻底解决了无人机转向后方向感错乱的问题。 +1. 智能迷宫寻路 (DFS 深度优先) + 侧路优先策略：打破常规直行逻辑，采用 DFS (深度优先搜索) 思想，在遇到新岔路口时优先转弯探索，确保遍历所有路径。 + 死胡同记忆与回溯：当检测到死路时，自动倒车并生成“虚拟墙（禁区）”封锁该区域，随后掉头返回主路。 + 精准转向与纠偏：摒弃时间控制转向，采用闭环检测精准对准路口；若转向后对准墙壁，自动执行横向平移 (Side-Step) 进行修正。 + 机身坐标系控制：使用 Body Frame 控制飞行，结合“急刹-原地决策”逻辑，彻底解决路口冲过头的问题。 2. 记忆与决策系统 - - 栅格化记忆地图：将世界划分为 2米 x 2米的栅格，实时记录“去过的地方”。 - - 射线检测评分：在路口决策时，向各个方向发射虚拟射线，检测路径是否为“新路”，优先探索未知区域。 - - 防抖动冷却：决策后强制执行一段距离，防止在路口反复纠结。 + 栅格化记忆地图：将世界划分为 1.5米 x 1.5米的栅格，实时记录“去过的地方”。 + 射线检测评分：在路口决策时，向各个方向发射虚拟射线，检测路径深度与新旧程度，优先选择未探索区域。 + 出口诱导机制：当雷达检测到极远距离（>15米）的开阔地时，判定为出口并全速冲刺。 3. 基础飞行保障 - - 紧急避险：配备物理反推刹车逻辑，在距离障碍物 <1.0米 时强制反向推力，防止惯性撞墙。 - - 高度锁定：通过 PID 控制器将飞行高度严格限制在设定值（如 -1.5米）。 + 慢速精细操控：巡航速度降至 1.0m/s，配合高灵敏度刹车逻辑，适应狭窄迷宫环境。 + 高度硬限位 (Hard Clamp)：在 PID 控制基础上增加垂直速度硬锁（限制在 ±0.8m/s），无论误差多大，强制防止无人机“飞天”或失控。 + 紧急避险：配备物理反推刹车逻辑，在距离障碍物 <1.0米 时强制反向推力，防止惯性撞墙。 [环境依赖] 在运行代码之前，请确保已安装以下 Python 库： @@ -66,11 +67,12 @@ pip install airsim numpy keyboard [可视化调试说明] 代码运行时，会在 AirSim 窗口中绘制彩色小球，代表无人机的“思维过程”： -- 绿色球体：新路 (New Path) - 未探索的区域，优先级最高。 -- 蓝色方块：足迹 (Visited) - 已经走过的路径点。 -- 红色球体：老路 (Old Path) - 探测到前方是走过的路，尽量避免。 -- 黑色大球：死路/禁区 (Forbidden) - 已确认为死胡同，生成虚拟墙封锁。 -- 黄色大球：出口 (Exit) - 检测到终点开阔地。 +绿色球体：新路 (New Path) - 未探索的区域，优先级最高。 +青色球体：岔路优先 (Priority) - 发现侧方新路口，强制优先探索。 +蓝色方块：足迹 (Visited) - 已经走过的路径点。 +红色球体：老路 (Old Path) - 探测到前方是走过的路，尽量避免。 +黑色大球：死路/禁区 (Forbidden) - 已确认为死胡同，生成虚拟墙封锁。 +黄色大球：出口 (Exit) - 检测到终点开阔地。 [手动操控 (辅助)] 虽然本系统设计为全自动运行，但在紧急情况下或使用手动模式代码时，可用以下按键： diff --git a/src/airsim_control/maze.py b/src/airsim_control/maze.py index 547dd16019..2a9a82e9ee 100644 --- a/src/airsim_control/maze.py +++ b/src/airsim_control/maze.py @@ -3,26 +3,25 @@ import time import math -# --- 配置 --- +# --- 精细操控版配置 --- VEHICLE_NAME = "Drone_1" LIDAR_NAME = "lidar_1" -# 飞行参数 +# 飞行参数 (慢速、精准) TARGET_HEIGHT = -1.5 -CRUISE_SPEED = 2.5 # 稍微提速 -TURN_SPEED = 90.0 # 转快点，别磨叽 -STOP_DIST = 3.5 # 刹车距离 -PASS_DIST = 4.5 # 判定通行的距离 -GRID_SIZE = 2.0 # 记忆格大小 - -# 极远距离 (视为出口) +CRUISE_SPEED = 1.0 # 速度降至 1.0，防止冲过头 +TURN_SPEED = 30.0 # 转向速度 +STOP_DIST = 2.0 # 刹车距离 +PASS_DIST = 2.0 # 通行门槛 (大于2米就敢进) +GRID_SIZE = 1.5 EXIT_DIST_THRESHOLD = 15.0 -# 可视化开关 +SIDE_MARGIN = 1.5 # 左右保持距离 + VISUALIZE = True -# --- 记忆模块 --- +# --- 记忆模块 (保持不变) --- class MemoryMap: def __init__(self, grid_size): self.grid_size = grid_size @@ -39,7 +38,7 @@ def mark_visited(self, pos_x, pos_y, client): self.visited.add((gx, gy)) if VISUALIZE: client.simPlotPoints([airsim.Vector3r(gx * self.grid_size, gy * self.grid_size, -1.5)], - color_rgba=[0.0, 0.0, 1.0, 1.0], size=15, is_persistent=True) + color_rgba=[0.0, 0.0, 1.0, 1.0], size=10, is_persistent=True) def mark_forbidden(self, pos_x, pos_y, client): gx, gy = self._to_grid(pos_x, pos_y) @@ -47,18 +46,26 @@ def mark_forbidden(self, pos_x, pos_y, client): self.forbidden.add((gx, gy)) if VISUALIZE: client.simPlotPoints([airsim.Vector3r(gx * self.grid_size, gy * self.grid_size, -1.5)], - color_rgba=[0.0, 0.0, 0.0, 1.0], size=30, is_persistent=True) + color_rgba=[0.0, 0.0, 0.0, 1.0], size=25, is_persistent=True) - def check_status(self, pos_x, pos_y): - gx, gy = self._to_grid(pos_x, pos_y) - if (gx, gy) in self.forbidden: return 2 + def calculate_path_score(self, start_x, start_y, angle_rad, check_dist): + steps = int(check_dist / self.grid_size) + if steps == 0: return 0, False, start_x, start_y + visited_count = 0 + target_x = start_x + math.cos(angle_rad) * check_dist + target_y = start_y + math.sin(angle_rad) * check_dist + + for i in range(1, steps + 1): + d = i * self.grid_size + tx = start_x + math.cos(angle_rad) * d + ty = start_y + math.sin(angle_rad) * d + gx, gy = self._to_grid(tx, ty) + if (gx, gy) in self.forbidden: return -1000, True, tx, ty + if (gx, gy) in self.visited: visited_count += 1 - # 范围检查：如果目标点或其相邻点去过，都算去过（模糊匹配） - for dx in [-1, 0, 1]: - for dy in [-1, 0, 1]: - if (gx + dx, gy + dy) in self.visited: - return 1 - return 0 + density = visited_count / steps + score = 100 - (density * 150) + return score, False, target_x, target_y # 初始化 @@ -68,18 +75,42 @@ def check_status(self, pos_x, pos_y): client.enableApiControl(True, vehicle_name=VEHICLE_NAME) client.armDisarm(True, vehicle_name=VEHICLE_NAME) -print("🚀 起飞中...") +print(" 精细操控模式起飞 (低速抗过冲)...") client.takeoffAsync(vehicle_name=VEHICLE_NAME).join() client.moveToZAsync(TARGET_HEIGHT, 1, vehicle_name=VEHICLE_NAME).join() -print("\n=== 终极寻路: 直觉 + 出口诱导 ===") +# 暴力起步 (依然需要，但稍微温柔点) +print("💨 起步...") +client.moveByVelocityBodyFrameAsync(1.5, 0, 0, 2.0, + drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=airsim.YawMode(is_rate=True, yaw_or_rate=0), + vehicle_name=VEHICLE_NAME).join() + + +# --- 控制函数 --- + +def calculate_z_correction(current_z): + z_error = TARGET_HEIGHT - current_z + vz = z_error * 1.5 + return float(np.clip(vz, -0.8, 0.8)) + + +def calculate_y_correction(l_dist, r_dist): + if l_dist > SIDE_MARGIN and r_dist > SIDE_MARGIN: + return 0.0 + vy = 0.0 + if l_dist < SIDE_MARGIN: + push = (SIDE_MARGIN - l_dist) * 0.8 + vy += push + if r_dist < SIDE_MARGIN: + push = (SIDE_MARGIN - r_dist) * 0.8 + vy -= push + return float(np.clip(vy, -0.8, 0.8)) # 降低横向修正力度 def get_lidar_info(): - """获取前、左、右距离""" lidar_data = client.getLidarData(lidar_name=LIDAR_NAME, vehicle_name=VEHICLE_NAME) if not lidar_data or len(lidar_data.point_cloud) < 3: return 99, 99, 99 - points = np.array(lidar_data.point_cloud, dtype=np.float32) points = np.reshape(points, (int(points.shape[0] / 3), 3)) valid = points[(points[:, 2] > -0.5) & (points[:, 2] < 0.5)] @@ -89,10 +120,16 @@ def get_lidar_info(): l_mask = (valid[:, 1] < -1.0) & (np.abs(valid[:, 0]) < 1.0) r_mask = (valid[:, 1] > 1.0) & (np.abs(valid[:, 0]) < 1.0) + valid = points[(points[:, 2] > -0.4) & (points[:, 2] < 0.4)] + if len(valid) == 0: return 99, 99, 99 + + f_mask = (valid[:, 0] > 0) & (np.abs(valid[:, 1]) < 0.6) # 前方判定变窄 + l_mask = (valid[:, 1] < -1.0) & (np.abs(valid[:, 0]) < 1.0) + r_mask = (valid[:, 1] > 1.0) & (np.abs(valid[:, 0]) < 1.0) + f_d = np.min(valid[f_mask][:, 0]) if np.any(f_mask) else 99 l_d = np.min(np.linalg.norm(valid[l_mask][:, :2], axis=1)) if np.any(l_mask) else 99 r_d = np.min(np.linalg.norm(valid[r_mask][:, :2], axis=1)) if np.any(r_mask) else 99 - return f_d, l_d, r_d @@ -102,52 +139,32 @@ def get_global_yaw(): math.atan2(2.0 * (o.w_val * o.z_val + o.x_val * o.y_val), 1.0 - 2.0 * (o.y_val * o.y_val + o.z_val * o.z_val))) -def turn_rel(angle): - print(f" ↪️ 转向 {angle}°...") - client.moveByVelocityAsync(0, 0, 0, abs(angle) / TURN_SPEED, - drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, - yaw_mode=airsim.YawMode(is_rate=True, - yaw_or_rate=float(TURN_SPEED if angle > 0 else -TURN_SPEED)), - vehicle_name=VEHICLE_NAME).join() - client.moveByVelocityAsync(0, 0, 0, 0.2, vehicle_name=VEHICLE_NAME).join() - - -def check_direction_score(pos, curr_yaw, angle, lidar_dist): - """评分系统""" - # 1. 物理阻挡 - if lidar_dist < PASS_DIST: - return -1000, "🧱 阻挡" - - # 2. 【核心优化】出口检测 - # 如果雷达距离极远(>15米)，说明前面是开阔地(出口)，给予超高分！ - if lidar_dist > EXIT_DIST_THRESHOLD: - client.simPlotPoints([airsim.Vector3r(pos.x_val, pos.y_val, -1.5)], color_rgba=[1.0, 1.0, 0.0, 1.0], size=30, - duration=5.0) - return 10000, "🎉 出口/开阔地" +def turn_to_angle_gentle(target_angle_rel): + print(f" ↪️ 缓慢转向 {target_angle_rel}°") + start_yaw = get_global_yaw() + target_yaw = start_yaw + target_angle_rel - # 3. 记忆检查 - rad = math.radians(curr_yaw + angle) - check_dist = 4.0 - target_x = pos.x_val + math.cos(rad) * check_dist - target_y = pos.y_val + math.sin(rad) * check_dist - - status_code = memory.check_status(target_x, target_y) - - if status_code == 2: # 死路 - return -1000, "⚫ 死路" - - elif status_code == 1: # 老路 - # 调试：红点 - client.simPlotPoints([airsim.Vector3r(target_x, target_y, -1.5)], color_rgba=[1.0, 0.0, 0.0, 1.0], size=10, - duration=2.0) - return -50, "👣 老路" - - else: # 新路 - # 调试：绿点 - client.simPlotPoints([airsim.Vector3r(target_x, target_y, -1.5)], color_rgba=[0.0, 1.0, 0.0, 1.0], size=20, - duration=2.0) - return 100, "✨ 新路" + if target_yaw > 180: + target_yaw -= 360 + elif target_yaw < -180: + target_yaw += 360 + while True: + current_yaw = get_global_yaw() + error = target_yaw - current_yaw + if error > 180: + error -= 360 + elif error < -180: + error += 360 + if abs(error) < 1.0: break # 精度更高 1.0度 + + yaw_rate = np.clip(error * 1.0, -30, 30) # 转得更慢 + if abs(yaw_rate) < 8: yaw_rate = 8 * np.sign(yaw_rate) + + client.moveByVelocityAsync(0, 0, 0, 0.05, + drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=airsim.YawMode(is_rate=True, yaw_or_rate=float(yaw_rate)), + vehicle_name=VEHICLE_NAME).join() def scan_and_decide(): print("\n🛑 决策中...") @@ -157,114 +174,133 @@ def scan_and_decide(): curr_yaw = get_global_yaw() f_d, l_d, r_d = get_lidar_info() +def scan_and_decide(): + # ⚡ 急刹！多停一会儿，完全消除惯性 + client.moveByVelocityAsync(0, 0, 0, 1.5, vehicle_name=VEHICLE_NAME).join() + + pos = client.simGetVehiclePose(vehicle_name=VEHICLE_NAME).position + curr_yaw = get_global_yaw() + f_d, l_d, r_d = get_lidar_info() + options = [ {"angle": 0, "dist": f_d, "name": "前方"}, {"angle": -90, "dist": l_d, "name": "左侧"}, {"angle": 90, "dist": r_d, "name": "右侧"} ] - candidates = [] + print(" 📊 深度评分:") - print(" 📊 评分:") for opt in options: - score, status = check_direction_score(pos, curr_yaw, opt["angle"], opt["dist"]) - - # 只有非墙壁才加入 - if score > -500: - candidates.append({ - "angle": opt["angle"], - "score": score, - "name": opt["name"], - "dist": opt["dist"] - }) - print(f" -> {opt['name']}: {status} ({score})") - - if len(candidates) > 0: - # 排序逻辑优化： - # 1. 分数高的优先 - # 2. 【核心优化】分数相同时，优先选角度为0的(直行)！避免左右乱转 - # 3. 最后选距离远的 - # 我们用 tuple 排序: (score, is_straight, dist) - # angle == 0 转换为 1 (是直行), 否则 0 + if opt["dist"] < PASS_DIST: continue + if opt["dist"] > EXIT_DIST_THRESHOLD: + candidates.append({"angle": opt["angle"], "score": 99999, "name": opt["name"], "dist": opt["dist"]}) + continue - candidates.sort(key=lambda x: (x["score"], 1 if x["angle"] == 0 else 0, x["dist"]), reverse=True) + rad = math.radians(curr_yaw + opt["angle"]) + score, is_dead_end, tx, ty = memory.calculate_path_score(pos.x_val, pos.y_val, rad, 10.0) + # ⚡ 侧路优先逻辑 (DFS Bias) + if not is_dead_end and score > 0 and opt["angle"] != 0: + print(f" -> {opt['name']}: 🚪 岔路优先探索") + score += 300 # 权重加大，确保一定转 + + if not is_dead_end and opt["dist"] > 8.0 and score > 0: + bonus = min(opt["dist"] * 5, 50) + score += bonus + + status_text = "⚫ 死路" if is_dead_end else ("✨ 新路" if score > 0 else "👣 老路") + print(f" -> {opt['name']}: {status_text} ({score:.1f})") + + if not is_dead_end and VISUALIZE: + color = [0.0, 1.0, 0.0, 1.0] if score > 0 else [1.0, 0.0, 0.0, 1.0] + client.simPlotPoints([airsim.Vector3r(tx, ty, -1.5)], color_rgba=color, size=15, duration=2.0) + + if score > -900: + candidates.append({"angle": opt["angle"], "score": score, "name": opt["name"], "dist": opt["dist"]}) + + if candidates: + candidates.sort(key=lambda x: (x["score"], x["dist"]), reverse=True) best = candidates[0] print(f"✅ 决定: {best['name']}") if best["angle"] != 0: - turn_rel(best["angle"]) - - return True # 找到了路 - + turn_to_angle_gentle(best["angle"]) + + # ⚡ [关键修复] 转弯后检查：是否对准了墙？ + # 如果转过去了，但前方距离很近 (<1.5m)，说明冲过头了，对准了墙 + # 这时候需要做一个横向平移修正 + time.sleep(0.5) + check_f, check_l, check_r = get_lidar_info() + if check_f < 1.5: + print(" ⚠️ 检测到过冲 (对准墙壁) -> 尝试横向修正") + # 尝试左右平移看看哪边空 + # 这是一个盲猜逻辑：通常往回退一点(机身反方向平移)能对准路口 + # 假设我们刚刚右转，说明路口在机身右后方，如果对准了墙，说明机身太靠前 + # 我们尝试向后倒一点点 (Body Frame X 负方向) + client.moveByVelocityBodyFrameAsync(-0.5, 0, 0, 1.0, vehicle_name=VEHICLE_NAME).join() + + print(" 💨 缓慢进入...") + curr_z = client.simGetVehiclePose(vehicle_name=VEHICLE_NAME).position.z_val + vz_fix = calculate_z_correction(curr_z) + + # 缓慢推进进入 + client.moveByVelocityBodyFrameAsync(CRUISE_SPEED, 0, float(vz_fix), 2.0, + drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=airsim.YawMode(is_rate=True, yaw_or_rate=0), + vehicle_name=VEHICLE_NAME).join() + return True else: - print("⚠️ 全是死路! 掉头并封锁") - memory.mark_forbidden(pos.x_val, pos.y_val, client) - turn_rel(180) - return False # 被迫掉头 + print("⚠️ 绝境! 后撤封锁...") + client.moveByVelocityBodyFrameAsync(-1.0, 0, 0, 2.0, vehicle_name=VEHICLE_NAME).join() # 慢速后撤 + rad = math.radians(curr_yaw) + memory.mark_forbidden(pos.x_val + math.cos(rad) * 2.5, pos.y_val + math.sin(rad) * 2.5, client) + turn_to_angle_gentle(180) + return False + candidates.sort(key=lambda x: (x["score"], 1 if x["angle"] == 0 else 0, x["dist"]), reverse=True) try: - # 强制冷却时间 (防止刚转完头又觉得不对劲) cooldown_until = 0 - while True: - # 记录足迹 pos = client.simGetVehiclePose(vehicle_name=VEHICLE_NAME).position memory.mark_visited(pos.x_val, pos.y_val, client) - f_d, l_d, r_d = get_lidar_info() - # 状态判定 + vz_fix = calculate_z_correction(pos.z_val) + vy_fix = calculate_y_correction(l_d, r_d) + is_stuck = f_d < STOP_DIST - # 只有当侧面非常宽敞(>5m)，且没在冷却期内，才视为岔路 - is_junction = (l_d > 5.0 or r_d > 5.0) and time.time() > cooldown_until + # 只要侧面距离 > 2.5米 (放宽一点)，立刻触发 + is_junction = (l_d > 2.5 or r_d > 2.5) and (time.time() > cooldown_until) - # --- 优先级 1: 看到出口 (Exit) --- - # 如果前方一片空旷 (>15米)，说明要出去了，无视所有逻辑直接冲 if f_d > EXIT_DIST_THRESHOLD: - print(f"\r[🎉 发现出口!] 前方开阔 {f_d:.1f}m - 全速前进!", end="") - client.moveByVelocityBodyFrameAsync(CRUISE_SPEED * 1.5, 0, 0, 0.1, + print(f"\r[🚀 冲刺] 开阔地 {f_d:.1f}m", end="") + client.moveByVelocityBodyFrameAsync(3.0, 0, float(vz_fix), 0.1, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=True, yaw_or_rate=0), vehicle_name=VEHICLE_NAME).join() - continue # 跳过后面所有逻辑 + continue - # --- 优先级 2: 遇阻 --- if is_stuck: - print(f"\r[🛑 遇阻] 前方 {f_d:.1f}m", end="") + print(f"\r[🛑 遇阻] {f_d:.1f}m", end="") scan_and_decide() - # 决策完后，给 3秒 冷却时间，让它先飞离路口，别原地纠结 - cooldown_until = time.time() + 3.0 + cooldown_until = time.time() + 2.0 - # --- 优先级 3: 岔路 --- elif is_junction: - print(f"\r[✨ 岔路] 左:{l_d:.1f}m 右:{r_d:.1f}m", end="") - print(" -> 决策...") - # 往前送 2米 - client.moveByVelocityBodyFrameAsync(CRUISE_SPEED, 0, 0, 1.5, - drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, - yaw_mode=airsim.YawMode(is_rate=True, yaw_or_rate=0), - vehicle_name=VEHICLE_NAME).join() + print(f"\r[🚪 发现路口] 左:{l_d:.1f}m 右:{r_d:.1f}m -> ⚡ 急刹决策", end="") + # ⚡ 关键修改：取消向前送的动作！立即停车！ + client.moveByVelocityAsync(0, 0, 0, 0.5, vehicle_name=VEHICLE_NAME).join() scan_and_decide() cooldown_until = time.time() + 3.0 - # --- 优先级 4: 巡航 --- else: - print(f"\r[🚀 巡航] 前:{f_d:.1f}m ", end="", flush=True) - - z_curr = client.simGetVehiclePose(vehicle_name=VEHICLE_NAME).position.z_val - vz = (TARGET_HEIGHT - z_curr) * 1.0 - - # 简单的居中 - vy = 0 - if l_d < 2.0 and r_d < 2.0: - vy = (l_d - r_d) * 0.5 - vy = np.clip(vy, -1.0, 1.0) - + print(f"\r[🚀 巡航] H:{pos.z_val:.1f} Y:{vy_fix:.2f}", end="", flush=True) client.moveByVelocityBodyFrameAsync( - vx=CRUISE_SPEED, vy=float(vy), vz=float(vz), duration=0.1, + vx=CRUISE_SPEED, + vy=float(vy_fix), + vz=float(vz_fix), + duration=0.1, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=True, yaw_or_rate=0), vehicle_name=VEHICLE_NAME