Slevový týden - Květen Office week
Pouze tento týden sleva až 80 % na e-learning týkající se MS Office
30 % bodů zdarma na online výuku díky naší Slevové akci!

MPU6050 akcelerometr a gyroskop pro Arduino

Modul MPU6050 IMU pro Arduino se skládá z akcelerometru a gyroskopu, díky kterým můžeme měřit zrychlení a úhlovou rychlost ve 3 osách. Celkově máme k dispozici 6 hodnot (6 stupňů volnosti), proto se tento modul označuje jako 6 DOF (angl. Six Degrees of Freedom). Z naměřených hodnot umíme určit úhly pootočení (v angličtině se označují jako yaw, roll a pitch). Modul je tak vhodný např. pro určení orientace dronu nebo robotické ruky.

Princip měření

Oba senzory, akcelerometr a gyroskop, patří do tzv. MEMS (Mikro-Elektro-Mechanické Systémy). Jedná se o konstrukce velmi malých rozměrů (od 0.001 mm po 0.1 mm). K výrobě se využívá technologií z produkce mikroelektroniky, jako je např. selektivní leptání nebo iontové odprašování.

Akcelerometr

Zrychlení získáváme měřením kapacity mezi pohyblivým tělem a fixními plotnami. Při zrychlení tělesa dochází ke změně kapacity mezi deskami, kterou následně softwarově přepočítáme na zrychlení v jednotkách m/s2 nebo v jednotkách přetížení (násobky zemské tíze v jednotkách g).

Zdroj obrázku: howtomechatro­nics.com

Gyroskop

Gyroskop měří úhlovou rychlost (změna úhlu pootočení za určitý čas) využitím Coriolisovy síly. Při pohybu tělesa ve směru vektoru rychlosti (viz obrázek níže) a vystavení tělesa vnějšímu otáčení (úhlová rychlost) dochází k tomu, že na těleso působí Coriolisova síla. Tato síla způsobí přemístění tělesa z jeho původní polohy někam jinam (angl. displacement). Tato změna polohy způsobí změnu v kapacitě, která se dá změřit a zpracovat podobně jako v případě akcelerometru.

Zdroj obrázku: howtomechatro­nics.com

Zapojení modulu

Modul komunikuje prostřednictvím I^2C protokolu, takže zapojení není problém, viz obrázek. Napájet senzor můžeme s 3.3 V nebo 5 V:

Zdroj obrázku: howtomechatro­nics.com

Kód

Tentokrát si kód ztížíme tím, že nebudeme využívat externí knihovnu od Adafruitu. Tyto senzory lze totiž koupit i levněji neoriginální a již se mi několikrát stalo, že klony pak s Adafruit knihovnou nefungovaly. Pro komunikaci se senzorem budeme potřebovat pouze knihovnu Wire.h. Takže se těšte na práci s registry.

Nejprve knihovnu naincludujeme:

#include <Wire.h>

Na začátek si deklarujeme všechny proměnné:

  • 6 stupňů volnosti (zrychlení a úhlová rychlost v osách x, y, z)
  • 2 úhly získáme trigonometrickou metodou z akcelerometru
  • 3 úhly z gyroskopu pro určení orientace senzoru
  • dále si budeme ukládat celkové zrychlení a celkové přetížení G v násobcích zemské tíhy
  • dále naměřené odchylky v klidu, které slouží k zpřesnění měření (chybové hodnoty)
  • a nakonec proměnné k určení času
Tento výukový obsah pomáhají rozvíjet následující firmy, které dost možná hledají právě tebe!

V kódu to vše bude vypadat následovně:

const int MPU = 0x68;                                             // MPU6050 I2C adresa
float AccX, AccY, AccZ;                                           // zrychlení ve směru osy x, y, z
float GyroX, GyroY, GyroZ;                                        // úhlová rychlost ve směru osy x, y, z
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;   // úhly pootočení ve směrech x, y, z
float roll, pitch, yaw;                                           // názvy proměnných, které se běžně používají v letecké terminologii
float a;                                  // celkové zrychlení v m/s^2
float G;                                  // přetížení v násobcích zemské tíhy (1g = 9.81 m/s^2)
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;   // chybové hodnoty
int c = 0;                                                        // proměnná pro cyklus while

