Навигация и Одометрия. Как робот понимает, где он находится

Представьте, что вас разбудили ночью в незнакомой комнате. Вы используете:

1. Осязание (нащупать стены) → Энкодеры, лидар
2. Равновесие (не упасть) → IMU
3. Зрение (окно, звезды) → Камеры, GPS

Робот решает ту же задачу, используя свой набор «чувств». Это навигационная стека — комбинация сенсоров и алгоритмов.

Принцип: «Я отсчитываю шаги от стартовой точки».

Математика: Интегрирование во времени: \[ x(t) = x_0 + \int_0^t v(\tau) d\tau, \quad \theta(t) = \theta_0 + \int_0^t \omega(\tau) d\tau \] где \(v\) — скорость (м/с), \(\omega\) — угловая скорость (рад/с).

Проблема: Накопление ошибки. Для гироскопа с дрейфом \(b\): \[ \theta_{\text{err}}(t) = b \cdot t \] Через 60 секунд при \(b = 0.1^\circ/\text{с}\): ошибка 6°.

Принцип: «Я смотрю на карту и вижу свою точку».

Пример GPS: Трилатерация по \(N\) спутникам: \[ \sqrt{(x - x_i)^2 + (y - y_i)^2 + (z - z_i)^2} = c \cdot \Delta t_i \] где \(c = 299792458\ \text{м/с}\) — скорость света, \(\Delta t_i\) — разность времён.

Движение описывается уравнениями Ньютона: \[ \mathbf{a} = \frac{d\mathbf{v}}{dt}, \quad \mathbf{v} = \frac{d\mathbf{x}}{dt} \] где \(\mathbf{a}\) измеряется акселерометром.

Фильтр Калмана для IMU: Объединение акселерометра и гироскопа: \[ \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k(\mathbf{z}_k - \mathbf{H}\hat{\mathbf{x}}_{k|k-1}) \] где \(\mathbf{z}_k\) — измерения акселерометра, \(\mathbf{K}_k\) — матрица усиления Калмана.

Курс вычисляется через арктангенс: \[ \psi = \arctan2(-B_y, B_x) \] где \(B_x, B_y\) — компоненты магнитного поля.

Компенсация наклона: При углах крена \(\phi\) и тангажа \(\theta\):

\[ \mathbf{B}_h = \mathbf{R}_{\text{comp}} \cdot \mathbf{B}_{\text{raw}} \]

\[ \mathbf{B}_h = \begin{bmatrix} B_{hx} \\ B_{hy} \\ B_{hz} \end{bmatrix}, \quad \mathbf{R}_{\text{comp}} = \begin{bmatrix} \cos\theta & 0 & -\sin\theta \\ \sin\theta\sin\phi & \cos\phi & \cos\theta\sin\phi \\ \sin\theta\cos\phi & -\sin\phi & \cos\theta\cos\phi \end{bmatrix}, \quad \mathbf{B}_{\text{raw}} = \begin{bmatrix} B_x \\ B_y \\ B_z \end{bmatrix} \]

где:

• \(\phi\) — крен (roll), \(\theta\) — тангаж (pitch) из IMU
• \(B_x, B_y, B_z\) — сырые измерения магнитометра
• \(B_{hx}, B_{hy}\) — скорректированные горизонтальные компоненты

Горизонтальный курс (heading): \[ \psi = \arctan2(-B_{hy}, B_{hx}) \]

DOP (Dilution of Precision): Мера геометрического качества: \[ \text{HDOP} = \sqrt{\sigma_x^2 + \sigma_y^2} \] где \(\sigma_x, \sigma_y\) — стандартные отклонения.

RTK-коррекция: Фазовая разность между базой и ровером: \[ \Delta \phi = \frac{c \cdot \Delta t}{\lambda} + N + \varepsilon \] где:

• \(c = 299792458\ \text{м/с}\) — скорость света
• \(\lambda\) — длина волны сигнала (~0.19 м для L1)
• \(N\) — неоднозначность целых чисел
• \(\varepsilon\) — ошибки измерений

ICP (Iterative Closest Point): Минимизация расстояния между облаками точек: \[ E(\mathbf{R}, \mathbf{t}) = \sum_{i=1}^N |\mathbf{p}_i - (\mathbf{R}\mathbf{q}_i + \mathbf{t})|^2 \] где \(\mathbf{p}_i, \mathbf{q}_i\) — соответственные точки.

