SLAM: картографування та локалізація

1. Задача SLAM

Формально SLAM оцінює спільну апостеріорну ймовірність траєкторії робота та карти, враховуючи зашумлені керуючі впливи та зашумлені спостереження орієнтирів:

Чому це складно? Одометрія дрейфує — прослизання коліс та похибка інтегрування накопичуються необмежено з відстанню. Якби робот довіряв лише одометрії, його оцінена позиція розходилася б з реальністю тим більше, чим довше він рухається. Орієнтири, спостережувані з карти, можуть коригувати цей дрейф, але позиції орієнтирів відомі лише відносно того, де робот думає, що він побував. Похибки локалізації спотворюють карту; похибки карти спотворюють локалізацію. Алгоритми SLAM — це клас оцінювачів, що статистично узгоджено розплутують цю пов'язану невизначеність.

2. Моделі руху та сенсорів

Кожен алгоритм SLAM потребує двох імовірнісних моделей. Модель руху передбачає, як змінюється поза при заданому керуючому впливі, а модель сенсора передбачає, яке спостереження очікувати за заданої пози та карти.

R і Q — коваріаційні матриці, що відображають зашумленість моделі руху та сенсора відповідно. Робот з точним лазерним далекоміром, але прослизаючими колесами, матиме малу Q і велику R. У своїй основі кожен бекенд SLAM — це спосіб об'єднання цих двох зашумлених джерел інформації в єдину найкращу оцінку; розрізняється лише спосіб обчислення цього об'єднання.

3. EKF-SLAM

Розширений фільтр Калмана доповнює класичний вектор стану фільтра Калмана, включаючи як позу робота, так і позицію кожного орієнтира, а потім лінеаризує (загалом нелінійні) моделі руху та сенсора навколо поточної оцінки за допомогою розкладу Тейлора першого порядку (якобіана).

Ключова ідея полягає в тому, що повна коваріаційна матриця Σ щільна — корекція пози робота від одного спостереження орієнтира також зсуває оцінену позицію кожного іншого орієнтира через спільні недіагональні кореляційні члени. Саме це пов'язує картографування та локалізацію в одному оцінювачі. Ціна — квадратична складність за кількістю орієнтирів (O(N²) на оновлення, оскільки Σ має розмір N×N), що обмежує класичний EKF-SLAM кількома сотнями орієнтирів, перш ніж він стане обчислювально непрактичним.

4. FastSLAM і фільтри часток

FastSLAM (Монтемерло та ін., 2002) використовує факторизацію: за умови відомої траєкторії робота оцінки орієнтирів стають умовно незалежними одна від одної. Це дозволяє FastSLAM представляти траєкторію фільтром часток (багато дискретних гіпотез «де побував робот»), надаючи кожній частці власний незалежний набір малих EKF, по одному на орієнтир.

Оскільки кожна частка несе повну незалежну гіпотезу карти, FastSLAM природно обробляє неоднозначні або багатомодальні ситуації (наприклад, симетричний коридор), які одногіпотезний EKF представити не може. Компроміс — виснаження часток: після багатьох кроків перевибірки всі частки можуть звестися до нащадків одного предка, втрачаючи різноманітність траєкторій — що особливо шкідливо якраз перед замиканням циклу.

5. Графове SLAM

Сучасні системи SLAM (GTSAM, g2o, Cartographer, бекенд ORB-SLAM) формулюють задачу як граф поз: вузли — це пози робота (і опціонально орієнтири), а ребра — обмеження, отримані з одометрії або зі зіставлення спостережень між непослідовними позами. Розв'язання SLAM стає нелінійною оптимізацією найменших квадратів по всьому графу.

Ключова властивість, що тут використовується, — це розрідженість: кожне ребро з'єднує лише дві близькі пози, тому матриця інформації JᵀΩJ розріджена і може бути ефективно розкладена навіть для графів із сотнями тисяч вузлів — саме тому графове (повне) SLAM масштабується значно краще за щільний підхід EKF зі складністю O(N²).

6. Замикання циклу та асоціація даних

У процесі дослідження середовища дрейф одометрії призводить до того, що оцінена траєкторія робота повільно розходиться з реальним шляхом. Замикання циклу — це момент, коли робот розпізнає, що повернувся до раніше відвіданого місця — це одне нове ребро в графі поз дозволяє оптимізатору «прищепити» накопичений дрейф назад до глобально узгодженої карти.

7. Картографування сіткою зайнятості

Для мобільних роботів, обладнаних дальномірами (LiDAR, сонар), карти часто представляються у вигляді сітки зайнятості — регулярного масиву комірок, кожна з яких зберігає ймовірність того, що вона зайнята перешкодою. Комірки оновлюються незалежно за допомогою логарифмічної форми правила Байєса для чисельної стабільності.

Трасування променя від пози робота до кожного виміру дальності (наприклад, алгоритмом Брезенхема) позначає вільний простір вздовж променя та зайнятий простір у його кінцевій точці. Логарифмічне накопичення означає, що комірка, яку неодноразово спостерігали як вільну, стає впевнено вільною, тоді як одне хибне зчитування не може перевернути стан уже добре встановленої комірки — проста, але ефективна властивість стійкості.

8. Реалізація на JavaScript

// Мінімальне 2D SLAM на сітці зайнятості: логарифмічне оновлення + трасування променя Брезенхема
class OccupancyGrid {
  constructor(width, height, cellSize) {
    this.width = width; this.height = height; this.cellSize = cellSize;
    this.logOdds = new Float32Array(width * height); // 0 = невідомо (p=0.5)
    this.L_OCC = 0.85; this.L_FREE = -0.4; this.L_MIN = -5; this.L_MAX = 5;
  }

  worldToCell(x, y) {
    return { cx: Math.floor(x / this.cellSize), cy: Math.floor(y / this.cellSize) };
  }

  idx(cx, cy) { return cy * this.width + cx; }

  updateCell(cx, cy, logOddsDelta) {
    if (cx < 0 || cy < 0 || cx >= this.width || cy >= this.height) return;
    const i = this.idx(cx, cy);
    this.logOdds[i] = Math.max(this.L_MIN, Math.min(this.L_MAX, this.logOdds[i] + logOddsDelta));
  }

  // Алгоритм Брезенхема: позначаємо вільні комірки вздовж променя, зайняту — у точці влучення
  integrateScan(pose, ranges, maxRange, angleStep) {
    for (let i = 0; i < ranges.length; i++) {
      const angle = pose.theta + i * angleStep;
      const r = Math.min(ranges[i], maxRange);
      const hitX = pose.x + r * Math.cos(angle);
      const hitY = pose.y + r * Math.sin(angle);
      const { cx: x0, cy: y0 } = this.worldToCell(pose.x, pose.y);
      const { cx: x1, cy: y1 } = this.worldToCell(hitX, hitY);
      this.traceLine(x0, y0, x1, y1, ranges[i] < maxRange);
    }
  }

  traceLine(x0, y0, x1, y1, hitOccupied) {
    let dx = Math.abs(x1 - x0), dy = -Math.abs(y1 - y0);
    let sx = x0 < x1 ? 1 : -1, sy = y0 < y1 ? 1 : -1;
    let err = dx + dy, x = x0, y = y0;
    while (true) {
      const isEndpoint = (x === x1 && y === y1);
      this.updateCell(x, y, isEndpoint && hitOccupied ? this.L_OCC : this.L_FREE);
      if (isEndpoint) break;
      const e2 = 2 * err;
      if (e2 >= dy) { err += dy; x += sx; }
      if (e2 <= dx) { err += dx; y += sy; }
    }
  }

  probability(cx, cy) {
    const l = this.logOdds[this.idx(cx, cy)];
    return 1 - 1 / (1 + Math.exp(l));
  }
}

// Використання: будуємо сітку 200x200 з роздільною здатністю 0.1 м, інтегруємо один скан LiDAR
const grid = new OccupancyGrid(200, 200, 0.1);
const pose = { x: 10, y: 10, theta: 0 };
const ranges = [3.2, 3.1, 3.0, 5.8, 5.9]; // симульовані виміри дальності LiDAR (м)
grid.integrateScan(pose, ranges, 8.0, Math.PI / 180);
console.log(grid.probability(130, 100)); // p(зайнято) для комірки поблизу влучення скана

9. Сучасні системи SLAM

Попри десятиліття прогресу, SLAM залишається активною сферою досліджень: стійка робота в динамічних середовищах (люди в русі, мінливі меблі), довгострокове підтримання карти та спільне багаторобототехнічне SLAM — усе це досі відкриті інженерні виклики. Але базовий рецепт — модель руху, модель сенсора та принциповий спосіб об'єднати їхню невизначеність — лежить в основі кожного рішення, від робота-пилососа за 30 доларів до мультисенсорного стека безпілотного автомобіля.