Создание собственной системы стабилизации

ИльяПРо

да я еще ничего не сделал, хотел заняться если время позволит) на ф4 не потянет, то что я хочу и ф7 не потянет. Тут либо тот же i7 либо систему на кристалле типа zynq 7000. Но пока рано говорить, пока даже алгоритма нет.

zealot01
oleg70:

Правда делает это с частотой 100 Гц максимум, короче аппаратный IMU…

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

SergDoc
zealot01:

В даташите на 9250 вскользь упоминается 200Гц. И возможность подключения внешнего магнитометра по i2c прямо к чипу.

он там внутри - раз, читать какой-то другой компас через i2c, когда датчик подключен по SPI, а тем паче использовать DMP с другим компасом - далеко не тривиальная задача (проверено на собственном опыте)…
ну и использование DMP без обратной связи (корректировка значений внешними датчиками GPS, optical flow и т.д.) смысла в жизнь не принесёт т.к. нарастающую ошибку DMP исправить не чем…

zealot01
SergDoc:

ну и использование DMP без обратной связи (корректировка значений внешними датчиками GPS, optical flow и т.д.) смысла в жизнь не принесёт т.к. нарастающую ошибку DMP исправить не чем…

Так всегда можно подкоректировать DMP внешним контроллером со своими датчиками, никто не спорит, просто делать это можно не часто в случае DMP. А так только по шине данные
гиро качать и считать кватернионы на ЦП.

oleg70
zealot01:

В даташите на 9250 вскользь упоминается 200Гц.

Тут ситуация такая: гира и аксель читаются как и у других датчиков с частотой 2000 и 1000 Гц соответсятвенно… т.е. при желании цикл можно смело и килогерц сделать… (у меня работает на 500 гц), а вот с внутренним магнитометром, как всегда маленькая “засада” его чтение организовано очень “оригинально” через мост SPI-I2c,
т.е. физически внутри чипа магнитометр подключен по i2c а обращение к нему идет по SPI через подачу команд (скорость опроса соответственно 20 гц) …
Нафига такие костыли мне не понятно, наверно у производителей есть весские аргументы на этот счет…

zealot01
oleg70:

т.е. физически внутри чипа магнитометр подключен по i2c а обращение к нему идет по SPI через подачу команд (скорость опроса соответственно 20 гц) …
Нафига такие костыли мне не понятно, наверно у производителей есть весские аргументы на этот счет…

Это ,наверное, чтобы можно было внешний магнитометр подключить по i2c не меняя логики построения, посмотри 4.4 Block Diagram датащита MPU-9250

Кстати нашёл, действительно DMP обновление 200Гц
#define MAX_DMP_SAMPLE_RATE 200 // Maximum sample rate for the DMP FIFO (200Hz)

oleg70
zealot01:

не меняя логики построения

Да видимо так, к тому же, магнитометр вообще довольно низкоскоростное устройство, наверно из-за чисто физических свойств сенсора или его ацп, поэтому подключать его по SPI смысла мало…

SergDoc
zealot01:

Так всегда можно подкоректировать DMP внешним контроллером со своими датчиками, никто не спорит, просто делать это можно не часто в случае DMP. А так только по шине данные
гиро качать и считать кватернионы на ЦП.

Смысла нет корректировать выходные данные с ошибкой! Она будет только нарастать, надо корректировать, как бы по проще, сам алгоритм в самом начале ( например, для расчета скорости из ускорения, вместо предыдущей скорости, которую посчитал DMP надо подставлять уже скорректированную алгоритмом ЦП)…

По сей причине, в наших условиях жудких вибраций и общей болтанки, алгоритм DMP, мягко говоря - не применим

oleg70:

Да видимо так, к тому же, магнитометр вообще довольно низкоскоростное устройство, наверно из-за чисто физических свойств сенсора или его ацп, поэтому подключать его по SPI смысла мало…

То что мы будем делать выборку раз в час или 200 Раз в секунду - это не имеет никакого значения, скажу по секрету, для простых комплиментарников как в мультивие и всех его производных, шины i2c за глаза, недостаточная скорость шины - это от лукавого… Я сторонник SPI в плане надёжности, да её сложнее организовать аппаратно , в частности развести плату, ну тут как кому - хочешь дёшево и сердито выбирай i2c и если руки не кривые в плане кодятства, её вполне хватит!

oleg70
SergDoc:

То что мы будем делать выборку раз в час или 200 Раз в секунду - это не имеет никакого значения

