🔮 Слияние датчиков

Урок 3.2 | Level 3: Инженер

Sensor Fusion — искусство извлечения правды из шума

Проблема

Каждый датчик врёт по-своему:

Датчик	Хорошо	Плохо
Акселерометр	Статика (где гравитация)	Динамика (вибрации!)
Гироскоп	Быстрые изменения	Дрейф со временем
Магнитометр	Абсолютный курс	Магнитные помехи
GPS	Абсолютная позиция	Медленно, неточно

Решение: Объединить их умно! 🧠

IMU: Акселерометр + Гироскоп

Акселерометр

Измеряет: Линейное ускорение (включая гравитацию!)

В покое:                В движении:
                        
    ↑ g = 9.8 м/с²          ↑ g
    │                       │ ← a (ускорение)
    ●                       ●→
   🤖                      🤖

Из направления $\vec{g}$ можно вычислить Roll и Pitch:

$$ \text{Roll} = \arctan2(a_y, a_z) $$$$ \text{Pitch} = \arctan2(-a_x, \sqrt{a_y^2 + a_z^2}) $$

Проблема акселерометра

При движении и вибрациях — шум!

Реальный угол: ────────────────────────────
                
Акселерометр:  ─╱╲─╱╲╱╲─╱╲─╱╲╱╲─╱╲─╱╲╱╲─╱╲─
               (шум от вибраций)

Гироскоп

Измеряет: Угловую скорость (°/сек)

Чтобы получить угол, интегрируем:

$$ \theta(t) = \theta_0 + \int_0^t \omega(\tau) d\tau $$

В коде:

angle += gyro_rate * dt;

Проблема гироскопа: ДРЕЙФ

Даже при $\omega = 0$ датчик выдаёт маленькое значение (bias):

Реальный угол: ────────────────────────────
                                         ╱
Гироскоп:      ──────────────────────────╱
                                      ↗
                              дрейф (накапливается!)

За 1 минуту может накопиться 10-20° ошибки!

Комплементарный фильтр

Идея

Объединяем лучшее от обоих:

Гироскоп: Точный на коротких интервалах
Акселерометр: Стабильный на длинных интервалах

Гироскоп:       Высокие частоты (быстрые движения)
                      │
                      ▼
              ┌───────────────┐
              │ Комплементарный│ ──► Угол
              │    фильтр     │
              └───────────────┘
                      ▲
                      │
Акселерометр:  Низкие частоты (медленные, стабильные)

Формула

$$ \theta = \alpha \cdot (\theta_{prev} + \omega \cdot dt) + (1 - \alpha) \cdot \theta_{acc} $$

где:

$\alpha \approx 0.96-0.98$ — коэффициент доверия к гироскопу
$\theta_{prev}$ — предыдущий угол
$\omega$ — угловая скорость от гироскопа
$\theta_{acc}$ — угол от акселерометра

Код комплементарного фильтра

const float ALPHA = 0.98;
float angle = 0;

void loop() {
  float dt = 0.01;  // 100 Hz
  
  // Читаем датчики
  float gyro_rate = readGyro();        // °/сек
  float accel_angle = readAccelAngle(); // ° (из atan2)
  
  // Комплементарный фильтр
  angle = ALPHA * (angle + gyro_rate * dt) + (1 - ALPHA) * accel_angle;
  
  Serial.println(angle);
  delay(10);
}

Визуализация работы

                  Вибрация    Дрейф гироскопа
                     ↓             ↓
Акселерометр: ──╱╲╱╲╱╲╱╲──────────────────────
                                            
Гироскоп:     ─────────────────────────────╱
                                         ↗
                                        
Комплементарный: ─────────────────────────────
                  ↑ Убрал шум    ↑ Убрал дрейф

Фильтр Калмана

Зачем нужен, если есть комплементарный?

Комплементарный	Калман
Простой	Сложный
1 параметр (α)	Много параметров
Не учитывает шумы	Учитывает статистику шумов
Хорош для IMU	Хорош для ЛЮБЫХ датчиков

Калман — оптимальный по статистике! 📊

Идея фильтра Калмана

Два этапа в каждом цикле:

Predict (Предсказание):
«На основе модели, где я должен быть?»
Update (Коррекция):
«Датчик говорит другое. Кому верить больше?»

