Инерциальная система навигации своими руками

Содержание
  1. Инерциальная навигационная система: как это работает
  2. В автономном полете
  3. Устройство инерциальной системы
  4. Авионика из Раменского
  5. Инерциальная навигационная система: общие сведения, принцип действия, классификация и способы ориентации
  6. Общие сведения о технологии
  7. Компоненты системы
  8. Принцип работы системы
  9. Классификация инерциальных навигационных систем
  10. Способы определения ориентации платформенных систем
  11. Особенности инерциально-спутниковых систем
  12. Области применения инерциальных систем
  13. Заключение
  14. Что такое инерциальная навигация
  15. Инерциальные навигационные системы для беспилотного транспорта
  16. Комплексирование инерциальной навигации с ГНСС
  17. Об инерциальных системах в беспилотном автомобиле
  18. Общий принцип вычисления навигации инерциальным модулем с коррекцией от ГНСС приемника (или как работает комплексированное навигационное решение?)
  19. Типы выставки навигационного алгоритма (или типы навигационной выставки)
  20. Как происходит коррекция по поведенческой модели автомобиля?
  21. Почему ошибается инерциальная навигационная система?
  22. Планы на будущее
  23. О других кейсах
  24. Контакты
  25. О компании
  26. Авторы материала
  27. Использование инерциальной навигационной системы (ИНС) с несколькими датчиками на примере задачи стабилизации высоты квадрокоптера
  28. Какие задачи необходимо решать в полете?
  29. Что мы имеем в распоряжении?
  30. Сглаживаем
  31. Интегрируем
  32. Комплементарный фильтр
  33. ПИД-регулятор приходит на помощь
  34. А как же гироскоп?

Инерциальная навигационная система: как это работает

1896a0a46c86fc53d18598f317d47b50

Долгое время отсутствие точной информации о местоположении самолета или вертолета было серьезным препятствием на пути развития авиации. Пилотам была необходима навигационная система, которая не зависела бы от земных ориентиров и капризов природы. Появление автономных инерциальных систем навигации стало большим шагом в истории авиации. Первые инерциальные системы в нашей стране были разработаны в 1960-х годах в Раменском приборостроительном КБ, входящем сегодня в концерн «Радиоэлектронные технологии» (КРЭТ) Госкорпорации Ростех.

В автономном полете

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

ec0d69e33f37594abb81588ef7f78d44
Бесплатформенная навигационная система БИНС-СП-1

Такой метод теоретически был разработан в 1930-е годы, но реализовать его удалось только спустя 20 лет. Инерциальная навигация основывается на применении законов механики, в частности на теории устойчивости механических систем, которую разрабатывали русские математики А.М. Ляпунов и А.В. Михайлов. Первые инерциальные навигационные системы (ИНС) были созданы в 1950-е годы в США и СССР. Они были довольно громоздкими и могли занимать все свободное пространство самолета. Современные ИНС изготавливаются с применением микроэлектронных технологий и занимают гораздо меньше места.

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

Устройство инерциальной системы

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

d32ce8fabd4424aa85b6c36458322d0a
Лазерный гироскоп

Для сбора информации о полете в состав ИНС включаются акселерометры, считывающие линейное ускорение, и гироскопы, позволяющие определить углы наклона самолета относительно основных осей: тангаж, рысканье и крен. Точность полученной информации зависит от характеристик этих приборов. Анализом данных занимается компьютер, который затем по определенным навигационным алгоритмам корректирует движение объекта.

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

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

Недостатком ИНС можно назвать ошибки, которые накапливаются с течением времени в получаемой от приборов информации. Это могут быть как методические ошибки, так и ошибки, связанные с неверной начальной настройкой оборудования. Для их коррекции создаются интегрированные навигационные системы, где данные, получаемые ИНС, дополняются данными, поступающими от неавтономных систем, например спутниковой навигации. Еще одним относительным минусом ИНС является высокая стоимость входящего в их состав оборудования.

Авионика из Раменского

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

1c79492220fb60cb0231d0a6d8a1f5ba

В 1960-е годы первые в СССР инерциальные навигационные системы для авиации были разработаны Раменским приборостроительным конструкторским бюро (РПКБ). Начиная с 1958 года специалистами РПКБ проводилось эскизное проектирование ИНС для различных классов и типов летательных аппаратов. Совершенствование чувствительных элементов – разработка поплавковых гироскопов и акселерометров, а затем динамически настраиваемых гироскопов – и применение цифровой вычислительной техники обеспечили создание и широкое применение ИНС.

