Question

我目前正在使用我的Arduino项目中的MPU9150Lib来读取我的IMU数据（当然是MPU9150）。到目前为止效果很好。但有时候我注意到了一些小故障。我认为这是一个万向节锁问题。当我查看库时，似乎它们融合了数据，然后简单地将得到的欧拉角转换为四元数。这会导致万向节锁吗？如果是这样，你们可以帮我改写代码吗？我真的不明白数据融合背后的数学。所以我不知道如何更正库。

你可以在这里找到它： https://github.com/zarthcode/MPU9150Lib/tree/master/libraries/MPU9150Lib

该库包含以下功能。我不确定问题是否在函数内，但对我来说它看起来很奇怪，因为它最终将euler坐标转换为四元数。

void MPU9150Lib::dataFusion()
{
  float qMag[4];
  float deltaDMPYaw, deltaMagYaw;
  float newMagYaw, newYaw;
  float temp1[4], unFused[4];
  float unFusedConjugate[4];

  // *** NOTE *** pitch direction swapped here

  m_fusedEulerPose[VEC3_X] = m_dmpEulerPose[VEC3_X];
  m_fusedEulerPose[VEC3_Y] = -m_dmpEulerPose[VEC3_Y];
  m_fusedEulerPose[VEC3_Z] = 0; 
  MPUQuaternionEulerToQuaternion(m_fusedEulerPose, unFused);    // create a new quaternion

  deltaDMPYaw = -m_dmpEulerPose[VEC3_Z] + m_lastDMPYaw;         // calculate change in yaw from dmp
  m_lastDMPYaw = m_dmpEulerPose[VEC3_Z];                        // update that

  qMag[QUAT_W] = 0;
  qMag[QUAT_X] = m_calMag[VEC3_X];
  qMag[QUAT_Y] = m_calMag[VEC3_Y];
  qMag[QUAT_Z] = m_calMag[VEC3_Z];

  // Tilt compensate mag with the unfused data (i.e. just roll and pitch with yaw 0)

  MPUQuaternionConjugate(unFused, unFusedConjugate);
  MPUQuaternionMultiply(qMag, unFusedConjugate, temp1);
  MPUQuaternionMultiply(unFused, temp1, qMag);

  // Now fuse this with the dmp yaw gyro information

  newMagYaw = -atan2(qMag[QUAT_Y], qMag[QUAT_X]);

  if (newMagYaw != newMagYaw) {                                 // check for nAn
#ifdef MPULIB_DEBUG
    Serial.println("***nAn\n");
#endif
    return;                                                     // just ignore in this case
  }
  if (newMagYaw < 0)
    newMagYaw = 2.0f * (float)M_PI + newMagYaw;                 // need 0 <= newMagYaw <= 2*PI

  newYaw = m_lastYaw + deltaDMPYaw;                             // compute new yaw from change
  if (newYaw > (2.0f * (float)M_PI))                            // need 0 <= newYaw <= 2*PI
    newYaw -= 2.0f * (float)M_PI;
  if (newYaw < 0)
    newYaw += 2.0f * (float)M_PI;

  deltaMagYaw = newMagYaw - newYaw;                             // compute difference
  if (deltaMagYaw >= (float)M_PI)
    deltaMagYaw = deltaMagYaw - 2.0f * (float)M_PI;
  if (deltaMagYaw <= -(float)M_PI)
    deltaMagYaw = (2.0f * (float)M_PI + deltaMagYaw);

  newYaw += deltaMagYaw/4;                                      // apply some of the correction

  if (newYaw > (2.0f * (float)M_PI))                            // need 0 <= newYaw <= 2*PI
    newYaw -= 2.0f * (float)M_PI;
  if (newYaw < 0)
    newYaw += 2.0f * (float)M_PI;

  m_lastYaw = newYaw;

  if (newYaw > (float)M_PI)
    newYaw -= 2.0f * (float)M_PI;

  m_fusedEulerPose[VEC3_Z] = newYaw;                            // fill in output yaw value

  MPUQuaternionEulerToQuaternion(m_fusedEulerPose, m_fusedQuaternion);
}

Answer 1

我能够解决问题。我找到了另一个上传到mpu9150的sparkfun分线板的库。我也无法让这个工作，但出于其他原因。原始数据非常嘈杂，我无法通过调整选项来修复它。但是在库中附带的示例是数据融合的功能。我将其复制到MPU9150Lib并调整变量。所以基本上我现在使用MPU9150Lib和sparkfun分线板的数据融合算法。现在一切都像魅力一样。你可以在这里找到sparkfun lib： https://github.com/sparkfun/MPU-9150_Breakout/tree/master/firmware/MPU6050/Examples

这里的例子最终是我一直在寻找的东西。记录良好的使用mpu9150的方法。花了我一些时间来完成这个事情并以我想要的方式运行。

MPU9150Lib和万向节锁？

1 个答案: