[alexPec 3]

Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 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;

[deeper_79 0]

Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 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;

 

 

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

[alexPec 3]

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

 

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

 

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?). Ну и углы ориентации это вторично, главное чтобы кватернион считался правильно. Меня это и интересует. Правильно ли считаю? Углы ориентации то можно и не считать в основном цикле, изредка делать преобразование кватерниона в углы и все.

 

[deeper_79 0]

1. да это так

 

 

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

 

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

 

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

 

 

[amaora 20]

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

[ibk66 0]

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

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 гр. В реальных системах коррекцию проволят по внешней информации, включая параметры датчиков в число неизвестных.

Изменено 6 октября, 2013 пользователем ibk66

[OldMann 0]

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