Создание собственной системы стабилизации
Я про то, что шина не так важна для выборки датчиков, и то что i2c куда-то там не успеет…
Ещё один “секрет” простая аналоговая серва на ВИШ на 50гц. Сделает лучше и быстрее, то что 500гц. Регулятор насилующий мотор и батарею
Сами-то подумайте соотношение шаг винта/скорость вращения/ потребляемая мощность/тяга…
шина не так важна для выборки датчиков
Говорим, по моему, о минимальном времени цикла: чтение датчиков /обсчет данных /выдача управляющего сигнала (?)
В этой цепи шина имеет немалое значение всетаки SPI на порядок шустрее i2c…
А то что эффективней управление шагом или оборотами, это уже другой вопрос. (конечно серва+механика еффективней)
Привет всем, написал алгоритм калибровки компаса. Работает в реал-тайме, не требует сбора полной сферы магнитных векторов, дополнительно выдает магнитное склонение и наклонение в данной местности. Думается, что DJI используют аналогичный алгоритм, так как, я попробовал провести такие же манипуляции, и компас откалибровался.
Замечания: результатом алгоритма является только 3 коэффициента смещения, коэффициенты масштаба и взаимовлияния осей отсутствуют, но я думаю что этого достаточно, чтобы определить направление на север с точностью в несколько градусов.
В качестве датчиков: акселерометр, ДУС, магнитометр. Алгоритм основан угадайте на чем? правильно моем любимом UKF=) в принципе всего 6 перемнных сосотяния, не жрущий. полный цикл прогноза занимает 208 мкс, а цикл прогноза и коррекции вместе 500 мкс на STM32F407 с включенным FPU.
Видео:
Давно не интересовался как люди калибруют компас. Может я отстал от жизни и это прошлый век??
Может я отстал от жизни и это прошлый век??
По мне, так это скорей день завтрашний ))) А что дает такой подход ?
У меня примитивно находится пресловутая сфера при нескольких вращениях в “реальном времени” (фактически поиск максимумов и минимумов по осям, с последующим смещением) - в результате те же ~ 2-5 градусов точности обеспечены…
К тому же не понимаю: зачем для компаса нужен расчет магнитного склонения (?) лететь точно на северный полюс ?
как я понимаю для возврата домой можно обойтись и без “честного” севера.
А что дает такой подход ?
Ну примерно экономит время на калибровке компаса, провернул пару раз по осям, как у Dji - все готово.
(фактически поиск максимумов и минимумов по осям, с последующим смещением)
Чтобы попасть в максимумы и минимумы магнитной сферы - это надо знать как расположен магнитный вектор в данной местности, либо собрать кучу точек в предполагеамых областях максимумов/минимумов (а их целых 6 штук), отсеять выбросы, я бы сказал, что это сложнее, чем собрать полную сферу, хотя честно я не пробовал и могу ошибаться.
К тому же не понимаю: зачем для компаса нужен расчет магнитного склонения (?) лететь точно на северный полюс ?
Ну а как комплексировать данные с GPS? Тут в любом случае надо выставить виртуальные оси с реальными осями. Во многих случаев можно пренебречь магнитным склонением, так как оно 0-15 градусов и система использующая магнитометр и GPS будет на грани, но есть места на планете, где магнитное склонение достигает 120 градусов и 50, как можно это не учитывать? посмотрите карту магнитного склонения.
Да и написал я этот алгоритм как проверку концепции, не было конкретной цели.
Ну а как комплексировать данные с GPS?
Да, согласен, у меня просто все примитивно (хотя можно и так))…
алгоритм как проверку концепции
Интересная концепция. У меня проще сделано - при калибровке компаса выбирается минимум 6 более-менее ортогональных отсчетов и по ним аппроксимируется эллипсоид. Центр эллипсоида - есть магнитные смещения. Но самокалибровка в реальном времени - это же еще интереснее.
если алгоритм работает, в условиях посторонних магнитных полей и без них показывает с точностью до градуса на север, то роли не играет какой это век.
главное чтоб это работало )
дополнительно выдает магнитное склонение и наклонение в данной местности
А каким образом склонение определяется, да еще с точность 1-2 градуса? По моему это лучше все же по карте склонений определить.
Кстати, масштабы по осям зря игнорируешь - они так же важны как и у акселерометра, а у компасса еще зависят от места установки, так что даже точные калибровки с завода не помогут.
Давно не интересовался как люди калибруют компас. Может я отстал от жизни и это прошлый век??
Самый популярный способ вроде как фиттинг эллипсоида. Для него достаточно покрутить по двум осям, не обязательно всю сферу собирать. DJI могут и его использовать.
Вот еще одно видео, там уже я запустил непрерывный режим калибровки. Попеременно подношу и убираю магнит.
А каким образом склонение определяется, да еще с точность 1-2 градуса? По моему это лучше все же по карте склонений определить.
Тут вы правы, я поспешил с выводами. Изначально были сомнения на этот счет. Провел побольше тестов, я просто удачно попал в первый раз. А работает он так: В UKF отлсеживаются 6 переменных состояния (3 искомых смещения и 3 составляющих магнитного вектора в мировой СК), в качестве алгоритма ориентации используется обычный Mahony. В сам UKF в качестве управляющего сигнала идет кватернион с Mahony. UKF пытается подобрать такие коэффициенты смещения и такой вектор магнитного поля в мировой СК, чтобы его прогноз совпал с реальным значением магнитометра. (это что то типа Parameter Estimation, так можно нейросеть например настраивать, или просто подбирать физические параметры системы, просто для проверки этой концепции попробовал откалибровать магнитометр).
Кстати, масштабы по осям зря игнорируешь - они так же важны как и у акселерометра, а у компасса еще зависят от места установки, так что даже точные калибровки с завода не помогут.
Вот эта мысль меня тоже волновала, но я прообовал подносить магнитомягкие материалы в виде ферромагнитного сердечника трансформатора, сфера почти не исказилась. Поэтому я сделал предположение, что это не так важно.
А как еще можно проверить? Как сильно может плющить сферу?
Самый популярный способ вроде как фиттинг эллипсоида. Для него достаточно покрутить по двум осям, не обязательно всю сферу собирать. DJI могут и его использовать.
Ух ты, я про это не слышал. А можно поподробнее?
А как еще можно проверить? Как сильно может плющить сферу?
Я себе это так представляю: мягкое железо уплотняет поле в одном месте, растягивает в другом, и вдобавок поворачивает. Вот первое что нагуглил:
ybex.com/…/qugl3xvn4bgj302i7syzwomq5mi2ycqq1grnyun… Степень искажений сильно зависит от расстояния до железа, так что какой нибудь болтик в паре миллиметров от компасса может сделать неприятный сюрприз. Если место установки заренее не известно, нужно предполагать самое худшее.
Ух ты, я про это не слышал. А можно поподробнее?
Хм, думаю что слышали, это ж первый способ калибровки который гуглится 😃 Если точек мало, он все равно работает. Например, если просто покрутиться по двум осям как предлагает DJI, можно собрать вот такие точки: ybex.com/…/gyjo6rz5qzqo8drmjxmevnorzdb5yonpc1e76nv… (отмечены красным цветом). В них однозначно ложится только один элиипсоид (если подбирать только scale и offset). Наклон оси подбирать сложнее математически, но тоже можно. Методы фиттинга есть разные, я пробовал градиентный спуск с отжигом - он очень прост в реализации, минимум математики и работает нормально на такой задаче. Можно уравнения решать, матрицы инвертировать - задача минимизации среднеквадратичного расстояния очень хорошо изучена и описана.
Вот еще одно видео, там уже я запустил непрерывный режим калибровки. Попеременно подношу и убираю магнит.
Я думаю, что именно такие способы калибровки всех сенсоров и используются в продвинутых IMU. В описании VectorNav видел, что нужно погонять систему в тепличных условиях (когда сенсоры не перегружаются и врут по минимуму), а потом сохранить текущее состояние фильтра как “начальное”.
Интересные вещи говорите. Спасибо.
Кому нужен алгоритм, могу выложить в свободный доступ, вопрос в каком виде? Могу только пару файлов кинуть, или сделать порт на F4BY. Вопрос, есть ли реальное применение этой штуке, и кому оно нужно? Кто готов проверить и развить тему?
Кому нужен алгоритм, могу выложить в свободный доступ, вопрос в каком виде? Могу только пару файлов кинуть, или сделать порт на F4BY.
Спасибо! Пары файлов будет достаточно для желающих поиграться. Под какой лицензией будет код? MIT?
Вопрос, есть ли реальное применение этой штуке, и кому оно нужно? Кто готов проверить и развить тему?
Думаю применение есть, если эта штука в реальном времени работает и не сползает если показания компаса не меняются (стабильный прямолинейный полет или нахождение на месте)
Ок, да, делайте с кодом, что хотите. Только хотелось бы узнать о результатах тестов, или какие то дополнения, усовершенствования. Вопрос: куда скинуть код?
Кстати какие у вас впечатления о прошивке квадрокоптера? Что то успели перенять полезного?
Ок, да, делайте с кодом, что хотите. Только хотелось бы узнать о результатах тестов, или какие то дополнения, усовершенствования.
Спасибо! Можно куда угодно, хоть на гитхаб, хоть на мыло.
Кстати какие у вас впечатления о прошивке квадрокоптера? Что то успели перенять полезного?
Честно говоря, в деталях еще даже не смотрел - времени не было. Реализовано удобно, модульно, если в тестах будет хорошо работать с разными наборами датчиков (аксель+гира, аксель+гира+компас, аксель+гира+GPS, аксель+гира+компас+GPS) - возможно заменит комплиментарник в INAVе.
Думаю применение есть, если эта штука в реальном времени работает и не сползает если показания компаса не меняются (стабильный прямолинейный полет или нахождение на месте)
Конкретно у этого алгоритма, на управляющий вход подается кватернион. В данном случае кватернион рассчитывается с помощью Mahony (гира+аксель). Поэтому он потихоньку уплывает по оси Z. При этом этот уплывающий кватернион поступает в UKF, который занимается только рассчетом коэффициентов и глобального магнитного вектора. Главная цель - это конечно смещения. будут ли уплывать смещения, когда плывет кватернион, я не проверял. Но скорее всего будет уплывать глобальный вектор, а смещения останутся, но это не точно.
Собственно эти самые коэффиценты смещения могут пойти обратно в алгоритм расчета ориентации (уже аксель+гира+магнитометр), получается обратная связь, поэтому хз, как поведет себя эта связка. Поэтому если смещения уходят с плывущим кватернионом, но смысла включать реал-тайм калибровку нет, и тогда единственное применение - единождая калиборовка (просто она будет чуть проще). Если смещения не уплывают от плывущего кватерниона, тогда можно пустить его непрерывно. Но тут надо настроить коэффициенты адаптации, как быстро алгоритм должен реагировать на магнитные возмущения, надо настроить так, чтобы кратковременные выбросы были учтены и отбрасовались алгоритмом ориентации, но долгосрочные изменения магнитного поля перестраивали коэффициенты. Это все в случае раздельного алгоритма ориентации и алгоритма калибровки.
Можно опять все собрать в кучу и сделать единый UKF, настроить коэффициенты адаптации, коэффициенты игнорирования кратковременных магнитных возмущений. Чтобы с этим всем экспериментировать, надо хорошо так разобраться с UKF.
Я не предлагаю готовое решение, тут еще много работы, тестов, подгонки.
Честно говоря, в деталях еще даже не смотрел - времени не было. Реализовано удобно, модульно, если в тестах будет хорошо работать с разными наборами датчиков (аксель+гира, аксель+гира+компас, аксель+гира+GPS, аксель+гира+компас+GPS) - возможно заменит комплиментарник в INAVе.
Да на самом деле, можете говорить как есть, все это писалось паралельно изучению STM, стилей программирования, поэтому код - это сборная солянка, грязный, не гибкий, не удобный. Многое я бы сделал иначе.
Конкретно в той прошивке работает набор - аксель, гира, GPS, барометр(хотя на самом деле, его можно убрать, толку от него 0). Чтобы добавить магнитометр надо добавить функцию simDoMagUpdate. Но я не вижу в этом смысла, так как тот вариант который там есть - это законченное решение, магнитометр там не нужен. Разве что, для того чтобы еще добавить в мат модель магнитные смещения и коэффициенты масштаба. Но тогда в ресурсы STM32F4 уже не впишемся. Поэтому Для коптера я считаю это самый оптимальный вариант.
Для отдельного универсального IMU можно еще поэкспериментировать с разными датчиками, мат моделями, алгоритмами.
А также для полного UKF, который будет иметь мат модели акселя, гиры, компаса, видеокамеры, GPS, учитывающий и коэффиценты смещения и масштаба, работащий внутри и вне помещений. Но тут уже сложнее, и железо как минимум intel core i7.
Ссылка на код:
www.dropbox.com/sh/…/AADAb2jiAQuKgIAE1Eje6mNDa?dl=…
А как вам тот проект с OpenCV и логами, когда видео поворачивается с помощью логов? кто нибудь его использовал?
Но я не вижу в этом смысла, так как тот вариант который там есть - это законченное решение, магнитометр там не нужен.
Т.е. вектор ориентации считается по голому GPS, без какого-либо компаса? Отлично!
Там связка аксель, гира, GPS. А так да, магнитометр не участвует в расчетах во время полета, только в самом начале, при соотношении осей. Я же об этом раз 5 говорил в предыдущих сообщениях
А как вам тот проект с OpenCV и логами, когда видео поворачивается с помощью логов? кто нибудь его использовал?
В планах что-то подобное для стабилизации видео с камеры, жестко зарепленной на борту.
Я же об этом раз 5 говорил в предыдущих сообщениях
Я за темой не следил какое-то время 😃
Я за темой не следил какое-то время
Обкатал UKF на коптере, штука своебразная. Летает без использования магнитометра
rcopen.com/forum/f134/topic224458/6562
Вот есть видео, где показано как GPS корректирует ориентацию в фильтре Калмана.
rcopen.com/forum/f134/topic224458/6608
Обратите внимание чем больше неверен начальный курс компаса, по которому была начальная калибровка осей, тем сильнее пила. Эта пила - это коррекция GPS. чем сильнее пила, тем больше различается истинный курс и прогнозируемый фильтром. Можно заметить, что через некоторое время даже в 3-ьем тесте пила становиится все меньше и меньше, а можно предположить, что полетав я еще немного, курс приблизился бы к настоящему
rcopen.com/forum/f134/topic224458/6626
Фильтр сам находит как из показания GPS правильно скорректировать позицию и ориентацию.
rcopen.com/forum/f134/topic224458/6628
А вот Александр говорит тоже самое:
Как не помощник ? По нему видны итоги перемещения которые можно сравнить с интгралом акселя и выполнить корректировку.
rcopen.com/forum/f134/topic224458/6717
А кстати забыл сказать. Данный UKF, который я взял у AutoQuad можно еще хорошо так оптимизировать. Дело в том, что сигма точки которые генерятся в количестве 2*A+1. Где A - это кол-во переменных состояния + переменные шума. Так вот почитав форум Autoquad я выяснил, что при настройке UKF Билл использовал его способность к Parameter Estimation, то есть брал этот фильтр гонял в поле, где то брал рефференсные значения по переменным состояния. И загонял эти рефференсные значения в UKF. таким образом настраивались все параметры шумов Q, N, V. Но после настройки из вектора состояния Билл не убрал как раз эти V и N - параметры шумов процесса и шумов измерения. По сути они теперь константы и их рассчитывать не надо. То есть теперь по идее не надо вырабатывать сигма точки с учетом переменных шума (так как они константы), а только лишь для переменных состояния. Вот пример в текушем фильтре используется 17 переменных состояния и 12 переменных шума процесса и 3 переменных шума измерения, то есть всего оценивается 17+12+3=32 переменных. Для них вырабатываются сигма точки аж целых 32*2+1 = 65 точек. Каждая точка имеет размерность 32. То есть имеем 65 векторов с 32 измерениями. Каждая точка проходит через уравнения прогноза и измерения.
Представьте как можно это оптимизировать, если убрать из расчета шумы процесса и шумы измерения, ведь они все равно константы и были включены в расчет, только лишь для первоначальной настройки.
Поэтому кто это сделает может существенно повысить частоту UKF или загрузить еще переменных состояния.
Но чтобы это сделать надо хорошо так разобраться во всех функциях SRCDKF. У меня с разбегу не получилось.
Поздравляю с Днем Рождения Сергея, основателя этой темы и создателя платы F4BY))