Фильтр Калмана: разбор навигационной системы БПЛА исходный код / Хабр

Фильтр Калмана: разбор навигационной системы БПЛА   исходный код / Хабр Роботы

Безопасность на квадрокоптере: калибруйте компас и придерживайте рукой

Посадка в руку - приручение

Месяц назад мы приобрели квадрокоптер DJI Phantom 3 Advanced. Аппарат доставляет массу удовольствия всей семье, особенно, когда после полёта возвращаешься домой и просматриваешь записи с высоты на большом экране. Но самые сильные эмоции мы испытали, когда дрон терял управление.

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

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

Интересную мысль высказал в своём интервью один дизайнер из нашей компании. Суть в следующем: все эти гаджеты и виджеты вокруг нас формируют отличную от натуральной действительность, по-нашему — дополненную реальность. Если взрослые люди ещё как-то осознают, где и что, то детишки приходят в этот мир, наполненный дронами, планшетами и прочей техникой, ощущая его, наверное, вполне себе целостно. Конечно, я успел помечтать о сооружении квадрокоптера своими руками. К своему творению у меня вряд ли осталось бы такое детское любопытство, какое возникает к сложной китайской поделке. Фантом от DJI возник в моей жизни весьма спонтанно, словно доселе невиданное физическое явление, которое хотелось бы не только использовать по назначению, но и познавать как бы изнутри. Следует сразу подчеркнуть, что килограммовая машина с мощными пропеллерами действительно опасна со всех сторон. Однако опасность, как известно, резко снижается в процессе освоения аппарата, особенно, освоения на грани безопасности.

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

Кто-то скажет, что цена летающего гаджета сама по себе ограничивает фантазию на эксперименты, но, даже если этот «кто-то» в большинстве, то дронов, убитых в первом же полёте, всё равно уже не вернуть. И вообще, разве какие-то там презренные деньги могут охладить пыл настоящего русского естествоиспытателя? Тем более, уже потраченные деньги.

Меня лично остановили чисто технические препятствия: батарейка в коробке была заряжена всего на 20%, и приложение DJI Go почему-то не показывало видео с камеры на моём престарелом смартфоне. Пока разбирался с видео, батарейка села ещё больше. Так что с первым полётом Фантом у меня выше полутора метров от земли не оторвался.

Дальнейшие ковыряния с видео привели к мысли обновить прошивки. Затем настала ночь, а за ней — рабочий день. Вот когда мне посчастливилось прочистить мозги от перехватывающей дыхание тяги устремиться в небо на всех парусах.

Так вот, прежде чем запускать новый квадрокоптер в опасные трёхмерные путешествия, я настоятельно рекомендую не мануалы читать, а посмотреть на Ютюбе видео падений и других неприятных ситуаций с мультикоптерами. Я-то просто собирался глянуть, как люди летают, точнее, пилотируют дронов, но глаз сам зацепился за картинки с подписями вида «phantom crash». После таких роликов инструкции уже не читаются, а как бы перечитываются в поисках важных сведений. Поэтому к вечеру я был спокоен и уравновешен. Летать, как полагается, начал в режиме новичка. А когда отключил его, поначалу значительно ограничивал высоту и дальность полёта.

Более всего впечатлили видео, показывающие, как Фантом уносится в неизведанное, подхваченный сильным ветром. Есть в них какая-то тревога и безнадёжность, которая не чувствуется в подборке забавных падений и столкновений. Успокаивает только одна мысль: «Если видео выложили, значит, дрон с бортовой флэшкой таки нашёлся». Но снова тревожит другая: «А что если нашёл его кто-то другой? А что если выжила только флэшка?» Честно говоря, мы тоже уже запускали при значительных скоростях воздушных потоков, но дрон, отметим, противостоит им весьма уверенно. Трудно представить, какие ветра имели место дуть в приведённых видео. Там, небось, оператора самого сносило, а он ещё дроном пытался управлять. Хотя, ещё раз, верховые воздушные течения могут сильно отличаться от низовых.

Просматривая видео с бьющимися и сдуваемыми квадрокоптерами, я заметил однажды простую рекомендацию: калибруйте компас перед полётом — это позволит Фантому без проблем вернуться к месту взлёта. Действительно, возврат «домой» является чуть ли не самой важной функцией летательного аппарата. Потому что при подъёме на 150 метров над землёй даже в чистом дневном небе его уже непросто разглядеть. А если не знать примерный сектор телесного угла, то и просто не разглядеть. Если же дрон взлетает на 250 метров и по горизонтали удаляется на 500, то визуально его контролировать нет никакой возможности. В незнакомом однообразном ландшафте по видео с его камеры тоже не сориентируешься. Подозреваю, на заре коптеростроения не один аппарат потерялся в небе таким образом. А нам повезло. Умные китайцы встроили такой функционал, что квадрокоптер возвращается домой не только тогда, когда его просят, но и в том случае, если теряет сигнал с пульта. Кстати, в городе потеря сигнала происходит регулярно.

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

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

Но не совсем обычно. С руки. На мой взгляд, полезно знать, что DJI Phantom 3 без проблем запускается, если держать его одной рукой за ногу. Может пригодиться при запуске на каком-нибудь поле с высокой травой. В конце видео под этой статьёй показано, как осуществлять таким образом приземление, то есть приручение. Ну и просто полезно знать тяжесть и силу квадрокоптера на всякий случай. При первом запуске с руки я просто поддал газу, чтобы вывесить дрона в воздухе и, не разжимая полностью пальцев, убедился, что его никуда не сносит.

