Skip to main content
STEM:Hub

SLAM и Навигация — Геометрия автономного сознания

SLAM (Simultaneous Localization and Mapping) — это не алгоритм, а фундаментальный парадокс автономии: как найти себя в мире, который не знаешь, и как узнать мир, не зная, где находишься. В 2026 году это симбиоз геометрии, теории вероятностей и машинного обучения, где робот становится картографом собственной реальности.

Философские основы: Парадокс автономного познания

Проблема курицы и яйца в робототехнике:

  • Локализация требует карты: \(P(x_t | m, z_{1:t})\)
  • Картографирование требует позиции: \(P(m | x_{1:t}, z_{1:t})\)
\[ P(x_{1:t}, m | z_{1:t}, u_{1:t}) = P(x_{1:t} | z_{1:t}, u_{1:t}) \cdot P(m | x_{1:t}, z_{1:t}) \]

Эволюция SLAM: От геометрии к семантике

Поколение 1: Геометрический SLAM (2000-2010)

  • Данные: Точечные облака (LiDAR), особенности (features)
  • Математика: Наименьшие квадраты, оптимизация графов
  • Представители: EKF-SLAM, FastSLAM, GraphSLAM

Поколение 2: Визуальный SLAM (2010-2020)

  • Данные: Камеры (моно, стерео, RGB-D)
  • Математика: Пучковая настройка (bundle adjustment)
  • Представители: ORB-SLAM, LSD-SLAM, DSO

Поколение 3: Нейросетевый SLAM (2020-2026)

  • Данные: Мультимодальные (изображение, LiDAR, IMU)
  • Математика: Глубокое обучение, трансформеры
  • Представители: Neural SLAM, Scene Representation Networks

Поколение 4: Семантический SLAM (2026+)

  • Данные: Геометрия + семантика
  • Математика: Геометрическое глубокое обучение
  • Идея: Карта как граф объектов с отношениями

Математические основы SLAM

Формальная постановка задачи

Дано:

  • Наблюдения: \(z_{1:t} = \{z_1, ..., z_t\}\)
  • Управления: \(u_{1:t} = \{u_1, ..., u_t\}\)
  • Карта: \(m\) (сетка, граф признаков, семантические объекты)
\[ \arg\max_{x_{1:t}, m} P(x_{1:t}, m | z_{1:t}, u_{1:t}) \]

Фактор-граф представление

\[ P(X, m) \propto \prod_i \phi_i(\mathcal{X}_i) \]

где:

  • Вершины: Позиции \(x_i\), признаки карты \(m_j\)
  • Факторы: Ограничения между вершинами

Пример графа:

x0 --(odom)--> x1 --(odom)--> x2
 |              |              |
(obs)          (obs)          (obs)
 |              |              |
m1              m2             m3

Оптимизация: Решение нелинейной задачи наименьших квадратов

\[ X^*, m^* = \arg\min_{X, m} \sum_k \|h_k(\mathcal{X}_k) - z_k\|_{\Sigma_k}^2 \]\[ (H^T \Sigma^{-1} H) \Delta \xi = -H^T \Sigma^{-1} r \]

где \(H\) — Якобиан, \(r\) — вектор невязок.

Ключевые компоненты современного SLAM (2026)

1. Фронтенд (Frontend) — Восприятие и ассоциация данных

Визуальная одометрия (Visual Odometry)

Задача: Оценка движения по последовательности изображений.

\[ E = \sum_{\mathbf{p} \in \Omega} \|I_2(\omega(\mathbf{p}; \xi)) - I_1(\mathbf{p})\|_\delta \]

где \(\omega\) — деформация изображения.

Основанный на признаках (Feature-based):

class FeatureBasedVO {
    vector<KeyPoint> extract_features(Mat image) {
        // ORB, SIFT, SuperPoint (2026)
        vector<KeyPoint> kps;
        ORB::detect(image, kps);
        return kps;
    }
    
    SE3 estimate_motion(vector<Point2f> pts1, vector<Point2f> pts2) {
        // 5-точечный алгоритм + RANSAC
        Mat E = findEssentialMat(pts1, pts2, K);
        recoverPose(E, pts1, pts2, K, R, t);
        return SE3(R, t);
    }
};

