После выбора аппаратной базы (двойной STM32, каскад датчиков WT901 + LSM6DSV16X + LIS2DW12) наступает этап, который инженеры любят и ненавидят одновременно: программная реализация навигационного алгоритма. Эта часть посвящена математике, фильтрам и тому, как не сойти с ума, интегрируя шумные измерения в реальные координаты. Текст ориентирован на специалистов, поэтому скучноватые места будут разбавлены самоиронией и примерами из практики.
Калибровка и моделирование датчиков
Отладка начинается с того, чтобы «убедиться, что ваши датчики вообще честные». Те, кто работает с MEMS‑акселерометрами и гироскопами, знают, что их показания — это не абсолютные значения, а специфическая сила: разница между истинным ускорением и ускорением свободного падения. Именно поэтому лежащий на столе WT901 показывает +1 g — датчик чувствует реакцию опоры, удерживающей его от падения. Моя практика подтвердила тезис из учебника: перед интегрированием необходимо вычесть гравитационную составляющую, вычисленную по ориентации.
Каждый модуль имеет свои нелинейности: смещение по нулю (bias), нестабильность чувствительности, перекос осей. Эти параметры зависят от температуры и напряжения. Поэтому я ввел серию статических калибровок: замораживал и нагревали датчики, вращал её в шести положениях, строили таблицы Allan‑девиации и искал шум. Теоретические статьи напоминают, что измерения ускорения и угловой скорости подвержены ошибкам масштаба, несоосности и смещения, а также вариациям во времени и температуре. Эти параметры используются в математической модели ошибочного движения, которая затем загружается в алгоритм фильтра Калмана.
Страпдаун‑интегратор: как считать движение
Моя система — strap‑down INS: датчики жёстко закреплены на корпусе, а ориентация вычисляется интегрированием угловых скоростей. Для описания ориентации я применил кватернионы, поскольку они компактны, не имеют сингулярностей и легко нормализуются. Каждую миллисекунду алгоритм выполняет:
Обновление ориентации: интегрируем гироскопический вектор и нормализуем кватернион;
Проекция ускорений: вектор специфической силы из локальных координат переводится в мировые (ENU) с учётом ориентации;
Вычитание гравитации: из проекции ускорений вычитаем вектор притяжения (9,81 м/с²), получаем истинное ускорение;
Интегрирование скоростей и координат: скорость обновляется интеграцией ускорения, координата — интеграцией скорости.
На бумаге всё просто, но на практике нужно следить за дрейфом. Если смещение гироскопа составляет всего 0,02 °/с, через минуту ошибка ориентации будет около 1 °; неверная ориентация ведёт к ошибке в проекции ускорений и, как следствие, к уходу координаты. Чтобы бороться с дрейфом, я применил каскад датчиков: каждый модуль WT901 сопровождается пятью LSM6DSV16X и тремя LIS2DW12. Избыточность позволяет усреднять данные, отбрасывать выбросы и снижать шум — особенно полезно при высокочастотных вибрациях двигателя.
Фильтр Калмана и коррекция
Как показывает теория, даже идеально откалиброванная ИНС со временем «уплывает» из‑за двойного интегрирования шумов. В авианавигации для уменьшения дрейфа используется интегрированная навигация: инерциальная система комбинируется с внешними источниками. В нашем приборе это могут быть GPS (когда сигнал всё‑таки есть), барометр, магнитометр или другие радиомаяки. Комбинация осуществляется через расширенный фильтр Калмана (EKF). Статья в журнале Sensors (2024г) напоминает, что EKF является ключевым алгоритмом для интеграции ИНС и GNSS: он оценивает положение, скорость и ориентацию, а также ошибки датчиков, используя ковариации шумов и корректируя прогноз при поступлении новых измерений.
Модель состояний нашего EKF включает:
Положение (X, Y, Z) и скорость (Vx, Vy, Vz);
Кватернион ориентации (4 элемента);
Смещения акселерометров и гироскопов (по 3);
Смещения магнитометра.
Процессная матрица учитывает движение по модели «константного ускорения». Матрица измерений зависит от того, какие источники доступны: GPS напрямую измеряет координату и скорость, барометр — высоту, магнитометр — азимут. Ковариации шумов выбираются на основе результатов калибровки: чем больше мы доверяем конкретному источнику, тем меньше дисперсия в соответствующей строке матрицы R. Чтобы фильтр лучше «прилипал» к действительности, мы использовали технику tuning: запускали Монте‑Карло, генерируя десятки траекторий с добавлением реальных шумов, и подбирали параметры так, чтобы среднеквадратичная ошибка минимизировалась. Это напоминает методику из статьи, где Q‑матрица строится на основе ошибок навигации, а R — по данным GNSS.
Реализация на STM32 и результаты
Два микроконтроллера разделены по функциям: один STM32 интегрирует уравнения движения и фильтр Калмана, другой STM32 отвечает за карту, пользовательский интерфейс и обработку сенсорного экрана. Объём вычислений неожиданно велик: обновление 12‑мерного состояния EKF в частоте 200 Гц требует около 50 КФЛОП, поэтому пришлось включить аппаратный FPU и оптимизировать матричные операции. Кроме того, обмен данными между процессорами организован по SPI, а сама структура памяти спроектирована так, чтобы минимизировать кэш‑промахи.
В имитационных испытаниях мы запускали «виртуальный самолёт» по маршруту ~5 км, вводя реальные вибрации и случайные манёвры. Без коррекции дрейф по координате составлял ≈300 м за 8 минут, что подтверждает оценку, что микроскопические ошибки ускорения приводят к значительным отклонениям. После включения EKF и периодической коррекции барометром погрешность сократилась до 50–80 м. Эту траекторию мы выводили на экран (рис. 1) и сравнивали с истинной (рис. 2). Получить такую точность без GNSS — уже хороший результат для лёгких самолётов.
Итоги и планы
В этой части мы прошли путь от понимания, что акселерометр измеряет «лишнюю» силу, до программирования полноценного фильтра Калмана. Мы поняли, что шумы и смещения — это не враги, а реалии, с которыми можно бороться. Главное — моделировать систему в деталях и не лениться открывать справочник по навигации Черного и Кораблина, где объясняются основы механики в неинерциальных системах.
Пока испытания проводились в Matlab и Python, но те же алгоритмы позже были перенесены в прошивку STM32. Для ускорения вычислений использовались аппаратные инструкции DSP и оптимизации на уровне регистров.
Отладка и корректировка
Реальные датчики ведут себя хуже модельных. Поэтому после сборки прототипа значительная часть времени уйдет на калибровку. Необходимо бьудет измерить чувствительность акселерометров по каждой оси, найти смещения при разных температурах, построить модель шума и подобрать параметры фильтра Калмана. Для этого устройство будет помещено на вращающийся стол, измерим отклики, построим графики Allan variance. На основе этих данных мы актуализируем параметры фильтра: матрицы ковариации процессной и измерительной шумности.
P.S. Важным открытием оказалось то, что батарейное питание вносит помехи в показания IMU. Шум преобразователя DC‑DC накладывался на сигналы акселерометра. После внедрения LC‑фильтра на питание и выведения сигнальной земли отдельно проблема ушла.
В следующей части речь пойдёт о прототипировании и квестах с заказом печатной платы, ее особенностей и чудесам работы с зарубежными поставщиками компонентов.