// proměnné pro určení času, za který se senzor pootočil o daný úhel
float elapsedTime, currentTime, previousTime;

setup()

V setup() nejprve resetujeme senzor. Komunikaci začneme pomocí příkazů Wire.begin() a Wire.beginTransmission("adresa senzoru"). Adresu senzoru jsme na začátku nastavili na 0x68. Následně, abychom resetovali senzor, musíme do registru s adresou 6B do 7. bitu zadat 0. Do ostatních bitů zapíšeme také 0, protože nic jiného, než reset nepotřebujeme:

Serial.begin(9600);

Wire.begin();           // inicializace I2C komunikace
Wire.beginTransmission (MPU);   // inicializace komunikace přímo se senzorem MPU6050 // MPU = 0x68
Wire.write(0x6B);       // komunikace s registrem 6B
Wire.write(0x00);       // reset senzoru - umístění nul do registru 6B
Wire.endTransmission(true);     // konec přenosu

Je třeba dodat, že I^2C protokol přenáší data se sekvencí 8 bitů (8 nul/jedniček). Do příkazu Wire.write("sekvence bitů") můžeme zadat sekvenci buď v hexadecimální nebo binární soustavě. Takže Wire.write(00000000) je ekvivalentní Wire.write(0x00). My budeme dále pracovat v hexadecimální soustavě. Nakonec ukončíme přenos s příkazem Wire.endTransmission(true).

Dále nastavíme rozsah akcelerometru tak, že přistoupíme k registru 1C a změníme 4. a 5. bit. Rozsahy lze měnit od +/- 2G až po +/- 16 G. V kódu stačí odkomentovat příslušný řádek, aktuálně je zde ponecháno +/- 2G.

// Nastavení rozsahu akcelerometru

Wire.beginTransmission (MPU);
Wire.write (0x1C); // přístup k ACCEL_CONFIG registru (1C v hex), nastav rozsah, odkomentuj řádek
Wire.write(0x00); // +/- 2G AFS_SEL = 0, v binární soustavě 00000000
//Wire.write(0x08); // +/- 4G AFS_SEL = 1, v binární soustavě 00001000
//Wire.write(0x10); // +/- 8G AFS_SEL = 2, v binární soustavě 00010000
//Wire.write(0x18); // +/- 16G AFS_SEL = 3, v binární soustavě 00011000
Wire.endTransmission (true);

Nastavíme rozsah gyroskopu. Gyroskop měří úhlovou rychlost, v podstatě se dá změřit úhlová rychlost přibližně 5 otáček za sekundu (360 °/s je 1 otáčka za sekundu). K nastavení slouží registr 1B, princip je stejný jako u akcelerometru:

// Nastavení rozsahu gyroskopu

Wire.beginTransmission(MPU);
Wire.write (0x1B); // komunikace s registrem 1B
//Wire.write(0x00); // +/- 250 ° / s AFS_SEL = 0, v binární soustavě 000 00 000
//Wire.write(0x08); // +/- 500 ° / s AFS_SEL = 1, v binární soustavě 000 01 000
//Wire.write(0x10); // +/- 1000 ° / s AFS_SEL = 2, v binární soustavě 000 10 000
//Wire.write(0x18); // +/- 2000 ° / s AFS_SEL = 3, v binární soustavě 000 11 000
Wire.endTransmission(true);
delay(20);

Žádný senzor není 100%. Proto je vhodné na začátku zjistit odchylku od skutečné hodnoty, kterou bychom chtěli naměřit. Stačí senzor nechat v klidu a funkce nám vrátí odchylky, které je třeba odečíst od naměřených hodnot. V podstatě jde o cyklus, v němž 200 krát zjistíme raw hodnoty z akcelerometru a gyroskopu, a následně hodnoty zprůměrujeme. Tělo funkce si můžete prohlédnout v závěru článku.

// zavolání funkce calculate_IMU_error() pro získání odchylky (chybové hodnoty)
calculate_IMU_error();

loop()

Raw data z akcelerometru získáme tak, že si vyžádáme 6 registrů (3B, 3C, 3D, 3E, 3F, 40) příkazem Wire.requestFrom(MPU, 6, true). Předtím si však zapíšeme první registr, 3B. Získáme tak sekvenci 48 bitů (6 registrů po 8 bitů). Příkazem Wire.read() získáme jednotlivé bity, posun provádíme operátorem <<:

