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

[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]).

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

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

[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; курс=текущий угол;

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