К началу 1970-х годов предприятием были решены проблемы точного управляемого полета на большие расстояния. В дальнейшие годы инерциальные системы многократно совершенствовались. Раменское предприятие создавало навигационные комплексы и другое оборудование для самолетов ОКБ Сухого, Микояна, Туполева, Ильюшина, Камова, Миля и др. В 2012 году предприятие вошло в состав концерна «Радиоэлектронные технологии». Сегодня РПКБ – один из мировых лидеров в производстве авиационной электроники.

Источник

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

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

Общие сведения о технологии

53c656668eaa5c78a6c653085cfb774c

42c522ad237e2155ea4fad4a90d621c1 Вам будет интересно: GPS-трекер для автомобиля. Какой GPS-трекер выбрать для автомобиля? Установка GPS-трекера на автомобиль. Подключение GPS-трекера

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

Компоненты системы

97849d16f8b11ac4ffdd155c4217f78a

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

Принцип работы системы

f8f9e6ac83c8f6b2ecfc604577d4e6b3

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

Читайте также:  Забавные коты своими руками

Классификация инерциальных навигационных систем

48913cc826af402d225343db94d1c41e

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

Способы определения ориентации платформенных систем

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

d4112af03c9a8a16273f6fd0a2dfb437

Особенности инерциально-спутниковых систем

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

Области применения инерциальных систем

Среди потенциальных потребителей технологии инерциальной навигации значатся представители самых разных отраслей. Это не только космонавтика и авиация, но также автомобилестроение (системы навигации), робототехника (средства контроля кинематических характеристик), спорт (определение динамики движения), медицина и даже бытовая техника т. д.

Заключение

c8cb9835a01a61c1556268bfdd744a90

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

Источник

Авторы: Андрей Михеев, технический директор ООО «ЛМП» и Андрей Родин, инженер-программист ООО «ЛМП».
Присоединяйтесь к обсуждению темы в Facebook
Подписывайтесь на наш телеграм-канал

art 2636

Что такое инерциальная навигация

Неавтономные методы решения задач навигации основываются на использовании внешних ориентиров или сигналов (например, звёзд, маяков, радиосигналов и т. п.). Эти методы достаточно просты, но в ряде случаев не могут быть реализованы из-за отсутствия видимости или наличия помех для радиосигналов и т.п. Причиной возникновения инерциальной навигации стала необходимость создания автономных навигационных систем.

Инерциальные навигационные системы для беспилотного транспорта

В 2018 году Лаборатория Микроприборов разработала инерциальную навигационную систему (ИНС) ГКВ-11 с встроенным ГНСС (Global Navigation Satellite System, глобальная навигационная спутниковая система) приемником метровой точности. Данный модуль позволяет получить курс автомобиля в движении, рассчитать ориентацию и поддерживать автономную навигацию при кратковременной потере сигнала ГНСС.

ИНС ГКВ состоят из МЭМС-датчиков (датчиков угловой скорости и акселерометров), высокопроизводительного вычислителя, приемника ГНСС, магнитометра и необходимой периферии. Для вычисления навигации инерциальный модуль комплексует инерциальные и ГНСС данные. Ошибки, вызванные шумами и нестабильностью инерциальных датчиков, корректируются по данным ГНСС приемника.

Датчики ГКВ-6 и ГКВ-11.

art 2624

Антенны ГКВ-12 и ГКВ-11.

art 2623

В квалификационных заездах конкурса по беспилотному транспорту «Зимний город» в марте 2019 г. и в финале, в декабре 2019 г., с инерциальной навигационной системой от ЛМП участвовало два беспилотных автомобиля команды МАДИ (ГКВ-11 с RTK) и команды НГТУ (с ГКВ-10 и ГНСС приемником от Ориент Системс).

На текущий момент Лаборатория Микроприборов продолжает совершенствовать свои инерциальные навигационные системы. Команде МАДИ поставлены несколько новых инерциальных модулей ГКВ-12 с двухантенным решением для получения курса из статики.

Беспилотный автомобиль МАДИ с инерциальной навигационной системой ООО «ЛМП».

art 2625

art 2626

Комплексирование инерциальной навигации с ГНСС

