Гироскоп - помогите, как туда притянули акселерометр? Опции

[alexPec]

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

Господа математики, не могу понять как в расчете положения (динамического) может участвовать акселерометр (магнитомера у меня не будет).
Есть там (проект freeIMU) функция:

Код

void  imu_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { //gx,gy,gz - угловые ускорения, рад/сек
  float recipNorm;                                                                                                                                          //ax,ay,az - линейные ускорения
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f;
  float halfey = 0.0f;
  float halfez = 0.0f;
  float qa, qb, qc;

  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;

  // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
  if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {                                                          //Магнитомер не интересует
    float hx, hy, bx, bz;
    float halfwx, halfwy, halfwz;

    // Normalise magnetometer measurement
    recipNorm = imu_invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;

    // Reference direction of Earth's magnetic field
    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    bx = sqrt(hx * hx + hy * hy);
    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

    // Estimated direction of magnetic field
    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);

    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex = (my * halfwz - mz * halfwy);
    halfey = (mz * halfwx - mx * halfwz);
    halfez = (mx * halfwy - my * halfwx);
  }

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;

    // Normalise accelerometer measurement                     Нормируем вектор ускорения
    recipNorm = imu_invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity  //через направляющие косинусы вычисляем вектор гравитации
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;

    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);     //Непонятно зачем комбинируем вектор гравитации и измеренные ускорения!!!!
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {                        
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);  //Интегрируем по времени и добавляем к угловым ускорениям - зачем!!!!!!
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);  //Самое непонятное это!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      gx += integralFBx;  // apply integral feedback          
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

  // 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;
}

Делалось все это как я понял по этой книге (вложение)

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

Конкретно непонятно это (кусок взят из кода выше):

Код

    halfex += (ay * halfvz - az * halfvy);     //Непонятно зачем комбинируем вектор гравитации и измеренные ускорения!!!!
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {                        
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);  //Интегрируем по времени и добавляем к угловым ускорениям - зачем!!!!!!
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);  //Самое непонятное это!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      gx += integralFBx;  // apply integral feedback          
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

Прикрепленные файлы

 An_efficient_orientation_filter_for_inertial_and_inertialmagnetic_sensor_arrays.pdf ( 1.47 мегабайт ) Кол-во скачиваний: 83

 

[MrAlex]

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

[alexPec]

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

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

Гироскоп не измеряет углы положения в пространстве, а только их приращения, угловые скорости.

Это датчик, а я подразумеваю под гироскопом систему датчик - программа. Так вот как раз интегрируя их (данные с датчика) во времени через кватерионы и получают текущие углы ориентации в пространстве. По крайней мере я всегда так думал.

Вот например, софт от free imu arduino:



Неужели никто не задумывался над математикой?

[deeper_79]

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




Это датчик, а я подразумеваю под гироскопом систему датчик - программа. Так вот как раз интегрируя их (данные с датчика) во времени через кватерионы и получают текущие углы ориентации в пространстве. По крайней мере я всегда так думал.

Вот например, софт от free imu arduino:



Неужели никто не задумывался над математикой?

есть смысл почитать во это http://www.twirpx.com/file/126419/?rand=2257909
это хотя для моря, но можно продолжить и на авиацию
попробую кратко описать алгоритм работы гировертикали построенной на похожем модуле:
1. По гироскопам решая уравнение Пусаона находим кватернион определяющий ориентацию связанной системы координат(ССК) и инерциальной (неподвижной относительно звезд) системой координат(ИСК).
2. Интегрируем показания акселерометров получаем скорости в связанной системе координат
3. Вводим кватернион который определяет ориентацию ИСК относительно географической системы координат (ГСК), неподвижной относительно Земли (на начальном этапе единичный)
4. Перемножаем к. из п.1 и п.3
5. используя кватернион в п.3 проектируем линейные скорости в ГСК (естественно в начале получаем ошибочные должен быть вектор [0 0 0], линейное перемещение объекта пока опускаю)
6. По полученную разницу в п.5 подаем на фильтры (фильтры это отдельная БОЛЬШАЯ тема) и формируем кватернион коррекции который умножаем кватернион в п.3 (замыкаем обратную связь)

как мне кажется в вашем случае скорости как раз из пункта 2 и 4. Это если очень кратко, а так бесплатформенные гировертикали это отдельная большая тема
может проще вы скажете чего вам надо найти? есть кое какие примеры в матлабе

[alexPec]

попробую кратко описать алгоритм работы гировертикали построенной на похожем модуле:
1. По гироскопам решая уравнение Пусаона находим кватернион определяющий ориентацию связанной системы координат(ССК) и инерциальной (неподвижной относительно звезд) системой координат(ИСК).
2. Интегрируем показания акселерометров получаем скорости в связанной системе координат
3. Вводим кватернион который определяет ориентацию ИСК относительно географической системы координат (ГСК), неподвижной относительно Земли (на начальном этапе единичный)
4. Перемножаем к. из п.1 и п.3
5. используя кватернион в п.3 проектируем линейные скорости в ГСК (естественно в начале получаем ошибочные должен быть вектор [0 0 0], линейное перемещение объекта пока опускаю)
6. По полученную разницу в п.5 подаем на фильтры (фильтры это отдельная БОЛЬШАЯ тема) и формируем кватернион коррекции который умножаем кватернион в п.3 (замыкаем обратную связь)