// data z akcelerometru
Wire.beginTransmission(MPU);
Wire.write(0x3B); // začneme s registrem 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // vyžádání 6 registrů, každá hodnota pro 1 osu je uložena v 2 registrech

/* Pro rozsah + -2G vyděl raw hodnoty 8192
 * Pro rozsah + -4G vyděl raw hodnoty 4096
 * Pro rozsah + -8G vyděl raw hodnoty 2048
 * Pro rozsah + -16G vyděl raw hodnoty 1024
 */
// pro každou hodnotu zjistíme hodnoty v 2 registrech, sečteme (symbol | = the bitwise OR operátor)

AccX = (Wire.read () << 8 | Wire.read ()) / 8192.0; // X osa
AccY = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Y osa
AccZ = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Z osa

Raw hodnotu např. pro osu x získáme sečtením bitů z registru 3B a 3C. Každá hodnota je ukryta v 2 registrech. Nejde přímo o sčítání, ale o operaci OR v binární soustavě (symbol |). Získanou hodnotu vydělíme číslem 8192 (pokud jsme si zvolili rozsah 2G), které zjistíme z datasheetu. Pro jiné rozsahy dělíme jiným číslem, viz kód.

Z naměřených hodnot zrychlení můžeme určit úhly pootočení pro osu x a y. Postačí na to funkce atan(), která zjistí úhel z poměru jednotlivých hodnot:

// určení úhlu x a y trigonometrickou metodou a ošetření hodnot odečtením odchylky
accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (+0.58)
accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58)

U gyroskopu je to podobné, liší se však výpočet úhlů. Ze senzoru získáme úhlovou rychlost, kterou je třeba vynásobit časem, za který se senzor pootočil (podobně jako dráha = rychlost * čas). Ten určíme jako rozdíl aktuálního času a čas z předchozího cyklu.

// data z gyroskopu
previousTime = currentTime;                 // získání času z předchozí smyčky
currentTime = Millis ();                // aktuální čas
elapsedTime = (currentTime - previousTime) / 1000;  // vypočítá čas, za který se senzor pootočil o určitý úhel

Ukažme si kód pro gyroskop, princip je stejný jako u akcelerometru. Začneme registrem 0x43 a změníme dělitele pro raw hodnotu dle rozsahu gyroskopu:

Wire.beginTransmission(MPU);
Wire.write(0x43); // začneme s registrem 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // podobně jako u akcelerometru

/* Pro rozsah 250deg / s vyděl raw hodnoty 131
 * Pro rozsah 250deg / s vyděl raw hodnoty 65.5
 * Pro rozsah 250deg / s vyděl raw hodnoty 32.8
 * Pro rozsah 250deg / s vyděl raw hodnoty 16.4
 */

GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
Gyro = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

// ošetření hodnot odečtením odchylky, pro každý senzor jsou hodnoty jiné!
GyroX = GyroX +0.16; // GyroErrorX ~ (-0.56)
Gyro = gyro -4.22; // GyroErrorY ~ (2)
GyroZ = GyroZ -0.26; // GyroErrorZ ~ (-0.8)

Následuje vyčíslení úhlů. Je třeba dodat, že k aktuálnímu úhlu přičítáme úhel z předchozího cyklu. Můžeme tak naměřit i pootočení o 1000° (rozsah je pro úhlovou rychlost, ne pro úhel!).

// úhly získáme tak, že naměřené hodnoty úhlové rychlosti vynásobíme časovým intervalem
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // stupeň / sekunda * sekunda = stupeň
gyroAngleY = gyroAngleY + gyro * elapsedTime;
yaw = yaw + GyroZ * elapsedTime; // namísto yaw jsme mohli dát gyroAngleZ

// pro získání přesnějších měření kombinujeme, 96% hodnoty bude tvořit hodnota z gyroskopu, 4% z akcelerometru
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

Yaw, roll a pitch jsou jednoduše názvy úhlů, které jsou zažité v letecké terminologii. V kódu by se dal např. roll vypočítat takto: roll = gyroAngleX + GyroX * elapsedTime. V kódu jsme vzali 96% z gyroskopu a 4% z akcelerometru pro určení úhlů roll a pitch, proto to vypadá trošku jinak.