Ну “раз в час” это конечно перебор ))), а так, по идее, регулирующее воздействие на аппарат должно соответствовать его физическим свойствам, таких как масса, инерция, другими словами - вряд ли квадрокоптер сможет “колебаться” в пространстве с частотой 100 Гц (ды даже 50) тогда какой смысл иметь данные для стабилизации чаще… к томуж сама винтомоторная группа не сможет эффективно компенсировать положение на таких частотах, опять же из за собственной суммарной инерционности…
Я себе сделал 500 гц цикл только в надежде на то, что сам алгоритм “слияния” будет более шустро корректировать углы по акселерометру и тогда “коэфициент доверия” к акселю можно занизить, и тем самым снизить влияние линейных ускорений на результат.
Вроде как лучше, но не факт, наверно чтоб получить ощутимый результат частоту надо поднять еще на порядок, а это невозможно…

SergDoc

Я про то, что шина не так важна для выборки датчиков, и то что i2c куда-то там не успеет…

Ещё один “секрет” простая аналоговая серва на ВИШ на 50гц. Сделает лучше и быстрее, то что 500гц. Регулятор насилующий мотор и батарею

Сами-то подумайте соотношение шаг винта/скорость вращения/ потребляемая мощность/тяга…

oleg70
SergDoc:

шина не так важна для выборки датчиков

Говорим, по моему, о минимальном времени цикла: чтение датчиков /обсчет данных /выдача управляющего сигнала (?)
В этой цепи шина имеет немалое значение всетаки SPI на порядок шустрее i2c…
А то что эффективней управление шагом или оборотами, это уже другой вопрос. (конечно серва+механика еффективней)

ИльяПРо

Привет всем, написал алгоритм калибровки компаса. Работает в реал-тайме, не требует сбора полной сферы магнитных векторов, дополнительно выдает магнитное склонение и наклонение в данной местности. Думается, что DJI используют аналогичный алгоритм, так как, я попробовал провести такие же манипуляции, и компас откалибровался.
Замечания: результатом алгоритма является только 3 коэффициента смещения, коэффициенты масштаба и взаимовлияния осей отсутствуют, но я думаю что этого достаточно, чтобы определить направление на север с точностью в несколько градусов.
В качестве датчиков: акселерометр, ДУС, магнитометр. Алгоритм основан угадайте на чем? правильно моем любимом UKF=) в принципе всего 6 перемнных сосотяния, не жрущий. полный цикл прогноза занимает 208 мкс, а цикл прогноза и коррекции вместе 500 мкс на STM32F407 с включенным FPU.
Видео:

Давно не интересовался как люди калибруют компас. Может я отстал от жизни и это прошлый век??

oleg70
ИльяПРо:

Может я отстал от жизни и это прошлый век??

По мне, так это скорей день завтрашний ))) А что дает такой подход ?
У меня примитивно находится пресловутая сфера при нескольких вращениях в “реальном времени” (фактически поиск максимумов и минимумов по осям, с последующим смещением) - в результате те же ~ 2-5 градусов точности обеспечены…
К тому же не понимаю: зачем для компаса нужен расчет магнитного склонения (?) лететь точно на северный полюс ?
как я понимаю для возврата домой можно обойтись и без “честного” севера.

ИльяПРо
oleg70:

А что дает такой подход ?

Ну примерно экономит время на калибровке компаса, провернул пару раз по осям, как у Dji - все готово.

oleg70:

(фактически поиск максимумов и минимумов по осям, с последующим смещением)

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

oleg70:

К тому же не понимаю: зачем для компаса нужен расчет магнитного склонения (?) лететь точно на северный полюс ?

Ну а как комплексировать данные с GPS? Тут в любом случае надо выставить виртуальные оси с реальными осями. Во многих случаев можно пренебречь магнитным склонением, так как оно 0-15 градусов и система использующая магнитометр и GPS будет на грани, но есть места на планете, где магнитное склонение достигает 120 градусов и 50, как можно это не учитывать? посмотрите карту магнитного склонения.
Да и написал я этот алгоритм как проверку концепции, не было конкретной цели.

oleg70
ИльяПРо:

Ну а как комплексировать данные с GPS?

Да, согласен, у меня просто все примитивно (хотя можно и так))…

jShadow
ИльяПРо:

алгоритм как проверку концепции

