Построение ИНС на современных бюджетных MEMS датчиках
Рынок малогабаритных сенсоров на основе технологии MEMS в текущий момент быстро растет. Эти сенсоры встраиваются уже практически во все мобильные телефоны, используются в любительских системах стабилизации и управления. Цена за простейшие сенсоры начинается примерно с 1.5$ в розницу, 10$ стоит 3х осевой датчик среднего качества. Более профессиональные модели стоят от 100-200$ и доходят до 1000$, они отличаются большей точностью и стабильностью характеристик. Но купить такой, чтобы просто поиграться, получается дороговато.
В статье проведен небольшой анализ того, что получится, если попытаться построить ИНС (инерциальную навигационную систему) на основе бюджетных MEMS датчиков (примерно по 10$ за датчик). Основной вопрос - какие будут накапливаться погрешности перемещения. Из этих значений, можно будет определить, на что же сможет сгодиться такая ИНС.
Сразу скажу: подобная тематика не мое направление работы. С фильтрацией, цифровой обработкой сигналов и прочими смежными тематиками я знаком довольно поверхностно, поэтому где-то могу наврать в терминологии, где-то пропустить что-то важное. По большому счету вся реализация работает "в лоб", без дополнительных фильтров и уловок. С применением продвинутых фильтраций, можно будет существенно улучшить результаты, но в текущем виде, полученные данные уже позволяют сделать некоторые выводы.
Система инерциальной навигации (ИНС) призвана информировать о перемещении объекта относительно начальной точки. Наблюдение производится при помощи наборов датчиков. В идеальном случае - это акселерометр и гироскоп. Этих 2х датчиков достаточно для отслеживания перемещения объекта в пространстве. Но мы живем не в идеальном мире, так что при обработке показаний с датчиков накапливается ошибка (происходит интегрирование показаний во времени). В условиях гравитационного поля Земли акселерометр используют для компенсации углов крена-тангажа, а магнетометр необходим для компенсации курса. Для атмосферы можно добавить барометрический датчик и сделать компенсацию по высоте, т.е. оси Z (но с длинным списком ограничений).
Далее, если точно известна ориентация, по ускорениям с акселерометра определяется перемещение объекта. Но здесь выявляется 2 особенности:
- Акселерометр фиксирует ускорение свободного падения. Величина эта достаточно большая (относительно прочих ускорений). Зная ориентацию, можно избавиться от G, однако, все-равно небольшие погрешности добавляет ошибку не только в перемещения объекта, но и в значения его ускорения. К примеру, 1 градус в отклонении гировертикали даст линейное ускорение в плоскости земли в 0.15м/с^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 для коррекции его показаний. В частности, добавляется точный курс, точнее определяется высота, есть некоторый резерв отслеживания положения на моменты потери сигнала.
Шаговая инерциалка
Интересная идея была в завязке перемещения на шаги человека. В данном случае в моменты опоры на ноги гарантированна неподвижность. Такие моменты повторяются часто, поэтому сбросы так же возможны через небольшие промежутки времени. Некоторая концепция была разработана, однако в итоге отказался от экспериментов из-за отвратительных результатов по накоплению ошибок. Так же был вырезан полностью раздел по ней с зачатками описания. Однако, случайно найденная демонстрация вселяет оптимизм в данном направлении.
По этой причине решил вернуться к ИНС позже, после того, как будут улучшены алгоритмы калибровки датчиков.