使用 9 轴 IMU 将加速度双重积分到位置

Question

First post on this forum, hope I'm doing this right. I know there have been several threads on double integration of acceleration in the past, and I know about the errors inherent in an accelerometer that isn't a 200k+ military grade sensor. Fortunately my purposes I just need it to be approximately correct (+/- 3 inches) for no longer than ten seconds.

I'm almost there already. I am using the linear acceleration off a bno055 IMU. I'm sampling at a rate of 50hz (every 20ms). Every time I sample, I use basic trapezoidal integration to move from acc to velocity and velocity to position. I have a "discrimination window" to throw out at-rest error, and a "movement end detect" code to set the velocity back to 0 after the acceleration is 0 for a given amount of counts.

It's working after a fashion, I just need it to work a little bit better. I'm seeing some really odd kickback where I move the accelerometer and the position moves with it pretty correctly until I stop, then the position "kicks back" by several inches - sometimes nearly back to where I started. Brought in a friend much smarter than I am and he recommended I integrate smarter, using 4 or 5 data points instead of the last two that I use in trapezoidal integration.

So my question: How can I use the last four or five data points to integrate more precisely than basic trapezoidal integration? I tried looking into Euler and RK4 but it's been a long time since I've done higher-level math and I didn't know where to start. If someone could explain rather simply, that would be awesome. Thanks. For background, this code is all running on a microcomputer so I can't run the data through matlab.

PS. I was also recommended using a high-pass filter, but again, when I tried to start reading up on digital high pass filters I just couldn't make sense of it. I thought I'd start with the smarter integration and see what that does.

Answer 1

Although the OP has undoubtedly solved this problem in the two years since posting, I am responding because I encountered the same issue and discovered the solution after reading this question.

For the OP's purpose the trapezoidal integration or even a midpoint estimate of integration would work fine. More complex or more accurate integrations are not necessary for the curves representing this kind of simple motion and will not help with this particular problem.

The problem described is that the position is tracked properly for a period of time, but the position jumps backwards when the device stops moving. This behavior occurs because of improper use of velocity damping.

When the device is moved and then stopped there is a momentary acceleration and then some period of constant velocity motion (zero acceleration) and then a period of deceleration (decreasing velocity) until the device is once again at rest.

The OP reports using

"movement end detect" code to set the velocity back to 0 after the acceleration is 0 for a given amount of counts.

The OPs problem was very likely that during the period of constant velocity motion, the acceleration was zero so the velocity was artificially reset to zero in this attempt at a noise-reducing (dampening) algorithm. The subsequent deceleration then causes a negative velocity for some period of time, returning the calculated position almost all the way back to the starting point.

If tracking small discrete movements, one solution to this particular problem is to detect and track the cycle of acceleration -> coasting -> deceleration and to apply dampening algorithms only after the deceleration part of the cycle has completed.