Кроме того, после первого удара о дерево мы немедленно приобрели защитные рамки для пропеллеров. Они, конечно, делают конструкцию громоздкой и непригодной к быстрой разборке, но так спокойнее. Между прочим, на том же Ютюбе есть такие видео, где квадрокоптеры внезапно устремляются в стену или в людей без всякой видимой причины. С нами такого не случалось, но во время съёмок около высоких зданий аппарат действительно иногда непредсказуемо болтает. Трындит, как прожжённая торговка… Подозреваю, дело тут в сложных воздушных завихрениях, которые локально могут быть очень стремительны (яркий пример — торнадо). Так что пусть лучше он будет в защите.

Таким образом, перед каждым запуском мы калибруем компас. К сожалению, я так пока и не понял, как в приложении DJI Go отображается состояние неудачной калибровки. В любом случае, сначала взлетаю сантиметров на тридцать над поверхностью, проверяю управляемость и только потом отправляю гаджет в дальний полёт. Теперь, по крайней мере, я могу его запросто поймать и удержать в ситуации сноса.

В процессе изучения лётных характеристик дрона, у нас родилась идея снять небольшой фильм о жизни этих летательных аппаратов. Сказка, конечно, ложь, но получилось забавно. (Смотреть со звуком.)

Обн. Образовалось много интересных комментариев, за которые я очень благодарен. Конечно, статья и не претендовала на истину в последней инстанции, но лично на мой вкус ценности своей из-за критики не потеряла. Насчёт калибровки компаса и, вообще, аккуратного взлёта есть обширный комментарий опытного коптеровода fly4546. Мне кажется, это чересчур, но для справки интересно. На днях сам взлетал с автомобильной стоянки на крыше здания: с пола Фантом взлетать отказывался, т.к. в окружении такого количества железа компасу сносило крышу, зато с руки спокойно взлетел и вёл себя абсолютно предсказуемо. Думаю, мозги у Фантомов всё лучше, поэтому великая доля осторожности от прежних поколений теряет актуальность.

Видео

FPV (First Person View/Вид от первого лица) — термин применим к полёту от первого лица, когда беспилотник оборудован FPV камерой, а пользователь используя FPV монитор/FPV очки получает видео с этой камеры в режиме реального времени.

VR (Virtual Reality/Виртуальная реальность) очки/Glasses/Goggles — предоставляют пользователю более «захватывающий» опыт.

Подвес/Gimbal — устройства, которые сопряжены с камерой и как правило приводятся в действие с помощью серводвигателя или бесколлекторного мотора постоянного тока. Подвес — это то, что может стабилизировать камеру в полёте.

GoPro — cерия экшн камер GoPro широко использующихся для съёмки и/или передачи видео.

LCD/ЖКД (Liquid Crystal Display/Жидкокристаллический дисплей) — это тип дисплея/монитора, используемого для отображения изображения, полученного видеоприёмником.

OSD (On Screen Display/На дисплее) — отображает данные полёта на дисплее/мониторе, которые отправляются с БПЛА (например: высота, спутниковые координаты местоположения и т.д.).

Вы действительно хотите собрать бпла с нуля?

Каким должен быть беспилотник (собранным/или кастомным) зависит от того, насколько вы хотите узнать об этой области. Создание беспилотника с нуля может быть довольно сложным и даже опасным процессом. Если вы предпочитаете просто и без заморочек «подняться в воздух», в порядке возрастания сложности рекомендуется следующее:

Смотрите про коптеры:  Выбираем маленький квадрокоптер с FPV для гонок по квартире

Игрушка/toy

  • Мультироторные игрушки становятся все более популярными. Большинство из них маленькие и могут уместиться на ладони, но встречаются и крупные, такие, как А.R. Drone Parrot. Игрушечными многомоторными беспилотниками не всегда легко управлять, но они более устойчивы к авариям/крашам. Игрушки, как правило, меньше по размеру, а роль рамы выполняет эстетическая оболочка.

Rtf

  • В комплект «Ready To Fly/Готов к полёту» входят все детали, необходимые для комплектного БПЛА. Сюда входит сам БПЛА (чаще всего предварительно собранный и готовый к работе), аппаратура управления, аккумулятор и зарядное устройство. Аппарат откалиброван и должен летать с относительной легкостью. Тем не менее они имеют ограниченный предел прочности, и очередная авария может повредить систему настолько, что будет лучшим, просто купить новый беспилотник, а не пытаться его отремонтировать.

Arf

  • Комплект «Almost Ready to Fly/Почти готов к полёту» это комплект, в который входят полностью собранные: рама, моторы и большинство «основных» частей (в некоторых случаях может потребоваться сборка нескольких частей/деталей, в основном с целью облегчения доставки). Обычно ARF комплектация требует дооснащения передатчиком/приёмником, реже, батареей и зарядным устройством. Некоторые ARF комплекты не включают в себя сам контроллер полёта. Возможно, пользователю потребуется выполнить некоторую калибровку из-за необходимости дооснащения дополнительными деталями. Комплект BNF не предлагается, так как не все передатчики и приёмники совместимы друг с другом.

Бпла набор/uav kit

  • Набор обычно включает в себя большинство важных деталей, необходимых для создания БПЛА, но при этом могут отсутствовать контроллер полёта, передатчик/приёмник, аккумулятор и зарядное устройство. Разные комплекты имеют разное содержимое, поэтому важно точно знать, что входит в комплект и какие дополнительные компоненты/детали понадобятся. Содержимое комплекта должно быть совместимо друг с другом.

