0

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

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

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

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);
}
4

1 回答 1

0

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

那里的例子终于是我一直在寻找的。一个有据可查的使用 mpu9150 的方法。我花了一些时间来让这件事按照我想要的方式运行。

于 2014-07-01T23:09:56.183 回答