Лидарная одометрия (LiDAR Odometry)

LOAM (LiDAR Odometry and Mapping):

class LOAM:
    def extract_features(self, point_cloud):
        # Вычисление кривизны точек
        curvatures = compute_curvature(point_cloud)
        
        # Выбор реберных (edge) и плоскостных (planar) особенностей
        edge_features = select_points(curvatures > threshold_edge)
        planar_features = select_points(curvatures < threshold_planar)
        
        return edge_features, planar_features
    
    def align_frames(self, features1, features2):
        # Минимизация расстояния от точек до линий/плоскостей
        # edge: расстояние до ближайшей линии
        # planar: расстояние до ближайшей плоскости
        transformation = icp_variant(features1, features2)
        return transformation

2. Бэкенд (Backend) — Оптимизация и замыкание петель

Оптимизация графа (Pose Graph Optimization)

class PoseGraphOptimizer {
    struct Constraint {
        int id_from, id_to;
        SE3 measurement;    // Измеренное преобразование
        Matrix6d information; // Матрица информации
    };
    
    vector<SE3> optimize(vector<SE3> initial_poses, 
                         vector<Constraint> constraints) {
        // Использование g2o или GTSAM
        g2o::SparseOptimizer optimizer;
        // ... добавление вершин и ограничений
        optimizer.initializeOptimization();
        optimizer.optimize(100);
        return optimized_poses;
    }
};

Замыкание петель (Loop Closure)

Проблема: Распознать, что робот вернулся в ранее посещённое место.

Методы 2026:

  1. Визуальные дескрипторы: NetVLAD, DBoW3
  2. Геометрические: ICP (Iterative Closest Point) для LiDAR
  3. Семантические: Распознавание объектов как ориентиров
class LoopClosureDetector:
    def detect_loop(self, current_frame, map_frames):
        # Извлечение дескриптора сцены
        current_desc = self.netvlad(current_frame)
        
        # Поиск ближайших соседей в базе данных
        scores = []
        for map_frame in map_frames:
            score = cosine_similarity(current_desc, map_frame.descriptor)
            scores.append(score)
        
        # Проверка геометрической согласованности
        if max(scores) > threshold:
            candidate = map_frames[argmax(scores)]
            if self.geometric_verification(current_frame, candidate):
                return candidate.id
        return None

3. Представление карты (Map Representation)

Карты занятости (Occupancy Grid Maps)

\[ \log \frac{p(m_i | z_{1:t})}{1 - p(m_i | z_{1:t})} = \log \frac{p(m_i | z_t)}{1 - p(m_i | z_t)} + \log \frac{p(m_i | z_{1:t-1})}{1 - p(m_i | z_{1:t-1})} - \log \frac{p(m_i)}{1 - p(m_i)} \]

Эффективные структуры 2026:

  • Октодеревья (OctoMap): Иерархическое представление 3D
  • TSDF (Truncated Signed Distance Function): Для RGB-D камер

Точечные карты (Point Cloud Maps)

class VoxelGridMap {
    struct Voxel {
        Eigen::Vector3f centroid;
        int point_count;
        Eigen::Matrix3f covariance;
    };
    
    unordered_map<VoxelKey, Voxel> voxels;
    
    void insert_point(const Point& p) {
        VoxelKey key = get_voxel_key(p);
        voxels[key].add_point(p);
    }
};

Семантические карты (Semantic Maps)

Карта как граф объектов:

Узел: {тип: "стол", положение: (x,y,z), размер: (w,h,d)}
Ребро: {отношение: "на_вершине", объект1: "чашка", объект2: "стол"}

Навигация: От карты к действию

Архитектура навигационного стека (ROS 2 Nav2)

[Глобальный планировщик] → [Локальный планировщик] → [Контроллер]
         ↑                       ↑                       ↑
  [Глобальная карта]      [Локальная карта]      [Одометрия]
         ↑                       ↑
     [SLAM]                  [Сенсоры]

Глобальное планирование (Global Planning)

Алгоритм A с эвристикой:*

\[ f(n) = g(n) + h(n) \]

где \(h(n)\) — эвристика (манхэттенское, евклидово, диагональное расстояние).

Оптимизации 2026:

