diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 2c20f8cd25..a6ec61ff74 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -4,6 +4,8 @@ 所有可调参数集中在此管理 """ +import math + # ==================== 飞行与探索参数 ==================== EXPLORATION = { 'TOTAL_TIME': 120, # 总探索时间 (秒),建议120-180 @@ -34,6 +36,42 @@ } } +# ==================== 智能决策参数 ==================== +INTELLIGENT_DECISION = { + # 向量场算法参数 + 'VECTOR_FIELD_RADIUS': 8.0, # 向量场影响半径 (米) + 'OBSTACLE_REPULSION_GAIN': 3.0, # 障碍物排斥增益 + 'GOAL_ATTRACTION_GAIN': 2.0, # 目标吸引力增益 + 'SMOOTHING_FACTOR': 0.3, # 向量平滑因子 + 'MIN_TURN_ANGLE_DEG': 10, # 最小转弯角度 (度) + 'MAX_TURN_ANGLE_DEG': 60, # 最大转弯角度 (度) + + # 探索网格参数 + 'GRID_RESOLUTION': 2.0, # 网格分辨率 (米) + 'GRID_SIZE': 50, # 网格大小 (单元格数) + 'INFORMATION_GAIN_DECAY': 0.95, # 信息增益衰减率 + 'EXPLORATION_FRONTIER_THRESHOLD': 0.3, # 探索前沿阈值 + + # 控制参数 + 'PID_KP': 1.5, # 比例系数 + 'PID_KI': 0.05, # 积分系数 + 'PID_KD': 0.2, # 微分系数 + 'SMOOTHING_WINDOW_SIZE': 5, # 平滑窗口大小 + + # 自适应参数 + 'ADAPTIVE_SPEED_ENABLED': True, # 启用自适应速度 + 'MIN_SPEED_FACTOR': 0.3, # 最小速度因子 + 'MAX_SPEED_FACTOR': 1.5, # 最大速度因子 + + # 探索策略权重 + 'MEMORY_WEIGHT': 0.7, # 记忆权重 (避免重复访问) + 'CURIOUSITY_WEIGHT': 0.3, # 好奇心权重 (探索新区域) + + # 目标管理 + 'TARGET_LIFETIME': 15.0, # 目标有效期 (秒) + 'TARGET_REACHED_DISTANCE': 3.0, # 目标到达判定距离 (米) +} + # ==================== 手动控制参数 ==================== MANUAL = { 'CONTROL_SPEED': 3.0, # 水平移动速度 (米/秒) @@ -41,7 +79,7 @@ 'YAW_SPEED': 45.0, # 偏航角速度 (度/秒) 'ENABLE_AUTO_HOVER': True, # 松开按键时自动悬停 'DISPLAY_CONTROLS': True, # 在画面显示控制说明 - 'SAFETY_ENABLED': True, # 启用安全限制(高度、速度限制) + 'SAFETY_ENABLED': True, # 启用安全限制 (高度、速度限制) 'MAX_MANUAL_SPEED': 5.0, # 最大手动控制速度 'MIN_ALTITUDE_LIMIT': -5.0, # 最低飞行高度限制 'MAX_ALTITUDE_LIMIT': -30.0, # 最高飞行高度限制 @@ -49,11 +87,12 @@ # ==================== 前视窗口参数 ==================== DISPLAY = { - 'WINDOW_WIDTH': 640, # 窗口宽度 - 'WINDOW_HEIGHT': 480, # 窗口高度 + 'WINDOW_WIDTH': 640, # 窗口宽度 (像素) + 'WINDOW_HEIGHT': 480, # 窗口高度 (像素) 'ENABLE_SHARPENING': True, # 启用图像锐化,改善模糊 'SHOW_INFO_OVERLAY': True, # 显示信息叠加层 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒),建议30-50 + 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) } # ==================== 系统与安全参数 ==================== @@ -67,6 +106,8 @@ 'ENABLE_HEALTH_CHECK': True, # 启用健康检查 'HEALTH_CHECK_INTERVAL': 20, # 健康检查间隔 (循环次数) + + 'EMERGENCY_RESPONSE_TIME': 10.0, # 紧急响应超时时间 (秒) } # ==================== 相机配置 ==================== @@ -74,12 +115,19 @@ CAMERA = { 'DEFAULT_NAME': "0", # 默认相机名称 # 如果"0"无效,可以尝试以下名称: - # 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"] + 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"], + + # 深度相机参数 + 'DEPTH_FOV_DEG': 90, # 深度相机视野 (度) + 'MAX_DEPTH_RANGE': 100.0, # 最大深度范围 (米) } # ==================== 调试参数 ==================== DEBUG = { 'SAVE_PERCEPTION_IMAGES': False, # 是否保存感知图像用于调试 'IMAGE_SAVE_INTERVAL': 50, # 图像保存间隔 (循环次数) - 'LOG_DECISION_DETAILS': False, # 是否记录详细决策信息 + 'LOG_DECISION_DETAILS': True, # 是否记录详细决策信息 + 'SAVE_GRID_VISUALIZATION': True, # 是否保存网格可视化 + 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 + 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 } \ No newline at end of file diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index c88534a5e0..a099153357 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,9 +1,9 @@ """ -AirSimNH 感知驱动自主探索无人机 - 工程优化版 +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版(修复版) 核心:视觉感知 → 语义理解 → 智能决策 → 安全执行 集成:配置管理、日志系统、异常恢复、前视窗口显示 -新增:修复按键冲突,改进手动控制 -版本: 2.5 (修复按键冲突改进版) +新增:向量场避障算法、基于网格的信息增益探索、平滑飞行控制 +版本: 3.1 (修复配置和健康检查问题) """ import airsim @@ -18,10 +18,11 @@ import queue import signal import sys -from typing import Tuple, List, Optional, Dict +from typing import Tuple, List, Optional, Dict, Set import traceback import logging from datetime import datetime +import random # ============ 导入配置文件 ============ try: @@ -57,6 +58,40 @@ class DefaultConfig: 'MIN_ALTITUDE_LIMIT': -5.0, 'MAX_ALTITUDE_LIMIT': -30.0 } + # 新增:智能决策参数 - 修复键名问题 + INTELLIGENT_DECISION = { + 'VECTOR_FIELD_RADIUS': 8.0, # 向量场影响半径 + 'OBSTACLE_REPULSION_GAIN': 3.0, # 障碍物排斥增益 + 'GOAL_ATTRACTION_GAIN': 2.0, # 目标吸引力增益 + 'SMOOTHING_FACTOR': 0.3, # 向量平滑因子 + 'MIN_TURN_ANGLE_DEG': 10, # 最小转弯角度(度) + 'MAX_TURN_ANGLE_DEG': 60, # 最大转弯角度(度) + + 'GRID_RESOLUTION': 2.0, # 网格分辨率(米) + 'GRID_SIZE': 50, # 网格大小(单元格数) + 'INFORMATION_GAIN_DECAY': 0.95, # 信息增益衰减率 + 'EXPLORATION_FRONTIER_THRESHOLD': 0.3,# 探索前沿阈值 + + 'PID_KP': 1.5, # 比例系数 + 'PID_KI': 0.05, # 积分系数 + 'PID_KD': 0.2, # 微分系数 + 'SMOOTHING_WINDOW_SIZE': 5, # 平滑窗口大小 + + 'ADAPTIVE_SPEED_ENABLED': True, # 启用自适应速度 + 'MIN_SPEED_FACTOR': 0.3, # 最小速度因子 + 'MAX_SPEED_FACTOR': 1.5, # 最大速度因子 + + 'MEMORY_WEIGHT': 0.7, # 记忆权重(避免重复访问) + 'CURIOUSITY_WEIGHT': 0.3, # 好奇心权重(探索新区域) + + 'TARGET_LIFETIME': 15.0, # 目标有效期(秒) + 'TARGET_REACHED_DISTANCE': 3.0, # 目标到达判定距离(米) + } + DEBUG = { + 'SAVE_PERCEPTION_IMAGES': False, + 'IMAGE_SAVE_INTERVAL': 50, + 'LOG_DECISION_DETAILS': False + } config = DefaultConfig() @@ -70,6 +105,268 @@ class FlightState(Enum): LANDING = "降落" EMERGENCY = "紧急状态" MANUAL = "手动控制" + PLANNING = "路径规划" + + +class Vector2D: + """二维向量类""" + def __init__(self, x=0.0, y=0.0): + self.x = x + self.y = y + + def __add__(self, other): + return Vector2D(self.x + other.x, self.y + other.y) + + def __sub__(self, other): + return Vector2D(self.x - other.x, self.y - other.y) + + def __mul__(self, scalar): + return Vector2D(self.x * scalar, self.y * scalar) + + def __truediv__(self, scalar): + return Vector2D(self.x / scalar, self.y / scalar) + + def magnitude(self): + return math.sqrt(self.x**2 + self.y**2) + + def normalize(self): + mag = self.magnitude() + if mag > 0: + return Vector2D(self.x / mag, self.y / mag) + return Vector2D() + + def rotate(self, angle): + """旋转向量""" + cos_a = math.cos(angle) + sin_a = math.sin(angle) + return Vector2D( + self.x * cos_a - self.y * sin_a, + self.x * sin_a + self.y * cos_a + ) + + def to_tuple(self): + return (self.x, self.y) + + @staticmethod + def from_angle(angle, magnitude=1.0): + return Vector2D(magnitude * math.cos(angle), magnitude * math.sin(angle)) + + +class PIDController: + """PID控制器类""" + def __init__(self, kp, ki, kd, integral_limit=5.0, output_limit=10.0): + self.kp = kp + self.ki = ki + self.kd = kd + self.integral_limit = integral_limit + self.output_limit = output_limit + + self.previous_error = 0.0 + self.integral = 0.0 + self.previous_time = time.time() + + def update(self, error, dt=None): + if dt is None: + current_time = time.time() + dt = current_time - self.previous_time + self.previous_time = current_time + + # 积分项 + self.integral += error * dt + self.integral = max(-self.integral_limit, min(self.integral_limit, self.integral)) + + # 微分项 + derivative = (error - self.previous_error) / dt if dt > 0 else 0.0 + self.previous_error = error + + # 计算输出 + output = self.kp * error + self.ki * self.integral + self.kd * derivative + return max(-self.output_limit, min(self.output_limit, output)) + + +class ExplorationGrid: + """探索网格地图类""" + def __init__(self, resolution=2.0, grid_size=50): + self.resolution = resolution + self.grid_size = grid_size + self.half_size = grid_size // 2 + + # 初始化网格:0=未知,1=已探索,0.x=部分探索 + self.grid = np.zeros((grid_size, grid_size), dtype=np.float32) + + # 信息增益缓存 + self.information_gain = np.zeros((grid_size, grid_size), dtype=np.float32) + + # 障碍物标记 + self.obstacle_grid = np.zeros((grid_size, grid_size), dtype=bool) + + # 访问时间记录 + self.visit_time = np.zeros((grid_size, grid_size), dtype=np.float32) + + # 当前位置索引 + self.current_idx = (self.half_size, self.half_size) + + # 探索前沿 + self.frontier_cells = set() + + print(f"🗺️ 初始化探索网格: {grid_size}x{grid_size}, 分辨率: {resolution}m") + + def world_to_grid(self, world_x, world_y): + """世界坐标转网格索引""" + grid_x = int(world_x / self.resolution) + self.half_size + grid_y = int(world_y / self.resolution) + self.half_size + + # 边界检查 + grid_x = max(0, min(self.grid_size - 1, grid_x)) + grid_y = max(0, min(self.grid_size - 1, grid_y)) + + return (grid_x, grid_y) + + def grid_to_world(self, grid_x, grid_y): + """网格索引转世界坐标""" + world_x = (grid_x - self.half_size) * self.resolution + world_y = (grid_y - self.half_size) * self.resolution + return (world_x, world_y) + + def update_position(self, world_x, world_y): + """更新当前位置""" + self.current_idx = self.world_to_grid(world_x, world_y) + + # 标记当前位置为已探索 + x, y = self.current_idx + radius = 3 # 探索半径(网格单元) + + for dx in range(-radius, radius + 1): + for dy in range(-radius, radius + 1): + nx, ny = x + dx, y + dy + if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: + distance = math.sqrt(dx**2 + dy**2) + exploration_value = max(0, 1.0 - distance / radius) + self.grid[nx, ny] = max(self.grid[nx, ny], exploration_value) + self.visit_time[nx, ny] = time.time() + + # 更新探索前沿 + self._update_frontiers() + + def _update_frontiers(self): + """更新探索前沿""" + self.frontier_cells.clear() + + for x in range(1, self.grid_size - 1): + for y in range(1, self.grid_size - 1): + # 如果当前单元格已探索,检查邻居是否有未探索的 + if self.grid[x, y] > 0.7: # 足够探索 + neighbors = [ + (x-1, y), (x+1, y), (x, y-1), (x, y+1), + (x-1, y-1), (x-1, y+1), (x+1, y-1), (x+1, y+1) + ] + + for nx, ny in neighbors: + if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: + if self.grid[nx, ny] < 0.3 and not self.obstacle_grid[nx, ny]: + # 计算信息增益:基于未探索邻居数量 + unexplored_neighbors = 0 + for nnx in range(nx-1, nx+2): + for nny in range(ny-1, ny+2): + if 0 <= nnx < self.grid_size and 0 <= nny < self.grid_size: + if self.grid[nnx, nny] < 0.3: + unexplored_neighbors += 1 + + self.information_gain[nx, ny] = unexplored_neighbors / 9.0 + self.frontier_cells.add((nx, ny)) + + def update_obstacles(self, obstacles_world): + """更新障碍物信息""" + for obs_x, obs_y in obstacles_world: + grid_x, grid_y = self.world_to_grid(obs_x, obs_y) + + # 标记障碍物及其周围区域 + radius = 2 + for dx in range(-radius, radius + 1): + for dy in range(-radius, radius + 1): + nx, ny = grid_x + dx, grid_y + dy + if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: + self.obstacle_grid[nx, ny] = True + self.grid[nx, ny] = 0.0 # 障碍物区域不可探索 + + def get_best_exploration_target(self, current_pos): + """获取最佳探索目标""" + if not self.frontier_cells: + # 如果没有前沿,返回随机方向 + angle = random.uniform(0, 2 * math.pi) + distance = 10.0 # 10米外 + return ( + current_pos[0] + distance * math.cos(angle), + current_pos[1] + distance * math.sin(angle) + ) + + # 计算每个前沿单元格的得分 + best_score = -1 + best_target = None + current_x, current_y = current_pos + + for fx, fy in self.frontier_cells: + # 计算信息增益 + info_gain = self.information_gain[fx, fy] + + # 计算距离成本 + world_x, world_y = self.grid_to_world(fx, fy) + distance = math.sqrt((world_x - current_x)**2 + (world_y - current_y)**2) + distance_cost = min(1.0, distance / 30.0) # 归一化 + + # 计算最近访问时间(避免重复) + time_since_visit = time.time() - self.visit_time[fx, fy] + time_factor = min(1.0, time_since_visit / 60.0) # 1分钟内衰减 + + # 综合得分 + score = ( + config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - + distance_cost * 0.3 + ) + + if score > best_score: + best_score = score + best_target = (world_x, world_y) + + return best_target + + def visualize_grid(self, size=300): + """可视化网格""" + if self.grid.size == 0: + return None + + img_size = min(size, self.grid_size * 5) + img = np.zeros((img_size, img_size, 3), dtype=np.uint8) + + cell_size = img_size // self.grid_size + + for x in range(self.grid_size): + for y in range(self.grid_size): + color = (0, 0, 0) + + if (x, y) == self.current_idx: + color = (0, 255, 0) # 当前位置:绿色 + elif self.obstacle_grid[x, y]: + color = (0, 0, 255) # 障碍物:红色 + elif self.grid[x, y] > 0.7: + color = (200, 200, 200) # 已探索:灰色 + elif self.grid[x, y] > 0.3: + color = (100, 100, 100) # 部分探索:深灰色 + elif (x, y) in self.frontier_cells: + # 前沿单元格:根据信息增益着色 + gain = self.information_gain[x, y] + color = (0, int(255 * gain), int(255 * (1 - gain))) # 绿到黄 + + # 绘制单元格 + x1 = x * cell_size + y1 = y * cell_size + x2 = (x + 1) * cell_size + y2 = (y + 1) * cell_size + + cv2.rectangle(img, (y1, x1), (y2, x2), color, -1) + + return img @dataclass @@ -83,14 +380,134 @@ class PerceptionResult: recommended_height: float = config.PERCEPTION['HEIGHT_STRATEGY']['DEFAULT'] safe_directions: List[float] = None front_image: Optional[np.ndarray] = None + obstacle_positions: List[Tuple[float, float]] = None # 新增:障碍物位置列表 def __post_init__(self): if self.safe_directions is None: self.safe_directions = [] + if self.obstacle_positions is None: + self.obstacle_positions = [] + + +class VectorFieldPlanner: + """向量场规划器""" + def __init__(self): + self.repulsion_gain = config.INTELLIGENT_DECISION['OBSTACLE_REPULSION_GAIN'] + self.attraction_gain = config.INTELLIGENT_DECISION['GOAL_ATTRACTION_GAIN'] + self.field_radius = config.INTELLIGENT_DECISION['VECTOR_FIELD_RADIUS'] + self.smoothing_factor = config.INTELLIGENT_DECISION['SMOOTHING_FACTOR'] + + # 修复:使用正确的配置键名,将角度转换为弧度 + self.min_turn_angle = math.radians(config.INTELLIGENT_DECISION['MIN_TURN_ANGLE_DEG']) + self.max_turn_angle = math.radians(config.INTELLIGENT_DECISION['MAX_TURN_ANGLE_DEG']) + + # 历史向量用于平滑 + self.vector_history = deque(maxlen=config.INTELLIGENT_DECISION['SMOOTHING_WINDOW_SIZE']) + self.current_vector = Vector2D() + + def compute_vector(self, current_pos, goal_pos, obstacles): + """计算合成向量""" + # 目标吸引力 + attraction_vector = self._compute_attraction(current_pos, goal_pos) + + # 障碍物排斥力 + repulsion_vector = self._compute_repulsion(current_pos, obstacles) + + # 合成向量 + combined_vector = attraction_vector + repulsion_vector + + # 向量平滑 + smoothed_vector = self._smooth_vector(combined_vector) + + # 限制转向角度 + limited_vector = self._limit_turn_angle(smoothed_vector) + + self.current_vector = limited_vector + return limited_vector + + def _compute_attraction(self, current_pos, goal_pos): + """计算目标吸引力""" + if goal_pos is None: + return Vector2D() + + # 计算朝向目标的向量 + dx = goal_pos[0] - current_pos[0] + dy = goal_pos[1] - current_pos[1] + distance = math.sqrt(dx**2 + dy**2) + + if distance < 0.1: # 已到达目标 + return Vector2D() + + # 吸引力与距离成反比(接近目标时减速) + strength = min(self.attraction_gain, self.attraction_gain / max(1.0, distance)) + + return Vector2D(dx, dy).normalize() * strength + + def _compute_repulsion(self, current_pos, obstacles): + """计算障碍物排斥力""" + repulsion = Vector2D() + + for obs_x, obs_y in obstacles: + dx = current_pos[0] - obs_x + dy = current_pos[1] - obs_y + distance = math.sqrt(dx**2 + dy**2) + + if distance < self.field_radius and distance > 0.1: + # 排斥力与距离平方成反比 + strength = self.repulsion_gain * (1.0 / distance**2) + direction = Vector2D(dx, dy).normalize() + repulsion += direction * strength + + return repulsion + + def _smooth_vector(self, new_vector): + """平滑向量""" + self.vector_history.append(new_vector) + + if len(self.vector_history) < 2: + return new_vector + + # 指数加权平均 + smoothed = Vector2D() + total_weight = 0.0 + + for i, vec in enumerate(reversed(self.vector_history)): + weight = math.exp(-i * self.smoothing_factor) + smoothed += vec * weight + total_weight += weight + + if total_weight > 0: + smoothed = smoothed / total_weight + + return smoothed + + def _limit_turn_angle(self, vector): + """限制转向角度""" + if self.current_vector.magnitude() < 0.1: + return vector + + current_angle = math.atan2(self.current_vector.y, self.current_vector.x) + new_angle = math.atan2(vector.y, vector.x) + + angle_diff = new_angle - current_angle + # 将角度差归一化到[-π, π] + angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi + + # 限制角度变化率 + if abs(angle_diff) > self.max_turn_angle: + angle_diff = math.copysign(self.max_turn_angle, angle_diff) + elif abs(angle_diff) < self.min_turn_angle and vector.magnitude() > 0.1: + angle_diff = math.copysign(self.min_turn_angle, angle_diff) + + # 应用限制后的角度 + magnitude = vector.magnitude() + limited_angle = current_angle + angle_diff + + return Vector2D.from_angle(limited_angle, magnitude) class FrontViewDisplay: - """前视画面显示管理器 - 修复按键冲突改进版""" + """前视画面显示管理器""" def __init__(self, window_name="无人机前视画面", width=None, height=None, enable_sharpening=None, show_info=None): @@ -110,8 +527,8 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, # 手动控制状态 self.manual_mode = False - self.key_states = {} # 当前按下的键 - self.last_keys = {} # 上次按下的键(用于检测释放) + self.key_states = {} + self.last_keys = {} # 控制退出标志 self.exit_manual_flag = False @@ -148,7 +565,8 @@ def stop(self): self.display_thread.join(timeout=2.0) def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, - manual_info: Optional[List[str]] = None): + manual_info: Optional[List[str]] = None, + additional_images: Optional[Dict] = None): """更新要显示的图像""" if not self.display_active or self.paused or image_data is None: return @@ -172,6 +590,7 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, 'image': image_data.copy(), 'info': info.copy() if info else {}, 'manual_info': manual_info.copy() if manual_info else [], + 'additional_images': additional_images.copy() if additional_images else {}, 'timestamp': time.time() } @@ -197,7 +616,7 @@ def should_exit_manual(self): return self.exit_manual_flag def _display_loop(self): - """显示线程主循环 - 修复按键冲突""" + """显示线程主循环""" cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) cv2.resizeWindow(self.window_name, self.window_width, self.window_height) @@ -221,6 +640,7 @@ def _display_loop(self): display_image = None info = {} manual_info = [] + additional_images = {} try: # 获取最新图像 @@ -229,6 +649,7 @@ def _display_loop(self): display_image = packet['image'] info = packet['info'] manual_info = packet['manual_info'] + additional_images = packet.get('additional_images', {}) # 更新统计 self._update_stats() @@ -246,7 +667,7 @@ def _display_loop(self): if display_image is not None: # 添加信息叠加 if self.show_info: - display_image = self._add_info_overlay(display_image, info, manual_info) + display_image = self._add_info_overlay(display_image, info, manual_info, additional_images) cv2.imshow(self.window_name, display_image) elif self.paused: @@ -266,13 +687,11 @@ def _display_loop(self): # 根据模式处理按键 if self.manual_mode: - # 手动控制模式下的按键处理 self._handle_manual_mode_key(key) else: - # 通用模式下的窗口控制按键 self._handle_window_control_key(key, display_image) - # 更新按键状态(用于检测按键释放) + # 更新按键状态 self._update_key_states(current_keys) # 检查窗口是否被关闭 @@ -282,7 +701,6 @@ def _display_loop(self): self.display_active = False break except: - # 窗口可能已被销毁 self.display_active = False break @@ -310,8 +728,6 @@ def _handle_manual_mode_key(self, key): if key == 32: # 空格 print("⏸️ 悬停指令") - # 其他手动控制键已经在key_states中记录,会在主循环中处理 - def _handle_window_control_key(self, key, display_image): """处理通用窗口控制按键""" key_char = chr(key).lower() if 0 <= key <= 255 else '' @@ -359,7 +775,8 @@ def _update_stats(self): self.display_stats['frame_count'] = 0 self.display_stats['last_update'] = now - def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None) -> np.ndarray: + def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None, + additional_images: Dict = None) -> np.ndarray: """在图像上叠加状态信息""" if image is None or image.size == 0: return image @@ -371,8 +788,9 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str # 判断是否为手动模式 is_manual = info.get('state', '') == "手动控制" - # 信息栏高度(手动模式需要更多空间显示控制说明) - info_height = 140 if (is_manual and manual_info) else 100 + # 如果有附加图像(如网格图),调整信息栏高度 + grid_img = additional_images.get('grid') if additional_images else None + info_height = 180 if (is_manual and manual_info) or grid_img is not None else 100 # 创建半透明信息栏 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) @@ -389,18 +807,32 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) + # 智能决策信息 + decision_info = info.get('decision_info', {}) + if decision_info: + y_pos = 90 + for key, value in decision_info.items(): + if key == 'vector_angle': + cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) + y_pos += 20 + elif key == 'grid_score': + cv2.putText(image, f"探索得分: {value:.2f}", (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 255), 1) + y_pos += 20 + # 手动控制信息 if is_manual and manual_info: + y_start = 130 for i, line in enumerate(manual_info): - y_pos = 90 + i * 20 + y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) - # 显示手动控制提示 cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) elif not is_manual: - # 障碍物信息(如果不是手动模式) + # 障碍物信息 obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), @@ -411,6 +843,27 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) + # 如果有网格图,显示在右上角 + if grid_img is not None and grid_img.size > 0: + grid_size = 150 + grid_resized = cv2.resize(grid_img, (grid_size, grid_size)) + + # 在图像右上角放置网格图 + x_offset = width - grid_size - 10 + y_offset = info_height + 10 + + if y_offset + grid_size < height: + # 创建网格图的背景 + cv2.rectangle(image, (x_offset-2, y_offset-2), + (x_offset+grid_size+2, y_offset+grid_size+2), + (255, 255, 255), 1) + + # 将网格图放入指定位置 + image[y_offset:y_offset+grid_size, x_offset:x_offset+grid_size] = grid_resized + + cv2.putText(image, "探索网格", (x_offset, y_offset-5), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + return image except Exception as e: print(f"⚠️ 添加信息叠加层出错: {e}") @@ -428,13 +881,13 @@ def _save_screenshot(self, image: Optional[np.ndarray]): class PerceptiveExplorer: - """基于感知的自主探索无人机 - 修复按键冲突改进版""" + """基于感知的自主探索无人机 - 智能决策增强版(修复版)""" def __init__(self, drone_name=""): # 初始化日志系统 self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 修复按键冲突改进版") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 智能决策增强版(修复版)") self.logger.info("=" * 60) # 初始化AirSim连接 @@ -471,6 +924,28 @@ def __init__(self, drone_name=""): self.base_height = config.EXPLORATION['BASE_HEIGHT'] self.takeoff_height = config.EXPLORATION['TAKEOFF_HEIGHT'] + # 智能决策组件 + self.vector_planner = VectorFieldPlanner() + self.exploration_grid = ExplorationGrid( + resolution=config.INTELLIGENT_DECISION['GRID_RESOLUTION'], + grid_size=config.INTELLIGENT_DECISION['GRID_SIZE'] + ) + + # PID控制器 + self.velocity_pid = PIDController( + config.INTELLIGENT_DECISION['PID_KP'], + config.INTELLIGENT_DECISION['PID_KI'], + config.INTELLIGENT_DECISION['PID_KD'] + ) + self.height_pid = PIDController(1.0, 0.1, 0.3) + + # 探索目标 + self.exploration_target = None + self.target_update_time = 0 + # 修复:使用配置中的目标有效期 + self.target_lifetime = config.INTELLIGENT_DECISION.get('TARGET_LIFETIME', 15.0) + self.target_reached_distance = config.INTELLIGENT_DECISION.get('TARGET_REACHED_DISTANCE', 3.0) + # 记忆系统 self.visited_positions = deque(maxlen=100) @@ -490,6 +965,8 @@ def __init__(self, drone_name=""): 'state_changes': 0, 'front_image_updates': 0, 'manual_control_time': 0.0, + 'vector_field_updates': 0, + 'grid_updates': 0, } # 前视窗口 @@ -503,7 +980,7 @@ def __init__(self, drone_name=""): self.logger.info("✅ 系统初始化完成") self.logger.info(f" 开始时间: {datetime.now().strftime('%H:%M:%S')}") self.logger.info(f" 预计探索时长: {self.exploration_time}秒") - self.logger.info(f" 日志级别: {config.SYSTEM['LOG_LEVEL']}") + self.logger.info(f" 智能决策: 向量场避障 + 网格探索") def _setup_logging(self): """配置日志系统""" @@ -555,6 +1032,22 @@ def _connect_to_airsim(self): self.logger.error("请检查:1. AirSim是否启动 2. 网络设置 3. 防火墙") sys.exit(1) + def _check_connection_health(self): + """检查连接健康状态 - 新增方法""" + try: + # 简单的心跳检查 + self.client.ping() + self.logger.debug("✅ 连接健康检查通过") + return True + except Exception as e: + self.logger.warning(f"⚠️ 连接健康检查失败: {e}") + # 尝试重新连接 + try: + self._connect_to_airsim() + return True + except: + return False + def _setup_front_display(self): """初始化前视显示窗口""" try: @@ -571,12 +1064,12 @@ def _setup_front_display(self): self.front_display = None def get_depth_perception(self) -> PerceptionResult: - """获取并分析深度图像,理解环境""" + """获取并分析深度图像,理解环境 - 增强版(修复健康检查)""" result = PerceptionResult() self.stats['perception_cycles'] += 1 try: - # 健康检查 + # 健康检查 - 修复:使用新方法 if config.SYSTEM['ENABLE_HEALTH_CHECK']: current_time = time.time() if current_time - self.last_successful_loop > 10.0: @@ -633,6 +1126,9 @@ def get_depth_perception(self) -> PerceptionResult: if dir_distance > self.depth_threshold_safe: result.safe_directions.append(angle_rad) + # 提取障碍物位置(用于向量场) + result.obstacle_positions = self._extract_obstacle_positions(depth_array, h, w) + # 地形分析 ground_region = depth_array[3 * h // 4:, :] if ground_region.size > 10: @@ -682,13 +1178,20 @@ def get_depth_perception(self) -> PerceptionResult: # 准备显示信息 display_info = self._prepare_display_info(result) + # 更新探索网格 + self._update_exploration_grid(result) + # 更新前视窗口 if self.front_display: manual_info = None if self.state == FlightState.MANUAL: manual_info = self._get_manual_control_info() - self.front_display.update_image(img_bgr, display_info, manual_info) + # 获取网格可视化图像 + grid_img = self.exploration_grid.visualize_grid(size=150) + additional_images = {'grid': grid_img} if grid_img is not None else {} + + self.front_display.update_image(img_bgr, display_info, manual_info, additional_images) self.stats['front_image_updates'] += 1 except Exception as e: @@ -697,36 +1200,108 @@ def get_depth_perception(self) -> PerceptionResult: self.last_successful_loop = time.time() # 详细日志 - if self.loop_count % 50 == 0: + if self.loop_count % 50 == 0 and config.DEBUG.get('LOG_DECISION_DETAILS', False): self.logger.debug(f"感知结果: 障碍={result.has_obstacle}, 距离={result.obstacle_distance:.1f}m, " - f"开阔度={result.open_space_score:.2f}") - - except airsim.ClientException as e: - self.logger.error(f"❌ AirSim客户端异常: {e}") - self.stats['exceptions_caught'] += 1 - self._handle_airsim_exception(e) - except ConnectionError as e: - self.logger.error(f"❌ 网络连接异常: {e}") - self.stats['exceptions_caught'] += 1 - self._handle_connection_error() - except Exception as e: - self.logger.error(f"❌ 感知过程中发生未知异常: {e}") - self.logger.debug(f"异常详情: {traceback.format_exc()}") - self.stats['exceptions_caught'] += 1 + f"开阔度={result.open_space_score:.2f}, 障碍物数={len(result.obstacle_positions)}") + + except Exception as e: # 修复:捕获通用异常 + if "ClientException" in str(type(e)) or "Connection" in str(e): + self.logger.error(f"❌ AirSim客户端异常: {e}") + self.stats['exceptions_caught'] += 1 + # 尝试重新连接 + self._check_connection_health() + else: + self.logger.error(f"❌ 感知过程中发生未知异常: {e}") + self.logger.debug(f"异常详情: {traceback.format_exc()}") + self.stats['exceptions_caught'] += 1 return result + def _extract_obstacle_positions(self, depth_array, height, width): + """从深度图像中提取障碍物位置""" + obstacles = [] + + try: + # 获取当前无人机位置和朝向 + state = self.client.getMultirotorState(vehicle_name=self.drone_name) + pos = state.kinematics_estimated.position + orientation = state.kinematics_estimated.orientation + roll, pitch, yaw = airsim.to_eularian_angles(orientation) + + # 只处理近距离障碍物 + near_mask = depth_array < self.depth_threshold_near * 1.5 + + # 采样障碍点 + step = 4 # 采样步长 + for i in range(0, height, step): + for j in range(0, width, step): + if near_mask[i, j]: + distance = depth_array[i, j] + + # 将像素坐标转换为相对于相机的3D坐标 + # 简化模型:假设相机水平视角为90度 + fov_h = math.radians(90) + pixel_angle_x = (j - width/2) / (width/2) * (fov_h/2) + pixel_angle_y = (i - height/2) / (height/2) * (fov_h/2) + + # 计算相对位置 + z = distance # 深度方向 + x = z * math.tan(pixel_angle_x) + y = z * math.tan(pixel_angle_y) + + # 旋转到世界坐标系(考虑无人机偏航角) + world_x = x * math.cos(yaw) - y * math.sin(yaw) + pos.x_val + world_y = x * math.sin(yaw) + y * math.cos(yaw) + pos.y_val + + obstacles.append((world_x, world_y)) + + # 限制障碍物数量 + max_obstacles = 20 + if len(obstacles) > max_obstacles: + obstacles = random.sample(obstacles, max_obstacles) + + except Exception as e: + self.logger.warning(f"⚠️ 提取障碍物位置失败: {e}") + + return obstacles + + def _update_exploration_grid(self, perception: PerceptionResult): + """更新探索网格""" + try: + # 获取当前位置 + state = self.client.getMultirotorState(vehicle_name=self.drone_name) + pos = state.kinematics_estimated.position + + # 更新网格位置 + self.exploration_grid.update_position(pos.x_val, pos.y_val) + + # 更新障碍物 + if perception.obstacle_positions: + self.exploration_grid.update_obstacles(perception.obstacle_positions) + + self.stats['grid_updates'] += 1 + + except Exception as e: + self.logger.warning(f"⚠️ 更新探索网格失败: {e}") + def _prepare_display_info(self, perception: PerceptionResult) -> Dict: """准备显示信息""" try: state = self.client.getMultirotorState(vehicle_name=self.drone_name) pos = state.kinematics_estimated.position - return { + + info = { 'state': self.state.value, 'obstacle_distance': perception.obstacle_distance, 'position': (pos.x_val, pos.y_val, pos.z_val), 'loop_count': self.loop_count, } + + # 添加决策信息 + if hasattr(self, 'last_decision_info'): + info['decision_info'] = self.last_decision_info + + return info except: return {} @@ -864,7 +1439,7 @@ def change_state(self, new_state: FlightState): self.stats['state_changes'] += 1 def run_manual_control(self): - """改进的手动控制模式 - 修复按键冲突""" + """手动控制模式""" self.logger.info("=" * 60) self.logger.info("启动手动控制模式") self.logger.info("=" * 60) @@ -927,10 +1502,9 @@ def run_manual_control(self): self.apply_manual_control() last_control_time = current_time - # 定期获取并显示图像(降低频率,避免冲突) + # 定期获取并显示图像 if current_time - last_image_time >= 0.1: # 10Hz try: - # 只获取前视图像,避免深度图像获取冲突 camera_name = config.CAMERA['DEFAULT_NAME'] responses = self.client.simGetImages([ airsim.ImageRequest( @@ -965,7 +1539,6 @@ def run_manual_control(self): self.front_display.update_image(img_bgr, display_info, manual_info) last_image_time = current_time except Exception as img_error: - # 忽略图像获取错误,继续控制 pass # 短暂休眠 @@ -1015,9 +1588,8 @@ def run_manual_control(self): if choice == '1': self.logger.info("🔄 返回自动探索模式") - # 继续自动探索 remaining_time = self.exploration_time - (time.time() - self.start_time) - if remaining_time > 10: # 至少还有10秒才继续 + if remaining_time > 10: self.exploration_time = remaining_time self.run_perception_loop() else: @@ -1064,7 +1636,7 @@ def run_perception_loop(self): # 1. 感知阶段 perception = self.get_depth_perception() - # 2. 决策阶段 + # 2. 智能决策阶段 decision = self.make_intelligent_decision(perception) # 3. 控制执行阶段 @@ -1092,7 +1664,7 @@ def run_perception_loop(self): self.emergency_stop() def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: - """基于感知结果做出智能决策""" + """基于感知结果做出智能决策 - 增强版(修复配置键名)""" self.stats['decision_cycles'] += 1 try: @@ -1108,55 +1680,117 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.change_state(FlightState.HOVERING) elif self.state == FlightState.HOVERING: + # 扫描环境,选择探索目标 target_yaw = (time.time() % 10) * 0.2 - if len(perception.safe_directions) > 0: + # 检查是否需要更新探索目标 + current_time = time.time() + if (self.exploration_target is None or + current_time - self.target_update_time > self.target_lifetime): + + self.exploration_target = self.exploration_grid.get_best_exploration_target((pos.x_val, pos.y_val)) + self.target_update_time = current_time + + if self.exploration_target: + self.logger.info(f"🎯 新探索目标: {self.exploration_target[0]:.1f}, {self.exploration_target[1]:.1f}") + + if self.exploration_target: self.change_state(FlightState.EXPLORING) elif self.state == FlightState.EXPLORING: if perception.has_obstacle: self.change_state(FlightState.AVOIDING) - target_vx, target_vy = -vel.x_val, -vel.y_val + # 紧急避障:后退 + target_vx, target_vy = -vel.x_val * 2, -vel.y_val * 2 else: - if perception.safe_directions: - current_yaw = airsim.to_eularian_angles( - state.kinematics_estimated.orientation - )[2] - - diverse_dirs = [ - d for d in perception.safe_directions - if abs(d - current_yaw) > math.radians(45) - ] - - if diverse_dirs: - best_dir = diverse_dirs[0] - else: - best_dir = perception.safe_directions[0] - - speed_factor = min(1.0, perception.open_space_score * 1.5) - target_vx = self.preferred_speed * speed_factor * math.cos(best_dir) - target_vy = self.preferred_speed * speed_factor * math.sin(best_dir) - else: - target_z = pos.z_val - 5 - self.change_state(FlightState.AVOIDING) + # 使用向量场算法计算最佳方向 + current_pos = (pos.x_val, pos.y_val) + + # 获取探索目标 + if self.exploration_target is None: + self.exploration_target = self.exploration_grid.get_best_exploration_target(current_pos) + self.target_update_time = time.time() + + # 计算向量场 + vector = self.vector_planner.compute_vector( + current_pos, + self.exploration_target, + perception.obstacle_positions + ) + + # 自适应速度调整 + speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) + + # 应用PID控制平滑速度 + target_speed = self.preferred_speed * speed_factor + current_speed = math.sqrt(vel.x_val**2 + vel.y_val**2) + speed_error = target_speed - current_speed + speed_adjustment = self.velocity_pid.update(speed_error) + + # 计算最终速度向量 + final_vector = vector.normalize() * (target_speed + speed_adjustment) + target_vx = final_vector.x + target_vy = final_vector.y + + self.stats['vector_field_updates'] += 1 + + # 保存决策信息用于显示 + self.last_decision_info = { + 'vector_angle': math.atan2(vector.y, vector.x), + 'vector_magnitude': vector.magnitude(), + 'grid_score': len(self.exploration_grid.frontier_cells) / 100.0, + 'speed_factor': speed_factor + } + + # 检查是否到达目标附近 + if self.exploration_target: + distance_to_target = math.sqrt( + (self.exploration_target[0] - current_pos[0])**2 + + (self.exploration_target[1] - current_pos[1])**2 + ) + if distance_to_target < self.target_reached_distance: # 使用配置的距离 + self.exploration_target = None + self.change_state(FlightState.HOVERING) + self.logger.info("✅ 到达探索目标") elif self.state == FlightState.AVOIDING: if perception.has_obstacle: - if abs(perception.obstacle_direction) < math.radians(30): - target_z = pos.z_val - 3 - target_vx, target_vy = 0, 0 - else: - avoid_dir = perception.obstacle_direction + math.pi - target_vx = 1.5 * math.cos(avoid_dir) - target_vy = 1.5 * math.sin(avoid_dir) + # 使用向量场进行避障 + current_pos = (pos.x_val, pos.y_val) + + # 计算远离障碍物的方向 + avoid_vector = self.vector_planner.compute_vector( + current_pos, + None, # 没有目标,只有排斥力 + perception.obstacle_positions + ) + + if avoid_vector.magnitude() > 0.1: + avoid_vector = avoid_vector.normalize() * 1.5 # 避障速度 + target_vx = avoid_vector.x + target_vy = avoid_vector.y + + # 尝试改变高度 + target_z = pos.z_val - 3 else: - self.change_state(FlightState.HOVERING) + # 障碍物清除,回到探索状态 + self.change_state(FlightState.EXPLORING) time.sleep(1) elif self.state == FlightState.EMERGENCY: target_vx, target_vy, target_yaw = 0, 0, 0 target_z = max(pos.z_val, -20) + elif self.state == FlightState.PLANNING: + # 路径规划状态(预留) + target_vx, target_vy = 0, 0 + target_z = perception.recommended_height + + # 高度PID控制 + height_error = target_z - pos.z_val + height_adjustment = self.height_pid.update(height_error) + target_z += height_adjustment + # 高度安全限制 target_z = max(self.max_altitude, min(self.min_altitude, target_z)) @@ -1166,14 +1800,45 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.logger.error(f"❌ 决策过程异常: {e}") return 0.0, 0.0, self.base_height, 0.0 + def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitude: float) -> float: + """计算自适应速度因子""" + if not config.INTELLIGENT_DECISION['ADAPTIVE_SPEED_ENABLED']: + return 1.0 + + # 基于开阔度调整速度 + open_factor = min(1.0, perception.open_space_score * 1.2) + + # 基于障碍物距离调整速度 + if perception.obstacle_distance < self.depth_threshold_near * 2: + obs_factor = max(0.3, perception.obstacle_distance / (self.depth_threshold_near * 2)) + else: + obs_factor = 1.0 + + # 基于向量场稳定性调整速度 + vector_factor = min(1.0, vector_magnitude * 2) + + # 综合速度因子 + speed_factor = open_factor * obs_factor * vector_factor * 0.7 + + # 限制在允许范围内 + speed_factor = max( + config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], + min(config.INTELLIGENT_DECISION['MAX_SPEED_FACTOR'], speed_factor) + ) + + return speed_factor + def _execute_control_decision(self, decision): - """执行控制决策,增强异常处理""" + """执行控制决策,增强平滑性""" try: target_vx, target_vy, target_z, target_yaw = decision - if self.state in [FlightState.EXPLORING, FlightState.AVOIDING]: + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING]: + # 使用平滑的速度控制 self.client.moveByVelocityZAsync( - target_vx, target_vy, target_z, 0.5, + target_vx, target_vy, target_z, 0.5, # 增加持续时间以平滑 + drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=target_yaw), vehicle_name=self.drone_name ) else: @@ -1208,6 +1873,9 @@ def _report_status(self, exploration_start, perception): self.logger.info(f" 环境感知: 障碍{'有' if perception.has_obstacle else '无'} " f"| 距离={perception.obstacle_distance:.1f}m " f"| 开阔度={perception.open_space_score:.2f}") + self.logger.info(f" 智能决策: 向量场{self.stats['vector_field_updates']}次 " + f"| 网格更新{self.stats['grid_updates']}次") + self.logger.info(f" 探索网格: 前沿{len(self.exploration_grid.frontier_cells)}个") self.logger.info(f" 系统统计: 异常{self.stats['exceptions_caught']}次 " f"| 状态切换{self.stats['state_changes']}次") if self.stats['manual_control_time'] > 0: @@ -1275,6 +1943,9 @@ def _generate_summary_report(self): self.logger.info(f" 探索航点数量: {len(self.visited_positions)}") self.logger.info(f" 状态切换次数: {self.stats['state_changes']}") self.logger.info(f" 检测到障碍次数: {self.stats['obstacles_detected']}") + self.logger.info(f" 向量场计算次数: {self.stats['vector_field_updates']}") + self.logger.info(f" 网格更新次数: {self.stats['grid_updates']}") + self.logger.info(f" 探索前沿数量: {len(self.exploration_grid.frontier_cells)}") self.logger.info(f" 前视图像更新次数: {self.stats['front_image_updates']}") self.logger.info(f" 手动控制时间: {self.stats['manual_control_time']:.1f}秒") self.logger.info(f" 捕获的异常数: {self.stats['exceptions_caught']}") @@ -1284,19 +1955,26 @@ def _generate_summary_report(self): try: report_filename = f"mission_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.txt" with open(report_filename, 'w', encoding='utf-8') as f: - f.write("AirSimNH 无人机任务报告\n") + f.write("AirSimNH 无人机任务报告 (智能决策增强版 - 修复版)\n") f.write("=" * 50 + "\n") f.write(f"生成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}\n") f.write(f"总运行时间: {total_time:.1f}秒\n") f.write(f"总循环次数: {self.loop_count}\n") f.write(f"探索航点数量: {len(self.visited_positions)}\n") f.write(f"状态切换次数: {self.stats['state_changes']}\n") + f.write(f"向量场计算次数: {self.stats['vector_field_updates']}\n") + f.write(f"网格更新次数: {self.stats['grid_updates']}\n") + f.write(f"探索前沿数量: {len(self.exploration_grid.frontier_cells)}\n") f.write(f"手动控制时间: {self.stats['manual_control_time']:.1f}秒\n") f.write(f"异常捕获次数: {self.stats['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write("=" * 50 + "\n") + f.write("智能决策配置:\n") + for key, value in config.INTELLIGENT_DECISION.items(): + f.write(f" {key}: {value}\n") + f.write("=" * 50 + "\n") f.write("飞行航点记录:\n") - for i, pos in enumerate(self.visited_positions[:20]): # 只记录前20个航点 + for i, pos in enumerate(self.visited_positions[:20]): f.write(f" 航点{i+1}: ({pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f})\n") if len(self.visited_positions) > 20: f.write(f" ... 还有{len(self.visited_positions)-20}个航点\n") @@ -1338,16 +2016,22 @@ def main(): """主程序入口""" # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 修复按键冲突改进版") + print("AirSimNH 无人机感知探索系统 - 智能决策增强版(修复版)") print(f"启动时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}") print(f"配置状态: {'已加载' if CONFIG_LOADED else '使用默认配置'}") print(f"日志级别: {config.SYSTEM['LOG_LEVEL']}") print(f"探索时间: {config.EXPLORATION['TOTAL_TIME']}秒") print("=" * 70) + print("智能决策特性:") + print(" • 向量场避障算法 (VFH)") + print(" • 基于网格的信息增益探索") + print(" • PID平滑飞行控制") + print(" • 自适应速度调整") + print("=" * 70) # 用户选择模式 print("\n请选择运行模式:") - print(" 1. 自动探索模式 (AI自主决策)") + print(" 1. 智能探索模式 (AI自主决策)") print(" 2. 手动控制模式 (键盘控制)") print(" 3. 混合模式 (先自动探索,后可切换)") print("=" * 50) @@ -1372,7 +2056,7 @@ def signal_handler(sig, frame): if mode_choice == '1': # 自动探索模式 print("\n" + "="*50) - print("启动自动探索模式") + print("启动智能探索模式") print("="*50) explorer.run_perception_loop() @@ -1401,21 +2085,20 @@ def signal_handler(sig, frame): print("="*50) # 先运行一段时间的自动探索 - explorer.logger.info("🔍 开始自动探索...") + explorer.logger.info("🔍 开始智能探索...") original_time = config.EXPLORATION['TOTAL_TIME'] - # 设置较短的自动探索时间,然后询问 - explorer.exploration_time = min(60, original_time) # 最多自动探索60秒 + explorer.exploration_time = min(60, original_time) # 运行自动探索 explorer.run_perception_loop() - # 如果自动探索正常结束(非紧急停止) + # 如果自动探索正常结束 if not explorer.emergency_flag: print("\n" + "="*50) - print("自动探索阶段结束") + print("智能探索阶段结束") print("请选择下一步:") print(" 1. 进入手动控制模式") - print(" 2. 继续自动探索") + print(" 2. 继续智能探索") print(" 3. 结束任务返航") print("="*50)