Фундаментальная матрица \(\mathbf{F}\): Для двух кадров: \[ \mathbf{x}_2^\top \mathbf{F} \mathbf{x}_1 = 0 \] где \(\mathbf{x}_1, \mathbf{x}_2\) — соответственные точки.

Из \(\mathbf{F}\) извлекается движение \(\mathbf{R}, \mathbf{t}\) с точностью до масштаба.

Сенсорный граф:
  Энкодеры → Скорость v, ω → Интегратор → Сырая одометрия
                                         ↓
  Лидар → Облако точек → ICP/Сопоставление → Коррекция
                                         ↓
  Фильтр Калмана: x̂ = [x, y, θ] с ковариацией P

Уравнения фильтра Калмана:

Prediction (прогноз):

\[ \begin{aligned} \hat{\mathbf{x}}_{k|k-1} &= f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k) \\ \mathbf{P}_{k|k-1} &= \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top + \mathbf{Q}_k \end{aligned} \]

Update (коррекция):

\[ \begin{aligned} \mathbf{K}_k &= \mathbf{P}_{k|k-1} \mathbf{H}_k^\top (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k)^{-1} \\ \hat{\mathbf{x}}_{k|k} &= \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k[\mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})] \\ \mathbf{P}_{k|k} &= (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} \end{aligned} \]

Обозначения:

• \(\hat{\mathbf{x}}_{k|k-1}\) — априорная оценка состояния (прогноз)
• \(\hat{\mathbf{x}}_{k|k}\) — апостериорная оценка (после коррекции)
• \(\mathbf{P}_{k|k-1}, \mathbf{P}_{k|k}\) — ковариационные матрицы ошибок
• \(\mathbf{K}_k\) — матрица усиления Калмана
• \(\mathbf{z}_k\) — вектор измерений
• \(f(\cdot)\) — модель динамики системы
• \(h(\cdot)\) — модель измерений

Состояние системы: \[ \mathbf{x} = [\mathbf{p}, \mathbf{q}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a]^\top \] где \(\mathbf{q}\) — кватернион ориентации.

IMU модель: \[ \begin{aligned} \dot{\mathbf{p}} &= \mathbf{v} \\ \dot{\mathbf{v}} &= \mathbf{R}(\mathbf{q})(\mathbf{a}_m - \mathbf{b}_a) - \mathbf{g} \\ \dot{\mathbf{q}} &= \frac{1}{2}\mathbf{q} \otimes \begin{bmatrix}0 \\ \boldsymbol{\omega}_m - \mathbf{b}_g\end{bmatrix} \end{aligned} \]

Ковариационная матрица: \[ \mathbf{P} = \begin{bmatrix} \sigma_x^2 & \sigma_{xy} & \sigma_{x\theta} \\ \sigma_{xy} & \sigma_y^2 & \sigma_{y\theta} \\ \sigma_{x\theta} & \sigma_{y\theta} & \sigma_\theta^2 \end{bmatrix} \]

Fusion правило: Взвешенное среднее по обратным ковариациям: \[ \hat{x} = \frac{\sigma_{\text{odo}}^{-2} x_{\text{odo}} + \sigma_{\text{gps}}^{-2} x_{\text{gps}}}{\sigma_{\text{odo}}^{-2} + \sigma_{\text{gps}}^{-2}} \]

Исходные данные:
  - Точность энкодера: ±1% на метр
  - Путь: 100 метров
  
Расчет:
  Ошибка после 100 м: 1% × 100 = 1 метр
  Но реально: σ = 0.01 × √100 = 0.1 м (случайная)
  Систематическая ошибка: 0.01 × 100 = 1 м
  Итог: 1.0 ± 0.1 м

Частота IMU: 100 Гц
Частота магнитометра: 10 Гц

Комплементарный фильтр:
  θ_k = α·(θ_{k-1} + ω·Δt) + (1-α)·θ_mag
  где α = 0.98 (доверие к гироскопу)

При дрейфе гироскопа 0.1°/с:
  Через 10 с без коррекции: ошибка 1°
  С коррекцией каждые 0.1 с: ошибка < 0.1°

Базовая станция: известные координаты (σ = 0.001 м)
Расстояние база-ровер: 5 км
Атмосферная корреляция: ρ = 0.9 на 5 км