Сборка с нуля/custom

  • Кастомная сборка это то, где пользователь комбинирует различные изделия от разных производителей и заставляет их работать вместе. Такой подход требует понимания, какие компоненты будут необходимы для создания беспилотного летательного аппарата, и будут в центре внимания в этой серии статей.

Вы заметили недостающие термины, которые будут полезны? Не стесняйтесь добавлять их в комментариях ниже.

Выбор полетного контроллера

Так как мне нужно транслировать видео и телеметрию через интернет, то сразу приходит на ум поставить на дрон микрокомпьютер с 4G модемом и камерой, и сделать из этого комплекта web-трансляцию. Нашлись вот такие решения:

. Это обычные одноплатники с внешним USB 4G модемом и камерой. Для кодирования и трансляции видео используется

. Но эти штуки сами по себе дроном управлять не умеют, их нужно использовать совместно с полетным контроллером.

Полетный контроллер — это мозг дрона. Он следит за состоянием датчиков положения (гироскоп, акселерометр, компас), GPS-координатами, положением ручек на пульте управления и, исходя из этих данных, управляет моторами, чтобы висеть в одной точке или куда-то лететь.

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

С Ardupilot я игрался еще на 8-битных атмегах, в которых не было USB-bootloader’а, а прошивались они на программаторе. С тех пор с ним не сталкивался и был приятно удивлен, когда узнал, что сейчас он может работать на 64-битных компьютерах с Linux, у него огромное сообщество пользователей как хобби, так и профи, длинный список поддерживаемых “из коробки” датчиков и расписанные планы на 2022-2022 годы. За это время он успел перерасти в проект DroneCode, а потом и отсоединиться от него.

На первый взгляд в нем как раз реализованы все необходимые функции: автоматический взлет и посадка, загрузка полетных заданий, есть desktop и мобильные приложения под все основные семейства ОС. Программы управления (GCS — Ground Control Station) общаются с бортовым контроллером короткими сообщениями по открытому протоколу MAVLink через комплект радиомодемов (дрон шлет телеметрию, GCS шлет команды управления). Подозреваю, что эти сообщения получится пустить через интернет.

Взглянем на список поддерживаемых контроллеров и что-нибудь подберем. Вариантов там полтора десятка от мала до велика и с разными характеристиками.

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

Самый легкий комплект (84 г) получается из микрокомпьютера

(9 г), контроллера

(15 г), родного внешнего GNSS модуля (46 г) и дополнительного

(14 г).

Фильтр Калмана: разбор навигационной системы БПЛА   исходный код / Хабр

Рабочий процессор в этом комплекте один — на Raspberry Pi. На нем висит управление ШИМ регуляторов моторов, считывание показаний датчиков, ОС Linux со всеми потрохами и декодирование видео с камеры. Так как в Pi Zero не предусмотрены USB порты, то в этом варианте приходится использовать внешний концентратор. IMU датчики и вход питания без резервирования.

Следующий комплект (97 г) от гонконгской компании с русскими фамилиями в команде разработчиков — контроллер Emlid Edge (59 г) с GNSS модулем (38 г). GNSS модуль работает по протоколу UAVCAN и дополнительно оснащен магнитометром и датчиком воздушного давления. За ШИМ здесь отвечает отдельный процессор ARM Cortex-M3, ОС Linux крутится на основном ARM Cortex-A53 quad-core.

В контроллере имеется HDMI видеовход, что позволяет подключить к нему напрямую любую камеру с таким выходом, например GoPro 4 или 5. Относительно высокая стоимость объясняется дальнобойными wifi-приемопередатчиками в комплекте (до 2 км с трансляцией HD-видео).

Следующий вариант (98 г) состоит из знаменитого микрокомпьютера Raspberry Pi 3 (45 г) с контроллером-шилдом Navio 2 (23 г) от той же Emlid и внешней GNSS-антенны (30 г). На контроллере стоит отдельный процессор Cortex-M3 для управления ШИМ на 14 каналах и расшифровки входящих SBUS и PPM сигналов от приемника. Он, в свою очередь, управляется через драйвер в ядре ОС Linux, которая крутится на Raspberry.

Контроллер оснащен парой раздельных IMU датчиков (акселерометр, гироскоп, магнитометр) MPU9250 и LSM9DS1, одним датчиком воздушного давления и GNSS-модулем U-blox NEO-M8N, который видит GPS, Глонасс и BeiDou с внешней антенной через разъем MCX.

Интегрированные навигационные системы (ins)

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

INS потенциально имеют более высокую точность чем просто инерциальные навигационные системы или чисто спутниковые навигаторы, по этой причине они могут использоваться в качестве бортовых навигационных систем на БПЛА большой дальности (с радиусом полёта свыше 1000км).

  1. Одноантенный или двухантенный спутниковый приёмник (трёхантенный в данного рода системах используется редко). От этого зависит количество определяемых углов ориентирования.
  2. Количество доступных спутниковых каналов. Каждый канал отвечает за получение информации от определённой спутниковой группировки, которых на данный момент насчитывается несколько:
  • GPS (США) – наиболее старая и самая распространённая спутниковая система, характеризуемая широкой зоной охвата;
  • Galileo (ЕС) – в отличие от других спутниковых систем, она изначально имеет в основном гражданское применение, т.е. более доступна, но не отличается большой надёжностью, поскольку до сих пор находится в стадии формирования;
  • QZSS (Япония) – высокоточная (квазизенитная) спутниковая система, но зона охвата у неё пока что оставляет желать лучшего;
  • ГЛОНАСС (Россия) – наиболее стабильно работающая в нашей стране спутниковая система навигации;
  • BEIDOU или BD(Китай) – самая перспективная спутниковая система, судя по темпам развития орбитальной группировки;
  • NavIc (Индия) – наиболее «молодая» и наименее востребованная спутниковая система, в связи с тем, что она надёжно работает только на п-ве Индостан и окружающих его акваториях.
  1. Как известно, станция ГНСС также может быть одночастотной или работать сразу на нескольких частотах. На БПЛА применяются в основном одночастотные спутниковые приёмники, поскольку их показания точнее в режиме RTK (режим реального времени).
  2. Желательно также, чтобы ГНСС, установленная на БПЛА, могла получать псевдо-диапазонные спутниковые сигналы, т.е. «сырые» данные со спутника, а не переданные наземными ретрансляторами. Это значительно повышает надёжность навигационной системы в целом. 

