реклама на сайте
подробности

 
 
 
Reply to this topicStart new topic
> Гироскоп - вырождение угла, Как борются с этим?
alexPec
сообщение Feb 10 2014, 11:56
Сообщение #1


Профессионал
*****

Группа: Свой
Сообщений: 1 236
Регистрация: 9-04-06
Пользователь №: 15 968



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

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

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

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

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

Go to the top of the page
 
+Quote Post
FPGAz
сообщение Feb 10 2014, 12:08
Сообщение #2


Участник
*

Группа: Участник
Сообщений: 30
Регистрация: 9-02-14
Пользователь №: 80 406



А это разве не эффект "схлопывания рамок"? ))
Go to the top of the page
 
+Quote Post
alexPec
сообщение Feb 10 2014, 14:01
Сообщение #3


Профессионал
*****

Группа: Свой
Сообщений: 1 236
Регистрация: 9-04-06
Пользователь №: 15 968



Цитата(FPGAz @ Feb 10 2014, 16:08) *
А это разве не эффект "схлопывания рамок"? ))


В кватернионах такого нет. Это в углах эйлера. Просто действительно при повороте на 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;

Работает отлично. Но вот сейчас даже не могу найти откуда это взял. Хотел перечитать откуда взялось там чего.
Go to the top of the page
 
+Quote Post
_Ivana
сообщение Feb 10 2014, 18:19
Сообщение #4


Местный
***

Группа: Свой
Сообщений: 352
Регистрация: 13-08-11
Из: Воронеж
Пользователь №: 66 710



В любом случае лучше разобраться в вопросе и принять осмысленное решение, я не разбираюсь. Но если подходить "втупую", то плавный переход можно реализовать сложением величин с нужными процентными весами, зависящими от аргумента желаемым образом.
Go to the top of the page
 
+Quote Post
amaora
сообщение Feb 10 2014, 18:33
Сообщение #5


Местный
***

Группа: Участник
Сообщений: 379
Регистрация: 2-01-08
Пользователь №: 33 778



Есть же операция пресчета векторов в связанную систему и наоборот для заданного кватерниона, произведение слева и справа на сопряженный. Вот распишите для нужного вам вектора эту операцию, и возмите угол который нужен, а в линейной алгебре с этим уже проще.
Go to the top of the page
 
+Quote Post
Timmy
сообщение Feb 11 2014, 03:24
Сообщение #6


Знающий
****

Группа: Участник
Сообщений: 821
Регистрация: 9-08-08
Из: Санкт-Петербург
Пользователь №: 39 515



Цитата(alexPec @ Feb 10 2014, 15:56) *
При угле тангажа в 90град. значение всегда примерно равно -90. Соответственно начинаются глюки, система управления чудит.

Система управления должна работать только с производными рысканья/крена/тангажа в связанной системе координат. Не пытайтесь переходить ни к какой другой системе координат и проблем при использовании кватернионов не будет.
Go to the top of the page
 
+Quote Post
Taran87
сообщение Feb 11 2014, 11:09
Сообщение #7


Участник
*

Группа: Участник
Сообщений: 18
Регистрация: 12-05-09
Пользователь №: 48 986



Сталкивался с ситуацией пересчета угла рысканья (yaw) для мобильных, носимых устройств. Пересчет из кватернион в угол не дает всегда адекватный результат. Решил проблему при переходе в матрицу ДСМ, в которой точно видно при каких углах работает та или иная формула. Если задача еще актуально жду ответа. rolleyes.gif
Go to the top of the page
 
+Quote Post
alexPec
сообщение Feb 11 2014, 12:43
Сообщение #8


Профессионал
*****

Группа: Свой
Сообщений: 1 236
Регистрация: 9-04-06
Пользователь №: 15 968



Цитата(Taran87 @ Feb 11 2014, 15:09) *
Сталкивался с ситуацией пересчета угла рысканья (yaw) для мобильных, носимых устройств. Пересчет из кватернион в угол не дает всегда адекватный результат. Решил проблему при переходе в матрицу ДСМ, в которой точно видно при каких углах работает та или иная формула. Если задача еще актуально жду ответа. rolleyes.gif

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

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 град, стает тем что нужно.
Go to the top of the page
 
+Quote Post
spl
сообщение Mar 15 2017, 08:22
Сообщение #9





Группа: Участник
Сообщений: 9
Регистрация: 26-01-07
Пользователь №: 24 779



Прошу прощения за некромантию.
Уважаемый AlexPec, удалось ли вам повернуть систему координат на угол обратный teta?
А то мне как-раз требуется проделать такой-же фокус, только дополнительно еще повернуть и на угол обратный phy. Т.е. чтобы осталось только вращение по одному углу psi.
Go to the top of the page
 
+Quote Post
spl
сообщение Mar 15 2017, 09:55
Сообщение #10





Группа: Участник
Сообщений: 9
Регистрация: 26-01-07
Пользователь №: 24 779



Попробую уточнить вопрос. Имеется некоторый кватернион 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; курс=текущий угол;

А может надо совсем все по другому делать т.к. в конечном итоге нужно определить углы между локальными осями координат (оси объекта) относительно плоскостей глобальной системы координат.
Go to the top of the page
 
+Quote Post
spl
сообщение Mar 16 2017, 13:09
Сообщение #11





Группа: Участник
Сообщений: 9
Регистрация: 26-01-07
Пользователь №: 24 779



Понемногу разбираюсь. Поскольку поворот на углы Эйлера осуществляется последовательно - XYZ, то и вычитание этих углов должно быть таким же только в обратном порядке - ZYX. Таким образом, методом вычитания можно найти только первый угол - X, осуществив последовательное вычитание вокруг ZY. На этом мысль заканчивается. Как найти два других угла непонятно.
Go to the top of the page
 
+Quote Post
alexPec
сообщение Mar 16 2017, 20:27
Сообщение #12


Профессионал
*****

Группа: Свой
Сообщений: 1 236
Регистрация: 9-04-06
Пользователь №: 15 968



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

Reply to this topicStart new topic
2 чел. читают эту тему (гостей: 2, скрытых пользователей: 0)
Пользователей: 0

 


RSS Текстовая версия Сейчас: 18th November 2017 - 10:27
Рейтинг@Mail.ru


Страница сгенерированна за 0.01282 секунд с 7
ELECTRONIX ©2004-2016