Розробка AI-системи автономної навігації дронів

Система автономної навігації дронів на основі AI

Автономний політ без GPS - завдання, яке доводиться вирішувати в тунелях між будівлями, всередині приміщень, при глушенні сигналу. Дрон повинен оцінювати своє становище, будувати карту оточення та уникати перешкод, спираючись лише на бортові сенсори: камери, IMU, барометр, далекоміри.

Visual-Inertial Odometry: позиціонування без GPS

import numpy as np
import cv2
from scipy.spatial.transform import Rotation

class VisualInertialOdometry:
    """
    VINS-Mono / ORB-SLAM3 логика — упрощённо.
    На практике используем готовые библиотеки с ROS2 интеграцией.
    """
    def __init__(self, camera_matrix: np.ndarray,
                 imu_noise: dict):
        self.K = camera_matrix
        self.orb = cv2.ORB_create(nfeatures=500)
        self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

        self.prev_frame = None
        self.prev_kp = None
        self.prev_desc = None

        # Состояние: позиция + ориентация
        self.position = np.zeros(3)
        self.rotation = np.eye(3)

        self.imu_noise = imu_noise

    def update(self, frame: np.ndarray,
               imu_data: dict) -> dict:
        """Обновление оценки позиции по кадру + IMU"""
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        kp, desc = self.orb.detectAndCompute(gray, None)

        if self.prev_frame is not None and desc is not None:
            matches = self.matcher.match(self.prev_desc, desc)
            matches = sorted(matches, key=lambda x: x.distance)[:100]

            if len(matches) > 20:
                pts1 = np.float32([self.prev_kp[m.queryIdx].pt
                                    for m in matches])
                pts2 = np.float32([kp[m.trainIdx].pt for m in matches])

                E, mask = cv2.findEssentialMat(pts1, pts2, self.K,
                                                method=cv2.RANSAC,
                                                prob=0.999, threshold=1.0)
                if E is not None:
                    _, R, t, _ = cv2.recoverPose(E, pts1, pts2, self.K)
                    # Интеграция движения
                    self.position += self.rotation @ t.flatten()
                    self.rotation = R @ self.rotation

        self.prev_frame = gray
        self.prev_kp = kp
        self.prev_desc = desc if desc is not None else self.prev_desc

        return {
            'position': self.position.copy(),
            'rotation': self.rotation.copy(),
            'tracked_features': len(kp) if kp else 0
        }

Виявлення та обхід перешкод

Для indoor та urban польотів - стереокамери або ToF-сенсори дають dense depth map. Intel RealSense D435 чи структуроване світло для ближніх дистанцій.

class ObstacleAvoidance:
    def __init__(self, depth_camera, safety_distance: float = 1.5):
        self.depth_cam = depth_camera
        self.safety_dist = safety_distance  # метры
        self.fov_h = 87  # градусов (RealSense D435)
        self.sectors = 5  # делим поле зрения на 5 секторов

    def compute_clear_directions(self,
                                  depth_frame: np.ndarray) -> dict:
        """Находим свободные направления полёта"""
        h, w = depth_frame.shape
        sector_width = w // self.sectors
        clearance = {}

        for i in range(self.sectors):
            sector = depth_frame[:, i*sector_width:(i+1)*sector_width]
            # Игнорируем нулевые значения (нет данных)
            valid = sector[sector > 0]
            if len(valid) == 0:
                clearance[i] = float('inf')
                continue

            # P10 — ближняя граница препятствия в секторе
            min_dist = float(np.percentile(valid, 10)) / 1000.0  # мм -> м

            clearance[i] = min_dist

        # Направление с максимальным clearance
        best_sector = max(clearance, key=clearance.get)
        angle = (best_sector - self.sectors // 2) * (self.fov_h / self.sectors)

        return {
            'clearance_by_sector': clearance,
            'best_direction_angle': angle,
            'is_path_clear': clearance[self.sectors//2] > self.safety_dist
        }

Планування маршруту: 3D Occupancy Grid

import heapq

class OccupancyGridPlanner:
    def __init__(self, resolution: float = 0.2):
        self.resolution = resolution  # метров на ячейку
        self.grid = {}  # 3D sparse grid: (ix, iy, iz) -> occupancy

    def update_from_depth(self, depth_frame: np.ndarray,
                           camera_pose: np.ndarray):
        """Обновляем карту препятствий"""
        # Конвертация depth в point cloud
        points = self._depth_to_pointcloud(depth_frame)
        # Трансформация в мировые координаты
        points_world = (camera_pose[:3, :3] @ points.T).T + camera_pose[:3, 3]

        for pt in points_world:
            ix, iy, iz = (int(pt[0] / self.resolution),
                           int(pt[1] / self.resolution),
                           int(pt[2] / self.resolution))
            self.grid[(ix, iy, iz)] = 1  # occupied

    def astar_3d(self, start: tuple, goal: tuple) -> list:
        """A* в 3D occupancy grid"""
        def heuristic(a, b):
            return np.sqrt(sum((a[i]-b[i])**2 for i in range(3)))

        heap = [(0, start)]
        came_from = {start: None}
        cost = {start: 0}

        while heap:
            _, current = heapq.heappop(heap)
            if current == goal:
                break

            for dx, dy, dz in [(1,0,0),(-1,0,0),(0,1,0),
                                 (0,-1,0),(0,0,1),(0,0,-1)]:
                neighbor = (current[0]+dx, current[1]+dy, current[2]+dz)
                if self.grid.get(neighbor, 0) == 1:
                    continue  # препятствие

                new_cost = cost[current] + 1
                if neighbor not in cost or new_cost < cost[neighbor]:
                    cost[neighbor] = new_cost
                    priority = new_cost + heuristic(neighbor, goal)
                    heapq.heappush(heap, (priority, neighbor))
                    came_from[neighbor] = current

        # Восстанавливаем путь
        path = []
        node = goal
        while node is not None:
            path.append(node)
            node = came_from.get(node)
        return list(reversed(path))

Кейс: автономний дрон для інспекції складу

Склад 8000 м ², 4 поверхи стелажів до 12м висоти. Завдання: автономний обліт стелажів, фото інвентаризація без GPS.

Стек: ROS2 Humble + PX4 Autopilot + Intel RealSense D435i (глибина + IMU) + ORB-SLAM3. Дрон DJI F450 із кастомним польотним контролером.

Точність локалізації: ±8 см у горизонтальній площині, ±5 см за висотою. Швидкість обльоту: 0,5 м/с. Час обльоту одного ряду стелажів (50м): 110 секунд.