В любом случае, сотрудники компании «Лазерные Компоненты» всегда готовы помочь Вам при выборе подходящего навигационного прибора.

Компас, встроенный в полётный контроллер apm 2.8

Этот внутренний компас оказался самым простым в подключении и настройке!  Для его работы необходимо наличие 3-ей перемычки на контроллере (возле гнезда с надписью GPS), и чтобы провод компаса, идущий от внешнего компаса к контроллеру, был отключен!

Настройка и калибровка этого компаса (при помощи программы Mission Planner) также не вызвала никаких проблем, все шаги в точности соответствовали многочисленным видеоинструкциям на ютубе: Выбрал тип компаса — встроенный (при этом исчезла настройка поворота компаса относительно полётного контроллера)

И как завершающая проверка — квадрокоптер, поставленый на подоконник со временем поймавший все спутники, показывал стороны горизонта правильно! То есть, если на подоконнике поставить летательный аппарат носом на север, то и на карте в ПО Mission Planner он тоже показывает на север!

Главный недостаток использования такого компаса — это влияние магнитных полей бортовой электроники на показания компаса! В первую очередь это силовые провода, идущие от аккумулятора к регуляторам оборотов, и далее — к двигателям. Среди создателей квадрокоптеров общепринятым считается использовать внешний компас (встроенный в модуль GPS) и устанавливать его на отдельной стойке, длиной 13-15 сантиметров, как раз с целью избежать таких вот магнитных помех. Ну или как шутят эти же создатели квадрокоптеров — «Чтобы приёмник GPS был поближе к спутникам».

Итак, для решения этой проблемы я соорудил жуткую конструкцию

из г0вна и палок

из детских кубиков и кабельных стяжек, которая позволила мне поднять Полётный контроллер APM 2.8 (а следовательно и встроенный в него компас) выше основной поверхности квадрокоптера на высоту около 10 сантиметров. Проблема влияния магнитных полей была решена: Процедура Compassmot показала влияние магнитных полей бортовой электроники менее 5% из допустимых 25%.

Для справки: Compassmot это процедура измерения интерференции (влияния) магнитных полей бортовой электроники на показания компаса. Для её выполнения пропеллеры квадрокоптера переворачиваются вверх ногами и сдвигаются по кругу на один двигатель, чтобы при вращении пропеллеров квадрокоптер не пытался взлететь, а наборот, только сильнее прижимался к земле!

В результате я отлетал на такой конфигурации свои первые полгода в роли владельца беспилотника! Компас в комплексе с GPS работал хорошо, Loiter успешно заставлял квадрокоптер висеть в точке, миссия отрабатывала, полёт по точкам просходил удачно, но этажерка из детских кубиков немного портила внешний вид.

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

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

Механика

CG (Center of Gravity/Центр тяжести) — это точка на воздушном судне, где вес распределён одинаково по всем сторонам.

Рама (Frame) — рама это своего рода «скелет» воздушного судна, на котором крепятся все детали. Простые рамы имеют моторы, подключенные к алюминиевым или другим лёгким профилям («лучам»), которые затем соединяются с центральным корпусом.

Смотрите про коптеры:  Выбираем маленький квадрокоптер с FPV для гонок по квартире

Оболочка/Корпус (Shell) — эстетическое/функциональное покрытие, используемое для улучшения сопротивления элементам, а иногда и для улучшения аэродинамики. Некоторые серийно выпускаемые БПЛА имеют только пластиковую оболочку, которая также выступает в качестве «рамы».

Посадочные опоры (Landing gear) — как правило, многомоторный дрон в отличии от самолёта не имеет колёс, их роль выполняют посадочные опоры. Опоры предотвращают перемещение по земле и снижают общий вес.

Retract/Retractable — обычно относится к шасси, которое имеет два положения: одно для посадки и взлёта, а другое, которое занимает меньше места или улучшает обзор во время полёта.

G10 — этот материал обычно используют вместо углеродного волокна для изготовления каркаса БПЛА, поскольку он не только очень жёсткий и лёгкий, но значительно дешевый.

Защита пропеллеров (Propeller guards/Prop Guards) — это материал, который окружает пропеллер, чтобы препятствовать контакту несущего винта с другими объектами. Защита реализована как средство обеспечения безопасности и способ минимизировать повреждение беспилотника.

Разъёмы/Коннекторы (Connectors) — для соединения и разъединения проводов на их концах применяют разъёмы. Распространённые разъёмы для батарей – «Deans» и «XT60», в то время как разъёмы для контроллера полёта и датчиков расположены с интервалом 0.1 дюйма (2.5мм).

Хомут (Clamp) — «Tube clamp/Трубный хомут» это устройство, обычно используемое на круглой трубе для соединения её с другим устройством (например, с креплением мотора или корпусом БПЛА).

Демпферы (Dampeners) — формованные резиновые детали, используемые для минимизации вибрации, передаваемой по беспилотнику.