Комплексированное навигационное решение позволяет вычислять ориентацию ИНС с высокой точностью при высокой динамике (в зависимости от характеристик датчиков от 0,5° до 0,01°), а также работать автономно при отключении или пропадании ГНСС сигнала. Чем стабильнее и точнее инерциальная часть, тем дольше будет сохраняться навигация без дополнительной коррекции. На рисунках 1, 2 и 3 показаны проезды под мостом и туннелем, а также при плотной городской застройке.

Рис. 1. Проезд на автомобиле под мостом.

art 2635

Рис. 2. Проезд на автомобиле под туннелем.

art 2634

Рис. 3. Проезд на автомобиле при плотной застройке.

art 2633

Об инерциальных системах в беспилотном автомобиле

Команда Лаборатории Микроприборов подготовила ответы на следующие вопросы, касающиеся применения инерциальных навигационных систем в беспилотных и автономных системах:

Общий принцип вычисления навигации инерциальным модулем с коррекцией от ГНСС приемника (или как работает комплексированное навигационное решение?)

ГНСС передает координаты в геодезической СК (геоцентрическая ECEF или эллипсоидная LLA), которые внутри алгоритма преобразуются в СК NED и выполняется коррекция инерциальной навигации, к начальной точке СК NED привязывается определенная координата в геодезической СК. Таким образом, абсолютная точность координат, выдаваемых навигационным алгоритмом, в геодезической СК полностью зависит от начальной вычисленной координаты ГНСС приемником.

После включения ИНС и ГНСС работа навигационного алгоритма построена следующим образом:

art 2627

Типы выставки навигационного алгоритма (или типы навигационной выставки)

На данный момент Лабораторией Микроприборов представлено 4 типа выставки для навигационного алгоритма с коррекцией от данных ГНСС приемника, которые где-то могут пересекаться, но так как их разработка проводилась последовательно, они имеют разные номера:

*рекомендуется такой временной интервал, исходя из времени нестабильности датчиков угловой скорости (ДУС), но этот параметр может быть задан пользователем.

Как происходит коррекция по поведенческой модели автомобиля?

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

В таком случае значительно повышается время автономной работы навигационного модуля. При проезде под мостом алгоритм с коррекцией по модели автомобиля приезжает в точку возобновления ГНСС решения (Рисунок 4а), в то время как чисто автономный навигационный алгоритм допускает бо́льшую ошибку (Рисунок 4б).

art 2632

art 2631

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

Читайте также:  Греческий кувшин своими руками

art 2630

Почему ошибается инерциальная навигационная система?

Основной источник ошибки — это ошибки ориентации. В сводной таблице приведен анализ ошибок на расчет скорости и положения.

Интегрирование ошибки датчиков угловой скорости (гироскопы)

Ошибка в 0,1° ведет к ошибке в скорости 17 мм/с
За 10 с ошибка скорости 170 мм/с
За 60 с ошибка скорости 1 м/с

Ошибка измерения линейного ускорения (акселерометры)

Ошибка в 1 mg ведет к ошибке в положении
За 1 с – 5 мм
За 10 с – 25 см
За 60 с – 18 м

Ошибка в масштабном коэффициенте особенно заметна при постоянном вращении в одном направлении. Как правило, движение автомобилей носит случайный характер, и сумма всех поворотов сводится к 0, поэтому ошибка масштабного коэффициента вносит в таких случаях малый вклад.

При ошибке ориентации угловая скорость по одной из осей вращения вносит свою проекцию на остальные оси, например, при ошибке в 1 мрад (0,057 °/с) проекция угловой скорости вдоль оси Z, равная 120 °/с на оси X, Y (крен, тангаж) составит 0,12 °/с, за 3 секунды вращения (поворот на 360°) ошибка ориентации увеличится по крену и тангажу на 0,36°.

Для списывания накопленных ошибок ИНС необходимо подключать дополнительные источники коррекции на других принципах работы. Для беспилотного автомобиля корректорами могут выступать:

art 2637

Планы на будущее

В 2021 г. Лаборатория Микроприборов планирует добавить коррекцию от двухантенного ГНСС приемника в движении на малой скорости, когда курс по динамической модели вычисляется с бо́льшей ошибкой в сравнении с ГНСС приемником. Также ведется разработка более точного ИНС с нестабильностью по ДУС менее 1 °/ч и менее 0,2 °/ч для увеличения времени автономной работы и точности вычисления навигации.

О других кейсах

Беспилотные летательные аппараты (БПЛА). Для БПЛА были разработаны малогабаритные инерциальные модули ГКВ-5 и ГКВ-6, которые могут быть использованы в составе автопилота или для проведения геодезических изысканий (фотограмметрия или облако точек с привязкой к координатам).

