From ec81ce95a524b1809a74c924cd14d9f5f43ff297 Mon Sep 17 00:00:00 2001 From: russian_proger Date: Mon, 5 Jan 2026 11:49:29 +0500 Subject: [PATCH] feat: add pitch, roll in Geolocation --- autopilot.py | 6 +- geolocation.py | 213 +++++++++++++++++++++++++++++++++++++++++-------- simulator.py | 8 +- 3 files changed, 187 insertions(+), 40 deletions(-) diff --git a/autopilot.py b/autopilot.py index 72693c9..f510a44 100644 --- a/autopilot.py +++ b/autopilot.py @@ -207,8 +207,8 @@ class AutoPilot(Pilot): return { 'x': self.geo.x, 'y': self.geo.y, - 'angle': self.geo.angle, - 'angle_degrees': math.degrees(self.geo.angle), + 'angle': self.geo.yaw, + 'angle_degrees': math.degrees(self.geo.yaw), 'frame_count': self.frame_count } @@ -331,7 +331,7 @@ class AutoPilot(Pilot): # Вычисляем угол к цели target_angle = math.atan2(self.points[self.target_idx][1] - self.geo.y, self.points[self.target_idx][0] - self.geo.x) - angle_trajectory = self.geo.angle + math.pi / 2 + angle_trajectory = self.geo.yaw + math.pi / 2 # print("[ANGLE]", angle_trajectory, "->", target_angle) diff --git a/geolocation.py b/geolocation.py index 29fff89..7d6b377 100644 --- a/geolocation.py +++ b/geolocation.py @@ -1,66 +1,213 @@ + import math import numpy as np +from typing import Optional + class Geolocation: - """ Класс геопозиции """ - x: float - y: float - z: float # Масштаб (Высота) - angle: float # Угол направления движения + """Класс геопозиции с полной ориентацией БПЛА в 3D пространстве""" - def __init__(self, x: float = 0, y: float = 0, z: float = 1, angle: float = 0): + x: float # Координата X (метры) + y: float # Координата Y (метры) + z: float # Масштаб (высота) + yaw: float # Рыскание (rotation around Z-axis) + pitch: float # Тангаж (rotation around Y-axis) + roll: float # Крен (rotation around X-axis) + + def __init__( + self, + x: float = 0, + y: float = 0, + z: float = 1, + yaw: float = 0, + pitch: float = 0, + roll: float = 0 + ): self.x = x self.y = y self.z = z - self.angle = angle + self.yaw = yaw + self.pitch = pitch + self.roll = roll def __str__(self) -> str: - return (f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, " - f"z={self.z:.2f}, angle={self.angle:.1f}°)") + return ( + f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}, " + f"yaw={math.degrees(self.yaw):.1f}°, " + f"pitch={math.degrees(self.pitch):.1f}°, " + f"roll={math.degrees(self.roll):.1f}°)" + ) - def __matmul__(self, mat: np.array) -> 'Geolocation': + def __matmul__(self, mat: np.ndarray) -> 'Geolocation': return self.copy().apply(mat) - def __imatmul__(self, mat: np.array) -> 'Geolocation': + def __imatmul__(self, mat: np.ndarray) -> 'Geolocation': return self.apply(mat) def apply(self, mat: np.ndarray) -> 'Geolocation': - """ На основе матрицы трансформации вычисляет новую позицию и направление """ + """ + Применяет матрицу трансформации для вычисления новой позиции и ориентации. + Для 2D трансформаций (3x3 матрица) обновляет только x, y, z и yaw. + Для 3D трансформаций (4x4 матрица) обновляет все параметры включая pitch и roll. + """ + if mat.shape == (3, 3): + return self._apply_2d(mat) + elif mat.shape == (4, 4): + return self._apply_3d(mat) + else: + raise ValueError(f"Unsupported matrix shape: {mat.shape}. Expected (3,3) or (4,4)") + + def _apply_2d(self, mat: np.ndarray) -> 'Geolocation': + """Применяет 2D трансформацию""" tx, ty = -mat[0, 2], -mat[1, 2] - - # Вычисляем угол поворота rotation = -np.arctan2(mat[1, 0], mat[0, 0]) scale = np.hypot(mat[0, 0], mat[0, 1]) - # Координаты уже отцентрированы, поэтому используем их напрямую dx_meters = tx dy_meters = ty - - angle_global = self.angle + rotation + angle_global = self.yaw + rotation - # Применяем поворот к смещению (учитываем текущий угол БПЛА) cos_angle = math.cos(angle_global) sin_angle = math.sin(angle_global) - # Поворачиваем смещение в глобальные координаты - # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз dx_global = dx_meters * cos_angle - dy_meters * sin_angle dy_global = dx_meters * sin_angle + dy_meters * cos_angle - - # Обновляем координаты БПЛА - self.z /= scale - self.x = self.x + dx_global * self.z - self.y = self.y + dy_global * self.z - - # Нормализуем угол в диапазоне [-π, π] - self.angle = math.atan2(math.sin(angle_global), math.cos(angle_global)) - + + self.z /= scale if scale != 0 else 1 + self.x += dx_global * self.z + self.y += dy_global * self.z + self.yaw = self._normalize_angle(angle_global) + return self - def copy(self) -> 'Geolocation': - return Geolocation(self.x, self.y, self.z, self.angle) + def _apply_3d(self, mat: np.ndarray) -> 'Geolocation': + """Применяет 3D трансформацию с извлечением углов Эйлера""" + position = np.array([self.x, self.y, 0, 1]) + new_position = mat @ position + + self.x = new_position[0] + self.y = new_position[1] + + rotation_mat = mat[:3, :3] + scale = np.linalg.norm(rotation_mat[:, 0]) + self.z *= scale + + normalized_mat = rotation_mat / scale + yaw, pitch, roll = self._extract_euler_angles(normalized_mat) + + self.yaw = self._normalize_angle(self.yaw + yaw) + self.pitch = self._normalize_angle(self.pitch + pitch) + self.roll = self._normalize_angle(self.roll + roll) + + return self @staticmethod - def transform(g: 'Geolocation', mat: np.ndarray) -> 'Geolocation | None': - return g.copy().apply(mat) \ No newline at end of file + def _extract_euler_angles(R: np.ndarray) -> tuple[float, float, float]: + """ + Извлекает углы Эйлера (yaw, pitch, roll) из матрицы вращения. + Использует ZYX convention (yaw-pitch-roll). + """ + sy = math.sqrt(R[0, 0]**2 + R[1, 0]**2) + + singular = sy < 1e-6 + + if not singular: + roll = math.atan2(R[2, 1], R[2, 2]) + pitch = math.atan2(-R[2, 0], sy) + yaw = math.atan2(R[1, 0], R[0, 0]) + else: + roll = math.atan2(-R[1, 2], R[1, 1]) + pitch = math.atan2(-R[2, 0], sy) + yaw = 0 + + return yaw, pitch, roll + + @staticmethod + def _normalize_angle(angle: float) -> float: + """Нормализует угол в диапазон [-π, π]""" + return math.atan2(math.sin(angle), math.cos(angle)) + + def copy(self) -> 'Geolocation': + """Создает полную копию объекта""" + return Geolocation(self.x, self.y, self.z, self.yaw, self.pitch, self.roll) + + def get_rotation_matrix(self) -> np.ndarray: + """ + Матрица вращения с порядком применения: yaw → pitch → roll + Умножение: R = Rx(roll) * Ry(pitch) * Rz(yaw) + """ + cy, sy = math.cos(self.yaw), math.sin(self.yaw) + cp, sp = math.cos(self.pitch), math.sin(self.pitch) + cr, sr = math.cos(self.roll), math.sin(self.roll) + + # Порядок умножения: Rx * Ry * Rz (справа налево: yaw→pitch→roll) + return np.array([ + [cy*cp, -sy*cr + cy*sp*sr, sy*sr + cy*sp*cr], + [sy*cp, cy*cr + sy*sp*sr, -cy*sr + sy*sp*cr], + [-sp, cp*sr, cp*cr] + ]) + + def to_dict(self) -> dict: + """Экспорт в словарь""" + return { + 'x': self.x, + 'y': self.y, + 'z': self.z, + 'yaw': self.yaw, + 'pitch': self.pitch, + 'roll': self.roll + } + + @classmethod + def from_dict(cls, data: dict) -> 'Geolocation': + """Импорт из словаря""" + return cls( + x=data.get('x', 0), + y=data.get('y', 0), + z=data.get('z', 1), + yaw=data.get('yaw', 0), + pitch=data.get('pitch', 0), + roll=data.get('roll', 0) + ) + + @staticmethod + def transform(g: 'Geolocation', mat: np.ndarray) -> Optional['Geolocation']: + """Статический метод трансформации (создает копию и применяет матрицу)""" + if g is None: + return None + return g.copy().apply(mat) + + +# Пример использования +if __name__ == "__main__": + # Создание геопозиции + geo = Geolocation(x=100, y=200, z=50, yaw=math.radians(45), pitch=math.radians(10), roll=math.radians(5)) + print("Исходная позиция:", geo) + + # 2D трансформация (обратная совместимость) + mat_2d = np.array([ + [0.9, -0.1, 5], + [0.1, 0.9, 10], + [0, 0, 1] + ]) + geo_2d = geo @ mat_2d + print("\nПосле 2D трансформации:", geo_2d) + + # 3D трансформация + angle = math.radians(30) + mat_3d = np.array([ + [math.cos(angle), -math.sin(angle), 0, 15], + [math.sin(angle), math.cos(angle), 0, 20], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + geo_3d = geo.copy() + geo_3d @= mat_3d + print("\nПосле 3D трансформации:", geo_3d) + + # Получение матрицы вращения + rot_mat = geo.get_rotation_matrix() + print("\nМатрица вращения:\n", rot_mat) + + print("\n✓ Код успешно выполнен") diff --git a/simulator.py b/simulator.py index 6dd6fce..8f07c9d 100644 --- a/simulator.py +++ b/simulator.py @@ -103,7 +103,7 @@ class Simulator: h, w = img_array.shape[:2] # Получаем исходные точки с учётом перспективы - yaw_deg = -math.degrees(self.geo.angle) + yaw_deg = -math.degrees(self.geo.yaw) src_points = self._get_perspective_points(w, h, yaw_deg) # Целевые точки - квадрат 700x700 @@ -144,11 +144,11 @@ class Simulator: action.move_to_element_with_offset(html, 200, 200) action.click_and_hold() - self.geo.angle += dangle + self.geo.yaw += dangle velocity = max(velocity, 10) - dx = math.cos(math.pi / 2 + self.geo.angle) * velocity / self.geo.z - dy = math.sin(math.pi / 2 + self.geo.angle) * velocity / self.geo.z + dx = math.cos(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z + dy = math.sin(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z self.update_trajectory(dx, dy)