Светодиоды (LED — Light Emitting Diode) — применяются с целью обозначить БПЛА в полёте, особенно ночью или в условиях слабого освещения.

Силовая установка

Тяга/Thrust — это сила, которую может обеспечить конкретный мотор и пропеллер (при определенном напряжении). Обычно измеряется в килограммах (кг/kg) или фунтах (Фунтах/Pounds/Lbs).

BEC — (Battery Eliminator Circuit/Преобразователь бортового питания) встроен в ESC (регулятор напряжения), который может обеспечить регулируемое 5В напряжение постоянного тока для любой электроники, которая в этом нуждается.

ESC (Electronic Speed Controller/Электронный регулятор скорости) — устройство, которое подключается к аккумулятору, мотору и контроллеру полёта и контролирует скорость вращения мотора.

Мотор/Motor — то, что применяется для вращения пропеллеров; в небольших беспилотных летательных аппаратах чаще всего используется коллекторный (Brushed) мотор, тогда как для более крупных беспилотных летательных аппаратов – бесколлекторный (Brushless).

Лопасти/Blades (лопасти пропеллера) — аэродинамическая поверхность создающая подъёмную силу. Как правило пропеллер имеет от двух до четырёх лопастей, которые могут быть как фиксированными, так и складными.

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

Адаптер пропеллера/Prop Adapter — устройство, используемое для сопряжения пропеллера с мотором.

Пропсейвер/Prop Saver — тип втулки, которая устанавливается на верхней части используемого мотора и заменяет адаптер пропеллера/Prop Adapter. В случае аварии часть пропсейвера идёт в расход, чтобы сохранить целостность пропеллера.

PCB (Printed Circuit Board/Печатная плата) — стеклопластиковая пластина, на которой припаяно множество компонентов. Многие электронные продукты имеют печатную плату.

LiPo (Lithium Polymer/Литий-Полимерный) — является наиболее распространенной батареей, используемой в дронах и беспилотных летательных аппаратах из-за её малого веса (относительно ёмкости хранения), а также высокой скорости разряда тока. Помимо LiPo на рынке доступны и другие типы литиевых батарей (LiFe, LiMn, LiOn и т.д.).

Распределение питания/Power Distribution — чтобы питать различное множество устройств, используемых в БПЛА, ресурсы батареи должны быть распределены, и именно здесь в игру вступает распределение питания (плата или кабель). Задействует одну положительную и отрицательную клеммы батареи и предоставляет множество различных клемм/точек подключения, от которых другие устройства (работающие на том же напряжении) могут получать питание.

Сервопривод/Servo — тип привода, который при правильном сигнале может перемещаться в определенное угловое положение.

Фильтр калмана: разбор навигационной системы бпла исходный код

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

Всем привет! Меня зовут Илья, я работаю программистом-инженером в компании “SID Logic”, мы занимаемся разработкой электроники и сопутствующего ПО для различных применений, мои области интересов касаются систем навигации (оптические, инерциальные, ГНСС, UWB, GSM/LTE), систем управления (ПИД, MRAC, MPC) преимущественно для задач робототехники, беспилотников. Одну из важных тем в навигации занимает тема Фильтра Калмана.

На Хабре фильтру Калману (ФК) посвящено уже много статей, но я пока не встречал объяснения (возможно, плохо смотрел?:))принципиального преимущества его использования перед, например, альфа-бета, различными комбинациями фильтров низкой частоты (ФНЧ), фильтров высокой частоты (ФВЧ), интеграторами, дифференциаторами и т.д… Однако, даже если вы знали, то о чем я буду дальше говорить, возможно, вам будет полезен код, которым я пользуюсь уже несколько лет, на основе которого было проверено множество гипотез, подтверждено их применение.

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

Для начала нужно рассмотреть такую формулировку задачи. Абстрактной задачи.

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

Фильтр Калмана: разбор навигационной системы БПЛА   исходный код / Хабр

Или другой пример, более реальной задачи – у вас есть 2 фотографии, сделанные под разным углом (части панорамы), вы открываете фотошоп и пытаетесь сместить второе фото так, чтобы с виду обе фотографии не имели разрыва между собой, иначе говоря пытаетесь сшить панораму.

Сшивка панорамы
Сшивка панорамы

В обоих примерах вы можете регулировать переменные (крутилки и смещения фото) и смотреть, как они влияют на результат, и вы можете оценить, как далеко вы от цели или близко (близка ли установленная температура к желаемой, сильно ли видно границу стыка двух фотографий). То есть у вас есть функция прямой зависимости (температура = f(крутилки) и визуальная сходимость стыка = f(смещение 2 фото)), но нет обратной зависимости.

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

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

Итак, фильтр Калмана является частным упрощенным случаем большого семейства алгоритмов поиска оптимальных параметров и состояния системы на основе математической модели.

ФК помогает решать задачи:

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

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

Или проще говоря, ФК подбирает такие параметры, чтобы на выходе получить тоже самое, что показывают датчики, при этом не противореча предыдущим состояниям.

Будем работать с более применительным к реальности видом ФК – то есть нелинейным, так как большинство систем имеют нелинейные взаимосвязи.

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

Все нелинейные виды ФК так или иначе используют линеаризацию. Распространены 2 вида линеаризации ФК:

EKF и UKF линеаризации
EKF и UKF линеаризации

В инженерном контексте для нас можно выделить следующие особенности:

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

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

Согласно целям статьи, понизить порог вхождения в тему, дать универсальный алгоритм для быстрого прототипирования и проверки гипотез в условиях слабой вычислительной базы и малого кол-ва памяти лучшим выбором будет являться второй вид ФК – UKF.

