AI Drone Autonomous Navigation System Development

AI-based autonomous drone navigation system

Autonomous flight without GPS is a challenge that must be addressed in tunnels, between buildings, indoors, and in situations where the signal is jammed. The drone must assess its position, map its surroundings, and avoid obstacles using only its onboard sensors: cameras, IMU, barometer, and rangefinders.

Visual-Inertial Odometry: Positioning Without 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
        }

Obstacle detection and avoidance

For indoor and urban flights, stereo cameras or ToF sensors provide a dense depth map. Intel RealSense D435 or structured light are used for close-range missions.

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
        }

Route Planning: 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))

Case Study: Autonomous Warehouse Inspection Drone

8,000 m² warehouse, 4 levels of racks up to 12 m high. Task: autonomous rack inspection, photo inventory without GPS.

Stack: ROS2 Humble + PX4 Autopilot + Intel RealSense D435i (depth + IMU) + ORB-SLAM3. DJI F450 drone with a custom flight controller.

Localization accuracy: ±8 cm horizontally, ±5 cm vertically. Flight speed: 0.5 m/s. Flight time for one row of racks (50 m): 110 seconds.