Вопрос по математике кватернионов Опции

[Mityan]

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

В литературе написано: получаем начальный кватернион q0 q1 q2 q3, потом есть формулы, которые вычисляют производную кватерниона на основе угловых скоростей. Интегрируя эту производную, получаем кватернион, который можно также по соответствующим формулам пересчитать в углы эйлера.

Сделал так (в МАТЛАБе):
% Задал начальное положение - курс, тангаж, крен.
yaw = deg2rad(54);
pitch = deg2rad(-20);
roll = 0;
% вычислил кватернион (начальный)
dcm = angle2dcm(yaw, pitch, roll);
quat = dcm2quat(dcm);
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
% Здесь можно было сразу angle2quat - не суть важно.
% Затем задал, как у меня будет вращаться тело -
% по одной из осей со скоростью 1 градус в секунду 36 секунд.
p = [zeros(30,1); deg2rad(1)*ones(36,1); zeros(34,1)]; % крен
q = zeros(100,1);
r = zeros(100,1);
U = zeros(100,3); % Углы
% Затем в цикле вычисляю углы Эйлера - фи, тэта, пси
% После этого производную кватерниона на основе q0 q1 q2 q3 p q r
% Обновляю значение q = q + dq - и на следующий круг.
for i = 1:1:len
theta = asind(-2*(q1*q3 - q0*q2));
s = sign(2*(q2*q3 + q0*q1));
u = q0^2 - q1^2 - q2^2 + q3^2;
l = sqrt(1 - 4*(q1*q3 - q0*q2)^2);
phi = acosd(u/l)*s;
s = sign(2*(q1*q2 + q0*q3));
u = q0^2 + q1^2 - q2^2 - q3^2;
psi = acosd(u/l)*s;
U(i,1) = theta;
U(i,2) = phi;
U(i,3) = psi;
dq0 = -(q1*p(i) + q2*q(i) + q3*r(i))/2;
dq1 = (q0*p(i) + q2*r(i) - q3*q(i))/2;
dq2 = (q0*q(i) + q3*p(i) - q1*r(i))/2;
dq3 = (q0*r(i) + q1*q(i) - q2*p(i))/2;
q0 = q0 + dq0;
q1 = q1 + dq1;
q2 = q2 + dq2;
q3 = q3 + dq3;
end

Математика взята из книги Performance, Stability, Dynamics and Control of Airplanes автора Pamadi
Получаю вот что (задавая разные скорости вращения):
1) Когда угол поворота переваливает за 180 градусов, он дальше становится комплексным и растет в отрицательную сторону,
например, 185 градусов видно как 180 - 5i
2) Углы другие тоже изменяются. Например, если у меня крен 4град*36с = 148град, то курс вместо 54 стал 51.8, а тангаж вместо -20 стал -20.9
3) Когда подставил вектор отсчетов реальных с датчика угловых скоростей (за вычетом zero-rate level), получил сообщение от МАТЛАба, что аргумент арккосинуса должен быть действительным. Это величина l = sqrt(1 - 4*(q1*q3 - q0*q2)^2); становится меньше 0.

Вопросы:
1) правильна ли сама логика, или я в чем-то запутался и что-то не учел?
2) почему изменяются остальные углы
4) почему получается комплексный угол
3) в книге (стр.347) ничего не говорится о каких-то пределах кватернионов, эта формула приведена ничтоже сумняшеся,
и непонятно, у тех, кто тоже занимается кватернионами, q1*q3 - q0*q2 никогда-никогда не бывает > 0.5?
(это чтобы корень стал мнимым).

Спасибо.

[AndreyVN]

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

В литературе написано: получаем начальный кватернион q0 q1 q2 q3,....

Поясните, pls, с чего, вообще, задача классической механики решается через алгебру кватернионов?
Чем не устраивает интегрирование уравнений Ньютона?

[Mityan]

Ну, вообще типа пишут, что при поворотах с использованием углов эйлера и матрицы направляющих косинусов существует неоднозначность при +/- 90 градусов, а этих недостатков лишена алгебра кватернионов, и везде - в инерциальных навигационных системах, а также в 3D математике компьютерных игр именно они и применяются (кватернионы).

Сообщение отредактировал Mityan - Jun 15 2012, 09:10

[Aner]

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

[Mityan]

