diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 3c950b3fc7..9e53e27276 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -41,6 +41,15 @@ 'MAX_AREA': 10000, # 最大检测面积(像素) 'UPDATE_INTERVAL': 1.0, # 检测更新间隔(秒) 'MEMORY_TIME': 5.0, # 物体记忆时间(秒),避免重复计数 + }, + + # 蓝色物体检测参数(新增) + 'BLUE_OBJECT_DETECTION': { + 'ENABLED': True, # 启用蓝色物体检测 + 'MIN_AREA': 50, # 最小检测面积(像素) + 'MAX_AREA': 10000, # 最大检测面积(像素) + 'UPDATE_INTERVAL': 1.0, # 检测更新间隔(秒) + 'MEMORY_TIME': 5.0, # 物体记忆时间(秒) } } @@ -85,6 +94,14 @@ 'DETECTION_RADIUS': 10.0, # 检测半径(米) 'MIN_DISTANCE': 2.0, # 最小接近距离(米) 'EXPLORATION_BONUS': 0.5, # 探索奖励分数 + }, + + # 蓝色物体探索参数(新增) + 'BLUE_OBJECT_EXPLORATION': { + 'ATTRACTION_GAIN': 1.2, # 蓝色物体吸引力增益 + 'DETECTION_RADIUS': 8.0, # 检测半径(米) + 'MIN_DISTANCE': 2.0, # 最小接近距离(米) + 'EXPLORATION_BONUS': 0.3, # 探索奖励分数 } } @@ -110,6 +127,7 @@ 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒),建议30-50 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 + 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体(新增) } # ==================== 系统与安全参数 ==================== @@ -142,6 +160,12 @@ 'UPPER1': [10, 255, 255], # 红色上限1 'LOWER2': [170, 120, 70], # 红色下限2(170-180度) 'UPPER2': [180, 255, 255], # 红色上限2 + }, + + # 蓝色物体检测颜色范围(HSV空间)(新增) + 'BLUE_COLOR_RANGE': { + 'LOWER': [100, 150, 50], # 蓝色下限 + 'UPPER': [130, 255, 255], # 蓝色上限 } } @@ -154,6 +178,7 @@ 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 'SAVE_RED_OBJECT_IMAGES': False, # 是否保存检测到红色物体的图像 + 'SAVE_BLUE_OBJECT_IMAGES': False, # 是否保存检测到蓝色物体的图像(新增) } # ==================== 数据记录参数 ==================== @@ -167,6 +192,7 @@ 'PERFORMANCE_MONITORING': True, # 启用性能监控 'SYSTEM_METRICS_INTERVAL': 5.0, # 系统指标记录间隔(秒) 'RECORD_RED_OBJECTS': True, # 记录红色物体信息 + 'RECORD_BLUE_OBJECTS': True, # 记录蓝色物体信息(新增) } # ==================== 性能监控参数 ==================== diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index a99c3c8bf9..292541a1a1 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,10 +1,10 @@ """ -AirSimNH 感知驱动自主探索无人机 - 智能决策增强版(红色物体检测版) +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版(红色与蓝色物体检测版) 核心:视觉感知 → 语义理解 → 智能决策 → 安全执行 集成:配置管理、日志系统、异常恢复、前视窗口显示 新增:向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -新增:性能监控与数据闭环系统、红色物体检测与记录 -版本: 3.3 (红色物体检测版) +新增:性能监控与数据闭环系统、红色与蓝色物体检测与记录 +版本: 3.4 (双色物体检测版) """ import airsim @@ -48,15 +48,20 @@ class DefaultConfig: 'OPENNESS_THRESHOLD': 0.7}, 'RED_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.0}} + 'MEMORY_TIME': 5.0}, + 'BLUE_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, + 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, + 'MEMORY_TIME': 5.0}} DISPLAY = {'WINDOW_WIDTH': 640, 'WINDOW_HEIGHT': 480, 'ENABLE_SHARPENING': True, - 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True} + 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True, + 'SHOW_BLUE_OBJECTS': True} SYSTEM = {'LOG_LEVEL': 'INFO', 'LOG_TO_FILE': True, 'LOG_FILENAME': 'drone_log.txt', 'MAX_RECONNECT_ATTEMPTS': 3, 'RECONNECT_DELAY': 2.0, 'ENABLE_HEALTH_CHECK': True, 'HEALTH_CHECK_INTERVAL': 20} CAMERA = {'DEFAULT_NAME': "0", 'RED_COLOR_RANGE': {'LOWER1': [0, 120, 70], 'UPPER1': [10, 255, 255], - 'LOWER2': [170, 120, 70], 'UPPER2': [180, 255, 255]}} + 'LOWER2': [170, 120, 70], 'UPPER2': [180, 255, 255]}, + 'BLUE_COLOR_RANGE': {'LOWER': [100, 150, 50], 'UPPER': [130, 255, 255]}} MANUAL = { 'CONTROL_SPEED': 3.0, 'ALTITUDE_SPEED': 2.0, @@ -91,13 +96,16 @@ class DefaultConfig: 'TARGET_LIFETIME': 15.0, 'TARGET_REACHED_DISTANCE': 3.0, 'RED_OBJECT_EXPLORATION': {'ATTRACTION_GAIN': 1.5, 'DETECTION_RADIUS': 10.0, - 'MIN_DISTANCE': 2.0, 'EXPLORATION_BONUS': 0.5} + 'MIN_DISTANCE': 2.0, 'EXPLORATION_BONUS': 0.5}, + 'BLUE_OBJECT_EXPLORATION': {'ATTRACTION_GAIN': 1.2, 'DETECTION_RADIUS': 8.0, + 'MIN_DISTANCE': 2.0, 'EXPLORATION_BONUS': 0.3} } DEBUG = { 'SAVE_PERCEPTION_IMAGES': False, 'IMAGE_SAVE_INTERVAL': 50, 'LOG_DECISION_DETAILS': False, - 'SAVE_RED_OBJECT_IMAGES': False + 'SAVE_RED_OBJECT_IMAGES': False, + 'SAVE_BLUE_OBJECT_IMAGES': False } DATA_RECORDING = { 'ENABLED': True, @@ -108,7 +116,8 @@ class DefaultConfig: 'JSON_FILENAME': 'flight_data.json', 'PERFORMANCE_MONITORING': True, 'SYSTEM_METRICS_INTERVAL': 5.0, - 'RECORD_RED_OBJECTS': True + 'RECORD_RED_OBJECTS': True, + 'RECORD_BLUE_OBJECTS': True } PERFORMANCE = { 'ENABLE_REALTIME_METRICS': True, @@ -133,6 +142,7 @@ class FlightState(Enum): MANUAL = "手动控制" PLANNING = "路径规划" RED_OBJECT_INSPECTION = "红色物体检查" + BLUE_OBJECT_INSPECTION = "蓝色物体检查" # 新增 @dataclass @@ -148,6 +158,19 @@ class RedObject: visited: bool = False +@dataclass +class BlueObject: # 新增蓝色物体数据结构 + """蓝色物体数据结构""" + id: int + position: Tuple[float, float, float] + pixel_position: Tuple[int, int] + size: float + confidence: float + timestamp: float + last_seen: float + visited: bool = False + + class Vector2D: """二维向量类""" def __init__(self, x=0.0, y=0.0): @@ -232,6 +255,7 @@ def __init__(self, resolution=2.0, grid_size=50): self.obstacle_grid = np.zeros((grid_size, grid_size), dtype=bool) self.visit_time = np.zeros((grid_size, grid_size), dtype=np.float32) self.red_object_grid = np.zeros((grid_size, grid_size), dtype=bool) + self.blue_object_grid = np.zeros((grid_size, grid_size), dtype=bool) # 新增 self.current_idx = (self.half_size, self.half_size) self.frontier_cells = set() @@ -317,7 +341,21 @@ def update_red_objects(self, red_objects): if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: self.red_object_grid[nx, ny] = True - def get_best_exploration_target(self, current_pos, red_objects=None): + def update_blue_objects(self, blue_objects): # 新增 + self.blue_object_grid.fill(False) + + for obj in blue_objects: + grid_x, grid_y = self.world_to_grid(obj.position[0], obj.position[1]) + + radius = 1 + 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.blue_object_grid[nx, ny] = True + + def get_best_exploration_target(self, current_pos, red_objects=None, blue_objects=None): + # 优先检查红色物体 if red_objects and len(red_objects) > 0: nearest_obj = None min_distance = float('inf') @@ -334,6 +372,23 @@ def get_best_exploration_target(self, current_pos, red_objects=None): if nearest_obj and min_distance < 15.0: return (nearest_obj.position[0], nearest_obj.position[1]) + # 其次检查蓝色物体 + if blue_objects and len(blue_objects) > 0: # 新增 + nearest_obj = None + min_distance = float('inf') + current_x, current_y = current_pos + + for obj in blue_objects: + if not obj.visited: + distance = math.sqrt((obj.position[0] - current_x)**2 + + (obj.position[1] - current_y)**2) + if distance < min_distance: + min_distance = distance + nearest_obj = obj + + if nearest_obj and min_distance < 12.0: + return (nearest_obj.position[0], nearest_obj.position[1]) + if not self.frontier_cells: angle = random.uniform(0, 2 * math.pi) distance = 10.0 @@ -360,11 +415,15 @@ def get_best_exploration_target(self, current_pos, red_objects=None): if self.red_object_grid[fx, fy]: red_bonus = config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] + blue_bonus = 0.0 # 新增 + if self.blue_object_grid[fx, fy]: + blue_bonus = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] + score = ( config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - distance_cost * 0.3 + - red_bonus + red_bonus + blue_bonus # 新增蓝色奖励 ) if score > best_score: @@ -391,7 +450,9 @@ def visualize_grid(self, size=300): elif self.obstacle_grid[x, y]: color = (0, 0, 255) elif self.red_object_grid[x, y]: - color = (0, 100, 255) + color = (0, 100, 255) # 红色物体显示为橙色 + elif self.blue_object_grid[x, y]: + color = (255, 100, 0) # 蓝色物体显示为青色 elif self.grid[x, y] > 0.7: color = (200, 200, 200) elif self.grid[x, y] > 0.3: @@ -452,6 +513,7 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi } self.red_objects_detected = [] + self.blue_objects_detected = [] # 新增 self.csv_columns = [ 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', @@ -463,7 +525,8 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi 'battery_level', 'cpu_usage', 'memory_usage', 'loop_time', 'grid_frontiers', 'grid_explored', 'vector_field_magnitude', 'adaptive_speed_factor', 'decision_making_time', 'perception_time', - 'red_objects_count', 'red_objects_detected', 'red_objects_visited' + 'red_objects_count', 'red_objects_detected', 'red_objects_visited', + 'blue_objects_count', 'blue_objects_detected', 'blue_objects_visited' # 新增 ] if self.enable_csv: @@ -528,6 +591,28 @@ def record_red_object(self, red_object): except Exception as e: print(f"⚠️ 记录红色物体时出错: {e}") + def record_blue_object(self, blue_object): # 新增 + try: + blue_object_data = { + 'id': blue_object.id, + 'position': blue_object.position, + 'pixel_position': blue_object.pixel_position, + 'size': blue_object.size, + 'confidence': blue_object.confidence, + 'timestamp': blue_object.timestamp, + 'visited': blue_object.visited + } + + self.blue_objects_detected.append(blue_object_data) + + if 'blue_objects' not in self.json_data: + self.json_data['blue_objects'] = [] + + self.json_data['blue_objects'].append(blue_object_data) + + except Exception as e: + print(f"⚠️ 记录蓝色物体时出错: {e}") + def _collect_system_metrics(self): try: cpu_percent = psutil.cpu_percent(interval=0.1) @@ -576,6 +661,7 @@ def save_json_data(self): try: self._calculate_performance_stats() + # 红色物体统计 if 'red_objects' in self.json_data: red_count = len(self.json_data['red_objects']) visited_count = sum(1 for obj in self.json_data['red_objects'] if obj.get('visited', False)) @@ -585,6 +671,16 @@ def save_json_data(self): 'visit_rate': visited_count / red_count if red_count > 0 else 0 } + # 蓝色物体统计(新增) + if 'blue_objects' in self.json_data: + blue_count = len(self.json_data['blue_objects']) + visited_count = sum(1 for obj in self.json_data['blue_objects'] if obj.get('visited', False)) + self.json_data['blue_objects_summary'] = { + 'total_detected': blue_count, + 'total_visited': visited_count, + 'visit_rate': visited_count / blue_count if blue_count > 0 else 0 + } + with open(self.json_filename, 'w', encoding='utf-8') as f: json.dump(self.json_data, f, indent=2, ensure_ascii=False) @@ -666,6 +762,11 @@ def generate_performance_report(self): visited_count = sum(1 for obj in self.json_data['red_objects'] if obj.get('visited', False)) report += f"红色物体检测: 总数{red_count}个, 已访问{visited_count}个\n" + if 'blue_objects' in self.json_data: # 新增 + blue_count = len(self.json_data['blue_objects']) + visited_count = sum(1 for obj in self.json_data['blue_objects'] if obj.get('visited', False)) + report += f"蓝色物体检测: 总数{blue_count}个, 已访问{visited_count}个\n" + report += "="*60 + "\n" warnings = [] @@ -704,6 +805,9 @@ class PerceptionResult: red_objects: List[RedObject] = None red_objects_count: int = 0 red_objects_image: Optional[np.ndarray] = None + blue_objects: List[BlueObject] = None # 新增 + blue_objects_count: int = 0 # 新增 + blue_objects_image: Optional[np.ndarray] = None # 新增 def __post_init__(self): if self.safe_directions is None: @@ -712,6 +816,8 @@ def __post_init__(self): self.obstacle_positions = [] if self.red_objects is None: self.red_objects = [] + if self.blue_objects is None: # 新增 + self.blue_objects = [] class VectorFieldPlanner: @@ -722,6 +828,7 @@ def __init__(self): self.field_radius = config.INTELLIGENT_DECISION['VECTOR_FIELD_RADIUS'] self.smoothing_factor = config.INTELLIGENT_DECISION['SMOOTHING_FACTOR'] self.red_attraction_gain = config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['ATTRACTION_GAIN'] + self.blue_attraction_gain = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['ATTRACTION_GAIN'] # 新增 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']) @@ -729,14 +836,19 @@ def __init__(self): self.vector_history = deque(maxlen=config.INTELLIGENT_DECISION['SMOOTHING_WINDOW_SIZE']) self.current_vector = Vector2D() - def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None): + def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None, blue_objects=None): attraction_vector = self._compute_attraction(current_pos, goal_pos) repulsion_vector = self._compute_repulsion(current_pos, obstacles) red_attraction_vector = Vector2D() + blue_attraction_vector = Vector2D() + if red_objects: red_attraction_vector = self._compute_red_attraction(current_pos, red_objects) - combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + if blue_objects: # 新增 + blue_attraction_vector = self._compute_blue_attraction(current_pos, blue_objects) + + combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + blue_attraction_vector smoothed_vector = self._smooth_vector(combined_vector) limited_vector = self._limit_turn_angle(smoothed_vector) @@ -788,6 +900,22 @@ def _compute_red_attraction(self, current_pos, red_objects): return attraction + def _compute_blue_attraction(self, current_pos, blue_objects): # 新增 + attraction = Vector2D() + + for obj in blue_objects: + if not obj.visited: + dx = obj.position[0] - current_pos[0] + dy = obj.position[1] - current_pos[1] + distance = math.sqrt(dx**2 + dy**2) + + if distance < config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['DETECTION_RADIUS']: + strength = self.blue_attraction_gain / max(1.0, distance) + direction = Vector2D(dx, dy).normalize() + attraction += direction * strength + + return attraction + def _smooth_vector(self, new_vector): self.vector_history.append(new_vector) @@ -1097,14 +1225,20 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str red_objects_count = info.get('red_objects_count', 0) red_objects_visited = info.get('red_objects_visited', 0) - if red_objects_count > 0: + blue_objects_count = info.get('blue_objects_count', 0) # 新增 + blue_objects_visited = info.get('blue_objects_visited', 0) # 新增 + + if red_objects_count > 0 or blue_objects_count > 0: red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" + blue_text = f"蓝色物体: {blue_objects_visited}/{blue_objects_count}" cv2.putText(image, red_text, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 255), 2) + cv2.putText(image, blue_text, (10, 110), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 0), 2) decision_info = info.get('decision_info', {}) if decision_info: - y_pos = 120 if red_objects_count > 0 else 90 + y_pos = 130 if red_objects_count > 0 or blue_objects_count > 0 else 90 for key, value in decision_info.items(): if key == 'vector_angle': cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), @@ -1116,7 +1250,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str y_pos += 20 if is_manual and manual_info: - y_start = 130 if red_objects_count > 0 else 100 + y_start = 150 if (red_objects_count > 0 or blue_objects_count > 0) else 100 for i, line in enumerate(manual_info): y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), @@ -1124,7 +1258,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - elif not is_manual and red_objects_count == 0: + elif not is_manual and red_objects_count == 0 and blue_objects_count == 0: 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), @@ -1179,12 +1313,12 @@ 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) self.client = None @@ -1240,6 +1374,12 @@ def __init__(self, drone_name=""): self.red_detection_interval = config.PERCEPTION['RED_OBJECT_DETECTION']['UPDATE_INTERVAL'] self.red_object_memory_time = config.PERCEPTION['RED_OBJECT_DETECTION']['MEMORY_TIME'] + self.blue_objects = [] # 新增 + self.blue_object_id_counter = 0 # 新增 + self.last_blue_detection_time = 0 # 新增 + self.blue_detection_interval = config.PERCEPTION['BLUE_OBJECT_DETECTION']['UPDATE_INTERVAL'] # 新增 + self.blue_object_memory_time = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MEMORY_TIME'] # 新增 + self.visited_positions = deque(maxlen=100) self.loop_count = 0 @@ -1273,6 +1413,8 @@ def __init__(self, drone_name=""): 'min_loop_time': 100.0, 'red_objects_detected': 0, 'red_objects_visited': 0, + 'blue_objects_detected': 0, # 新增 + 'blue_objects_visited': 0, # 新增 } self.front_display = None @@ -1284,11 +1426,13 @@ 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" 智能决策: 向量场避障 + 网格探索 + 红色物体检测") + self.logger.info(f" 智能决策: 向量场避障 + 网格探索 + 双色物体检测") if config.DATA_RECORDING['ENABLED']: self.logger.info(f" 数据记录: CSV + JSON 格式") if config.PERCEPTION['RED_OBJECT_DETECTION']['ENABLED']: self.logger.info(f" 红色物体检测: 已启用") + if config.PERCEPTION['BLUE_OBJECT_DETECTION']['ENABLED']: + self.logger.info(f" 蓝色物体检测: 已启用") def _setup_logging(self): self.logger = logging.getLogger('DroneExplorer') @@ -1489,7 +1633,7 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - label = f"ID:{red_object.id} ({confidence:.2f})" + label = f"R:{red_object.id} ({confidence:.2f})" cv2.putText(marked_image, label, (x, y-10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) @@ -1506,6 +1650,133 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra return red_objects, marked_image + def _detect_blue_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[BlueObject], np.ndarray]: # 新增 + blue_objects = [] + marked_image = image.copy() if image is not None else None + + if not config.PERCEPTION['BLUE_OBJECT_DETECTION']['ENABLED'] or image is None: + return blue_objects, marked_image + + try: + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + + lower_blue = np.array(config.CAMERA['BLUE_COLOR_RANGE']['LOWER']) + upper_blue = np.array(config.CAMERA['BLUE_COLOR_RANGE']['UPPER']) + + blue_mask = cv2.inRange(hsv, lower_blue, upper_blue) + + kernel = np.ones((5, 5), np.uint8) + blue_mask = cv2.morphologyEx(blue_mask, cv2.MORPH_CLOSE, kernel) + blue_mask = cv2.morphologyEx(blue_mask, cv2.MORPH_OPEN, kernel) + + contours, _ = cv2.findContours(blue_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + try: + state = self.client.getMultirotorState(vehicle_name=self.drone_name) + drone_pos = state.kinematics_estimated.position + orientation = state.kinematics_estimated.orientation + roll, pitch, yaw = airsim.to_eularian_angles(orientation) + except: + drone_pos = None + yaw = 0.0 + + for contour in contours: + area = cv2.contourArea(contour) + min_area = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MIN_AREA'] + max_area = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MAX_AREA'] + + if min_area <= area <= max_area: + x, y, w, h = cv2.boundingRect(contour) + center_x = x + w // 2 + center_y = y + h // 2 + + aspect_ratio = w / h if h > 0 else 1.0 + confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) + + world_pos = None + if drone_pos is not None and depth_array is not None: + try: + if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: + distance = depth_array[center_y, center_x] + + if 0.5 < distance < 50.0: + height, width = depth_array.shape + fov_h = math.radians(90) + + pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) + pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) + + z = distance + x_rel = z * math.tan(pixel_angle_x) + y_rel = z * math.tan(pixel_angle_y) + + world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val + world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val + world_z = drone_pos.z_val + + world_pos = (world_x, world_y, world_z) + except: + pass + + blue_object = BlueObject( + id=self.blue_object_id_counter, + position=world_pos if world_pos else (0.0, 0.0, 0.0), + pixel_position=(center_x, center_y), + size=area, + confidence=confidence, + timestamp=time.time(), + last_seen=time.time(), + visited=False + ) + + is_new_object = True + for existing_obj in self.blue_objects: + if self._is_same_object_blue(blue_object, existing_obj): + existing_obj.last_seen = time.time() + existing_obj.pixel_position = blue_object.pixel_position + existing_obj.confidence = max(existing_obj.confidence, confidence) + if world_pos: + existing_obj.position = world_pos + blue_object = existing_obj + is_new_object = False + break + + if is_new_object: + self.blue_object_id_counter += 1 + blue_objects.append(blue_object) + self.stats['blue_objects_detected'] += 1 + self.logger.info(f"🔵 检测到蓝色物体 #{blue_object.id} (置信度: {confidence:.2f})") + + if self.data_logger and config.DATA_RECORDING['RECORD_BLUE_OBJECTS']: + self.data_logger.record_blue_object(blue_object) + else: + blue_objects.append(blue_object) + + if marked_image is not None: + color = (255, 100, 0) + if blue_object.visited: + color = (0, 200, 0) + + cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) + cv2.circle(marked_image, (center_x, center_y), 5, color, -1) + + label = f"B:{blue_object.id} ({confidence:.2f})" + cv2.putText(marked_image, label, (x, y-10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + + current_time = time.time() + self.blue_objects = [obj for obj in self.blue_objects + if current_time - obj.last_seen < self.blue_object_memory_time] + + visited_count = sum(1 for obj in self.blue_objects if obj.visited) + if len(blue_objects) > 0: + self.logger.debug(f"🔵 当前蓝色物体: {len(self.blue_objects)}个, 已访问: {visited_count}个") + + except Exception as e: + self.logger.warning(f"⚠️ 蓝色物体检测失败: {e}") + + return blue_objects, marked_image + def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2.0) -> bool: if obj1.position != (0.0, 0.0, 0.0) and obj2.position != (0.0, 0.0, 0.0): distance = math.sqrt( @@ -1522,6 +1793,22 @@ def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2 return pixel_distance < 50 and time_diff < 5.0 + def _is_same_object_blue(self, obj1: BlueObject, obj2: BlueObject, distance_threshold=2.0) -> bool: # 新增 + if obj1.position != (0.0, 0.0, 0.0) and obj2.position != (0.0, 0.0, 0.0): + distance = math.sqrt( + (obj1.position[0] - obj2.position[0])**2 + + (obj1.position[1] - obj2.position[1])**2 + ) + return distance < distance_threshold + + pixel_distance = math.sqrt( + (obj1.pixel_position[0] - obj2.pixel_position[0])**2 + + (obj1.pixel_position[1] - obj2.pixel_position[1])**2 + ) + time_diff = abs(obj1.timestamp - obj2.timestamp) + + return pixel_distance < 50 and time_diff < 5.0 + def _check_red_object_proximity(self, current_pos): for obj in self.red_objects: if not obj.visited: @@ -1552,6 +1839,36 @@ def _check_red_object_proximity(self, current_pos): return False + def _check_blue_object_proximity(self, current_pos): # 新增 + for obj in self.blue_objects: + if not obj.visited: + distance = math.sqrt( + (obj.position[0] - current_pos[0])**2 + + (obj.position[1] - current_pos[1])**2 + ) + + min_distance = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['MIN_DISTANCE'] + if distance < min_distance: + obj.visited = True + obj.last_seen = time.time() + self.stats['blue_objects_visited'] += 1 + + self.logger.info(f"✅ 已访问蓝色物体 #{obj.id} (距离: {distance:.1f}m)") + + if self.data_logger: + event_data = { + 'object_id': obj.id, + 'position': obj.position, + 'distance': distance, + 'timestamp': time.time() + } + self.data_logger.record_event('blue_object_visited', event_data) + + self.change_state(FlightState.BLUE_OBJECT_INSPECTION) + return True + + return False + def get_depth_perception(self) -> PerceptionResult: result = PerceptionResult() self.stats['perception_cycles'] += 1 @@ -1649,6 +1966,8 @@ def get_depth_perception(self) -> PerceptionResult: img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR) current_time = time.time() + + # 检测红色物体 if current_time - self.last_red_detection_time >= self.red_detection_interval: red_objects, red_marked_image = self._detect_red_objects(img_bgr, depth_array) result.red_objects = red_objects @@ -1656,6 +1975,14 @@ def get_depth_perception(self) -> PerceptionResult: result.red_objects_image = red_marked_image self.last_red_detection_time = current_time + # 检测蓝色物体(新增) + if current_time - self.last_blue_detection_time >= self.blue_detection_interval: + blue_objects, blue_marked_image = self._detect_blue_objects(img_bgr, depth_array) + result.blue_objects = blue_objects + result.blue_objects_count = len(blue_objects) + result.blue_objects_image = blue_marked_image + self.last_blue_detection_time = current_time + result.front_image = img_bgr display_info = self._prepare_display_info(result) @@ -1672,7 +1999,15 @@ def get_depth_perception(self) -> PerceptionResult: grid_img = self.exploration_grid.visualize_grid(size=150) additional_images = {'grid': grid_img} if grid_img is not None else {} - display_image = result.red_objects_image if result.red_objects_image is not None else img_bgr + # 合并红色和蓝色物体标记 + display_image = img_bgr.copy() + if config.DISPLAY['SHOW_RED_OBJECTS'] and result.red_objects_image is not None: + red_mask = cv2.inRange(result.red_objects_image, (0, 100, 0), (0, 255, 255)) + display_image[red_mask > 0] = result.red_objects_image[red_mask > 0] + + if config.DISPLAY['SHOW_BLUE_OBJECTS'] and result.blue_objects_image is not None: + blue_mask = cv2.inRange(result.blue_objects_image, (255, 100, 0), (255, 255, 255)) + display_image[blue_mask > 0] = result.blue_objects_image[blue_mask > 0] if config.PERFORMANCE['ENABLE_REALTIME_METRICS']: cpu_usage = psutil.cpu_percent(interval=0) @@ -1689,7 +2024,7 @@ def get_depth_perception(self) -> PerceptionResult: 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}, 红色物体={result.red_objects_count}个") + f"开阔度={result.open_space_score:.2f}, 红色物体={result.red_objects_count}个, 蓝色物体={result.blue_objects_count}个") except Exception as e: if "ClientException" in str(type(e)) or "Connection" in str(e): @@ -1729,6 +2064,9 @@ def _record_flight_data(self, perception: PerceptionResult): red_objects_count = perception.red_objects_count red_objects_visited = sum(1 for obj in self.red_objects if obj.visited) + blue_objects_count = perception.blue_objects_count # 新增 + blue_objects_visited = sum(1 for obj in self.blue_objects if obj.visited) # 新增 + data_dict = { 'timestamp': datetime.now().isoformat(), 'loop_count': self.loop_count, @@ -1759,6 +2097,9 @@ def _record_flight_data(self, perception: PerceptionResult): 'red_objects_count': red_objects_count, 'red_objects_detected': self.stats['red_objects_detected'], 'red_objects_visited': red_objects_visited, + 'blue_objects_count': blue_objects_count, # 新增 + 'blue_objects_detected': self.stats['blue_objects_detected'], # 新增 + 'blue_objects_visited': blue_objects_visited, # 新增 } self.data_logger.record_flight_data(data_dict) @@ -1820,6 +2161,9 @@ def _update_exploration_grid(self, perception: PerceptionResult): if perception.red_objects: self.exploration_grid.update_red_objects(perception.red_objects) + if perception.blue_objects: # 新增 + self.exploration_grid.update_blue_objects(perception.blue_objects) + self.stats['grid_updates'] += 1 except Exception as e: @@ -1837,6 +2181,8 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: 'loop_count': self.loop_count, 'red_objects_count': perception.red_objects_count, 'red_objects_visited': sum(1 for obj in self.red_objects if obj.visited), + 'blue_objects_count': perception.blue_objects_count, # 新增 + 'blue_objects_visited': sum(1 for obj in self.blue_objects if obj.visited), # 新增 } if hasattr(self, 'last_decision_info'): @@ -1883,6 +2229,10 @@ def _get_manual_control_info(self): visited_count = sum(1 for obj in self.red_objects if obj.visited) info_lines.append(f"红色物体: {visited_count}/{len(self.red_objects)}") + if self.blue_objects: # 新增 + visited_count = sum(1 for obj in self.blue_objects if obj.visited) + info_lines.append(f"蓝色物体: {visited_count}/{len(self.blue_objects)}") + if self.manual_control_start > 0: elapsed = time.time() - self.manual_control_start info_lines.append(f"手动模式: {elapsed:.1f}秒") @@ -2071,6 +2421,7 @@ def run_manual_control(self): pos = state.kinematics_estimated.position current_pos = (pos.x_val, pos.y_val) self._check_red_object_proximity(current_pos) + self._check_blue_object_proximity(current_pos) # 新增 except: pass @@ -2105,6 +2456,7 @@ def run_manual_control(self): print("手动控制模式已结束") print(f"控制时间: {manual_time:.1f}秒") print(f"检测到红色物体: {self.stats['red_objects_detected']}个") + print(f"检测到蓝色物体: {self.stats['blue_objects_detected']}个") # 新增 print("="*60) print("请选择下一步:") print(" 1. 继续自动探索") @@ -2166,6 +2518,9 @@ def run_perception_loop(self): if self._check_red_object_proximity(current_pos): time.sleep(2) self.change_state(FlightState.EXPLORING) + if self._check_blue_object_proximity(current_pos): # 新增 + time.sleep(2) + self.change_state(FlightState.EXPLORING) except: pass @@ -2268,7 +2623,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( (pos.x_val, pos.y_val), - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) self.target_update_time = current_time @@ -2288,7 +2644,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float if self.exploration_target is None: self.exploration_target = self.exploration_grid.get_best_exploration_target( current_pos, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) self.target_update_time = time.time() @@ -2296,7 +2653,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float current_pos, self.exploration_target, perception.obstacle_positions, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) @@ -2318,6 +2676,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 'grid_score': len(self.exploration_grid.frontier_cells) / 100.0, 'speed_factor': speed_factor, 'red_objects_in_view': perception.red_objects_count, + 'blue_objects_in_view': perception.blue_objects_count, # 新增 'decision_time': time.time() - decision_start } @@ -2339,7 +2698,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float current_pos, None, perception.obstacle_positions, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) if avoid_vector.magnitude() > 0.1: @@ -2357,6 +2717,11 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float time.sleep(2) self.change_state(FlightState.EXPLORING) + elif self.state == FlightState.BLUE_OBJECT_INSPECTION: # 新增 + target_vx, target_vy = 0.0, 0.0 + time.sleep(2) + self.change_state(FlightState.EXPLORING) + elif self.state == FlightState.EMERGENCY: target_vx, target_vy, target_yaw = 0, 0, 0 target_z = max(pos.z_val, -20) @@ -2396,8 +2761,10 @@ def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitu vector_factor = min(1.0, vector_magnitude * 2) red_factor = 0.8 if perception.red_objects_count > 0 else 1.0 + blue_factor = 0.8 if perception.blue_objects_count > 0 else 1.0 # 新增 + color_factor = min(red_factor, blue_factor) # 新增 - speed_factor = open_factor * obs_factor * vector_factor * red_factor * 0.7 + speed_factor = open_factor * obs_factor * vector_factor * color_factor * 0.7 # 更新 speed_factor = max( config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], @@ -2410,7 +2777,8 @@ def _execute_control_decision(self, decision): try: target_vx, target_vy, target_z, target_yaw = decision - if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING, FlightState.RED_OBJECT_INSPECTION]: + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING, + FlightState.RED_OBJECT_INSPECTION, FlightState.BLUE_OBJECT_INSPECTION]: self.client.moveByVelocityZAsync( target_vx, target_vy, target_z, 0.5, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, @@ -2451,6 +2819,8 @@ def _report_status(self, exploration_start, perception): f"| 开阔度={perception.open_space_score:.2f}") self.logger.info(f" 红色物体: 检测到{perception.red_objects_count}个 " f"| 已访问{self.stats['red_objects_visited']}个") + self.logger.info(f" 蓝色物体: 检测到{perception.blue_objects_count}个 " + f"| 已访问{self.stats['blue_objects_visited']}个") # 新增 self.logger.info(f" 智能决策: 向量场{self.stats['vector_field_updates']}次 " f"| 网格更新{self.stats['grid_updates']}次") self.logger.info(f" 探索网格: 前沿{len(self.exploration_grid.frontier_cells)}个") @@ -2533,6 +2903,8 @@ def _generate_summary_report(self): self.logger.info(f" 检测到障碍次数: {self.stats['obstacles_detected']}") self.logger.info(f" 红色物体检测: {self.stats['red_objects_detected']}个") self.logger.info(f" 红色物体访问: {self.stats['red_objects_visited']}个") + self.logger.info(f" 蓝色物体检测: {self.stats['blue_objects_detected']}个") # 新增 + self.logger.info(f" 蓝色物体访问: {self.stats['blue_objects_visited']}个") # 新增 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)}") @@ -2548,7 +2920,7 @@ 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") @@ -2562,6 +2934,8 @@ def _generate_summary_report(self): f.write(f"手动控制时间: {self.stats['manual_control_time']:.1f}秒\n") f.write(f"红色物体检测总数: {self.stats['red_objects_detected']}个\n") f.write(f"红色物体已访问数: {self.stats['red_objects_visited']}个\n") + f.write(f"蓝色物体检测总数: {self.stats['blue_objects_detected']}个\n") + f.write(f"蓝色物体已访问数: {self.stats['blue_objects_visited']}个\n") f.write(f"异常捕获次数: {self.stats['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write(f"平均循环时间: {self.stats['average_loop_time']*1000:.1f}ms\n") @@ -2616,7 +2990,7 @@ def emergency_stop(self): 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']}") @@ -2628,17 +3002,18 @@ def main(): print(" • PID平滑飞行控制") print(" • 自适应速度调整") print(" • 性能监控与数据闭环") - print(" • 红色物体检测与记录") + print(" • 红色与蓝色物体检测与记录") print("=" * 70) print("数据记录:") print(f" • CSV格式: {config.DATA_RECORDING.get('SAVE_TO_CSV', False)}") print(f" • JSON格式: {config.DATA_RECORDING.get('SAVE_TO_JSON', False)}") print(f" • 性能监控: {config.DATA_RECORDING.get('PERFORMANCE_MONITORING', False)}") print(f" • 红色物体记录: {config.DATA_RECORDING.get('RECORD_RED_OBJECTS', False)}") + print(f" • 蓝色物体记录: {config.DATA_RECORDING.get('RECORD_BLUE_OBJECTS', False)}") print("=" * 70) print("\n请选择运行模式:") - print(" 1. 智能探索模式 (AI自主决策,包含红色物体检测)") + print(" 1. 智能探索模式 (AI自主决策,包含双色物体检测)") print(" 2. 手动控制模式 (键盘控制)") print(" 3. 混合模式 (先自动探索,后可切换)") print("=" * 50) @@ -2659,7 +3034,7 @@ def signal_handler(sig, frame): if mode_choice == '1': print("\n" + "="*50) - print("启动智能探索模式(含红色物体检测)") + print("启动智能探索模式(含双色物体检测)") print("="*50) explorer.run_perception_loop() @@ -2683,7 +3058,7 @@ def signal_handler(sig, frame): print("启动混合模式") print("="*50) - explorer.logger.info("🔍 开始智能探索(含红色物体检测)...") + explorer.logger.info("🔍 开始智能探索(含双色物体检测)...") original_time = config.EXPLORATION['TOTAL_TIME'] explorer.exploration_time = min(60, original_time) @@ -2693,7 +3068,7 @@ def signal_handler(sig, frame): print("\n" + "="*50) print("智能探索阶段结束") print(f"检测到红色物体: {explorer.stats['red_objects_detected']}个") - print(f"已访问红色物体: {explorer.stats['red_objects_visited']}个") + print(f"检测到蓝色物体: {explorer.stats['blue_objects_detected']}个") print("请选择下一步:") print(" 1. 进入手动控制模式") print(" 2. 继续智能探索")