Построение ИНС на современных бюджетных MEMS датчиках

Рынок малогабаритных сенсоров на основе технологии MEMS в текущий момент быстро растет. Эти сенсоры встраиваются уже практически во все мобильные телефоны, используются в любительских системах стабилизации и управления. Цена за простейшие сенсоры начинается примерно с 1.5$ в розницу, 10$ стоит 3х осевой датчик среднего качества. Более профессиональные модели стоят от 100-200$ и доходят до 1000$, они отличаются большей точностью и стабильностью характеристик. Но купить такой, чтобы просто поиграться, получается дороговато.

В статье проведен небольшой анализ того, что получится, если попытаться построить ИНС (инерциальную навигационную систему) на основе бюджетных MEMS датчиков (примерно по 10$ за датчик). Основной вопрос - какие будут накапливаться погрешности перемещения. Из этих значений, можно будет определить, на что же сможет сгодиться такая ИНС.

Сразу скажу: подобная тематика не мое направление работы. С фильтрацией, цифровой обработкой сигналов и прочими смежными тематиками я знаком довольно поверхностно, поэтому где-то могу наврать в терминологии, где-то пропустить что-то важное. По большому счету вся реализация работает "в лоб", без дополнительных фильтров и уловок. С применением продвинутых фильтраций, можно будет существенно улучшить результаты, но в текущем виде, полученные данные уже позволяют сделать некоторые выводы.

Система инерциальной навигации (ИНС) призвана информировать о перемещении объекта относительно начальной точки. Наблюдение производится при помощи наборов датчиков. В идеальном случае - это акселерометр и гироскоп. Этих 2х датчиков достаточно для отслеживания перемещения объекта в пространстве. Но мы живем не в идеальном мире, так что при обработке показаний с датчиков накапливается ошибка (происходит интегрирование показаний во времени). В условиях гравитационного поля Земли акселерометр используют для компенсации углов крена-тангажа, а магнетометр необходим для компенсации курса. Для атмосферы можно добавить барометрический датчик и сделать компенсацию по высоте, т.е. оси Z (но с длинным списком ограничений).

Далее, если точно известна ориентация, по ускорениям с акселерометра определяется перемещение объекта. Но здесь выявляется 2 особенности:

  1. Акселерометр фиксирует ускорение свободного падения. Величина эта достаточно большая (относительно прочих ускорений). Зная ориентацию, можно избавиться от G, однако, все-равно небольшие погрешности добавляет ошибку не только в перемещения объекта, но и в значения его ускорения. К примеру, 1 градус в отклонении гировертикали даст линейное ускорение в плоскости земли в 0.15м/с^2, оно сравнимо с обычными ускорениями при ходьбе.
  2. Акселерометр выдает ускорение. Чтобы получить позицию, требуется 2 интегрирования по времени. Учитывая что ошибка имеется в самом значении ускорения, она накапливается уже в значении скорости. А в значении позиции ошибка накапливается квадратично.

По-моему мнению, это основные моменты, которые могут свести на нет всю затею ИНС на MEMS датчиках.

Тестовый стенд

Для тестирования взяты 2 платы, изготовленные мной для других проектов и задумок. Плата с контроллером и плата с датчиками. В качестве управляющего контроллера искользуется NXP LPC1342 (Cortex M3, 72MHz). Так же задействованы датчики: LSM303 (спареный 3х осевой акселерометр и магнетометр), L3G4200 (3х осевой гироскоп) и BMP085 (барометр).

Из этих плат собран такой бутерброд:

Он работает по интерфейсу USB, как HID устройство. Функция у него только одна: постоянно отсылать в PC пакеты с выборками датчиков и меткой времени для последующего интегрирования показаний.

Обработка данных

В первую очередь при обработке данных с датчиков, вычисляется ориентация в пространстве. Для этого используется реализация алгоритма MARG описанная в документе. На выходе алгоритма имеется 4х мерный вектор - кватернион. Вектор однозначно описывает ориентацию в пространстве. По кватернионам есть хорошее описание: документ буквально на пару страничек, объясняющий их суть и описывающий все основные операции с ними.

Итак, теперь остается только рассчитать перемещение объекта. Расчет удобнее всего проводить в некоторой глобальной системе координат, приняв за начало - точку старта/инициализации, а за базисные вектора - направление вектора силы тяжести и направление на магнитный север (не магнитного вектора, а вектора в плоскости земли). Исходя из ориентации, полученный на каждом шаге вектор ускорения переводится в глобальную систему отсчета, из него вычитается ускорение свободного падения и проводится интегрирование, сначала для получения скорости, затем для получения позиции.

Данные с барометра позволяют избавиться от накопления погрешности по высоте, но в текущий момент эти показания не используются.

Софт

Для отображения данных на этапах отладки использовались логирования датчиков:

В качестве финального теста была написана простенькая программа. В ней отображается посчитанная ориентация и положение:

У меня только цифры. На YouTube есть более красочные демонстрации, к примеру, эта или эта. В принципе, мне так же можно сделать подобные визуализации, но это чисто эстетическая вещь и тратить на них времени не захотелось.

Тестирование

Уже была продумана методика тестирования, благо до написания самих тестов дойти не успел. Собственно, тестирование так и не потребовалось: в самом простом случае наблюдается быстрый уход позиции. За 10 секунд работы накопленная ошибка может достигать 1 метра, и это в неподвижном состоянии! Конечно, сказывается плохая калибровка датчиков, причем похоже в первую очередь акселерометра. Как результат "в лоб" сделать реализацию не получилось.

При поворотах так же замечается задержка в вычислении позиции. Эта задержка в итоге сказывается при компенсации ускорения свободного падения - при поворотах накопление ошибки по скорости происходит намного быстрее, соответственно и быстрее уходит позиция.

Выводы

Первый и самый важный вывод - ИНС не получился. Накапливаемые ошибки слишком велики. Вообще возможно, что улучшение калибровок, фильтраций и более точные отстройки ядра MARG позволят улучшить характеристики ИНС, однако вряд ли стоит ожидать от нее выдающихся характеристик и рассчитывать что такой ИНС станет применим в задачах топосъемки.

В качестве эксперимента была добавлена компенсация по барометру. Сам барометр довольно сильно шумит, поэтому требуется фильтровать его показания. Я использовал простой lowpass фильтр, побочный эффект которого - задержка в сигнале. Для избавления от этой задержки, можно смешивать фильтрованные данные барометра с данными от ИНС. В некоторых условиях работало довольно неплохо - задержка устраняется.

Пока что единственная идея, где может пригодиться ИНС с подобными характеристиками - использование в паре с приемником GPS для коррекции его показаний. В частности, добавляется точный курс, точнее определяется высота, есть некоторый резерв отслеживания положения на моменты потери сигнала.

Шаговая инерциалка

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

По этой причине решил вернуться к ИНС позже, после того, как будут улучшены алгоритмы калибровки датчиков.

Ссылки

  1. Исходные тексты программного комплекса и использованных библиотек
  2. Описание использованного MARG ядра
  3. Введение в кватернионы
Original: http://igorkov.org/imu,
Author: igorkov