Помимо UKF, существуют следующие вариации сигма-точечного фильтра Калмана: CDKF (central difference Kalman filter – фильтр Калмана центральной разности), SRUKF (square-root unscented Kalman filter – ансцентный фильтр Калмана квадратного корня), SRCDKF (square-root central difference Kalman filter – фильтр Калмана квадратного корня центральной разности) и их итеративные модификации IUKF, ICDKF, ISRUKF, ISRCDKF, которые рассматриваются в работах [1, 2].

В инженерном контексте задачи оптимальным выбором я считаю т.н. Square Root Central Difference Kalman Filter (SRCDKF). Выбор сигма точек осуществляется согласно центральной разности (в отрицательное направление от линеаризуемой точки и в положительное направление) пропорционально матрице квадратного корня ковариации. При этом не используется разложение Холецкого для матрицы ковариации, которое может быть не стабильным в условиях с присутствием одновременно малых и больших значений в матрице.  

Я решил не загромождать статью лишними формулами, кому будет интересно может почитать литературу в списке.

Это универсальное ядро SRCDKF, которое можно использовать в разных приложениях, реализуя разные математические модели.

Исходный код ядра можно скачать:

Ядро SRCDKF

С Теорией пока всё. Теперь практика.

Пример, иллюстрирующий суть ФК

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

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

Фильтр Калмана: разбор навигационной системы БПЛА   исходный код / Хабр

Нам для управления квадрокоптером нужно знать положение и ориентацию в пространстве в каждый момент времени.

Раз уж я сказал, что задача из реального мира, а реальный мир трехмерный, то мы будем рассматривать математическую модель описания вращения и положения в трехмерном пространстве. Для описания вращения в трехмерном пространстве воспользуемся кватернионом, можно сказать стандартом для таких случаев. А для описания положения – обычным 3-х мерным вектором.

Смотрите про коптеры:  Как установить GPS в квадрокоптер, а также как настроить - Все о квадрокоптерах | PROFPV.RU

Q=[q_w q_x q_y q_z]^T\ \ P=[p_x p_y p_z]^T

Кто работал с дешевыми MEMS IMU знает о необходимости калибровки для нивелирования масштабных искажений, межосевых взаимосвязей, а также смещений (biases).

Так выглядят данные с акселерометра, при вращении по всем осям. Центр эллипсоида смещен на некоторый вектор (bias). Сам эллипсоид масштабно искажен.
Так выглядят данные с акселерометра, при вращении по всем осям. Центр эллипсоида смещен на некоторый вектор (bias). Сам эллипсоид масштабно искажен.

Упрощенная, но достаточная для нашей точности математическая модель (без масштабных и межосевых коэффициентов) акселерометра и ДУСа одинаковая и имеет вид:

A_{raw}=A_{true} A_{bias}\W_{raw}=W_{true} W_{bias}

Где A_{raw}  W_{raw}– 3-х мерный вектор данных непосредственно с сенсора (ускорения и угловой скорости соответственно), A_{true}  W_{true} – истинный 3-х мерный вектор данных, A_{bias}  W_{bias}– 3-х мерный вектор смещения (bias).

Модель IMU у нас есть.

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

Для более-менее достаточной для нашей точности математической модели нужно 3 параметра для описания смещения (bias) акселерометра (в СК IMU), 3 параметра для описания смещения (bias) датчика угловой скорости (в СК IMU), 3 параметра описания положения (3-х мерный вектор в глобальной СК), 4 параметра для описания кватерниона ориентации ( кватернион, выражающий вектор из СК коптера в глобальную СК), дополнительно включим 3-х мерный вектор скорости (в глобальной СК). Итого 16 параметров. Это называется вектор состояния системы, так как он описывает полное состояние (в контексте нашей задачи) нашего квадрокоптера в конкретный момент времени, то есть какое он имеет положение, ориентацию, скорость и т.д.

X=[A_{bias}^T  W_{bias}^T  P^T  Q^T  V^T]^TA_{bias}=[a_{bx}  a_{by}  a_{bz}]^T\W_{bias}=[w_{bx}  w_{by}  w_{bz}]^T\Q=[q_w q_x q_y q_z]^T\ \ P=[p_x p_y p_z]^T\V=[v_x  v_y  v_z]^TФильтр Калмана: разбор навигационной системы БПЛА   исходный код / Хабр

Эволюция системы описывается тем, как будет меняться вектор состояния во времени, то есть зависимость нынешнего момента времени от прошлого, еще этот этап называется прогнозом. (t) – текущий момент времени, (t-1) – прошлый момент времени.

P(t)=P(t-1) V(t-1)dt

Здесь просто интегрируется по времени положение по скорости dt– шаг дискретизации.

A_{temp}=Q(t-1)*(A_{raw}-A_{bias}(t-1))\V(t)=V(t-1) A_{temp}dt S_{v_{vel}}

A_{temp}– временный вектор ускорений, состоящий из A_{raw}– вектора непосредственно из акселерометра A_{bias}(t-1)– смещение акселерометра в прошлый прогноз, * – операция вращения вектора кватернионом, в данном случае выражает локальный вектора ускорения в глобальную СК. Q(t-1) – кватернион в прошлый прогноз.

S_{v_{vel}} – вектор стандартных отклонений шумов прогноза для линейной скорости.

W_{temp}=W_{raw}-W_{bias}(t-1) S_{w_{rate}}dt\Q(t)=Q(t-1)otimes textrm{exp}(W_{temp}dt)