Zdroj obrázku: howtomechatro­nics.com

Celkové zrychlení a celkové přetížení vypočítáme jako vektorový součet zrychlení takto:

// celkové přetížení,
G = sqrt (AccX * AccX + AccY * AccY + AccZ * AccZ) -1.03;

// celkové zrychlení
a = 9.81 * G;

Možná se ptáte, proč je tam to číslíčko 1.03? Pokud je modul v klidu, senzor ve směru osy x a y v ideálním případě naměří zrychlení 0. Ve směru osy z působí však tíhové zrychlení i když je modul v klidu (vzpomeňte si na pohyblivé tělo na pružinách). AccZ je přibližně 1 g. Experimentálně jsem zjistil, že mi to zhruba vychází na 1.03 pro celkové zrychlení, viz obrázek:

Zdroj obrázku: howtomechatro­nics.com

Závěr

Senzor reaguje opravdu s dobrou odezvou a citlivost je vysoká. Senzor jsem položil na stůl a dokáže detekovat např. psaní na klávesnici nebo bouchání do stolu:

Celý kód

#include <Wire.h>
const int MPU = 0x68;                                             // MPU6050 I2C adresa
float AccX, AccY, AccZ;                                           // zrychlení ve směru osy x, y, z
float GyroX, GyroY, GyroZ;                                        // uhlová rychlost ve směru osy x, y, z
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;   // úhly pootočení ve směrech x, y, z
float roll, pitch, yaw;                                           // názvy proměnných, které se běžně používají v letecké terminologii
float a;                    // celkové zrychlení v m/s^2
float G;                    // přetížení v násobcích zemské tíže (1g = 9.81 m/s^2)
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;   // chybové hodnoty
int c = 0;                                                        // proměnná pro cyklus while

// proměnné pro určení času, za který se senzor pootočil o daný úhel
float elapsedTime, currentTime, previousTime;

void setup() {
Serial.begin(9600);
Wire.begin ();      // inicializace I2C komunikace
Wire.beginTransmission (MPU);   // inicializace komunikace přímo se senzorem MPU6050 // MPU = 0x68
Wire.write (0x6B);    // komunikace s registrem 6B
Wire.write (0x00);    // reset senzoru - umístění nul do registru 6B
Wire.endTransmission (true);  // konec přenosu

// Nastavení rozsahu akcelerometru

Wire.beginTransmission (MPU);
Wire.write (0x1C); // přístup k ACCEL_CONFIG registru (1C v hex), nastav rozsah, odkomentuj řádek
//Wire.write(0x00); // +/- 2G AFS_SEL = 0, v binární soustavě 00000000
//Wire.write(0x08); // +/- 4G AFS_SEL = 1, v binární soustavě 00001000
//Wire.write(0x10); // +/- 8G AFS_SEL = 2, v binární soustavě 00010000
//Wire.write(0x18); // +/- 16G AFS_SEL = 3, v binární soustavě 00011000
Wire.endTransmission (true);

// Nastavení rozsahu gyroskopu

Wire.beginTransmission (MPU);
Wire.write (0x1B); // komunikace s registrem 1B
//Wire.write(0x00); // +/- 250 ° / s AFS_SEL = 0, v binární soustavě 000 00 000
//Wire.write(0x08); // +/- 500 ° / s AFS_SEL = 1, v binární soustavě 000 01 000
//Wire.write(0x10); // +/- 1000 ° / s AFS_SEL = 2, v binární soustavě 000 10 000
//Wire.write(0x18); // +/- 2000 ° / s AFS_SEL = 3, v binární soustavě 000 11 000
Wire.endTransmission (true);
delay (20);


// zavolání funkce calculate_IMU_error () pro získání odchylky (chybové hodnoty)
calculate_IMU_error();
delay(20);
}

