Jump to content

    

Непонятки с кватернионами

Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 1с). Каждый шаг - интегрирование через кватернионы (см. код ниже). Смутило вот что: задаю скорости X и Z по 10 град/сек (обе сразу) и делаю шаг, получаю углы эйлера, затем последовательно делаю x = -10 град/сек, z=0, делаю шаг, затем x=0,z=-10 град/сек, делаю шаг, углы при этом в ноль не выходят. Это нормально?

 

Реализовал гироскоп через кватернионы в железе - такая ситуация:

 

Если наклонять плоскость гироскопа отдельно вдоль осей - все нормально, положишь в исходное положение - углы встают почти в 0. Дрейф ну совсем маленький. Если по кругу наклонять плоскость XY гироскопа (т.е. вперед, вперед-вправо, вправо, назад-вправо,назад, назад - влево, и т.д. по кругу) "по часовой стрелке" то через несколько кругов заметно прибывает угол по оси Z, когда ставишь в исходное положение. Если наклонять плоскость по кругу "против часовой стрелки", то при возврате в исходное положение угол по оси Z уменьшается, не дрейф точно, дрейф намного меньше. Почему так? Замечу: по оси Z вращения нет - только наклоны проскости XY. Отсчеты с гироскопа беру через регулярные промежутки времени, 2мс... В чем еще может быть затык?

Напрягает регулярность - по часовой - всегда прибывает угол, против - всегда убывает. Что-то с математикой неправильно?

В предыдущем топике выкладывал, но чтоб все под рукой было если кто захочет подсказать - вот еще раз код гироскопа:

 // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = imu_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

 

тут q0-q3 - кватернион, gx,gy,gz - угловые скорости, переведенные в рад/сек.

 

Углы считаю так:

 

angs[0]=atan2(2 * q1q2 - 2 * q0q3, 2 * q0q0 + 2 * q1q1 - 1) * 180/M_PI;
     angs[1]=-asin(2 * q1q3 + 2 * q0q2) * 180/M_PI;
     angs[2]=atan2(2 * q2q3 - 2 * q0q1, 2 * q0q0 + 2 * q3q3 - 1) * 180/M_PI;

Share this post


Link to post
Share on other sites
Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 1с). Каждый шаг - интегрирование через кватернионы (см. код ниже). Смутило вот что: задаю скорости X и Z по 10 град/сек (обе сразу) и делаю шаг, получаю углы эйлера, затем последовательно делаю x = -10 град/сек, z=0, делаю шаг, затем x=0,z=-10 град/сек, делаю шаг, углы при этом в ноль не выходят. Это нормально?

 

Реализовал гироскоп через кватернионы в железе - такая ситуация:

 

Если наклонять плоскость гироскопа отдельно вдоль осей - все нормально, положишь в исходное положение - углы встают почти в 0. Дрейф ну совсем маленький. Если по кругу наклонять плоскость XY гироскопа (т.е. вперед, вперед-вправо, вправо, назад-вправо,назад, назад - влево, и т.д. по кругу) "по часовой стрелке" то через несколько кругов заметно прибывает угол по оси Z, когда ставишь в исходное положение. Если наклонять плоскость по кругу "против часовой стрелки", то при возврате в исходное положение угол по оси Z уменьшается, не дрейф точно, дрейф намного меньше. Почему так? Замечу: по оси Z вращения нет - только наклоны проскости XY. Отсчеты с гироскопа беру через регулярные промежутки времени, 2мс... В чем еще может быть затык?

Напрягает регулярность - по часовой - всегда прибывает угол, против - всегда убывает. Что-то с математикой неправильно?

В предыдущем топике выкладывал, но чтоб все под рукой было если кто захочет подсказать - вот еще раз код гироскопа:

 // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = imu_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

 

тут q0-q3 - кватернион, gx,gy,gz - угловые скорости, переведенные в рад/сек.

 

Углы считаю так:

 

angs[0]=atan2(2 * q1q2 - 2 * q0q3, 2 * q0q0 + 2 * q1q1 - 1) * 180/M_PI;
     angs[1]=-asin(2 * q1q3 + 2 * q0q2) * 180/M_PI;
     angs[2]=atan2(2 * q2q3 - 2 * q0q1, 2 * q0q0 + 2 * q3q3 - 1) * 180/M_PI;

Все так и должно быть, смотрите уравнения угловых скоростей в связанной системе координат

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

 

Все так и должно быть, смотрите уравнения угловых скоростей в связанной системе координат

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

поторопилься сорри

 

у вас тут ошибки одни как мне кажеться

 

Fx, Fy, Fz - интегралы от гирскопов, тогда

 

FF2=sqrt(Fx^2+Fy^2+Fz^2)

if(FF2=0) FF3=0.5

else FF3=sin(FF2*0.5)/FF2

поулчаем кватернион

H[0]=cos(FF2*0.5);

H[1]=FF3*Fx;

H[2]=FF3*Fy;

H[3]=FF3*Fz;

 

//Матрица направляющих косинусов связывающая ГСК и объект (это кусок из кода в место L0a[0] подстваить Н[0] и так далее )

C0a[2][2] = L0a[0]*L0a[0]+ L0a[2]*L0a[2]- L0a[1]*L0a[1]- L0a[3]*L0a[3];

C0a[1][2] = 2.*(L0a[1]*L0a[2] + L0a[0]*L0a[3]);

C0a[3][1] = 2.*(L0a[1]*L0a[3] + L0a[0]*L0a[2]);

C0a[3][2] = 2.*(L0a[2]*L0a[3] - L0a[0]*L0a[1]);

C0a[3][3] = L0a[0]*L0a[0]+ L0a[3]*L0a[3]- L0a[1]*L0a[1]- L0a[2]*L0a[2] ;

 

