# ============================================================ # proportional_navigation.py # Модуль 2: Пропорциональная навигация (Proportional Navigation) # # Реализует: # - Pure Proportional Navigation (PPN) # - Augmented Proportional Navigation (APN) с компенсацией # ускорения цели # - Screen-space fallback (когда нет оценки дальности) # - Вычисление точки перехвата (intercept point) # ============================================================ import numpy as np from collections import deque from config import * from config_intercept import * from helpers import clamp, box_center, box_wh import cv2 class ProportionalNavigator: """ Proportional Navigation — классический закон наведения ракет. Принцип: ускорение перехватчика перпендикулярно линии визирования (LOS) пропорционально скорости вращения LOS и скорости сближения: a_cmd = N * Vc * dλ/dt где N — навигационная константа (3-5), Vc — closing velocity, dλ/dt — скорость изменения угла LOS. Augmented PN добавляет компенсацию ускорения цели: a_cmd = N * Vc * dλ/dt + (N/2) * a_target_normal При отсутствии 3D-информации работает в screen-space (2D аналог: стабилизировать положение цели в центре кадра). """ def __init__(self): self.enabled = bool(PN_ENABLE) self.N = float(PN_NAV_GAIN) self.augmented = bool(PN_AUGMENTED) # Состояние LOS self._prev_los = None # предыдущий угол LOS (рад) self._prev_los_t = None self._los_rate_smooth = 0.0 # сглаженная скорость LOS (рад/с) self._los_rate_raw = 0.0 # Состояние LOS по обеим осям (для 2D PN в screen space) self._prev_los_x = None self._prev_los_y = None # Для Augmented PN: оценка ускорения цели self._prev_target_vel = None self._prev_target_vel_t = None self._target_accel = np.zeros(2, dtype=np.float32) # История для отладки self._cmd_hist = deque(maxlen=60) # Выходы self.accel_cmd = np.zeros(2, dtype=np.float32) # [ax, ay] нормализованные self.intercept_point = None # предсказанная точка перехвата self.intercept_time = None # время до перехвата (сек) self.los_rate = 0.0 self.active = False def reset(self): self._prev_los = None self._prev_los_t = None self._los_rate_smooth = 0.0 self._los_rate_raw = 0.0 self._prev_los_x = None self._prev_los_y = None self._prev_target_vel = None self._prev_target_vel_t = None self._target_accel = np.zeros(2, dtype=np.float32) self.accel_cmd = np.zeros(2, dtype=np.float32) self.intercept_point = None self.intercept_time = None self.los_rate = 0.0 self.active = False self._cmd_hist.clear() def update_3d(self, my_pos, my_vel, target_pos, target_vel, closing_vel, timestamp): """ 3D Proportional Navigation (используется при наличии оценки дальности). Args: my_pos: np.array([x, y]) — позиция перехватчика (метры) my_vel: np.array([vx, vy]) — скорость перехватчика (м/с) target_pos: np.array([x, y]) — позиция цели (метры) target_vel: np.array([vx, vy]) — скорость цели (м/с) closing_vel: float — скорость сближения (м/с), > 0 = сближаемся timestamp: float — текущее время Returns: dict с полями accel_cmd, los_rate, intercept_point, intercept_time """ result = self._idle_result() if not self.enabled: return result r = target_pos - my_pos range_val = float(np.linalg.norm(r)) if range_val < 0.01: return result # Угол LOS los_angle = float(np.arctan2(r[1], r[0])) # Скорость LOS if self._prev_los is not None and self._prev_los_t is not None: dt = timestamp - self._prev_los_t if dt > 1e-4: # Обработка перехода через ±π d_los = los_angle - self._prev_los if d_los > np.pi: d_los -= 2.0 * np.pi elif d_los < -np.pi: d_los += 2.0 * np.pi self._los_rate_raw = d_los / dt alpha = float(PN_LOS_RATE_SMOOTH) self._los_rate_smooth = ( alpha * self._los_rate_smooth + (1.0 - alpha) * self._los_rate_raw ) self._prev_los = los_angle self._prev_los_t = timestamp self.los_rate = self._los_rate_smooth # Closing velocity vc = max(float(closing_vel), float(PN_MIN_CLOSING_VEL)) # ─── Pure PN ───────────────────────────────────────────── a_pn = self.N * vc * self._los_rate_smooth # ─── Augmented PN ──────────────────────────────────────── a_aug = 0.0 if self.augmented: target_vel_np = np.array(target_vel, dtype=np.float32) self._estimate_target_accel(target_vel_np, timestamp) # Нормальная компонента ускорения цели (перп. к LOS) if range_val > 0.1: los_unit = r / range_val los_perp = np.array([-los_unit[1], los_unit[0]], dtype=np.float32) a_target_normal = float(np.dot(self._target_accel, los_perp)) a_aug = float(PN_AUG_GAIN) * self.N * 0.5 * a_target_normal # Суммарная команда a_total = a_pn + a_aug # Нормализация max_cmd = float(PN_MAX_ACCEL_CMD) a_norm = float(clamp(a_total / max(20.0, vc), -max_cmd, max_cmd)) # Преобразование в декартовы координаты (перп. к LOS) los_perp_dir = np.array([-np.sin(los_angle), np.cos(los_angle)], dtype=np.float32) self.accel_cmd = a_norm * los_perp_dir # Точка перехвата self.intercept_point, self.intercept_time = self._compute_intercept( my_pos, my_vel, target_pos, target_vel ) self.active = True self._cmd_hist.append(float(a_norm)) result.update({ "active": True, "accel_x": float(self.accel_cmd[0]), "accel_y": float(self.accel_cmd[1]), "accel_mag": float(abs(a_norm)), "los_rate": float(self._los_rate_smooth), "los_angle_deg": float(np.degrees(los_angle)), "closing_vel": float(closing_vel), "range_m": float(range_val), "intercept_x": float(self.intercept_point[0]) if self.intercept_point is not None else None, "intercept_y": float(self.intercept_point[1]) if self.intercept_point is not None else None, "intercept_time": self.intercept_time, "a_pn": float(a_pn), "a_aug": float(a_aug), }) return result def update_screen(self, target_center, frame_w, frame_h, target_vel_px, timestamp): """ Screen-space PN fallback (без 3D-информации). Вместо 3D LOS используем углы от центра кадра к цели. LOS rate вычисляется из скорости смещения цели по экрану. Args: target_center: (cx, cy) — центр bbox цели в пикселях frame_w, frame_h: размеры кадра target_vel_px: (vx, vy) — скорость цели в пикселях/сек (из Kalman) timestamp: текущее время Returns: dict — команды наведения в screen space """ result = self._idle_result() if not self.enabled or not PN_SCREEN_FALLBACK: return result if target_center is None: return result cx, cy = float(target_center[0]), float(target_center[1]) scr_cx = float(frame_w) * 0.5 scr_cy = float(frame_h) * 0.5 # Нормализованная ошибка (как LOS angle в screen space) los_x = (cx - scr_cx) / max(1.0, scr_cx) los_y = (cy - scr_cy) / max(1.0, scr_cy) # LOS rate по каждой оси los_rate_x = 0.0 los_rate_y = 0.0 if self._prev_los_x is not None and self._prev_los_t is not None: dt = timestamp - self._prev_los_t if dt > 1e-4: raw_x = (los_x - self._prev_los_x) / dt raw_y = (los_y - self._prev_los_y) / dt alpha = float(PN_LOS_RATE_SMOOTH) los_rate_x = alpha * los_rate_x + (1.0 - alpha) * raw_x los_rate_y = alpha * los_rate_y + (1.0 - alpha) * raw_y self._prev_los_x = los_x self._prev_los_y = los_y self._prev_los_t = timestamp self.los_rate = float(np.hypot(los_rate_x, los_rate_y)) # Screen-space "closing velocity" = 1 (нет 3D) # Команда = N * los_rate (упрощённый PN) cmd_x = float(clamp(self.N * los_rate_x + 0.8 * los_x, -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) cmd_y = float(clamp(self.N * los_rate_y + 0.8 * los_y, -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) # Lead: добавляем упреждение по скорости цели if target_vel_px is not None: vx_n = float(target_vel_px[0]) / max(1.0, scr_cx) vy_n = float(target_vel_px[1]) / max(1.0, scr_cy) lead_t = float(PN_LEAD_TIME_SEC) cmd_x += 0.3 * vx_n * lead_t cmd_y += 0.3 * vy_n * lead_t cmd_x = float(clamp(cmd_x, -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) cmd_y = float(clamp(cmd_y, -PN_MAX_ACCEL_CMD, PN_MAX_ACCEL_CMD)) self.accel_cmd = np.array([cmd_x, cmd_y], dtype=np.float32) self.active = True result.update({ "active": True, "accel_x": cmd_x, "accel_y": cmd_y, "accel_mag": float(np.hypot(cmd_x, cmd_y)), "los_rate": self.los_rate, "los_angle_deg": float(np.degrees(np.arctan2(los_y, los_x))), "closing_vel": 0.0, "range_m": None, "intercept_x": None, "intercept_y": None, "intercept_time": None, "mode": "screen", }) return result # ─── Private ───────────────────────────────────────────────── def _estimate_target_accel(self, target_vel, timestamp): """Оценка ускорения цели из разности скоростей.""" if self._prev_target_vel is not None and self._prev_target_vel_t is not None: dt = timestamp - self._prev_target_vel_t if dt > 1e-4: raw_accel = (target_vel - self._prev_target_vel) / dt alpha = 0.6 self._target_accel = ( alpha * self._target_accel + (1.0 - alpha) * raw_accel ).astype(np.float32) self._prev_target_vel = target_vel.copy() self._prev_target_vel_t = timestamp def _compute_intercept(self, my_pos, my_vel, target_pos, target_vel, max_iter=12): """ Итеративное вычисление точки перехвата. Находит время t, при котором перехватчик, летящий со своей скоростью, достигает предсказанной позиции цели (target_pos + target_vel * t). """ my_speed = float(np.linalg.norm(my_vel)) if my_speed < 0.1: my_speed = 10.0 # fallback t = float(np.linalg.norm(target_pos - my_pos)) / max(my_speed, 0.1) for _ in range(max_iter): predicted = target_pos + target_vel * t dist = float(np.linalg.norm(predicted - my_pos)) t_new = dist / max(my_speed, 0.1) if abs(t_new - t) < 0.01: break t = 0.5 * t + 0.5 * t_new # демпфированная итерация intercept = target_pos + target_vel * t return intercept.astype(np.float32), float(t) @staticmethod def _idle_result(): return { "active": False, "accel_x": 0.0, "accel_y": 0.0, "accel_mag": 0.0, "los_rate": 0.0, "los_angle_deg": 0.0, "closing_vel": 0.0, "range_m": None, "intercept_x": None, "intercept_y": None, "intercept_time": None, } def status_line(self): if not self.enabled: return "Proportional Navigation disabled" mode = "APN" if self.augmented else "PPN" return f"PN ready: N={self.N:.1f} mode={mode}" def draw_overlay(self, frame_bgr, state, sx, sy): """Рисует вектор наведения и точку перехвата.""" if not self.enabled or not DRAW_PN_VECTOR: return if not state.get("active", False): return h, w = frame_bgr.shape[:2] cx = int(w * 0.5) cy = int(h * 0.5) # Вектор команды ax = float(state.get("accel_x", 0.0)) ay = float(state.get("accel_y", 0.0)) scale = 80.0 ex = int(cx + ax * scale) ey = int(cy + ay * scale) color = (0, 255, 128) if state.get("active") else (100, 100, 100) cv2.arrowedLine(frame_bgr, (cx, cy), (ex, ey), color, 2, cv2.LINE_AA) # Точка перехвата ix = state.get("intercept_x") iy = state.get("intercept_y") if ix is not None and iy is not None and DRAW_INTERCEPT_POINT: ipx = int(float(ix) / max(1e-6, float(sx))) ipy = int(float(iy) / max(1e-6, float(sy))) cv2.drawMarker(frame_bgr, (ipx, ipy), (0, 255, 128), cv2.MARKER_TILTED_CROSS, 14, 2) # Текст los_r = state.get("los_rate", 0.0) a_mag = state.get("accel_mag", 0.0) it = state.get("intercept_time") txt = f"PN: a={a_mag:.2f} LOS_r={los_r:.3f}" if it is not None: txt += f" t_int={it:.2f}s" cv2.putText(frame_bgr, txt, (20, 260), cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 2)