Спасибо за ответ, очень похоже. Только в этих алгоритмах вижу пока что ССК - объект и ИСК (подразумеваю ГСК). Читаю статейку которую прикладывал уже раз 15-й, начинаю втягиваться.

может проще вы скажете чего вам надо найти? есть кое какие примеры в матлабе

В идеале надо бы на ходу корректировать плывущие вертикаль и курс (в следствие дрейфа нулей гироскопа), т.е. компенсировать дрейфы гироскопа по показаниям акселерометра и магнитомера. Магнитомера на плате не имею, но похоже нужен будет.

есть смысл почитать во это http://www.twirpx.com/file/126419/?rand=2257909
это хотя для моря, но можно продолжить и на авиацию

Спасибо, почитаю, а пока накопившиеся вопросы, похоже Вы знаете на них ответы (в голове пока путаница, поэтому отнеситесь терпеливо к глупым вопросам ):

1. Приводил уже где-то алгоритм на картинке. Переосмыслил. Но до конца пока не понял, что он нам дает, что мы им добиваемся. Коррекцию вертикали? Постоянную? Если не постоянную, то при каких условиях? Или тут ситуация такая: чем стабильнее показания акселерометра (меньще дерганий объекта), тем точнее корректируем вертикаль? А как себя будет вести эта коррекция, если аксерерометр постоянно будет находится в трехмерном хаотическом движении (на модели самолета)?

2. Правильно ли понимание алгоритма: алгоритм решает задачу оптимизации, оптимизация - минимизация функции (2-я картинка), здесь в функции 1-й член - кватерион ориентации ССК(объект) относительно ИСК(земля) , второй - вектор поля (условно, подразумевается поле гравитации+поле магнитное земли, т.е. поле, трехмерно определяющее направление ИСК, третий - кватерион ориентации ИСК относительно ССК. Произведение всего этого дает нам (используя текущие углы гироскопа) положение ИСК относительно ССК, вычитаем из этого положения измеренное положение ИСК в ССК (акселерометрами и магнитомерами). Эту разность и минимизируем. Минимизируем за счет коррекции текущих углов гироскопа.

3. Минимизируем так: умножаем целевую функцию (картинка2) на матрицу Якоби - находим многомерный градиент, затем нормируем его, и с каким-то коэффициентом бета вычитаем из текущих угловых скоростей - я думал корректируем углы? или так и корректируем плавно за счет коррекции скоростей угловых?

Ну а с нижней частью алгоритма вроде все ясно: кватерион текущей ориентации ССК относительно ИСК домножаем итерационно на угловые скорости, интегрируем по времени численно, получаем новый кватерион ориентации.

Вникаю дальше, зреют новые вопросы...

И да, вся математика там построена на кватерионах.

Эскизы прикрепленных изображений

 

[deeper_79]

1. Приводил уже где-то алгоритм на картинке. Переосмыслил. Но до конца пока не понял, что он нам дает, что мы им добиваемся. Коррекцию вертикали? Постоянную? Если не постоянную, то при каких условиях? Или тут ситуация такая: чем стабильнее показания акселерометра (меньще дерганий объекта), тем точнее корректируем вертикаль? А как себя будет вести эта коррекция, если аксерерометр постоянно будет находится в трехмерном хаотическом движении (на модели самолета)?

2. Правильно ли понимание алгоритма: алгоритм решает задачу оптимизации, оптимизация - минимизация функции (2-я картинка), здесь в функции 1-й член - кватерион ориентации ССК(объект) относительно ИСК(земля) , второй - вектор поля (условно, подразумевается поле гравитации+поле магнитное земли, т.е. поле, трехмерно определяющее направление ИСК, третий - кватерион ориентации ИСК относительно ССК. Произведение всего этого дает нам (используя текущие углы гироскопа) положение ИСК относительно ССК, вычитаем из этого положения измеренное положение ИСК в ССК (акселерометрами и магнитомерами). Эту разность и минимизируем. Минимизируем за счет коррекции текущих углов гироскопа.

3. Минимизируем так: умножаем целевую функцию (картинка2) на матрицу Якоби - находим многомерный градиент, затем нормируем его, и с каким-то коэффициентом бета вычитаем из текущих угловых скоростей - я думал корректируем углы? или так и корректируем плавно за счет коррекции скоростей угловых?

Ну а с нижней частью алгоритма вроде все ясно: кватерион текущей ориентации ССК относительно ИСК домножаем итерационно на угловые скорости, интегрируем по времени численно, получаем новый кватерион ориентации.

Вникаю дальше, зреют новые вопросы...

И да, вся математика там построена на кватерионах.
[/quote]

1. В алгоритме который я приводил выше я указал что опускаю линейное перемещение объекта. Что бы алгриртм работал на линейном движении необходимо наличие внешней линейной скорости от спутников либо лага для моря, ну вообщем что то меряет скорость. Если ее нет то енто есть скоростная погрешность гировертикали. Если есть из полученных интегралов от акселерометров вычитаеться скорость полученная по внешней информации разложенная на соответсаующие оси.
Коррекция должна быть постояной, постоянная времени коррекции (фильтра) выбираеться исходя из дрейфа гисрокопа, для микромеханики это где то 10-15с, для более точных гироскопов 50с и выше, т.е. все высокочастоное будет вырезано.
2 и 3 вопрос я не готов так сходу подумаю может отвечу

есть еще одна статья моя во вложении

Прикрепленные файлы

 ______________________________________2.rar ( 1.58 мегабайт ) Кол-во скачиваний: 140