Интересная концепция. У меня проще сделано - при калибровке компаса выбирается минимум 6 более-менее ортогональных отсчетов и по ним аппроксимируется эллипсоид. Центр эллипсоида - есть магнитные смещения. Но самокалибровка в реальном времени - это же еще интереснее.

alexeykozin

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

alexmos
ИльяПРо:

дополнительно выдает магнитное склонение и наклонение в данной местности

А каким образом склонение определяется, да еще с точность 1-2 градуса? По моему это лучше все же по карте склонений определить.
Кстати, масштабы по осям зря игнорируешь - они так же важны как и у акселерометра, а у компасса еще зависят от места установки, так что даже точные калибровки с завода не помогут.

ИльяПРо:

Давно не интересовался как люди калибруют компас. Может я отстал от жизни и это прошлый век??

Самый популярный способ вроде как фиттинг эллипсоида. Для него достаточно покрутить по двум осям, не обязательно всю сферу собирать. DJI могут и его использовать.

ИльяПРо

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

alexmos:

А каким образом склонение определяется, да еще с точность 1-2 градуса? По моему это лучше все же по карте склонений определить.

Тут вы правы, я поспешил с выводами. Изначально были сомнения на этот счет. Провел побольше тестов, я просто удачно попал в первый раз. А работает он так: В UKF отлсеживаются 6 переменных состояния (3 искомых смещения и 3 составляющих магнитного вектора в мировой СК), в качестве алгоритма ориентации используется обычный Mahony. В сам UKF в качестве управляющего сигнала идет кватернион с Mahony. UKF пытается подобрать такие коэффициенты смещения и такой вектор магнитного поля в мировой СК, чтобы его прогноз совпал с реальным значением магнитометра. (это что то типа Parameter Estimation, так можно нейросеть например настраивать, или просто подбирать физические параметры системы, просто для проверки этой концепции попробовал откалибровать магнитометр).

alexmos:

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

Вот эта мысль меня тоже волновала, но я прообовал подносить магнитомягкие материалы в виде ферромагнитного сердечника трансформатора, сфера почти не исказилась. Поэтому я сделал предположение, что это не так важно.
А как еще можно проверить? Как сильно может плющить сферу?

alexmos:

Самый популярный способ вроде как фиттинг эллипсоида. Для него достаточно покрутить по двум осям, не обязательно всю сферу собирать. DJI могут и его использовать.

Ух ты, я про это не слышал. А можно поподробнее?

alexmos
ИльяПРо:

А как еще можно проверить? Как сильно может плющить сферу?

Я себе это так представляю: мягкое железо уплотняет поле в одном месте, растягивает в другом, и вдобавок поворачивает. Вот первое что нагуглил:
ybex.com/…/qugl3xvn4bgj302i7syzwomq5mi2ycqq1grnyun… Степень искажений сильно зависит от расстояния до железа, так что какой нибудь болтик в паре миллиметров от компасса может сделать неприятный сюрприз. Если место установки заренее не известно, нужно предполагать самое худшее.

ИльяПРо:

Ух ты, я про это не слышал. А можно поподробнее?

Хм, думаю что слышали, это ж первый способ калибровки который гуглится 😃 Если точек мало, он все равно работает. Например, если просто покрутиться по двум осям как предлагает DJI, можно собрать вот такие точки: ybex.com/…/gyjo6rz5qzqo8drmjxmevnorzdb5yonpc1e76nv… (отмечены красным цветом). В них однозначно ложится только один элиипсоид (если подбирать только scale и offset). Наклон оси подбирать сложнее математически, но тоже можно. Методы фиттинга есть разные, я пробовал градиентный спуск с отжигом - он очень прост в реализации, минимум математики и работает нормально на такой задаче. Можно уравнения решать, матрицы инвертировать - задача минимизации среднеквадратичного расстояния очень хорошо изучена и описана.

ИльяПРо:

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

Я думаю, что именно такие способы калибровки всех сенсоров и используются в продвинутых IMU. В описании VectorNav видел, что нужно погонять систему в тепличных условиях (когда сенсоры не перегружаются и врут по минимуму), а потом сохранить текущее состояние фильтра как “начальное”.

ИльяПРо

Интересные вещи говорите. Спасибо.
Кому нужен алгоритм, могу выложить в свободный доступ, вопрос в каком виде? Могу только пару файлов кинуть, или сделать порт на F4BY. Вопрос, есть ли реальное применение этой штуке, и кому оно нужно? Кто готов проверить и развить тему?