基于MPU6050的IMU加速度积分位置估计结果异常排查

尝试用MPU6050 IMU估计短周期（几秒内）的位置变化，已知漂移问题但暂不考虑。已通过Madgwick融合滤波补偿重力，调整后的加速度读数看似正常，但双积分得到的位置结果极小——实际移动IMU超过50cm，结果差数个数量级，怀疑运动学实现有误。

相关代码 #

VectorFloat getAccelerationNoGravity() {
  frame++;
  auto timeSinceLastSample = ReadTime(); 

  /**
   * Get the current acceleration readings compensated for gravity
   * 1. Get raw readings from acc & gyro
   * 2. Feed into madgwick filter, get quaternion
   * 3. Get gravity vector from quaternion
   * 4. subtract gravity from raw acceleration readings
   */

  // 1. Get raw readings from acc & gyro
  sensors_event_t acc, gyro, temp;
  mpu.getEvent(&acc, &gyro, &temp);

  // 2. Feed into madgwick filter, get quaternion
  float x = acc.acceleration.x;
  float y = acc.acceleration.y;
  float z = acc.acceleration.z;
  float gx = gyro.gyro.x;
  float gy = gyro.gyro.y;
  float gz = gyro.gyro.z;
  float deltat = timeSinceLastSample / 1000000.0;

  madgwickQuaternionUpdate(q, x, y, z, gx, gy, gz, deltat);
  Quaternion quat = Quaternion(q[0], q[1], q[2], q[3]);

  // 3. Get gravity vector from quaternion
  VectorFloat gravity = getGravity(&quat); // returns percentages of gravity

  // 4. subtract gravity from raw acceleration readings
  VectorFloat accAdj = getLinearAcceleration(VectorFloat(x, y, z), gravity);

  // 5. calculate bias & adjust
  calibrateBias(accAdj); // average of x samples
  accAdj.x -= bias.x;
  accAdj.y -= bias.y;
  accAdj.z -= bias.z;

  // calc position
  auto accMagnitude = accAdj.getMagnitude();
  if (biasComputed /*&& accMagnitude > 0.1*/) {
    // v0 (initial velocity) = v
    auto v0x = currVel.x;
    auto v0y = currVel.y;
    auto v0z = currVel.z;

    // currVel (current velocity) = v0 + a * t
    currVel.x = v0x + deltat * accAdj.x;
    currVel.y = v0y + deltat * accAdj.y;
    currVel.z = v0z + deltat * accAdj.z;

    // delta_x = v0 * t + 1/2 * a * t^2 * 100 (m -> cm)
    float deltat_sq = deltat * deltat;
    currPos.x += v0x * deltat + 0.5 * accAdj.x * deltat_sq * 100;
    currPos.y += v0y * deltat + 0.5 * accAdj.y * deltat_sq * 100;
    currPos.z += v0z * deltat + 0.5 * accAdj.z * deltat_sq * 100;
  }

  t_compute = micros() - t_compute;
  if (biasComputed && frame % 50 == 0) {
    Serial.printf("%f,%f,%f\n", accAdj.x, accAdj.y, accAdj.z);
  }

  return accAdj;
}

加速度输出数据 #

0.117377,0.135253,0.010953
0.117308,0.133007,0.010974
0.117446,0.129459,0.010982
0.117550,0.125331,0.010972
0.117732,0.120880,0.010971
0.117961,0.115567,0.011018
0.118101,0.111308,0.011070
0.118161,0.112330,0.011132
0.118256,0.114275,0.011229
0.118401,0.116992,0.011322

排查建议 #

• 检查加速度单位转换：确认acc.acceleration的单位是g还是m/s²。从输出的0.1左右数值判断，大概率是g单位未转换为m/s²（需乘以9.8），直接用会导致加速度值缩小10倍，积分后位置差一个数量级。
• 验证时间采样准确性：打印deltat数值，确认采样间隔是否符合实际频率（比如100Hz对应0.01s）。如果ReadTime()返回值错误，或deltat计算时单位转换出错（如误将微秒转成秒时多除1000），会过度缩小积分项。
• 校准逻辑验证：确认calibrateBias()是否在IMU静止时完成校准，biasComputed标志是否正确触发。若校准未完成就开始积分，或bias值计算错误，会错误抵消实际运动的加速度。
• 重置积分初始值：每次开始运动前，需将currVel和currPos重置为0。若初始值不为0，会导致积分结果偏移，掩盖实际运动的位置变化。
• 坐标系匹配检查：确认Madgwick滤波的坐标系与IMU原始坐标系一致，getGravity()和getLinearAcceleration()是否正确抵消重力分量。坐标系不匹配会导致运动加速度被错误补偿。
• 打印中间变量定位问题：实时打印currVel和currPos数值，对比accAdj的变化，判断是速度累积错误还是位置计算错误。

内容的提问来源于stack exchange，提问作者Paulius Velesko