Система ориентации: цепочка обработки IMU

Урок-практикум: собираем полную цепочку от сырых данных IMU до устойчивого угла наклона.

Система ориентации — конвейер, превращающий сырые данные акселерометра и гироскопа в стабильную оценку угла: фильтрация, наклон, слияние.

Здесь сходятся темы курса: датчик (IMU), фильтрация шума, слияние. Покажем полную цепочку: чистим акселерометр медианой, считаем наклон через arctan, сливаем с гироскопом комплементарным фильтром.

Шаг 1: наклон из акселерометра

Угол тангажа из проекций ускорения (урок про IMU):

$$ \theta_{acc} = \arctan\frac{a_x}{\sqrt{a_y^2 + a_z^2}} $$

import math, statistics

def pitch_from_acc(ax, ay, az):
    return math.degrees(math.atan2(ax, math.sqrt(ay*ay + az*az)))

# сырые отсчёты ax с одним выбросом
ax_raw = [4.9, 5.0, 9.5, 4.8, 5.1]
# медианный фильтр от выброса
ax = statistics.median(ax_raw)
print("ax после медианы:", ax)
print("угол:", round(pitch_from_acc(ax, 0.0, 8.5), 2), "°")

Вывод:

ax после медианы: 5.0
угол: 30.47 °

Шаг 2: слияние с гироскопом

Сливаем угол акселерометра с интегралом гироскопа комплементарным фильтром (раздел 6):

import math, random
random.seed(3)

dt = 0.1
alpha = 0.96
angle = 0.0
true = 0.0

for k in range(6):
    true += 3.0 * dt                              # реально 3 °/с
    gyro = 3.0 + random.uniform(-0.2, 0.2)        # шумная скорость
    acc = true + random.uniform(-2, 2)            # шумный угол
    angle = alpha*(angle + gyro*dt) + (1-alpha)*acc
    print("истина", round(true, 2), "-> оценка", round(angle, 2))

Вывод:

истина 0.3 -> оценка 0.3
истина 0.6 -> оценка 0.61
истина 0.9 -> оценка 0.84
истина 1.2 -> оценка 1.18
истина 1.5 -> оценка 1.43
истина 1.8 -> оценка 1.75

Как работает под капотом

Полная цепочка ориентации: сырые данные → калибровка нуля (датчик в покое не должен показывать наклон) → медианный/низкочастотный фильтр от выбросов и шума → расчёт угла акселерометра → слияние с гироскопом → готовый угол для управления. В реальных автопилотах вместо комплементарного фильтра часто стоит расширенный фильтр Калмана по всем трём осям плюс магнитометр для курса, но логика та же. Главное — каждый блок курса (датчик, АЦП, калибровка, фильтр, слияние) здесь занимает своё место в конвейере.

Частые ошибки

• Пропустить калибровку нуля — система покажет наклон в покое.
• Сливать нефильтрованный акселерометр — выбросы протекут в угол.
• Интегрировать гироскоп без слияния — угол уплывёт за минуты.

Итог

• Ориентация — это конвейер: калибровка → фильтр → наклон → слияние.
• Медиана убирает выбросы акселерометра, arctan даёт наклон, комплементарный фильтр сливает с гироскопом.
• Каждый блок курса находит здесь практическое применение.