INTRODUCTION
If you have used Drone or Rocket, you probably familiar with word ' IMU ' short for ' Inertial Measurement Unit '.
There are 3, 6, 9 axis IMU, but I used 6 axis one. IMU is consist of 3 - Accelerometers and 3 - Angular velocity meters, so we can obtain 3 Accelerations, 3 Angular Velocities of X,Y,Z Direction. (Direction is notated on Unit)
If you don't want conceptual explanation and just want Arduino Code, go down below. (It might not that really boring but somepeople thought Physics is boring,, I disagree with that. Complex mind makes complexity)
WHAT IS INERTIA ?
Then, What is ' Inertia ' ? Inertia means 'Property to maintain status quo'.
Suppose we're friends and it's time to say good bye. Oh, the Bus is just coming. You're get on the bus and I shake my hand outside of bus. Suddenly the bus leaves so all passengers are frightened and leaned backward. Not passengers but bus handles are leaned. At this moment, you and I see different sight. Check picture below.
I see whole bus include handle moves same speed as one component. They aren't seperated. Pulling Force's vertical force offset Gravitational Pull, so Pulling Force's horizontal force is net force(F= ma, Newton's 2nd Law).
But you're in the bus. Go inside to your point of view.
Inside of Bus is remain still. Somebody rolled forward but handles aren't move, they just lean backward. No Move means No Force. Then who removes Pulling's horizontal force?
The answer is ' Inertia '.
Inertia is conceptual force which we can't observe but realize there's something. If we accelerate something or give force, inertia gives exact same force on oppsite side. That's what IMU calculates. It is easy to think IMU has 3 handles inside for XYZ axis.
NUMERICAL INTEGRAL
Physical Meanings of Displacement, Velocity and Acceleration
In Physics, Displacement is obtained by integral of Velocity. Similarly, Velocity is obtained by integral of Acceleration. Precisely, If you want to know the Displacement of the specific time interval, integrate Velocities of the specific time interval.
For instance, suppose you have time function of Velocity y = x^2 (m/s) and want to know displacement of [0 2] (sec) time interval, Y = X^3/3 should be time function of Displacement and you moved (8/3 - 0) distance. (Be careful of Unit)
But in real world there isn't exact time function , we just approximate it. In that situation there are several ways of Integrations, I would like to introduce easy one.
Integral of Velocity just means calculate Area between function of Velocity and Time-axis (maybe X-axis). Function must be Curve but we just calculate it with shape of Rectangle because we can easily calculate area of rectangle (Area = Width*Height).
Next choose interval of time which I called Delta_Time(ΔT) and divide whole area with ΔT. Then we have specific moment T0 and T0+ΔT, also value of Velocity F(T0) and F(T0+ΔT). So the problem is which one do you choose for height? Nobody tells you which one is better. It depends on shape of function. Let's me explain the difference.
The Rectangle Method
Let's suppose function Y = X^2 and we want value of integral with X = [0 2] interval. We already knew the result is 8/3.
Compare between choosing height with former one F(T0) and latter one F(T0 +ΔT) by MATLAB and find out which one is more accurate. (ΔT = 0.1)
delt = 0.1;
X = [0:delt:2];
Temp = 0;
Area1 = 0;
Area2 = 0;
Origin = 8/3;
for i=1:1:length(X)-1
Temp = delt*(X(i)^2);
Area1 = Area1 + Temp;
end
for i=2:1:length(X)
Temp = delt*(X(i)^2);
Area2 = Area2 + Temp;
end
disp(Origin);
disp(Area1);
Err1 = ((Origin-Area1)/Origin) *100;
disp(Err1);
disp(Area2);
Err2 = ((Area2-Origin)/Area2) *100;
disp(Err2);According to result ( 8/3 = 2.6667), Former one is 2.47 with 7.375% error and Latter one is 2.87 with 7.085% error. We can obtain better one if we choose height for F(T0+ΔT). However the error is still more than 7%. If we expand time interval or deal with complex function, the error maybe increase exponentially. That means the result is useless.
Now we are at important crossroad. How could we improve quality? The answer is up to you. Error is reduced as you reduce time interval. But you cannot reduce ΔT infinitely, so you should build your own integration model.
The Trapezoidal Method
How about calculates Area with Trapezoid, not Rectangle. Let's check it.
First, how could you ask minimum question to distinguish different types. There are several types like (F(T0),F(T0+ΔT)) = (+,+), (+,-), (-,+), (-,-).
Second, If one is plus and the other is minus there are two options. One is just split ΔT into 2 segment and make two triangle with same width. The other is draw line cross F(T0) and F(T0+ΔT), calculates X-intercept and draw two triangle. It is certain that latter one is more complex but more useful. Check figure below.
clc; close all; clear all;
delt = 0.1;
X = [0:delt:2];
Area = 0;
Area_Temp = 0;
Origin = 8/3;
for i=1:1:length(X)-1
F0 = X(i)^2;
F1 = X(i+1)^2;
if (F0*F1>=0)
Area_Temp = (F0)*delt + ((F1-F0)*delt)/2;
else
X_Intercept = delt*F1/(F1-F0);
Area_Temp = (F0*(delt-X_Intercept))/2 + (F1*X_Intercept)/2;
end
Area = Area + Area_Temp;
end
disp(Origin);
disp(Area);
Err = ((Area-Origin)/Area) * 100;
disp(Err);Surprisingly, Calculated Area is 2.67 and the error has become 0.125% !! What a dramatic decrease.
Arduino PORT
So we're ready to process data obtained from IMU, but first we should learn how to use IMU and save Data to SD card. Let's use Arduino UNO, MPU 6050(IMU), SD card Reader Module.
| Arduino UNO | SD Card Reader Module |
| VCC | 5V |
| GND | GND |
| CS | Digital Pin 4 (DP 4) |
| MOSI | Digital Pin 11 (DP 11) |
| MISO | Digital Pin 12 (DP 12) |
| SCK | Digital Pin 13 (DP 13) |
| Arduino UNO | MPU6050 |
| VCC | 3.3V |
| GND | GND |
| SCL | Analog 5 (A5) |
| SDA | Analog 4 (A4) |
Be careful of SCL / SDA in MPU6050 !
Arduino CODE
Used Arduino IDE. Download " MPU6050_light " Library. It will tell you 3-Acceleration and 3-Angle.
#include <SPI.h>
#include <SD.h>
#include "Wire.h"
#include <MPU6050_light.h>
File Data;
MPU6050 mpu(Wire);
void setup() {
Serial.begin(9600);
Wire.begin();
Serial.println("Initializing SD card... \n");
if (!SD.begin(4)) {
Serial.println("Fail to Initialize \n");
while (1);
}
Serial.println("Suceed ! \n");
byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while(status!=0){ } // stop everything if could not connect to MPU6050
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcOffsets(true,true); // gyro and accelero
Serial.println("Done! \n");
}
void loop() {
Data = SD.open("TEST02.txt", FILE_WRITE);
mpu.update();
if (Data) {
Serial.println("Writing TXT file...");
Data.println("ACCELERO X,Y,Z: ");
Data.println(mpu.getAccX()),Data.println(mpu.getAccY()),Data.println(mpu.getAccZ());
Data.close();
Serial.println("Complete !");
delay(100);
}
else {
Serial.println("Error opening TEST02.txt...");
delay(100);
}
}Now you have data for 0.1 sec interval in your micro SD card. Next transfer your TXT file to EXCEL because MATLAB can read EXCEL file by ' xlsread ' inner function. Finally apply your integration algorithm and get Velocity Vector or Displacement, whatever you want !
As I mentioned above, there isn't exact time Function. So after IMU starts measuring, although I shaked it randomly from starting point, I remembered and laid on where it had started. It might be little unaccurate, I hope Error isn't big enough. Let's see how it works.
MATLAB CODE
clc; close all; clear all;
Data = xlsread("Test02");
len = (length(Data)+1)/4;
X_Acc = zeros(len,1);
Y_Acc = zeros(len,1);
delt = 0.1;
for i=1:1:len
X_Acc(i) = Data(4*i-3);
Y_Acc(i) = Data(4*i-2);
end
X_Vel = zeros(len-1,1);
for i=1:1:length(X_Vel)
if (X_Acc(i)*X_Acc(i+1)>=0)
Vel_Temp = (X_Acc(i))*delt + ((X_Acc(i+1)-X_Acc(i))*delt)/2;
else
X_Intercept = delt*X_Acc(i+1)/(X_Acc(i+1)-X_Acc(i));
Vel_Temp = (X_Acc(i)*(delt-X_Intercept))/2 + (X_Acc(i+1)*X_Intercept)/2;
end
X_Vel(i) = Vel_Temp;
end
X_Dis = zeros(len-2,1);
Total_Dis_X = 0;
Sum_Dis_X = zeros(len-2,1);
for i=1:1:length(X_Dis)
if (X_Vel(i)*X_Vel(i+1)>=0)
Dis_Temp = (X_Vel(i))*delt + ((X_Vel(i+1)-X_Vel(i))*delt)/2;
else
X_Intercept = delt*X_Vel(i+1)/(X_Vel(i+1)-X_Vel(i));
Dis_Temp = (X_Vel(i)*(delt-X_Intercept))/2 + (X_Vel(i+1)*X_Intercept)/2;
end
X_Dis(i) = Dis_Temp;
Total_Dis_X = Total_Dis_X + Dis_Temp;
Sum_Dis_X(i) = sum(X_Dis(1:i));
end
Y_Vel = zeros(len-1,1);
for i=1:1:length(Y_Vel)
if (Y_Acc(i)*Y_Acc(i+1)>=0)
Vel_Temp = (Y_Acc(i))*delt + ((Y_Acc(i+1)-Y_Acc(i))*delt)/2;
else
X_Intercept = delt*Y_Acc(i+1)/(Y_Acc(i+1)-Y_Acc(i));
Vel_Temp = (Y_Acc(i)*(delt-X_Intercept))/2 + (Y_Acc(i+1)*X_Intercept)/2;
end
Y_Vel(i) = Vel_Temp;
end
Y_Dis = zeros(len-2,1);
Total_Dis_Y = 0;
Sum_Dis_Y = zeros(len-2,1);
for i=1:1:length(Y_Dis)
if (Y_Vel(i)*Y_Vel(i+1)>=0)
Dis_Temp = (Y_Vel(i))*delt + ((Y_Vel(i+1)-Y_Vel(i))*delt)/2;
else
X_Intercept = delt*Y_Vel(i+1)/(Y_Vel(i+1)-Y_Vel(i));
Dis_Temp = (Y_Vel(i)*(delt-X_Intercept))/2 + (Y_Vel(i+1)*X_Intercept)/2;
end
Y_Dis(i) = Dis_Temp;
Total_Dis_Y = Total_Dis_Y + Dis_Temp;
Sum_Dis_Y(i) = sum(Y_Dis(1:i));
end
disp(Total_Dis_X);
disp(Total_Dis_Y);
T = [0:1:length(Sum_Dis_X)-1];
plot(T,Sum_Dis_X);
title('Displacement from Starting Point')
xlabel("Time (0.1 sec)")
ylabel("Displacement")
grid on
hold on
plot(T,Sum_Dis_Y);
grid on
hold on
legend('X-axis','Y-axis')
RESULT
Result : X-axis's Displacement from Starting Point : -0.0108
Y-axis's Displacement from Starting Point : 0.000025
As it came back to starting point, X & Y displacement should be zero. But according to result, even if there's error in integration algorithm, X-axis's error isn't neglectable. Check graph below. If you want to know 2 dimension displacemen vector value is square root of (Vel_X)^2 + (Vel_Y)^2. 3 dimension has similar process.
(X-axis's graph looks like my STOCK,,,)
So I will put IMU's Y-axis on ' Heading(Roll) ' side because rocket needs accurate result of maximum altitude, called 'Apogee'.
In this post we learned about how to get IMU's 3 - Acceleration and extract Velocity and Displacement. But as I said, there are also 3 - Angualr Velocity meters in IMU. We will use them for T.V.C system. Go to [T.V.C] part.
I hope there's a chance to introduce how to improve integration algorithm with Numerical Methods. But the best is building your own Algorithm. Thanks for reading :)
Thanks for your reading and I hope you got what you wanted !
댓글