Fire Net 搜索 贪心

import numpy as np import matplotlib.pyplot as plt import heapq import time import math from tqdm import tqdm class ThreatMap: def __init__(self, width, height, resolution=100): self.width = width self.height = height self.resolution = resolution # meters per grid cell self.terrain = np.zeros((width, height)) self.radars = [] self.fires = [] self.threat_map = np.zeros((width, height, 3)) # [terrain, radar, fire] self.combined_threat = np.zeros((width, height)) self.weights = np.array([0.2, 0.2, 0.3]) # Initial weights [wg, wR, wAA] def generate_terrain(self, terrain_type='mountain'): if terrain_type == 'mountain': # Generate mountain-like terrain x = np.linspace(0, 10, self.width) y = np.linspace(0, 10, self.height) X, Y = np.meshgrid(x, y) Z = 500 * np.sin(0.5*X) * np.cos(0.5*Y) + 300 self.terrain = Z.T elif terrain_type == 'valley': # Generate valley terrain for i in range(self.width): for j in range(self.height): self.terrain[i, j] = 100 + 50 * np.sin(i/20) + 50 * np.cos(j/20) # Normalize terrain self.terrain = (self.terrain - np.min(self.terrain)) / (np.max(self.terrain) - np.min(self.terrain)) * 1000 def add_radar(self, x, y, direction, max_range, fov=120): self.radars.append({ 'pos': (x, y), 'direction': direction, # in degrees 'max_range': max_range, # in meters 'fov': fov # field of view in degrees }) def add_fire(self, x, y, min_range, max_range): self.fires.append({ 'pos': (x, y), 'min_range': min_range, 'max_range': max_range }) def calculate_terrain_threat(self, x, y, z): h_max = np.max(self.terrain[x, y]) h_min = np.min(self.terrain[x, y]) return (h_max - h_min) / (z - h_max + 1e-5) def calculate_radar_threat(self, x, y, z): threat = 0 for radar in self.radars: rx, ry = radar['pos'] dx = (x - rx) * self.resolution dy = (y - ry) * self.resolution distance = np.sqrt(dx**2 + dy**2) # Check if within max range if distance > radar['max_range']: continue # Check if within field of view angle = math.degrees(math.atan2(dy, dx)) angle_diff = abs((angle - radar['direction'] + 180) % 360 - 180) if angle_diff > radar['fov'] / 2: continue # Calculate radar threat with exponential decay if distance < radar['max_range']: threat += np.exp(-10 * distance / radar['max_range']) return threat def calculate_fire_threat(self, x, y): threat = 0 for fire in self.fires: fx, fy = fire['pos'] dx = (x - fx) * self.resolution dy = (y - fy) * self.resolution distance = np.sqrt(dx**2 + dy**2) if distance < fire['min_range'] or distance > fire['max_range']: continue threat += np.exp(-10 * (distance - fire['min_range']) / (fire['max_range'] - fire['min_range'])) return threat def calculate_threats(self, z=500): for i in range(self.width): for j in range(self.height): # Terrain threat self.threat_map[i, j, 0] = self.calculate_terrain_threat(i, j, z) # Radar threat self.threat_map[i, j, 1] = self.calculate_radar_threat(i, j, z) # Fire threat self.threat_map[i, j, 2] = self.calculate_fire_threat(i, j) def variable_weighting(self, T): """Apply variable weighting based on threat levels""" S = np.array([ 1 + np.exp(T[0]), 1 + 1/(T[1] + 1), 1 + 1/(T[2] + 1) ]) W_prime = self.weights * S / np.sum(self.weights * S) return np.sum(W_prime * T) def combine_threats(self): for i in range(self.width): for j in range(self.height): T = self.threat_map[i, j] self.combined_threat[i, j] = self.variable_weighting(T) def plot_threat_map(self): plt.figure(figsize=(12, 10)) plt.imshow(self.combined_threat.T, origin='lower', cmap='hot') # Plot radars for radar in self.radars: x, y = radar['pos'] plt.scatter(x, y, s=100, c='blue', marker='^', label='Radar') # Plot fires for fire in self.fires: x, y = fire['pos'] plt.scatter(x, y, s=100, c='red', marker='o', label='Fire') plt.colorbar(label='Threat Level') plt.title('Combined Threat Map') plt.xlabel('X (grid cells)') plt.ylabel('Y (grid cells)') plt.legend() plt.show() class ImprovedARAStar: def __init__(self, threat_map, max_turn_angle=45): self.threat_map = threat_map self.width = threat_map.width self.height = threat_map.height self.resolution = threat_map.resolution self.max_turn_angle = max_turn_angle # in degrees self.k = 0.2 # Threat penalty coefficient self.epsilon = 2.0 # Initial epsilon def heuristic(self, a, b): # Euclidean distance return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2) * self.resolution def get_neighbors(self, node, parent): neighbors = [] # 8-connected grid for dx in [-1, 0, 1]: for dy in [-1, 0, 1]: if dx == 0 and dy == 0: continue x, y = node[0] + dx, node[1] + dy # Check bounds if x < 0 or x >= self.width or y < 0 or y >= self.height: continue # Check turn angle constraint if parent exists if parent: px, py = parent # Vector from parent to current node vec1 = (node[0] - px, node[1] - py) # Vector from current node to neighbor vec2 = (x - node[0], y - node[1]) # Calculate angle between vectors dot = vec1[0]*vec2[0] + vec1[1]*vec2[1] mag1 = math.sqrt(vec1[0]**2 + vec1[1]**2) mag2 = math.sqrt(vec2[0]**2 + vec2[1]**2) if mag1 > 0 and mag2 > 0: cos_angle = dot / (mag1 * mag2) angle = math.degrees(math.acos(max(min(cos_angle, 1), -1))) if angle > self.max_turn_angle: continue neighbors.append((x, y)) return neighbors def path_cost(self, a, b): # Euclidean distance dx = (a[0]-b[0]) * self.resolution dy = (a[1]-b[1]) * self.resolution return math.sqrt(dx**2 + dy**2) def plan(self, start, goal, max_time=60): OPEN = [] CLOSED = {} INCONS = {} # Initialize start node g_score = {start: 0} h_score = self.heuristic(start, goal) T_score = self.threat_map.combined_threat[start[0], start[1]] epsilon_s = self.epsilon * (1 + self.k * T_score) f_score = {start: g_score[start] + epsilon_s * h_score} heapq.heappush(OPEN, (f_score[start], start)) parent = {start: None} start_time = time.time() path_found = False while OPEN and time.time() - start_time < max_time: _, current = heapq.heappop(OPEN) if current == goal: path_found = True break CLOSED[current] = g_score[current] neighbors = self.get_neighbors(current, parent.get(current)) for neighbor in neighbors: # Calculate tentative g_score tentative_g = g_score[current] + self.path_cost(current, neighbor) # Get threat value for neighbor T_neighbor = self.threat_map.combined_threat[neighbor[0], neighbor[1]] epsilon_s = self.epsilon * (1 + self.k * T_neighbor) if neighbor in CLOSED and tentative_g >= CLOSED.get(neighbor, float('inf')): continue if tentative_g < g_score.get(neighbor, float('inf')): parent[neighbor] = current g_score[neighbor] = tentative_g h_val = self.heuristic(neighbor, goal) f_score[neighbor] = tentative_g + epsilon_s * h_val if neighbor in CLOSED: INCONS[neighbor] = g_score[neighbor] else: heapq.heappush(OPEN, (f_score[neighbor], neighbor)) # Reconstruct path path = [] if path_found: current = goal while current is not None: path.append(current) current = parent.get(current) path.reverse() # Calculate path metrics path_length = 0 threat_value = 0 if len(path) > 1: for i in range(1, len(path)): path_length += self.path_cost(path[i-1], path[i]) threat_value += self.threat_map.combined_threat[path[i][0], path[i][1]] threat_value /= len(path) return path, path_length, threat_value, time.time() - start_time def plot_path(self, path, title="Path Planning Result"): plt.figure(figsize=(12, 10)) plt.imshow(self.threat_map.combined_threat.T, origin='lower', cmap='hot') # Plot radars for radar in self.threat_map.radars: x, y = radar['pos'] plt.scatter(x, y, s=100, c='blue', marker='^', label='Radar') # Plot fires for fire in self.threat_map.fires: x, y = fire['pos'] plt.scatter(x, y, s=100, c='red', marker='o', label='Fire') # Plot path if path: path_x = [p[0] for p in path] path_y = [p[1] for p in path] plt.plot(path_x, path_y, 'g-', linewidth=2, label='Planned Path') plt.scatter(path_x[0], path_y[0], s=150, c='green', marker='o', label='Start') plt.scatter(path_x[-1], path_y[-1], s=150, c='purple', marker='*', label='Goal') plt.colorbar(label='Threat Level') plt.title(title) plt.xlabel('X (grid cells)') plt.ylabel('Y (grid cells)') plt.legend() plt.show() # Simulation and Experiment def run_simulation(): # Create threat map width, height = 100, 100 threat_map = ThreatMap(width, height, resolution=100) threat_map.generate_terrain('mountain') # Add threats (radars and fires) threat_map.add_radar(20, 80, 45, 5000) threat_map.add_radar(60, 40, 180, 4000) threat_map.add_radar(80, 70, 270, 6000) threat_map.add_fire(30, 30, 1000, 3000) threat_map.add_fire(50, 60, 1500, 4000) threat_map.add_fire(70, 20, 2000, 3500) # Calculate threats threat_map.calculate_threats(z=500) threat_map.combine_threats() # Plot threat map threat_map.plot_threat_map() # Create planner planner = ImprovedARAStar(threat_map) # Define scenarios scenarios = { 'Easy': {'start': (10, 90), 'goal': (90, 10), 'max_time': 60}, 'Medium': {'start': (5, 5), 'goal': (95, 95), 'max_time': 120}, 'Hard': {'start': (10, 50), 'goal': (90, 50), 'max_time': 300} } results = [] # Run simulations for name, params in scenarios.items(): print(f"\nRunning {name} scenario...") start, goal = params['start'], params['goal'] # Plan path path, length, threat, time_used = planner.plan(start, goal, params['max_time']) if path: print(f"Path found! Length: {length:.2f}m, Avg Threat: {threat:.4f}, Time: {time_used:.2f}s") planner.plot_path(path, title=f"{name} Scenario Path Planning") else: print("No path found!") results.append({ 'scenario': name, 'success': bool(path), 'path_length': length, 'avg_threat': threat, 'time': time_used }) # Print results print("\nSimulation Results:") print("{:<10} {:<10} {:<15} {:<15} {:<10}".format( "Scenario", "Success", "Path Length(m)", "Avg Threat", "Time(s)")) for res in results: print("{:<10} {:<10} {:<15.2f} {:<15.4f} {:<10.2f}".format( res['scenario'], str(res['success']), res['path_length'], res['avg_threat'], res['time'])) # Additional experiments (ablation study) print("\nRunning ablation study...") # Remove fire threats print("\nWithout fire threats:") threat_map_no_fire = ThreatMap(width, height, resolution=100) threat_map_no_fire.terrain = threat_map.terrain.copy() threat_map_no_fire.radars = threat_map.radars.copy() threat_map_no_fire.calculate_threats(z=500) threat_map_no_fire.combine_threats() planner_no_fire = ImprovedARAStar(threat_map_no_fire) path, length, threat, time_used = planner_no_fire.plan(scenarios['Medium']['start'], scenarios['Medium']['goal'], 120) planner_no_fire.plot_path(path, "Path Planning Without Fire Threats") # Remove radar threats print("\nWithout radar threats:") threat_map_no_radar = ThreatMap(width, height, resolution=100) threat_map_no_radar.terrain = threat_map.terrain.copy() threat_map_no_radar.fires = threat_map.fires.copy() threat_map_no_radar.calculate_threats(z=500) threat_map_no_radar.combine_threats() planner_no_radar = ImprovedARAStar(threat_map_no_radar) path, length, threat, time_used = planner_no_radar.plan(scenarios['Medium']['start'], scenarios['Medium']['goal'], 120) planner_no_radar.plot_path(path, "Path Planning Without Radar Threats") # Remove terrain threats print("\nWithout terrain threats:") threat_map_no_terrain = ThreatMap(width, height, resolution=100) threat_map_no_terrain.radars = threat_map.radars.copy() threat_map_no_terrain.fires = threat_map.fires.copy() threat_map_no_terrain.calculate_threats(z=500) threat_map_no_terrain.combine_threats() planner_no_terrain = ImprovedARAStar(threat_map_no_terrain) path, length, threat, time_used = planner_no_terrain.plan(scenarios['Medium']['start'], scenarios['Medium']['goal'], 120) planner_no_terrain.plot_path(path, "Path Planning Without Terrain Threats") if __name__ == "__main__": run_simulation() 生成的航迹为直线，且寻优时间总为0