Calculating displacement using Accelerometer and Gyroscope (MPU6050)

Question

I am a computer science student and working on an electronics project that requires to calculate yaw, pitch, roll and X,Y,Z displacement. I want to attach an IMU in a gun and track its orientation and displacement. I am able to get Yaw, Pitch and Roll but unfortunately cant understand how to calculate displacement or position of my gun. I am using a 10-DOF GY-87 sensor that contains MPU-6050.

I am getting values in term of g and m/s2 format. From the research that i have studied yet is that i need to get acceleration/time2 and then add all the values. But cant understand what time difference i should use. Refrence: How to calculate distance based on phone acceleration

#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
float dx, dy, dz = 0;
int16_t gx, gy, gz;





#define LED_PIN 13
bool blinkState = false;

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(38400);

    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    Serial.println("Updating internal sensor offsets...");

    accelgyro.setXGyroOffset(85);
    accelgyro.setYGyroOffset(1);
    accelgyro.setZGyroOffset(-4);
    accelgyro.setXAccelOffset(-4269);
    accelgyro.setYAccelOffset(-4836);
    accelgyro.setZAccelOffset(1080);

    pinMode(LED_PIN, OUTPUT);
}

void loop() {

        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

        dx=dx+(float)(((float)ax/(float)16384)*9.8*0.05*0.05);
        dy=dy+(float)(((float)ay/(float)16384)*9.8*0.05*0.05);
        dz=dz+(float)(((float)az/(float)16384)*9.8*0.05*0.05);
        Serial.print(dx); Serial.print("\t");
        Serial.print(dy); Serial.print("\t");
        Serial.print(dz); Serial.print("\t\n");


delay(1000);
    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

I want to track object as shown in the following youtube videos.

http://www.youtube.com/watch?v=ZYyyaJgKsDg

I would be grateful to you if anyone of you can guide me in this Regards. Thank you

P.S: Sorry for my bad english and use of non-technical terms.

Solution 1

I am afraid that the answer is not one you will want to hear. It is very, very hard to calculate position from a IMU unit. This video from Google is a very good reference for why (go to minute 24 for a detailed explanation). Basically, you need to integrate acceleration twice to get to position. You also need to remove gravity from the acceleration seen by your IMU. If this isn't done perfectly, the errors add up really fast.

The video you referenced used the information that the ball was rolling on the table to inform their model. They could track the orientation of their sensor to know which way the ball was rolling. They used the radius of the ball along with the angular changes from their board to track the ball in x and y. If you picked their ball off the table it wouldn't work at all.

If you need to track something, you should look for some sensor that can give you information on the position of your object (GPS, video analysis). Then you can use a Kalman filter to combine that with the IMU data to get good positional accuracy.

Good luck with your project.

Solution 2

I know that this an old Post but I thought I would post some corrections from experiments that I've done with the MPU 6050 and arduino.

First off, the equation your using to find displacement is incorrect, You need to use a kinematics equation. however, the equation Xf = 1/2at^2 + Vot + Xo is also incorrect because it is ONLY for CONSTANT accelerations. In this case the acceleration is CHANGING, so you can either take the average acceleration between two dataset points and plug it into the previous equation or use the following equation:

Xf = 1/4(Af + Ao)t^2 + Vot + Xo

Where Xf is the final distance in meters, Af is the final CURRENT acceleration in m/s^2, Ao is the previous acceleration of the last data set in m/s^2, t is the CHANGE in time BETWEEN Af and Ao sets of data in SECONDS, Vo is the instantaneous velocity of the last data set in m/s, and Xo is the final distance of the last dataset or the sum of all the previous distances in meters. Vo needs to be calculated using the previous acceleration and the acceleration from two previous datasets ago or Ao-1 using the following kinematics equation:

Vo = 1/2(Ao + Ao-1)*t + Vo-1

Where Vo is the previous instantaneous velocity in m/s, Ao is the previous acceleration in m/s^2, Ao-1 is the acceleration from two datasets ago in m/s^2, t is the change in time between Ao and Ao-1 datasets in SECONDS, and Vo-1 is the instantaneous velocity in m/s of the Ao-1 data set or two datasets ago.

Secondly you need to use a more reliable clock. I recommend using the micros() function and remember that t is the CHANGE in time between datasets. I'm not sure of it's reliability but it's the best I can think of. MAKE SURE to convert from microseconds to SECONDS when using the stated equations.

Third, I recommend that you calibrate your offsets every so often or even every time at the very beginning of your code by combining your code with a calibrating sketch such as that of Luis Ródenas. You can place it at the setup() routine and can use a small buffersize value or datasets like 200 or 300 to make sure you don't wait too long between experiments.

Fourth, you can either operate using an average between two dataset accelerations(which is what we're doing above) or take it a step further and use an average of various datasets such as using a fifo buffer array to store various acceleration values and take an average of all of the values in the buffer. Fifo buffers require that a set number of values remain in them at all times but as a new value goes in, an older one leaves. The larger the fifo, the more inaccurate your distance calculations will be but the fifo buffer will allow outlier acceleration values from affecting your data too much. The size of your buffer requires you to find a sweetspot between accuracy and lone outliers in acceleration values. If you do use a fifo buffer for your acceleration value then use the following equations:

Xf = 1/2At^2 + Vot + Xo

Vo = Aold *t + Vo-1

Where A is the new average acceleration derived from the hypothetical FIFO buffer, Aold is the old average acceleration from the last FIFO average, and t is the change in time between TWO individual dataset points. Everything in standard units of course, m/s s and so on.

You did a good job converting raw acceleration values to m/s^2 by dividing by 16384 and multiplying by 9.8m/s^2. 16384 value depends on the standard sensitivity setting of +-2g which can change if you choose another setting like +-4g.

Lastly, even with all the changes above it would be EXTREMELY difficult to get an accurate reading due to many different factors such as temperature. It is important to maintain a controlled environment for your accelerometer by using fans or anything to keep your gyro/ accelerometer at room temperature or 25C. There is a function in the Jeff Rowberg MPU6050 library to get the current temperature, mpu.getTemperature() I believe.

Even with all these changes, it will still be VERY difficult to get a good accurate reading due to mathematical reasons and small inaccuracies. You can try setting your gyro to a less sensitive setting because I know the mpu 6050 is set at the default +-2g setting, a higher setting could keep many issues from affecting your readings but it would become less sensitive to small displacements.

There are always more ways in which you can optimize your gyro/ accelerometer values, one way is if you increase the frequency of the MPU datastream in order to receive more data in a period of time, this could allow for you to receive more accurate readings. I believe there is a function in the Jeff Rowberg library to allow for this.

Solution 3

Yes.. in your loop() you have delay(1000);

That means your delta time is 1 sec, not 50ms.

try this in loop():

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    double t2 = millis() - pms;
    t2 /= 1000;  // convert ms to s
    t2 *=t2;


    // your Math and Serial here... 


    pms = millis(); 

Solution 4

It is late but maybe useful. You can search on INS stands on inertial navigation system to find the details. However in brief: the IMU measures the accelerations and angular velocities in a frame attached to it. For example acc in x direction is the x direction of IMU, not x direction of reference frame which you want to calculate the positions in. So you need to transform the accelerations to the reference frame. This can be done by Euler angles. As I understood MPU 6050 gives you Euler angles and even tranformation matrix. If so you should use

Acc_inert = T * Acc_body 

to transfer acc vector measured by sensor to acc vector in your reference frame. Then integrating twice gives you the position in reference frame. More details about the euler angles and calculation of transformation matrices are available in internet.

Solution 5

I understand that at the end your question is: "But I can't understand what time difference I should use."

The delta time you use in your code is 0.05. Meaning; you assume that your acceleration data rate is 20HZ. If it's not 20HZ, change it accordingly. Your calculations are based on the following formulas:

dis = 1/2 a t^2 + vt

v = v0 + at

where t is the time between two sequential acceleration samples.

Good luck

View more solutions

Comments

[Cerin]

This is the right idea. However, you should also account for the time it takes to perform your divide and multiply, and whatever you're doing for the serial connection, which on an embedded computer can actually take several milliseconds. In this case it adds an extra 395+7 to pms.