Упрощённые формулы

$$ \hat{x}_k^- = A \hat{x}_{k-1} $$$$ P_k^- = A P_{k-1} A^T + Q $$$$ K_k = P_k^- H^T (H P_k^- H^T + R)^{-1} $$$$ \hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) $$$$ P_k = (I - K_k H) P_k^- $$

где $K_k$ — коэффициент Калмана (сколько верить датчику).

Интуиция

Предсказание:    "Думаю, я здесь" ───● (неуверенность ≈)
                                      │
Измерение:       "Датчик говорит" ───● (неуверенность ○)
                                      │
                                      ▼
Результат:       "Скорее всего" ─────● (меньше неуверенности!)

Калман взвешивает предсказание и измерение по их точности.

Простой 1D Калман для угла

// Состояние
float angle = 0;     // Оценка угла
float bias = 0;      // Оценка смещения гироскопа
float P[2][2] = {{1, 0}, {0, 1}};  // Ковариация ошибки

// Параметры (настраиваются!)
float Q_angle = 0.001;   // Шум процесса (угол)
float Q_bias = 0.003;    // Шум процесса (bias)
float R_measure = 0.03;  // Шум измерения акселерометра

float kalmanFilter(float newAngle, float newRate, float dt) {
  // Predict
  float rate = newRate - bias;
  angle += dt * rate;
  
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;
  
  // Update
  float S = P[0][0] + R_measure;
  float K[2] = {P[0][0] / S, P[1][0] / S};
  
  float y = newAngle - angle;  // Innovation
  angle += K[0] * y;
  bias += K[1] * y;
  
  float P00_temp = P[0][0];
  float P01_temp = P[0][1];
  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;
  
  return angle;
}

Практика: стабилизация дрона

Задача

Квадрокоптер должен держать горизонт:

        Цель: θ = 0°
             ═══════
            ╱       ╲
           ╱         ╲
          O           O
          
        Реальность:
             ═══════
            ╱       ╲  ← наклон!
           ╱   θ     ╲
          O           O

Полный pipeline

┌─────────────┐     ┌──────────────┐     ┌───────────┐
│ Акселерометр│────►│              │     │           │
│   (ax,ay,az)│     │   Sensor     │     │    PID    │
└─────────────┘     │   Fusion     │────►│Controller │────► Моторы
┌─────────────┐     │ (Kalman/CF)  │     │           │
│  Гироскоп   │────►│              │     └───────────┘
│  (gx,gy,gz) │     └──────────────┘
└─────────────┘            │
                           ▼
                    Roll, Pitch, Yaw

Код для квадрокоптера

void loop() {
  // 1. Читаем IMU
  float ax, ay, az, gx, gy, gz;
  readIMU(&ax, &ay, &az, &gx, &gy, &gz);
  
  // 2. Sensor Fusion
  float accel_roll = atan2(ay, az) * RAD_TO_DEG;
  float accel_pitch = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG;
  
  roll = kalmanFilter_roll(accel_roll, gx, dt);
  pitch = kalmanFilter_pitch(accel_pitch, gy, dt);
  
  // 3. PID для стабилизации
  float roll_correction = computePID(0, roll);
  float pitch_correction = computePID(0, pitch);
  
  // 4. Микширование моторов
  motor1 = throttle + roll_correction + pitch_correction;
  motor2 = throttle - roll_correction + pitch_correction;
  motor3 = throttle - roll_correction - pitch_correction;
  motor4 = throttle + roll_correction - pitch_correction;
  
  setMotors(motor1, motor2, motor3, motor4);
}

Сравнение подходов

Метод	Сложность	Качество	Когда использовать
Только гироскоп	⭐	Дрейф	Никогда долго
Только акселерометр	⭐	Шум	Только в статике
Комплементарный	⭐⭐	Хорошо	IMU, простые проекты
Калман 1D	⭐⭐⭐	Отлично	IMU, дроны
EKF (Extended)	⭐⭐⭐⭐	Лучшее	SLAM, автопилоты

Подробнее в справочнике

📚 IMU датчики
📚 Алгоритмы фильтрации
📚 MPU6050

Следующий урок

🗺️ SLAM — Карта и локализация

Урок 3.3 →

Как робот строит карту и одновременно определяет своё положение!