class JumpPointSearch : public AStar {
    // Пропуск симметричных путей
    vector<Node> prune_neighbors(Node node, Map map) {
        // Правила прыжковых точек
        if (has_forced_neighbor(node, map)) {
            return {jump(node, direction)};
        }
        return {};
    }
};

RRT (Rapidly-exploring Random Trees Star) для сложных сред:*

class RRTStar:
    def plan(self, start, goal, obstacles):
        tree = Tree(start)
        for i in range(max_iterations):
            q_rand = self.sample_random()
            q_near = tree.nearest(q_rand)
            q_new = self.steer(q_near, q_rand)
            
            if self.collision_free(q_near, q_new, obstacles):
                tree.add_vertex(q_new)
                tree.add_edge(q_near, q_new)
                self.rewire(tree, q_new, obstacles)
        
        return tree.path_to_goal(goal)

Локальное планирование (Local Planning)

Dynamic Window Approach (DWA):

\[ G(v, \omega) = \alpha \cdot \text{heading}(v, \omega) + \beta \cdot \text{dist}(v, \omega) + \gamma \cdot \text{vel}(v, \omega) \]

Реализация:

class DWA {
    struct Velocity {
        float v, w;  // линейная и угловая скорости
    };
    
    Velocity plan(Pose current, Pose goal, vector<Obstacle> obstacles) {
        vector<Velocity> admissible = generate_velocities(current);
        Velocity best;
        float best_score = -INFINITY;
        
        for (auto& vel : admissible) {
            float score = evaluate_trajectory(vel, current, goal, obstacles);
            if (score > best_score) {
                best_score = score;
                best = vel;
            }
        }
        return best;
    }
};

Model Predictive Control (MPC) для плавной навигации:

\[ \min_{u_{0:N-1}} \sum_{k=0}^{N-1} \|x_k - x_{ref}\|_Q^2 + \|u_k\|_R^2 + \|x_N - x_{ref}\|_P^2 \]

subject to: \(x_{k+1} = f(x_k, u_k)\), \(u_{min} \leq u_k \leq u_{max}\)

Контроллер движения (Motion Controller)

Pure Pursuit:

\[ L = \sqrt{(x_g - x)^2 + (y_g - y)^2} \]\[ \alpha = \arctan\left(\frac{y_g - y}{x_g - x}\right) - \theta \]\[ \delta = \arctan\left(\frac{2L \sin(\alpha)}{k}\right) \]

Linear Quadratic Regulator (LQR) для следования траектории:

\[ \min_{u} \sum_{t=0}^{\infty} (x_t^T Q x_t + u_t^T R u_t) \]\[ u_t = -K_t x_t \]

Практикум: “Автономное исследование неизвестной среды”

Задача: Робот-исследователь (TurtleBot 3 + RPLiDAR + Intel RealSense) должен:

  1. Построить карту неизвестного помещения
  2. Найти все “интересные” объекты (определяемые семантически)
  3. Вернуться в исходную точку

Архитектура системы:

class ExplorationRobot:
    def __init__(self):
        self.slam = SemanticSLAM()
        self.planner = FrontierBasedPlanner()
        self.navigator = MpcNavigator()
        self.semantic_detector = YOLOv7()
        
    def explore(self):
        while not self.is_exploration_complete():
            # 1. Получение данных
            lidar_data = self.get_lidar()
            image = self.get_camera()
            odom = self.get_odometry()
            
            # 2. Обновление SLAM
            pose, map = self.slam.update(lidar_data, image, odom)
            
            # 3. Обнаружение семантики
            objects = self.semantic_detector.detect(image)
            self.slam.add_semantic_objects(objects, pose)
            
            # 4. Планирование следующей цели
            frontiers = self.planner.find_frontiers(map)
            next_goal = self.select_best_frontier(frontiers)
            
            # 5. Навигация к цели
            path = self.planner.plan_path(pose, next_goal, map)
            self.navigator.follow_path(path)

Метрики оценки:

  1. Полнота карты: \(\frac{\text{покрытая площадь}}{\text{общая площадь}}\)
  2. Точность позиции: RMSE относительно ground truth
  3. Эффективность исследования: \(\frac{\text{покрытая площадь}}{\text{пройденный путь}}\)
  4. Распознавание объектов: mAP (mean Average Precision)

