Гироскоп - вырождение угла, Как борются с этим? Опции

[alexPec]

Добрый день всем.

В гироскопе вычисляю угол рыскания через кватернионы по известной формуле:

atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1)

При угле тангажа в 90град. значение всегда примерно равно -90. Соответственно начинаются глюки, система управления чудит.

При этом (когда угол тангажа 90 град.) заметил, что в этом случае угол, вычисляемый как:

asin(2*q[1]*q[3]+2*q[0]*q[2]) это как раз то, что мне нужно.

Т.е. при приближении тангажа к 90 градусам регулируемая величина для системы управления должна плавно перейти от

atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1)

к

asin(2*q[1]*q[3]+2*q[0]*q[2]).

Может кто сталкивался и решал подобную задачу?

Или может классическое решение проблемы есть?

[FPGAz]

А это разве не эффект "схлопывания рамок"? ))

[alexPec]

А это разве не эффект "схлопывания рамок"? ))

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


PS А нет у кого геометрии как получаются эти углы из кватерниона?
Почему именно например именно такие аргументы 2*q[1]*q[2]-2*q[0]*q[3] и 2*q[0]*q[0]+2*q[1]*q[1]-1? Почему еще минус 1? Подозреваю что
в другой комбинации элементы кватерниона дадут нужный угол.

Например где-то откопал такое вычисление угла тангажа:

x=2*q[1]*q[3]-2*q[0]*q[2];
y=2*q[3]*q[2]+2*q[0]*q[1];
z=q[0]*q[0]+q[3]*q[3]-q[1]*q[1]-q[2]*q[2];
reg_ang=((atan(x/(sqrt(y*y+z*z))))*180)/3.141519;

Работает отлично. Но вот сейчас даже не могу найти откуда это взял. Хотел перечитать откуда взялось там чего.

[_Ivana]

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

[amaora]

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

[Timmy]

При угле тангажа в 90град. значение всегда примерно равно -90. Соответственно начинаются глюки, система управления чудит.

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

[Taran87]

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

[alexPec]

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

Да да да, актуально. Интересны подробности, если Вас не затруднит.

UPD Вроде понял, чего хочу. ))
Более формализованная задача:
Есть кватернион текущего поворота Qт, q[n] - его элементы По нему вычисляем углы, нужные мне, по формулам:

x=2*q[1]*q[3]-2*q[0]*q[2];
y=2*q[3]*q[2]+2*q[0]*q[1];
z=q[0]*q[0]+q[3]*q[3]-q[1]*q[1]-q[2]*q[2];

teta = ((atan(x/(sqrt(y*y+z*z))))*180)/3.141519;

phy = atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1))*180)/3.141519;

Вопрос: как повернуть систему координат (найти кватернион поворота) так, чтобы teta стала равна 0.
Вот так вполне конкретно. Понятно что надо повернуть на угол, обратный teta, но вот относительно какого вектора? Че-то не соображу.
Интуитивно чувствую, что решение такой задачи однозначное.
При таком повороте как раз угол, который вырождается в 90 град, стает тем что нужно.

[spl]

Прошу прощения за некромантию.
Уважаемый AlexPec, удалось ли вам повернуть систему координат на угол обратный teta?
А то мне как-раз требуется проделать такой-же фокус, только дополнительно еще повернуть и на угол обратный phy. Т.е. чтобы осталось только вращение по одному углу psi.

[spl]

Попробую уточнить вопрос. Имеется некоторый кватернион q определяющий текущее положение тела в пространстве.
Если проделать следующие математические операции

YPR = euler_from_quat(q); // Вычисляем углы Эйлера

q1 = Quat_from_Eueler(0, 0, -YPR[2]); // Формируем кватернионы обратного поворота
q2 = Quat_from_Eueler(0, -YPR[1], 0);

q4 = quat_mul_quat(q1, q); // Поворот
q5 = quat_mul_quat(q2, q4); // Поворот

то в q5 будет отсутствовать вращение по двум осям. Останется только одно.
Если я хочу заблокировать вращение по другим осям, то пытаюсь сделать так

YPR = euler_from_quat(q); // Вычисляем углы Эйлера

q1 = Quat_from_Eueler(0, -YPR[1], 0); // Формируем кватернионы обратного поворота
q2 = Quat_from_Eueler(-YPR[0], 0, 0);

q4 = quat_mul_quat(q1, q); // Поворот
q5 = quat_mul_quat(q2, q4); // Поворот

но вращение остается по всем осям.
Чувствую, что преобразование не корректное т.к. мне нужно исключить вращение по тангажу, курсу и крену, а я вычитаю углы Эйлера, но, в первом случае результат получается именно такой, какой и нужно.
Т.е. вычисляется общий кватернион поворота, а затем нужно вычислить отдельные его составляющие после обнуления поворота по другим осям.
Т.е. требуется найти три кватерниона поворота, после которых
1. тангаж=текущий угол; крен=0; курс=0;
2. тангаж=0; крен=текущий угол; курс=0;
3. тангаж=0; крен=0; курс=текущий угол;

А может надо совсем все по другому делать т.к. в конечном итоге нужно определить углы между локальными осями координат (оси объекта) относительно плоскостей глобальной системы координат.

[spl]

Понемногу разбираюсь. Поскольку поворот на углы Эйлера осуществляется последовательно - XYZ, то и вычитание этих углов должно быть таким же только в обратном порядке - ZYX. Таким образом, методом вычитания можно найти только первый угол - X, осуществив последовательное вычитание вокруг ZY. На этом мысль заканчивается. Как найти два других угла непонятно.

[alexPec]

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