Данные с датчиков снимаются 50 раз в секунду. Полагаю, вычислительных мощностей TMS320 хватит. Ничего огромного в приведенных формулах не вижу.
Кстати, если я в своем матлабовском коде, там, где углы сохраняются в матрицу U, добавляю U2 = quat2angle([q0 q1 q2q 3]); - то есть родную функцию, она почему-то вообще какую-то ерунду выдает.

[Mityan]

Вот что я скажу:

В другой книжке - Applied Mathematics in Integrated Navigation Systems автора Rogers
вычитал:

Disadvantages (of quaternions имеется ввиду) include nonlinear terms in the result and the necessity of
renormalizations within the computational cycles to maintain properties previously
discussed for DCMs.

Там кватерниноны вообще упоминаются мельком, буквально полстраницы (по крайней мере, докуда я долистал),
но стало ясно следующее: на каждом шаге необходимо нормирование кватернионов.

По этому я в коде перед оператором завершения цикла end добавил следующее:
e = sqrt(q0^2 + q1^2 + q2^2 + q3^2);
q0 = q0/e;
q1 = q1/e;
q2 = q2/e;
q3 = q3/e;

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

[Aner]

Данные с датчиков снимаются 50 раз в секунду. Полагаю, вычислительных мощностей TMS320 хватит. Ничего огромного в приведенных формулах не вижу.
Кстати, если я в своем матлабовском коде, там, где углы сохраняются в матрицу U, добавляю U2 = quat2angle([q0 q1 q2q 3]); - то есть родную функцию, она почему-то вообще какую-то ерунду выдает.

TMS320 не хватит.

[Mityan]

Посмотрим. В том коде, который приведен, не вижу огромного количества вычислений, тем более, что кватернионы - это числа от 0 до 1, т.е. можно обойтись операциями с фиксированной точкой. Наоборот, пишут (Книга Обработка информации в навигационных комплексах автора Бабича), что, дословно, "всеширотный алгоритм исчисления геодезических координат требует интегрирования системы дифференциальных уравнений 6 порядка..." и далее "выведен алгоритм счисления..., требующий решения системы 4 порядка (наименьшего). Теория его основана на использовании метода конечных поворотов в применением параметров Родрига-Гамильтона". А это и есть кватернионы.
Поэтому уверен, что по поводу вычислительной сложности вы несколько заблуждаетесь.

[Aner]

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

[AndreyVN]

Честно скажу, с кватернионами не работал, но доводилось писать программу по молекулярной механике.
Для визуализации 3D молекулы приходилось переходить из системы координат центра масс молекулы в глобальную систему координат и отрисовывать молекулу в соответствии с текущими углами поворота.

Матрицу с углами расписал, и к каждой операции приходилось добавлять, кажется пару if(), чтобы избежать неопределенности с углам, ок которой Вы говорили. Как я понимаю, у Вас задача та-же самая. Было это давно, я пользовался я книгой Дж. Фоли, А. вэн Дэм, Основы интерактивной машинной графики. М.Мир, 1985. К стати, там есть опечатки именно в формулах на повороты.

Стоит ли ради шести операций if() углубляться в кватернионные дебри?

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