W_{temp}– вектор угловой скорости, состоящий из W_{raw}– вектора данных непосредственно с ДУСа, W_{bias}(t-1)– рассчитанный на предыдущем шаге вектор смещения, S_{w_{bias}}– вектор стандартных отклонений шумов прогноза для угловой скорости.

otimes– символ, обозначающий перемножение кватернионов, exp()– это функция перевода 3-х мерного вектора вращения, находящегося в касательной плоскости относительно сферы кватерниона в пространство кватерниона, подробнее распишу попозже.

A_{bias}(t) = A_{bias}(t-1) S_{a_{bias}}dt\W_{bias}(t) = W_{bias}(t-1) S_{w_{bias}}dt

Смещения IMU не зависят от других переменных в векторе состояния, поэтому в нынешнем моменте времени они почти равны прошлым значениям. Почти, потому что все-таки они немного меняются со временем (температура может меняться), нужно чтобы ФК понимал, что это не константы и имел некоторую неопределенность в значении, для этого есть два параметра S_{a_{bias}}  S_{g_{bias}} – которые являются векторами стандартных отклонений шумов прогноза для смещений IMU.

Код этапа прогноза
void TimeUpdate(float *in, float *noise, float *out, float *u, float dt, int n) {
	float tmp[3], acc[3];
	float rate[3];
	float mat3x3[3 * 3];
	float q[4];
	int i;
	float qRate[4], qRes[4];

	// assume out == in
	out = in;

	for (i = 0; i < n; i  ) 
	{
		// create rot matrix from current quat
		q[0] = in[UKF_STATE_Q1*n   i];
		q[1] = in[UKF_STATE_Q2*n   i];
		q[2] = in[UKF_STATE_Q3*n   i];
		q[3] = in[UKF_STATE_Q4*n   i];
		quatToMatrix(mat3x3, q, 1);

		// pos
		out[UKF_STATE_POSX*n   i] = in[UKF_STATE_POSX*n   i]   in[UKF_STATE_VELX*n   i] * dt;
		out[UKF_STATE_POSY*n   i] = in[UKF_STATE_POSY*n   i]   in[UKF_STATE_VELY*n   i] * dt;
		out[UKF_STATE_POSZ*n   i] = in[UKF_STATE_POSZ*n   i]   in[UKF_STATE_VELZ*n   i] * dt;

		// acc
		tmp[0] = u[0]   in[UKF_STATE_ACC_BIAS_X*n   i];
		tmp[1] = u[1]   in[UKF_STATE_ACC_BIAS_Y*n   i];
		tmp[2] = u[2]   in[UKF_STATE_ACC_BIAS_Z*n   i];

		// rotate acc to world frame
		rotateVecByMatrix(acc, tmp, mat3x3);
		acc[1] -= GRAVITY;


		out[UKF_STATE_VELX*n   i] = in[UKF_STATE_VELX*n   i]   acc[0] * dt   noise[UKF_V_NOISE_VEL_X*n   i];
		out[UKF_STATE_VELY*n   i] = in[UKF_STATE_VELY*n   i]   acc[1] * dt   noise[UKF_V_NOISE_VEL_Y*n   i];
		out[UKF_STATE_VELZ*n   i] = in[UKF_STATE_VELZ*n   i]   acc[2] * dt   noise[UKF_V_NOISE_VEL_Z*n   i];


		// rate = rate   bias   noise
		rate[0] = (u[3]   in[UKF_STATE_GYO_BIAS_X*n   i]   noise[UKF_V_NOISE_RATE_X*n   i]) * dt;
		rate[1] = (u[4]   in[UKF_STATE_GYO_BIAS_Y*n   i]   noise[UKF_V_NOISE_RATE_Y*n   i]) * dt;
		rate[2] = (u[5]   in[UKF_STATE_GYO_BIAS_Z*n   i]   noise[UKF_V_NOISE_RATE_Z*n   i]) * dt;

		// rotate quat
		exp_mapQuat(rate, qRate);
		quatMultiply(qRes, q, qRate);

		out[UKF_STATE_Q1*n   i] = qRes[0];
		out[UKF_STATE_Q2*n   i] = qRes[1];
		out[UKF_STATE_Q3*n   i] = qRes[2];
		out[UKF_STATE_Q4*n   i] = qRes[3];

		// acc bias
		out[UKF_STATE_ACC_BIAS_X*n   i] = in[UKF_STATE_ACC_BIAS_X*n   i]   noise[UKF_V_NOISE_ACC_BIAS_X*n   i] * dt;
		out[UKF_STATE_ACC_BIAS_Y*n   i] = in[UKF_STATE_ACC_BIAS_Y*n   i]   noise[UKF_V_NOISE_ACC_BIAS_Y*n   i] * dt;
		out[UKF_STATE_ACC_BIAS_Z*n   i] = in[UKF_STATE_ACC_BIAS_Z*n   i]   noise[UKF_V_NOISE_ACC_BIAS_Z*n   i] * dt;

		// gbias
		out[UKF_STATE_GYO_BIAS_X*n   i] = in[UKF_STATE_GYO_BIAS_X*n   i]   noise[UKF_V_NOISE_GYO_BIAS_X*n   i] * dt;
		out[UKF_STATE_GYO_BIAS_Y*n   i] = in[UKF_STATE_GYO_BIAS_Y*n   i]   noise[UKF_V_NOISE_GYO_BIAS_Y*n   i] * dt;
		out[UKF_STATE_GYO_BIAS_Z*n   i] = in[UKF_STATE_GYO_BIAS_Z*n   i]   noise[UKF_V_NOISE_GYO_BIAS_Z*n   i] * dt;
	}
}

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

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