Современные вызовы и решения 2026

Проблема 1: “Вычислительная сложность в реальном времени”

Решение: Иерархические подходы + аппаратное ускорение

  • Фронтенд: На CPU, 60 Гц
  • Бэкенд: На GPU, 1 Гц
  • Оптимизация графа: Разреженные решатели (CHOLMOD, SuiteSparse)

Проблема 2: “Динамические среды”

Решение: Динамические карты + обнаружение изменений

class DynamicSLAM {
    void update_with_dynamics(PointCloud new_scan, PointCloud map) {
        // Сравнение с картой для обнаружения движущихся объектов
        vector<Point> changed_points = detect_changes(new_scan, map);
        
        if (is_moving_object(changed_points)) {
            // Игнорировать для картографирования
            remove_dynamic_points(new_scan);
        }
        
        // Обновить статическую карту
        update_static_map(new_scan);
    }
};

Проблема 3: “Большие масштабы (городская навигация)”

Решение: Иерархические карты

Уровень 1: Континенты → [Узлы: страны]
Уровень 2: Страны → [Узлы: города]  
Уровень 3: Города → [Узлы: районы]
Уровень 4: Районы → [Уровень карты занятости]

Проблема 4: “Отказ GPS (подземные, indoor)”

Решение: Мультимодальная сенсорная фьюжн

  • LiDAR-одометрия для точного короткого расстояния
  • Визуальная одометрия для среднего расстояния
  • Семантические ориентиры для глобальной согласованности

Будущее SLAM и навигации (2026+)

1. НейроСЛАМ (NeuroSLAM)

Энд-ту-энд обучение SLAM через нейросети:

class NeuroSLAM(nn.Module):
    def __init__(self):
        self.encoder = PointNet++()  # Кодирование облака точек
        self.lstm = nn.LSTM(256, 512)  # Траектория
        self.map_decoder = nn.ConvTranspose3d(512, 1, 4)  # Генерация карты
        
    def forward(self, point_clouds):
        # Кодирование
        features = [self.encoder(pc) for pc in point_clouds]
        
        # Временная обработка
        trajectory, _ = self.lstm(features)
        
        # Декодирование карты
        map = self.map_decoder(trajectory[-1])
        return trajectory, map

2. Спайковые SLAM для энергоэффективности

Событийные камеры + спайковые нейросети → в 100 раз эффективнее традиционных систем.

3. Квантовый SLAM

Использование квантовых алгоритмов для:

  • Квантового PCA для уменьшения размерности
  • Квантовой оптимизации графов

4. Коллективный SLAM (Crowd-SLAM)

\[ m^{global} = \bigcup_{i=1}^N \text{transform}(m^{local}_i, x_i) \]

5. SLAM в расширенной реальности

Единое картографирование физического и виртуального миров для AR-роботов.

Инструментарий 2026

Фреймворки и библиотеки:

  1. ROS 2 + Nav2: Промышленный стандарт
  2. Open3D: Обработка облаков точек
  3. g2o/GTSAM: Оптимизация графов
  4. TensorFlow Robotics: Нейросетевые компоненты
  5. Isaac Sim: Симуляция для обучения

Аппаратные платформы:

  1. NVIDIA Jetson Orin: 275 TOPS для AI на краю
  2. Intel RealSense D455: Глубина + цвет
  3. Ouster OS1: Высокоточный LiDAR
  4. event cameras: Даводары для высокоскоростного отслеживания

Что дальше?

  1. Искусственный интеллект в робототехнике — нейросетевые методы SLAM
  2. Сенсорная фьюжн — объединение данных для улучшения SLAM
  3. Управление роботами — использование SLAM в контуре управления
  4. Роевая робототехника — коллективное картографирование

Философский итог: SLAM в 2026 году — это не просто техническая задача, а фундаментальный акт кибернетического самосознания. Робот, строящий карту, создаёт не просто представление пространства, а внутреннюю вселенную, где геометрические отношения переплетаются с семантическим смыслом. Это путешествие от незнания к знанию, от дезориентации к локализации, от хаоса сырых данных к упорядоченной модели реальности. В этом процессе робот не просто находит путь из точки A в точку B — он обретает место в мире, и в этом обретении становится по-настоящему автономным существом.