/** Повернуть координаты атомов и оси инерции в молекуле MATH*****/
void TMain::RotMolPos( int N, float dA, int MovRot)
{
int i;
double CosdA,SindA;
float x,y,z,Ii1a,Ii1b,Ii1g,Ii2a,Ii2b,Ii2g,Ii3a,Ii3b,Ii3g;
CosdA = cos(dA);
SindA = sin(dA);
for(i=0; i< Calc->M[N].maxatom; i++){ /*повернуть все атомы в молекуле*/
x = Calc->M[N].X[i]-Calc->M[N].X0;
y = Calc->M[N].Y[i]-Calc->M[N].Y0;
z = Calc->M[N].Z[i]-Calc->M[N].Z0;
if(MovRot==1){ /* вокруг оси Z */
Calc->M[N].X[i]=(x*CosdA - y*SindA) + Calc->M[N].X0;
Calc->M[N].Y[i]=(x*SindA + y*CosdA) + Calc->M[N].Y0; }
if(MovRot==2){ /* вокруг оси Y */
Calc->M[N].X[i]=(x*CosdA + z*SindA) + Calc->M[N].X0;
Calc->M[N].Z[i]=(z*CosdA - x*SindA) + Calc->M[N].Z0; }
if(MovRot==3){ /* вокруг оси X */
Calc->M[N].Y[i]=(y*CosdA - z*SindA) + Calc->M[N].Y0;
Calc->M[N].Z[i]=(y*SindA + z*CosdA) + Calc->M[N].Z0; }
}
/*повернуть напрвляющие косинусы, задающие главные оси инерции*/
Ii1a = Calc->M[N].I1a;
Ii1b = Calc->M[N].I1b;
Ii1g = Calc->M[N].I1g;
Ii2a = Calc->M[N].I2a;
Ii2b = Calc->M[N].I2b;
Ii2g = Calc->M[N].I2g;
Ii3a = Calc->M[N].I3a;
Ii3b = Calc->M[N].I3b;
Ii3g = Calc->M[N].I3g;
if(MovRot==1){ /* вокруг оси Z */
Calc->M[N].I1a = Ii1a*CosdA - Ii1b*SindA;
Calc->M[N].I2a = Ii2a*CosdA - Ii2b*SindA;
Calc->M[N].I3a = Ii3a*CosdA - Ii3b*SindA;
Calc->M[N].I1b = Ii1a*SindA + Ii1b*CosdA;
Calc->M[N].I2b = Ii2a*SindA + Ii2b*CosdA;
Calc->M[N].I3b = Ii3a*SindA + Ii3b*CosdA; }
if(MovRot==2){ /* вокруг оси Y */
Calc->M[N].I1a = Ii1a*CosdA + Ii1g*SindA;
Calc->M[N].I2a = Ii2a*CosdA + Ii2g*SindA;
Calc->M[N].I3a = Ii3a*CosdA + Ii3g*SindA;
Calc->M[N].I1g = Ii1g*CosdA - Ii1a*SindA;
Calc->M[N].I2g = Ii2g*CosdA - Ii2a*SindA;
Calc->M[N].I3g = Ii3g*CosdA - Ii3a*SindA; }
if(MovRot==3){ /* вокруг оси X */
Calc->M[N].I1b = Ii1b*CosdA - Ii1g*SindA;
Calc->M[N].I2b = Ii2b*CosdA - Ii2g*SindA;
Calc->M[N].I3b = Ii3b*CosdA - Ii3g*SindA;
Calc->M[N].I1g = Ii1b*SindA + Ii1g*CosdA;
Calc->M[N].I2g = Ii2b*SindA + Ii2g*CosdA;
Calc->M[N].I3g = Ii3b*SindA + Ii3g*CosdA; }
}

[Mityan]

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

[Mityan]

В книге Обработка информации в навигационных комплексах автора Бабича, там, где выводится матрица производных кватерниона, каждый компонент домножается для нормирования на величину е = k*(1 - (p0^2 + p1^2 + p2^2 + p3^2)^(-1/2)), где (написано) k подбирается экспериментально.
Возможно, по моему предположению, в связи с конечной точность вычислений иногда возникает этот комплексный результат.

Пока в своем коде я величину е домножил еще на 1.0000000001, и комплексный коэффициент пропал. Если домножать на 1.0001, страдает точность.
В приведенном коде 40 умножений, 10 делений, 26 сложений, 2 sqrt, 3 acos.

[jks]

можно посмотреть как сделано здесь:

http://autopilot.sourceforge.net/index.html

Есть исходники и код математики кватернионов, интегрирование РК 4-го порядка, готовая (в какой-то мере) навигационная система .

Бортовой вычислитель работает на Mega163, частота опроса датчиков 25 Гц.

Версия Rev2 работает на ARM AT91R40008.

[brag]

Если есть угловые скорости, то зачем вам кватернионы? Они предназначены для хранения однозначного положения обьекта, где углы Эйлера вызывают кучу гемора, а хранение ориентации в виде матрицы вращения(3х векторов up,dir,side) жрет слишком много памяти(9 слов против 4):

Вот почитайте введение, дальше можно не читать,если алгебра понятна. http://www.gamedev.ru/articles/?id=30129
Что касается производительности, то не сильно тяжелее матриц. http://en.wikipedia.org/wiki/Quaternions_a...patial_rotation ,см "Performance comparisons with other rotation methods".

Теперь про определение положения по угловым скоростям. Есть 2 понятия угловых скоростей: абсолютная(вращение обьекта вокруг какой-то постоянной оси) и релятивная(называется Euler rates). В случаи с гироскопами именно с ними(Euler rates) приходится работать ибо система из гироскопов дает вектор угловой скорости вокруг оси, зависимой от этого вектора ) Кроме, как интегрировать их другого способа вроде нету http://answers.unity3d.com/questions/22173...-transform.html
А через что будете вращать обьект, хоть матрицы, хоть кватернионы - разница только в производительности и нумерической стабильности.