Ожидаемая точность:
  σ_ровер = σ_база / √(1 - ρ²) ≈ 0.001 / √(1 - 0.9²) ≈ 0.023 м

def calibrate_encoders(actual_distance):
    ticks = read_encoder_over_distance()
    ticks_per_meter = ticks / actual_distance
    # Сохраняем в EEPROM

float complementary_filter(float gyro_rate, float accel_angle, 
                          float mag_heading, float dt) {
    static float angle = 0;
    float alpha = 0.98;  // Доверие к гироскопу
    
    // Гироскоп: интеграция
    angle += gyro_rate * dt;
    
    // Акселерометр: низкочастотная коррекция наклона
    angle = alpha * angle + (1-alpha) * accel_angle;
    
    // Магнитометр: периодическая коррекция курса
    if (mag_update_available()) {
        angle = 0.95 * angle + 0.05 * mag_heading;
    }
    
    return angle;
}

\[ \begin{aligned} \Delta x &= \frac{N_L + N_R}{2} \cdot k_t \cdot \cos(\theta) \\ \Delta y &= \frac{N_L + N_R}{2} \cdot k_t \cdot \sin(\theta) \\ \Delta \theta &= \frac{N_R - N_L}{L} \cdot k_\theta \end{aligned} \] где:

• \(N_L, N_R\) — количество тиков левого и правого энкодеров
• \(k_t\) — коэффициент пересчета тиков в метры (м/тик)
• \(L\) — база робота (расстояние между колесами, м)
• \(k_\theta\) — коэффициент пересчета разницы тиков в угол (рад/тик)
• \(\theta\) — текущий курс робота

Неправильно: Простое усреднение GPS и одометрии Правильно: Взвешивание по обратным дисперсиям: \[ \hat{x} = \frac{\sigma_{\text{odo}}^{-2} x_{\text{odo}} + \sigma_{\text{gps}}^{-2} x_{\text{gps}}}{\sigma_{\text{odo}}^{-2} + \sigma_{\text{gps}}^{-2}} \]

Неправильно: \(\theta = \sum \omega \Delta t\) Правильно: Фильтр нижних частот или Калман

Пример: Для робота с дифференциальным приводом: \[ v = \frac{v_R + v_L}{2}, \quad \omega = \frac{v_R - v_L}{L} \] где \(L\) — расстояние между колесами (м), \(v_L, v_R\) — скорости колес (м/с).

Навигация — это комбинация физики (сенсоры) и математики (алгоритмы):

1. Фильтр Калмана: \[ \hat{x}^+ = \hat{x}^- + K(z - H\hat{x}^-) \] где \(\hat{x}^-\) — априорная оценка, \(\hat{x}^+\) — апостериорная

2. SLAM: Одновременная оптимизация карты и позиции \[ \arg\min_{\mathbf{X}, \mathbf{L}} \sum |\mathbf{z} - h(\mathbf{X}, \mathbf{L})|^2_{\Sigma} \] где \(\mathbf{L}\) — landmarks, \(\mathbf{X}\) — poses.

3. Оптимизация на графах: g2o, GTSAM \[ F(\mathbf{x}) = \sum e_{ij}^\top \Omega_{ij} e_{ij} \] где \(e_{ij}\) — ошибка между узлами \(i\) и \(j\).

Итог: Навигация — это вероятностный процесс. Мы никогда не знаем точное положение, только распределение вероятностей. Искусство в том, чтобы минимизировать дисперсию этого распределения, умно комбинируя сенсоры с разными характеристиками ошибок.

1. Галактический уровень → Солнечная система движется ~220 км/с относительно центра Галактики

   • Абсолютно: относительно центра Млечного Пути
   • Относительно: мы этого не чувствуем

2. Звездный уровень → Земля вращается вокруг Солнца (~30 км/с)

   • GPS спутники учитывают релятивистские поправки из-за скорости и гравитации: \[ \Delta t_{\text{rel}} = \frac{\Phi}{c^2} - \frac{v^2}{2c^2} \] где \(\Phi\) — гравитационный потенциал

3. Планетарный уровень → Земля вращается (~465 м/с на экваторе)

   • Кориолисова сила влияет на гироскопы и баллистику: \[ \mathbf{F}_c = -2m(\boldsymbol{\Omega} \times \mathbf{v}) \]