Наведение и стабилизация спутниковых антенн. Лабораторией Микроприборов был разработан алгоритм наведения и стабилизации спутниковых антенн при движении по суше, в воздухе и в море. В качестве корректора используется ГНСС приемник, но также возможна коррекция по уровню сигнала от космического спутника.

Контакты

Сайт Лаборатории Микроприборов: mp-lab.ru. По вопросам сотрудничества пишите: Этот адрес электронной почты защищён от спам-ботов. У вас должен быть включен JavaScript для просмотра.

Подписывайтесь на нас в социальных сетях:

О компании

Компания обладает компетенциями не только в сфере разработки и производства инерциальных систем, но и в проведении испытаний и калибровки, а также в оказании технической поддержки по внедрению устройств в беспилотные летательные аппараты и автомобили, спутниковые антенны, катера и другие системы. Благодаря дружной команде высококлассных специалистов и современному оборудованию, фирма может изготавливать чувствительные элементы и приборы на основе МЭМС в Зеленограде.

Сейчас у Лаборатории Микроприборов более 10 активных проектов, свыше 30 направлений по применению инерциальных систем и более 100 клиентов. Компания небольшая, поэтому достаточно мобильна и открыта для общения. Готовы помогать с интеграцией своих устройств, предлагать нестандартные решения и быть на связи не только в рабочие часы.

Авторы материала

Андрей Михеев, технический директор ООО «ЛМП», Этот адрес электронной почты защищён от спам-ботов. У вас должен быть включен JavaScript для просмотра.

art 2609

Андрей Родин, инженер-программист ООО «ЛМП», Этот адрес электронной почты защищён от спам-ботов. У вас должен быть включен JavaScript для просмотра.

Источник

Использование инерциальной навигационной системы (ИНС) с несколькими датчиками на примере задачи стабилизации высоты квадрокоптера

В данной статье я постараюсь рассказать о своем опыте создания и реализации алгоритма для обработки сигналов с нескольких стандартных датчиков, входящих в состав ИНС (в английской версии IMU), для решения задачи стабилизации высоты многороторного летательного аппарата (в моем случае — квадрокоптера). На хабре уже был ряд статей, описывающих, что это за игрушка и как её сделать самому. Как программисту по профессии, мне было интересно не только его собрать, но и поковыряться в «мозгах» и сделать что-то полезное для сообщества. В качестве «мозгов» я выбрал Arduino и замечательный проект MultiWii. Он полностью открытый, динамично развивается, но в нем пока есть «белые пятна». Например, неудовлетворительно работает стабилизация положения по высоте. И я решил разобраться, можно ли с имеющимся оборудованием улучшить эту часть системы.

Для начала немного вводной информации, чтобы прояснить, с чем предстоит иметь дело.
Мультиротор — аппарат с несколькими (3, 4, 6, 8) моторами с пропеллерами, каждый из которых создает вертикальную регулируемую тягу. В отличие от вертолета, стабилизация тут полностью электронная, и занимается ей микропроцессор при помощи ИНС (полетный контроллер).

Какие задачи необходимо решать в полете?

Что мы имеем в распоряжении?

Такой набор в сборе с Arduino-подобным процессором или в виде отдельной платки можно найти за сумму 70-100$

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

Первая задача — стабилизация ориентации — довольно успешно решается гироскопами. Гироскопы очень точно меряют угловую скорость и после интегрирования можно получить углы. Но у них есть проблема — показания уплывают со временем. Для коррекции этого дрифта применяется акселерометр, который всегда (ну или почти всегда в долгосрочной перспективе) знает, где земля. Но акселерометр ничего не почувствует, если его крутить вокруг оси Z, поэтому нам нужен магнетометр, который всегда знает, где север.
Вторая задача — нахождение высоты — частично решается барометром. Если обнулить показания на земле, то при подъеме на каждый метр мы знаем, насколько изменятся его показания (естественно, если мы не летаем 12 часов и не начала меняться погода). Но по условию задачи барометр у нас плохой, и он выдает высоту +-1м с диким шумом амплитудой примерно в этих же пределах. В реальности мой датчик показывает следующее (на 10-й секунде перемещен на 1метр):

3309c689b5afee44f32fea98f48f3e7d

