集成IMU加速度计数据以估计位置

huangapple go评论53阅读模式
英文:

Integrating IMU accelerometer data to estimate position

问题

我试图使用MPU6050 IMU来估计位置变化。我知道漂移是个问题,但我只是尝试在少于几秒的短时间内完成这个过程。

我使用了传感器融合的马德格威克滤波器来校正重力影响,调整后的加速度读数看起来不错。但当我对它们进行双重积分时,得到的位置非常非常小。

你对我可能做错了什么有什么见解吗?

VectorFloat getAccelerationNoGravity() {
  // 代码内容...
}

这导致了如下读数:

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

但是我移动了IMU超过50厘米,所以结果似乎差了几个数量级。

再次强调,我的调整后的加速度读数是有意义的,因此可能在实现运动学时犯了一个错误。

英文:

I am trying to estimate position change using an MPU6050 IMU. I know that drift is a problem, but I am only trying to do this over short periods of less than a few seconds.

I've adjusted for gravity using sensor fusion madgwick filter and after adjustment my acceleration readings look good, but when I double integrate them, the resulting positions are very very small.

Any insight into what I might be doing wrong?

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

This results in readings like this

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

but I am moving the IMU more than 50 cm so it seems the result is orders of magnitude off.

Again, my adjusted accelerometer readings make sense so perhaps I made a mistake implementing the kinematics?

答案1

得分: 1

我没有足够的声誉来发表评论,所以以回答的方式写下来,但实际上这是一条评论。我认为你的 deltat 应该是 1/sampling_rate(例如,如果你得到的是100Hz的数据,那就是1/100)。

英文:

I don't have enough reputation to comment that's why writing as an answer but this is actually a comment. I think your deltat should be 1/sampling_rate (ex: 1/100 if you get 100Hz data).

答案2

得分: 0

我的操作顺序有误 

currPos.x += v0x * deltat + 0.5 * accAdj.x * deltat_sq * 100;


应该为

currPos.x += (v0x * deltat + 0.5 * accAdj.x * deltat_sq) * 100;


<details>
<summary>英文:</summary>

My order of operations was off 

currPos.x += v0x * deltat + 0.5 * accAdj.x * deltat_sq * 100;


should be

currPos.x += (v0x * deltat + 0.5 * accAdj.x * deltat_sq) * 100;


</details>



huangapple
  • 本文由 发表于 2023年2月23日 20:09:14
  • 转载请务必保留本文链接:https://go.coder-hub.com/75544641.html
匿名

发表评论

匿名网友

:?: :razz: :sad: :evil: :!: :smile: :oops: :grin: :eek: :shock: :???: :cool: :lol: :mad: :twisted: :roll: :wink: :idea: :arrow: :neutral: :cry: :mrgreen:

确定