4. Земной уровень (инерциальная система ECI) → GPS работает здесь

   • Но! Земная кора движется (тектоника плит) ~2-10 см/год
   • ITRF (International Terrestrial Reference Frame) обновляется ежегодно

5. Локальный уровень (ENU: East-North-Up) → где живут роботы

   • Абсолютно: относительно здания/комнаты
   • Относительно: относительно Земли

WGS-84 (World Geodetic System) — “абсолютная” система для GPS:

• Но она привязана к движущейся Земле
• И сама обновляется (WGS-84 G1762 — реализация 2013 года)

Парадокс: GPS дает “абсолютные” координаты в системе, которая:

1. Вращается с Землей
2. Дрейфует с континентами
3. Обновляется каждые несколько лет

Пример: Здание в Сан-Франциско:

• Относительно Земли: движется ~5 см/год (разлом Сан-Андреас)
• Относительно центра Земли: вращается с периодом 1 день
• Относительно Солнца: 30 км/с по орбите

Для робота-пылесоса в этом здании:

• “Абсолютная” позиция = координаты комнаты в системе здания
• Но само здание “плывет” на плите

На практике используют иерархию систем отсчета:

Космическая (ICRS) → для астронавигации
    ↓
Земная инерциальная (ECI) → для спутников
    ↓
Земная вращающаяся (ECEF) → для GPS
    ↓
Локальная (ENU/NED) → для роботов
    ↓
Корпусная (Body) → для датчиков робота

В навигации “абсолютность” — всегда относительна к выбранной системе отсчета:

Практическое правило для робототехники:
Абсолютная навигация = навигация относительно системы, которая считается неподвижной на время задачи.

• Для робота-пылесоса: “абсолютно” = относительно стен квартиры
• Для сельхозробота: “абсолютно” = относительно полей (через RTK-GPS)
• Для марсохода: “абсолютно” = относительно марсианской поверхности

Ключевая мысль: Все системы отсчета относительны, но некоторые более полезны для конкретных задач. Искусство навигации — в выборе подходящего уровня “абсолютности” для решаемой проблемы.

😄 Если дошли до этого момента и поняли

✅ Физику сенсоров
✅ Математику обработки
✅ Архитектуру систем
✅ Философию навигации

— вы уже на 90% инженер по навигации!

Это как фильтр Калмана для знаний:

• Прогноз: “Это сложно, я не пойму”
• Коррекция: “Ага, вот же формулы и примеры!”
• Результат: Точная оценка своих способностей

\[ \Delta x = \frac{N_L + N_R}{2} \cdot k_t \cdot \cos(\theta) \]

Что это значит:

• \(N_L, N_R\) — тики энкодеров левого и правого колес
• \(k_t\) — коэффициент “метр на тик” (калибровка!)
• \(\theta\) — текущий курс робота
• Результат: Насколько робот продвинулся вперед (в метрах)

Понимаешь это → можешь сделать робота, который едет куда нужно, а не “примерно туда”

\[ \mathbf{B}_h = \mathbf{R}_{\text{comp}} \cdot \mathbf{B}_{\text{raw}} \]

Что это значит:

• \(\mathbf{B}_{\text{raw}}\) — сырые показания магнитометра [B_x, B_y, B_z]
• \(\mathbf{R}_{\text{comp}}\) — матрица компенсации наклона (зависит от углов крена и тангажа)
• \(\mathbf{B}_h\) — скорректированный вектор магнитного поля в горизонтальной плоскости

Понимаешь это → робот не сбивается при наклоне, компас показывает правильно даже на склоне

\[ \hat{x} = \frac{\sigma_A^{-2}x_A + \sigma_B^{-2}x_B}{\sigma_A^{-2} + \sigma_B^{-2}} \]

Что это значит:

• \(x_A, x_B\) — измерения от двух разных сенсоров
• \(\sigma_A^2, \sigma_B^2\) — их дисперсии (квадраты погрешностей)
• \(\sigma^{-2}\) — обратная дисперсия (вес измерения: чем точнее сенсор, тем больше вес)
• \(\hat{x}\) — оптимальная оценка

Понимаешь это → умеешь объединять данные разных датчиков, доверяя больше тем, которые сейчас точнее

И главное — философский вывод:

“Все системы отсчета относительны, но некоторые более полезны для конкретных задач”

Это уже уровень старшего инженера, который понимает не только как что-то сделать, но и почему это правильно в данном контексте.