На помощь барометру может прийти сонар, который меряет высоту с очень высокой точностью (даже тот, что я приобрел за 5$, выдает точность ± 3мм по заявлению производителя). Но сонар способен работать только невысоко над землей (2-10м), меряет долго (до 200мс), чувствителен к качеству поверхности, к углу наклона, и может терять сигнал.
Третья задача — определение координат — не решается никак указанными выше датчиками. Акселерометр в комбинации с гироскопом может выдать линейные горизонтальные ускорения, но тут есть две проблемы: постоянно действующий огромный (по сравнению с тем, что будем измерять) вектор 1G, и отсутствие привязок для коррекции. Так что определение координат остается прерогативой GPS-сенсора, и на высокую точность тут рассчитывать не приходится.

Во всех любительских полетных контроллерах задача нахождения ориентации решена хорошо и на ней останавливаться не буду. Задача довольно простая и расписана в интернете (один, два). В MultiWii используется красивое решение без сложностей типа кватернионов и матриц DCM (не забываем, что считать все это будет простенький 16-Мегагерцовый процессор), на основе упрощений для малых углов и комплементарного фильтра.

Читайте также:  Заклепки на сумку своими руками

Итак, ориентацию аппарата в пространстве мы знаем с высокой степенью точности. Теперь можно перейти к основной теме статьи, т. е. постараться улучшить результаты, которые выдает барометр (или сонар), чтобы их можно было скормить ПИД-регулятору. Для этого показания должны поступать без задержек, быть точными в короткой перспективе и не сильно уплывать со временем. Тема ПИД-регуляторов заслуживает отдельного пристального изучения, так как он широко используются в системах управления процессами. Я рекомендую сначала ознакомиться с их определением, чтобы лучше понять рассуждения, изложенные ниже.

Сглаживаем

Чем нам не подходят показания барометра в текущем виде? Ну во первых, сильная зашумленность сигнала будет вызывать лишние управляющие воздействия на моторы. Применив фильтр низких частот, мы уменьшим шум, но потеряем быстроту измерения. А это значит, что любые кратковременные возмущения останутся без внимания, резкие возмущения отработаются с большой задержкой, и самое главное, мы не получим дифференциальную составляющую (D) для ПИД-регулятора. А как следует из теории, регулятор без этой составляющей склонен к слабозатухающим осцилляциям вокруг целевой величины, что и наблюдается на практике.

Интегрируем

Хорошо, оставим барометр и возьмем акселерометр. Вроде все просто — из значения по оси Z вычтем константу 1G, получим линейное вертикальное ускорение. Дважды проинтегрируем его (фактически просуммируем в измерительном цикле) и получим скорость и относительное смещение. Для ПИД-регулятора это лакомые показатели, с ними можно построить хорошую динамическую модель. Но и тут не все так хорошо, как хотелось бы. Наклон аппарата вызовет изменение проекции вектора ускорения А на ось Z. Вибрации от мотора или изменение температуры могут вызвать «сдвиг» чувствительности, и наша константа 1G уже не будет соответствовать реальности. Но даже в случае идеально неподвижного аппарата и точно выставленной 1G, любой сенсор выдает шум. А ведь даже крохотная ошибка в течение десятка секунд двойного интегрирования вырастает до размера слона, и вот мы видим скорость 10м/с и высоту 20м (хотя от земли ещё даже не оторвались).

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

Если объяснять по простому — этот фильтр применяется к двум величинам, измеряемых разными датчиками, и корректирует одну из них так, что она медленно стремится ко второй. В измерительном цикле фильтр реализуется простой формулой:

d2e47628be10399f5ef57aeca7932f4b

При этом влияние величины A2 на A1 пропорционально разнице между ними и определяется коэффициентом k (чем больше, тем слабее влияние).
Если применить этот фильтр к высоте, найденной акселерометром, и показаниям барометра, получится интересная штука: дрифт акселерометра будет постоянно корректироваться барометром, а показания барометра будут сглаживаться (так как для A2 этот фильтр работает как фильтр низких частот). Но корректироваться будет только второй интеграл, а первый по прежнему спокойно «дрейфовать» в бесконечность, и в итоге из-за малого коэффициента k барометр просто не сможет повлиять на ситуацию.
Почему этот фильтр прекрасно работает для пары гироскоп + акселерометр? Потому что там мы корректируем первый интеграл, и он в конце концов перестает «уплывать», когда величина коррекции за время одного цикла сравняется с величиной ошибки гироскопа, прибавляемой в этом же цикле при интегрировании.

ПИД-регулятор приходит на помощь