void loop() {

// data z akcelerometru
Wire.beginTransmission (MPU);
Wire.write (0x3B); // začneme s registrem 0x3B (ACCEL_XOUT_H)
Wire.endTransmission (false);
Wire.requestFrom (MPU, 6, true); // vyžádání 6 registrů, každá hodnota pro 1 osu je uložena v 2 registrech

/* Pro rozsah + -2G vyděl raw hodnoty 8192
 * Pro rozsah + -4G vyděl raw hodnoty 4096
 * Pro rozsah + -8G vyděl raw hodnoty 2048
 * Pro rozsah + -16G vyděl raw hodnoty 1024
 */
// pro každou hodnotu zjistíme hodnoty v 2 registrech, sečteme (symbol | = the bitwise OR operátor)

AccX = (Wire.read () << 8 | Wire.read ()) / 8192.0; // X os
AccY = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Y os
AccZ = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Z os

// určení úhlu x a y trigonometrickou metodou a ošetření hodnot odečtením odchylky
accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (+0.58)
accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58)

// data z gyroskopu

Wire.beginTransmission (MPU);
Wire.write (0x43); // začneme s registrem 0x43
Wire.endTransmission (false);
Wire.requestFrom (MPU, 6, true); // podobně jako u akcelerometru

/* Pro rozsah 250deg / s vyděl raw hodnoty 131
 * Pro rozsah 250deg / s vyděl raw hodnoty 65.5
 * Pro rozsah 250deg / s vyděl raw hodnoty 32.8
 * Pro rozsah 250deg / s vyděl raw hodnoty 16.4
 */

GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0;
GyroY = (Wire.read () << 8 | Wire.read ()) / 131.0;
GyroZ = (Wire.read () << 8 | Wire.read ()) / 131.0;

// ošetření hodnot odečtením odchylky, pro každý senzor jsou hodnoty jiné!
GyroX = GyroX +0.16; // GyroErrorX ~ (-0.56)
GyroY = GyroY -4.22; // GyroErrorY ~ (2)
GyroZ = GyroZ -0.26; // GyroErrorZ ~ (-0.8)

// úhly získáme tak, že naměřené hodnoty úhlové rychlosti vynásobíme časovým intervalem
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // stupeň / sekunda * sekunda = stupeň
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime; // namísto yaw jsme mohli dát gyroAngleZ

// pro získání přesnějších měření kombinujeme, 96% hodnoty bude tvořit hodnota z gyroskopu, 4% z akcelerometru
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

// celkové přetížení,
G = sqrt (AccX * AccX + AccY * AccY + AccZ * AccZ) -1.03;

// celkové zrychlení
a = 9.81 * G;

// vypsání hodnot
Serial.print(" zrychlení ");
Serial.print(" osa x ");
Serial.print(AccX);
Serial.print(" osa y ");
Serial.print(AccY);
Serial.print(" osa z ");
Serial.println(AccZ);

Serial.print(" úhly ");
Serial.print(" osa x ");
Serial.print(gyroAngleX);
Serial.print(" osa y ");
Serial.print(gyroAngleY);
Serial.print(" osa z ");
Serial.println(yaw);

Serial.print(" přetížení ");
Serial.println(G);
Serial.print(" celkové zrychlení ");
Serial.println(a);

Serial.println(); // mezera

}

void calculate_IMU_error() {
// odchylku zobrazíme v serial monitoru, a následně přepíšeme do kódu
// cyklus while, průměr 200 krát
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);

// nezapomenout při změně rozsahu měnit dělitele
AccX = (Wire.read() << 8 | Wire.read()) / 8192.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 8192.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 8192.0 ;

// sčítání naměřených hodnot
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}

// vydělení naměřených hodnot číslem 200, získání průměrné odchylky
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;

// totéž pro gyroskop

while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}

GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;

// Vytiskněme chybové hodnoty na sériovém monitoru
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}

 

Stáhnout

Staženo 8x (7.16 kB)
Aplikace je včetně zdrojových kódů

 

 

Článek pro vás napsal michal
Avatar
Jak se ti líbí článek?
Ještě nikdo nehodnotil, buď první!
Autor sa venuje vzdelávaniu a má rad novinky vo vzdelávani.
Všechny články v sekci
Arduino
Aktivity (4)

 

 

Komentáře

Děláme co je v našich silách, aby byly zdejší diskuze co nejkvalitnější. Proto do nich také mohou přispívat pouze registrovaní členové. Pro zapojení do diskuze se přihlas. Pokud ještě nemáš účet, zaregistruj se, je to zdarma.

Zatím nikdo nevložil komentář - buď první!