Кватернионы и реальная механика, Вопрос синхронизации Опции

[alexPec]

Всем доброго дня. Неожиданно столкнулся с такой проблемой.

Есть механика, на рисунке. Внутренняя рамка С может поворачиваться вокруг осей Х и У относительно рамки А моторами М1 и М2. Вся система располагается в море, на корабле. Рамка А прикреплена жестко к кораблю. Задача - стабилизировать рамку С, так чтобы ее нормаль (g) совпадала с вектором гравитации.
Допустим, устанавливаю на рамку С гироскоп и акселерометр (трехосевые). При старте все хорошо - я точно знаю, как располагается подвижная система координат и точно знаю, что угол поворота вокруг оси Х я могу компенсировать мотором М1, а вокруг У- мотором М2. Гироскопы неизбежно плывут - с этим ничего не поделать. И если уплывания по осям Х и У можно компенсировать, зная вектор гравитации (измеряю акселерометром), то уплывание по оси g пока не вижу способа компенсировать. Да и компенсировать собственно не надо, проблема в том, что из-за этого уплывания со временем, когда уплывание достигнет например, 90 градусов, то мотором М1 я уже буду крутить кватернион вокруг оси Y, а мотором М1 - вокруг Х. Как-то бы привязать кватернион по оси g к рамке А.
Привязывать отдельно рамку А и отдельно кватернион к одной глобальной системе координат (например, по компасу) нет возможности.

Вопрос такой: как-то можно определить, каким мотором крутить, чтобы компенсировать появившийся крен рамки С? Т.е. крен -то я из кватерниона вытащу и разложу на составляющие по осям Х и У, а как определить, как сейчас расположены оси Х и У подвижной системы координат относительно осей моторов?

PS охота именно комплексно, через кватернионы решить задачу, а не отдельно по каждой оси мерить и компенсировать угловые скорости.

Эскизы прикрепленных изображений

 

[AlexandrY]

а как определить, как сейчас расположены оси Х и У подвижной системы координат относительно осей моторов?

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

[alexPec]

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

Да отследить - то отслежу, но они же плывут. К примеру: кладем на стол всю эту конструкцию (рамки А В С с моторами). Рассматриваем только ось g - остальные компенсирую по гравитации. При старте - да, знаю что как ориентировано. Но гироскопы плывут, соответственно при неподвижной механической конструкции кватернион будет вращаться (быстро ли, медленно ли - зависит от гироскопов, калибровки, но вращаться точно будет). Да, я отслежу что через некоторое время кватернион якобы повернулся на 90 градусов, а механика при этом лежит неподвижно. И в этом случае (когда механика лежит на столе) я знаю, что это кватернион уплыл, а не механика повернулась. А на корабле в море? Откуда мне знать, гироскоп это плывет или корабль медленно поворачивается? И через некоторое время я уже не смогу определить, на сколько повернулась механика, а на сколько уплыл кватернион.

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

[AlexandrY]

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

Да не сможете. Известный эффект.
Решается магнитометром или другими дополнительными средствами.
Неужели гугле вам об этом еще не сообщил?

[alexPec]

Да не сможете. Известный эффект.
Решается магнитометром или другими дополнительными средствами.
Неужели гугле вам об этом еще не сообщил?

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

[тау]

Задача - стабилизировать рамку С, так чтобы ее нормаль (g) совпадала с вектором гравитации.

почему нельзя решить задачу без гироскопа? прикрепите к рамке С груз внизу на жестком подвесе. Груз чувствует g и никуда не "плывет" в отличие от гироскопа. Груз все время будет ориентировать правильно рамку С , а вместо моторов поставьте энкодеры, чтобы снимать показания по крену. При разгоне корабля появится небольшая ошибка , но ее можно будет нейтрализовать по данным акселерометра.

[alexPec]

почему нельзя решить задачу без гироскопа? прикрепите к рамке С груз внизу на жестком подвесе. Груз чувствует g и никуда не "плывет" в отличие от гироскопа. Груз все время будет ориентировать правильно рамку С , а вместо моторов поставьте энкодеры, чтобы снимать показания по крену. При разгоне корабля появится небольшая ошибка , но ее можно будет нейтрализовать по данным акселерометра.

