Вычисление отклонения от курса по магнитометру и акселерометру

Я использую Arduino и датчик с 9 степенями свободы (гироскоп, акселерометр и магнитометр) и пытаюсь использовать угол наклона, крена и рыскания, которые датчик дает мне, чтобы вращать объект в единстве.

Мне удалось правильно вычислить тангаж и крен (ось z и x в единице) по акселерометру, но, похоже, я не могу правильно рассчитать рысканье. Под этим я подразумеваю, что когда я поворачиваю датчик по тангажу или крену, он странным образом вращает и мое отклонение.

Код в Arduino для получения заголовка

void getHeading(void)
{
  heading=180*atan2(Mxyz[0],Mxyz[1])/PI;
  if(heading <0) heading +=360;
}

void getTiltHeading(void)
{
  //float шаг = asin(-Axyz[0]);
  //floatroll = asin(Axyz[1]/cos(pitch));
  float pitch = atan( Axyz[0] / sqrt( Axyz[1] * Axyz[1] + Axyz[2] * Axyz[2] ) );
  float roll = atan( Axyz[1] / sqrt(Axyz[0] * Axyz[0] + Axyz[2] * Axyz[2]));

  float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
  float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
  float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
  tiltheading = 180 * atan2(yh, xh)/PI;
  if(yh<0)    tiltheading +=360;
}

Подача и перекат

    float _Pitch  = (float)(180 / Math.PI * Math.Atan( m_ResultX / Math.Sqrt( m_ResultY * m_ResultY + m_ResultZ * m_ResultZ ) ) );
    float _Roll = (float)(180 / Math.PI * Math.Atan(m_ResultY / Math.Sqrt(m_ResultX * m_ResultX + m_ResultZ * m_ResultZ)));
    float _Yaw = (float)(m_TiltHeadingResult);

Не стесняйтесь спрашивать подробности.

, 👍6

Обсуждение

ты понял, почему твое рыскание меняется, если ты наклоняешь/качаешь датчик? и как это решалось? любая информация мне тоже поможет., @ems


1 ответ

Лучший ответ:

3

Я не знаю, как выровнены оси всех ваших датчиков, но похоже, что вам следует выровнять показания магнитометра, вращая их на -pitch и -roll, чтобы вернуться в глобальную систему отсчета, вместо того, чтобы вращать показания магнитометра на положительные углы.

Однако углы тангажа и крена в коде оба являются относительными к горизонту, а не набором углов Эйлера, которые определяют вращение между двумя состояниями; из-за этого они не составляются для поворота магнитометра обратно в глобальную систему координат xy, как я предполагаю, что уравнения пытаются сделать. Код также использует только atan для тангажа и крена вместо atan2, поэтому их диапазон составляет всего 180 градусов.

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

Чтобы сделать это правильно, я рекомендую вам ознакомиться с углами Эйлера. Вы также можете попробовать поискать хорошую библиотеку, которая обрабатывает для вас слияние датчиков с 9 степенями свободы.

,

Я немного читал об углах Эйлера, но моя математика не так хороша, и у меня не было времени на дальнейшее исследование, но мне удалось найти очень хорошее слияние датчиков с 9 степенями свободы. Теперь все в порядке., @dnotol

Не могли бы вы предоставить ссылку на использованное вами сочетание датчиков?, @linhartr22

Смотрите также: