#!/usr/bin/env python3 """ 飛行任務規劃模組 新增：Circle Formation 和 Leader-Follower """ import math from typing import List, Tuple, Optional, Dict, Any from enum import Enum class MissionType(Enum): """任務類型""" M_FORMATION = "m_formation" CIRCLE_FORMATION = "circle_formation" LEADER_FOLLOWER = "leader_follower" class CoordinateConverter: """GPS 座標與 Local 座標的轉換器""" def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0): self.origin_lat = origin_lat self.origin_lon = origin_lon self.origin_alt = origin_alt self.R = 6371000.0 def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]: lat_rad = math.radians(lat) lon_rad = math.radians(lon) origin_lat_rad = math.radians(self.origin_lat) origin_lon_rad = math.radians(self.origin_lon) dlat = lat_rad - origin_lat_rad dlon = lon_rad - origin_lon_rad x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2) y = self.R * dlat z = alt - self.origin_alt return x, y, z def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]: origin_lat_rad = math.radians(self.origin_lat) origin_lon_rad = math.radians(self.origin_lon) lat_rad = origin_lat_rad + (y / self.R) lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2))) lat = math.degrees(lat_rad) lon = math.degrees(lon_rad) alt = self.origin_alt + z return lat, lon, alt class FormationPlanner: """隊形規劃器""" def __init__(self, spacing: float = 5.0, base_altitude: float = 10.0, altitude_diff: float = 2.0): self.spacing = spacing self.base_altitude = base_altitude self.altitude_diff = altitude_diff self.current_origin = None self.converter = None def plan_formation_mission(self, drone_gps_positions: List[Tuple[float, float, float]], target_gps: Tuple[float, float, float], mission_type: MissionType = MissionType.M_FORMATION, params: Optional[Dict[str, Any]] = None) -> Tuple[ List[Tuple[float, float, float]], List[Tuple[float, float, float]], Tuple[float, float, float]]: """ 規劃兩階段隊形任務 Args: drone_gps_positions: 當前無人機 GPS 位置列表 target_gps: 目標點 GPS 座標 mission_type: 任務類型 params: 任務參數 Returns: (stage1_gps, stage2_gps, origin) """ if len(drone_gps_positions) == 0: raise ValueError("無人機位置列表不能為空") # 計算中央點 center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions) center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions) self.current_origin = (center_lat, center_lon, center_alt) self.converter = CoordinateConverter(center_lat, center_lon, center_alt) # 轉換到 Local 座標 drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions] target_local = self.converter.gps_to_local(*target_gps) # 計算隊形 if mission_type == MissionType.M_FORMATION: stage1_local, stage2_local = self._calculate_m_formation(drone_local, target_local, params) elif mission_type == MissionType.CIRCLE_FORMATION: stage1_local, stage2_local = self._calculate_circle_formation(drone_local, target_local, params) elif mission_type == MissionType.LEADER_FOLLOWER: stage1_local, stage2_local = self._calculate_leader_follower(drone_local, target_local, params) else: raise ValueError(f"不支援的任務類型: {mission_type}") # 轉回 GPS stage1_gps = [self.converter.local_to_gps(*pos) for pos in stage1_local] stage2_gps = [self.converter.local_to_gps(*pos) for pos in stage2_local] return stage1_gps, stage2_gps, self.current_origin def _calculate_m_formation(self, drone_positions, target_point, params): """M字形編隊（原本的邏輯）""" params = params or {} N = len(drone_positions) spacing = params.get('spacing', self.spacing) base_altitude = params.get('base_altitude', self.base_altitude) altitude_diff = params.get('altitude_diff', self.altitude_diff) C_x = sum(pos[0] for pos in drone_positions) / N C_y = sum(pos[1] for pos in drone_positions) / N C_z = sum(pos[2] for pos in drone_positions) / N T_x, T_y, T_z = target_point V_x, V_y = T_x - C_x, T_y - C_y P_x, P_y = -V_y, V_x length = math.sqrt(P_x**2 + P_y**2) P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0) projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i) for i, pos in enumerate(drone_positions)] projections.sort() stage1_positions = [None] * N stage2_positions = [None] * N for rank, (_, original_idx) in enumerate(projections): offset = (rank - (N - 1) / 2) * spacing altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff) stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude) stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude) return stage1_positions, stage2_positions def _calculate_circle_formation(self, drone_positions, target_point, params): """ 圓形編隊 params: radius(10.0), altitude(10.0), start_angle(0.0) """ params = params or {} N = len(drone_positions) radius = params.get('radius', 10.0) altitude = params.get('altitude', 10.0) start_angle = params.get('start_angle', 0.0) center_x, center_y, center_z = target_point stage1_positions = [] stage2_positions = [] angle_step = 360.0 / N for i in range(N): angle_rad = math.radians(start_angle + angle_step * i) final_x = center_x + radius * math.cos(angle_rad) final_y = center_y + radius * math.sin(angle_rad) final_z = altitude current_x, current_y, current_z = drone_positions[i] stage1_positions.append(( current_x + (final_x - current_x) * 0.5, current_y + (final_y - current_y) * 0.5, current_z + (final_z - current_z) * 0.5 )) stage2_positions.append((final_x, final_y, final_z)) return stage1_positions, stage2_positions def _calculate_leader_follower(self, drone_positions, target_point, params): """ Leader-Follower 編隊 params: follow_distance(15.0), altitude_offset(1.0) """ params = params or {} N = len(drone_positions) follow_distance = params.get('follow_distance', 15.0) # 預設 15m altitude_offset = params.get('altitude_offset', 1.0) leader_x, leader_y, leader_z = drone_positions[0] target_x, target_y, target_z = target_point direction_x = target_x - leader_x direction_y = target_y - leader_y direction_length = math.sqrt(direction_x**2 + direction_y**2) dir_unit_x, dir_unit_y = (direction_x / direction_length, direction_y / direction_length) \ if direction_length > 0.01 else (0.0, 1.0) stage1_positions = [] stage2_positions = [] # Leader leader_stage1 = (leader_x + direction_x * 0.3, leader_y + direction_y * 0.3, target_z) stage1_positions.append(leader_stage1) stage2_positions.append(target_point) # Followers for i in range(1, N): back_offset = follow_distance * i height_offset = altitude_offset * (i % 2) current_x, current_y, current_z = drone_positions[i] follow1_x = leader_stage1[0] - dir_unit_x * back_offset follow1_y = leader_stage1[1] - dir_unit_y * back_offset follow1_z = leader_stage1[2] + height_offset stage1_positions.append(( current_x + (follow1_x - current_x) * 0.5, current_y + (follow1_y - current_y) * 0.5, current_z + (follow1_z - current_z) * 0.5 )) stage2_positions.append(( target_x - dir_unit_x * back_offset, target_y - dir_unit_y * back_offset, target_z + height_offset )) return stage1_positions, stage2_positions # 測試 if __name__ == "__main__": planner = FormationPlanner() drones = [(24.123450, 120.567800, 100.0), (24.123470, 120.567820, 102.0), (24.123440, 120.567810, 98.0)] target = (24.123600, 120.568000, 105.0) # Leader-Follower (15m 間距) s1, s2, origin = planner.plan_formation_mission(drones, target, MissionType.LEADER_FOLLOWER, {'follow_distance': 15.0}) print("Leader-Follower (15m):") for i, pos in enumerate(s2): print(f" {'Leader' if i == 0 else f'Follower {i}'}: {pos[0]:.6f}, {pos[1]:.6f}")