Груз будет раскачиваться, чувствуя ускорения при качке. С помощь связки гироскоп + акселерометр (тот же электронный груз) можно фильтровать акселерометр с достаточно большой постоянной времени, но не такой большой, чтобы уплывание гироскопов за это время было бы значительным. При этом "качку", т.е. достаточно быстрые повороты, можно как раз компенсировать по данным гироскопов. Медленные уходы гироскопов компенсировать по акселерометру.

Добавлено: при качке вся система испытывает и линейные ускорения, т.к. находится не в центре вращения корабля.

[AlexandrY]

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

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

[alexPec]

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

Как раз при качке, когда рамка С поворачивается на малый угол da вокруг Х или У или их комбинации (это чувствуют гироскопы, связанные с механикой), кватернион при пересчете повернется на угол db, но повернется он уже по-другому. И в случае кватерниона это чисто математический поворот, поэтому думаю можно после вычисления кватерниона произвести расчет соотношений поворота кватерниона и гироскопов. Хоть по крену это и очень малые повороты, но поскольку поворот кватерниона - математический, на основании показаний тех же гироскопов, то малые повороты тут не принципиальны.

Ну, условно, когда ось g уплыла на 90 градусов, при повороте механики вокруг оси мотора М1 (малого, но это не принципиально), пересчитываем кватернион, вычисляем требуемые углы - и видим, что кватернион повернулся вокруг оси У, а не Х, как было при старте после инициализации. Ну и из этого делаем вывод, что ось g уплыла на 90 градусов относительно механики.

[тау]

Груз будет раскачиваться, чувствуя ускорения при качке.
Добавлено: при качке вся система испытывает и линейные ускорения, т.к. находится не в центре вращения корабля.

добавьте в осях вязкое трение, демпфируйте раскачку.

[Rst7]

Я что-то не совсем понимаю, в чем проблема.

Вариант 1:

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

Вариант 2:

В коде, который считает поправки к дрейфу гироскопов сделайте значение референсного курса равным 0. И компенсацию дрейфа по курсу сделайте очень медленной.

Вариант 3:

Разместите IMU на внутренней рамке. И держите вертикальность. По акселерометрам (интегральное звено) и двум гироскопам (пропорциональная составляющая).

И еще. Я понимаю, что в кватернионах считать - это очень модно, аж два умножения можно сэкономить, но куда прозрачнее для понимания это все в терминах DCM делать.

[AlexandrY]

В коде, который считает поправки к дрейфу гироскопов сделайте значение референсного курса равным 0. И компенсацию дрейфа по курсу сделайте очень медленной.

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

[Aner]

добавьте в осях вязкое трение, демпфируйте раскачку.

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

[alexPec]

добавьте в осях вязкое трение, демпфируйте раскачку.

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

В коде, который считает поправки к дрейфу гироскопов сделайте значение референсного курса равным 0. И компенсацию дрейфа по курсу сделайте очень медленной.

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

Вариант1 - надо подумать.

[Rst7]

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

Еще раз повторюсь, заканчивайте с кватернионами. Рассматривайте физический смысл. Неважно, как Вы интегрируете данные IMU (кватернион, DCM - пофиг), у Вас должны быть две функции - одна - это пересчет вектора из системы координат IMU в звездную систему координат, и вторая - наоборот (если работать с DCM - то там все банально, сама по себе DCM и есть матрица пересчета из одной системы координат в другую, а пересчет в обратную сторону - это умножение на обратную матрицу). Вот, например, Вы измеряете вектор гравитации - этот вектор в системе координат IMU. Если Вы его пропустите через функцию перехода в звездную систему координат, то этот вектор будет представлять из себя вектор, у которого x, y равны 0, а z равен g. Когда x и y ненулевые - это и есть ошибка. Но можно точно так же переводить и не только вектора измерений, но и вектора ошибки. Т.е. если Вы возьмете вектор [cos(a-va),sin(a-va),0], где а - референсный курс, а va - курс по результатам интегрирования, то это будет вектор ошибки по курсу в звездной системе координат. Преобразовав этот вектор в систему координат IMU как раз и получатся ошибки по каждой из осей. Дальше PI-регулятор и добавка к данным с гироскопа. Все.