Но и из пары барометр+акселерометр можно извлечь нечто полезное, если применить к ним ПИД-регулятор (да-да, область их применения крайне обширна).
Итак, в чем главное слабое место нашего интегратора ускорения? В микро-ошибке, которая может возникнуть по разным причинам, описанным выше, при вычитании константы 1G. Если записать искомое ускорение в виде:

ba1f373d11d5645001d8a31f5fd36c67

то, регулируя величину bias, можно управлять и первым интегралом (скоростью), и вторым (смещением). Итак, цель нашего ПИД-регулятора найдена. Но надо ещё знать ошибку. Сделаем допущение, что bias зафиксируется после наступлении некоей стабилизации параметров системы (температурных, вибрационных и т. д.). Когда bias будет найден, показания акселерометра станут очень близки к истине и можно применить комплементарный фильтр, скрестив их с барометром. Величина коррекции этого фильтра и будет ошибкой, от которой будет отталкиваться ПИД-регулятор (он стремится свести ошибку к 0 за счет регулирования целевой переменной).
Дальше находим все три составляющие ПИД-регулятора. Пропорциональная (P) — это сама ошибка. Интегральная (I) — просто интегрируем её. Дифференциальная (D) — по теории надо дифференцировать ошибку. Но в ней сидят ужасные шумы барометра и акселерометра. Такое дифференцировать страшно, поэтому применим хитрость — возьмем за D-составляющую найденную акселерометром скорость с отрицательным знаком. Так как D призвана ввести затухание в регулятор, скорость тут вполне сгодится — чем она больше, тем больше надо её «погасить».

Умножим каждый из компонентов на свои коэффициенты, сложим и получим bias. Но тут применим вторую хитрость — не будем прибавлять bias напрямую к ускорению, а прибавим только I-часть. Именно она описывает величину постоянно действующей ошибки, что соответствует нашему допущению о медленном изменении bias.
P и D части прибавим к скорости, умножив на dT (так как позаимствовали их из ускорения).
Так как основная задача регулятора — найти постоянную составляющую ошибки, я решил настроить его достаточно «мягко», чтобы по минимуму влиять на кратковременные изменения. Но тут остается широкое поле для экспериментов, и все буде определяться поведением реальных датчиков.

А как же гироскоп?

Выше я упомянул, что для определения высоты нам понадобится ещё и гироскоп. Действительно, описанный выше алгоритм будет работать, только если вектор A (в локальной системе) смотрит точно вдоль оси Z. Как только аппарат наклонится, произойдут две неприятные вещи: проекция A на ось Z изменится и ПИД-регулятор начнет заново медленно и мучительно искать bias. И вторая — любое горизонтальное ускорение начнет давать ненулевую проекцию на локальную ось Z. При углах наклона в 45° уже и не поймешь, где какое ускорение.
Но так как мы знаем точную ориентацию локальной системы относительно глобальной, нет ничего сложного восстановить справедливость — просто спроектируем локальный вектор A на локальный вектор G (изначально найденный акселерометром и заботливо вращаемый гироскопом), который всегда смотрит в землю.
Операция эта простая и вытекает из определения векторного произведения:

7d253e5050771f54d0a73f9e5deb9d11

Сделать это надо до вычитания 1G.

Теперь можно посмотреть на код и результаты.

Данные сенсоров приходят в следующих переменных:
EstG.V – вектор G (он получен ранее при нахождении ориентации)
accADC – чистые данные с акселерометра
BaroAlt – данные с барометра, конвертированные в cm
На выходе получаем accZ, vel, alt.

Как видно, вычислительная сложность алгоритма довольно простая и Arduino его «переварит» легко (особенно если перевести в целочисленную арифметику, но тогда код станет плохо читаем).

PS: в видео есть фраза об отключении алгоритма при определенном угле наклона, и и-за этого возникает выброс ошибки. На самом деле это ограничение не нужно — алгоритм работает стабильно при любом угле от 0° до 360°

Если к этому алгоритму подключить сонар (вмето BaroAlt взять SonarAlt), то кривая высоты выглядит практически идеально. Таким образом, на низкой высоте используем сонар, при появлении ошибок или близко к пределу измерений переключаемся на барометр (предварительно согласовав высоты в период поступления достоверных данных с сонара).

К сожалению, погода пока не дает провести полетные испытания нового алгоритма. Как только появятся результаты, выложу графики сонара, отлаженный исходный код всего проекта и полетное видео.

Источник

Делаю сам
Adblock
detector