По поводу textrm{exp()}. Кватернион – это гиперпараметризированная модель вращения, так как имеет 4 параметра в пространстве вращения с 3 степенями свободы. Так как полноценно описать вращение 3 параметрами не получается (Gimbal lock), приходится прибегать к таким математическим конструкциям, как кватернионы. Для перехода из 4-х мерного пространства в 3-х мерное пространство и обратно есть специальные операции textrm{exp()}  textrm{log()}. Это можно представить как сферу (кватернион) и касательную плоскость (3-х мерный вектор вращения).

Фильтр Калмана: разбор навигационной системы БПЛА   исходный код / Хабр

Отсюда же проистекает проблема, которую я описывал выше (ФК не умеет работать с параметрами, у которых есть ограничения), у кватерниона должна быть единичная длина. В таком случае можно пойти на небольшую хитрость и после каждой итерации прогноза нормализовывать кватернион. Для нашей мат. модели такой способ сгодится, однако для более нелинейных систем, придется использовать специальную арифметику и операции с кватернионами внутри ФК. Подробнее про касательные пространства и фильтрацию с кватернионами в [3, 4].

В нашем случае есть только 2 датчика IMU и GNSS. IMU – используется непосредственно в модели прогноза как априорные значения и не моделируются.

GNSS моделируется так:

P_{gnss}=Q*P_{loc} P S_{p}

Q – кватернион из вектора состояния, P_{loc}– вектор из IMU в GNSS в СК IMU, P– вектор позиции из вектора состояния, S_p– вектор стандартного отклонения шума GNSS.

Чем выше S_p тем меньше доверие GNSS, данную величину можно варьировать в зависимости от качества данных с датчика.

Код, модели GNSS

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

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

Итак, для демонстрации данного ФК был написана демка на Unity.

Кратко расскажу подробности симулятора:

Сам ФК в Unity используется как динамическая библиотека, для WebGL – сырые исходники. Коптер не управляется по физическим законам, используются просто силы и моменты для имитации полета коптера в скрипте Control.cs. Для имитации IMU и GNSS созданы 2 скрипта IMUModel.cs и GNSSModel.cs. Обертка и взаимодействие с Unity ФК реализована в KalmanFilter.cs. Для графиков использовался открытый Asset.

Для физики используется внутренний движок PhysX. Для демонстрации 60 Гц – достаточно.

Вот видео демки и графики вектора состояния.

Вектор смещений ДУСа
Вектор смещений ДУСа
Вектор смещений акселерометра
Вектор смещений акселерометра
График углов Эйлера
График углов Эйлера
Вектор положения
Вектор положения
Вектор скоростей
Вектор скоростей

Пояснение по графикам: Estimate – это оценка ФК, Reference – это фактическая величина, Uncertainty – дисперсия из матрицы ковариации *100.

В центре графиков видно, что ошибка возросла, это произошло из-за уменьшения влияния GNSS данных (на видео видно, где я увеличил дисперсию GNSS corr).

Демку, что на видео можно скачать из репа:

Исходный код демки

А также можно поиграть интерактивно.

Можно заметить, что не использовался магнитометр для коррекции курса. Если применять обычный фильтр Magwick или Mahony тангаж и крен более менее наблюдаемы, когда нет линейных ускорений, то угол курса будет плыть и взять его неоткуда. А если постоянно присутствуют линейные, центростремительные ускорения, то все углы уплывут при коррекции акселерометром, это актуально для самолетов, например.

В случае применения ФК и данной мат. модели, такого не будет: при наличии движения с помощью датчика GNSS будет происходить разделение сырого вектора ускорения из акселерометра на линейную часть и на вектор ускорения свободного падения, из-за чего углы тангажа и крена будут максимально точными, а угол курса не будет уплывать. К тому же мы получаем еще и более точные векторы положения и скорости. Данную особенность приметил не только я:

В этом и заключается преимущество ФК – нахождении неявных полезных обратных зависимостей между вектором состояния и датчиками.

Вот график и видео иллюстрирующие коррекцию курсового угла, даже при начальном неправильном значении в 45 градусов.

Угол курса с начальной ошибкой в 45 градусов
Угол курса с начальной ошибкой в 45 градусов

В интерактивной демке, много чего нет, с чем можно поиграть непосредственно в Unity Edior, а именно: различные значения bias IMU, их изменения во времени, различные величины стандартных отклонений шумов прогноза и датчика GNSS, задержки между датчиками, неверные начальные условия и т.д. Так что советую скачать исходник и непосредственно в Unity Editor поиграть с различными условиями.

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

Вот например коптер, который я собрал, который летает почти с таким же ФК:

Суть ФК можно расширить на оптимизацию не только текущего вектора состояния в конкретный момент времени, но и оптимизацию, например, целого набора состояний, включенных в разные моменты времени (Sliding-Window Optimization, bundle adjustment/graph optimization). Такой подход требует гораздо большей вычислительной мощности и реализуется другими методами, однако гораздо более точен и стабилен.

За рамками статьи остались такие темы, как:

Если вам будет интересно можно продолжить цикл статей по практическому применению ФК.

Список литературы:

  1. Van Der Merwe, R. Sigma-point Kalman filters for probabilistic inference in dynamic state-space models

  2. Chaabane, M. Nonlinear State and Parameter Estimation Using Iterated Sigma Point Kalman Filter: Comparative Studies

  3. Christoph Hertzberga, Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds

  4. Joan Sol`, Quaternion kinematics for the error-state Kalman filter

Оцените статью
Радиокоптер.ру
Добавить комментарий

Adblock
detector