// Находим углы ориентации и передаем их на выход

psi_A = asin(C0a[3][2]) ;

if (fabs(C0a[3][3]) < 0.001) psi_A = M_PI*0.5 ;

 

tet_A = -atan2(C0a[3][1],C0a_A[3][3]);

 

KURa_A = atan2(C0a[1][2], C0a[2][2]);

if (KURa_A < 0.0) KURa_A += 2.*M_PI;

 

 

это будет как бы трехосный свободный гироскоп

Share this post


Link to post
Share on other sites
поторопилься сорри

 

у вас тут ошибки одни как мне кажеться

 

Fx, Fy, Fz - интегралы от гирскопов, то

 

1. Интегралы от гироскопов это подразумевается угловая скорость помноженная на dt?

 

2.Тут тоже непонятно:

 

FF2=sqrt(Fx^2+Fy^2+Fz^2)

if(FF2=0) FF3=0.5

else FF3=sin(FF2*0.5)/FF2

 

Это вроде как нормирующий множитель, но непонятно откуда sin взялся.

 

3. Нормируем интегралы скоростей (если верно п.1), а у Вас написано что получаем кватернион:

H[0]=cos(FF2*0.5);

H[1]=FF3*Fx;

H[2]=FF3*Fy;

H[3]=FF3*Fz;

 

Умножаем на существующий кватернион на получившийся трехмерный нормированный угол:

 

//Матрица направляющих косинусов связывающая ГСК и объект (это кусок из кода в место L0a[0] подстваить Н[0] и так далее )

C0a[2][2] = L0a[0]*L0a[0]+ L0a[2]*L0a[2]- L0a[1]*L0a[1]- L0a[3]*L0a[3];

C0a[1][2] = 2.*(L0a[1]*L0a[2] + L0a[0]*L0a[3]);

C0a[3][1] = 2.*(L0a[1]*L0a[3] + L0a[0]*L0a[2]);

C0a[3][2] = 2.*(L0a[2]*L0a[3] - L0a[0]*L0a[1]);

C0a[3][3] = L0a[0]*L0a[0]+ L0a[3]*L0a[3]- L0a[1]*L0a[1]- L0a[2]*L0a[2] ;

 

Если математика кватернионная почему 5 параметров? У меня все сделано вроде как по науке:

 

gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors

gy *= (0.5f * (1.0f / sampleFreq));

gz *= (0.5f * (1.0f / sampleFreq));

qa = q0;

qb = q1;

qc = q2;

q0 += (-qb * gx - qc * gy - q3 * gz);

q1 += (qa * gx + qc * gz - q3 * gy);

q2 += (qa * gy - qb * gz + q3 * gx);

q3 += (qa * gz + qb * gy - qc * gx);

 

 

На входе кватернион и на выходе, 4 параметра на входе помножаем на угловые скорости, на dt на 0.5 и получаем выходные 4 параметра.

 

Правило умножения взято из книжки(см рис), умножение кватернионов это я та понимаю и есть поворот одного на другой, если можно так выразится. В Вашем файле собственно тоже самое (см. второй рис) - умножаем кватернион H на омега (только почему на омега? Надо вроде как на омега*dt?). Ну и углы ориентации это вторично, главное чтобы кватернион считался правильно. Меня это и интересует. Правильно ли считаю? Углы ориентации то можно и не считать в основном цикле, изредка делать преобразование кватерниона в углы и все.

 

post-15968-1379623124_thumb.jpg

post-15968-1379623128_thumb.jpg

Share this post


Link to post
Share on other sites

1. да это так

 

 

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

 

3. это не параметры кватерниона а значения матрицы направляющих косинусов между ГСК и ССК, элементы матрицы составлены из параметров кватерниов, т.е. переход от парметров кватерниона к углам

 

4. на омега это дифференциальное уравнение см. выше

 

 

Share this post


Link to post
Share on other sites

Наверно порядок решателя не достаточен. Попробуйте шаг уменьшить для начала, если будет лучше можно перейти на методы более высокого порядка, чем метод Эйлера.

Share this post


Link to post
Share on other sites

Если Вы работаете с микромеханикой, то там желательно определять нули и коэффициенты пропорциональности. Причем все это плывет от температуры, питания и т.п. и эти параметры нужно подстраивать во времени. Получается что-то типа

mesCorrected0[0]=( mes[0]-gd.sensors.grs0[0])*gd.sensors.grs1[0];

mesCorrected0[1]=( mes[1]-gd.sensors.grs0[1])*gd.sensors.grs1[1];

mesCorrected0[2]=( mes[2]-gd.sensors.grs0[2])*gd.sensors.grs1[2];

а уже дальше пошла математика кватернионов.

Пару лет назад реализовывал БИНС на самых дешевых датчиках. Можно, работает, но то еще удовольствие. Хотя есть и плюсы: модели движения можно взять погрубее ;).

При работе с низкоточными датчиками Эйлера обычно хватает, т.к. все-равно идет постоянная коррекция по GNSS, а в периоды автономной работы все определяется качеством датчиков.

ИМХО.

PS. К чему я это. Нуль выставить легко в покое и повернув датчик вокруг оси и вернув назад, вы получите 0, причем погрешность за неточность коэффициента прапорционалоьности ушла, но если движения более сложные, то в общем случае это не так. Так что дополнительно, минимум, нужно проверить, что поворачивая датчик на угол A мы этот A и наинтегрировали, а не 0.98A или 1.01A. Это достаточно легко сделать и на столе, взяв A кратным 90 гр. В реальных системах коррекцию проволят по внешней информации, включая параметры датчиков в число неизвестных.

Edited by ibk66

Share this post


Link to post
Share on other sites

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

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now
Sign in to follow this