MPU 6050 Gyro, akcelerometr Komunikace s Arduino (Atmega328p): 5 kroků

2024 Autor: John Day | [email protected]. Naposledy změněno: 2024-01-30 08:19

MPU6050 IMU má na jednom čipu integrovaný 3osý akcelerometr i 3osý gyroskop.

Gyroskop měří rotační rychlost nebo rychlost změny úhlové polohy v čase, podél osy X, Y a Z.

Výstupy gyroskopu jsou ve stupních za sekundu, takže abychom získali úhlovou polohu, stačí integrovat úhlovou rychlost.

Na druhou stranu akcelerometr MPU6050 měří zrychlení měřením gravitačního zrychlení podél 3 os a pomocí nějaké trigonometrické matematiky můžeme vypočítat úhel, pod kterým je snímač umístěn. Pokud tedy spojíme nebo zkombinujeme data z akcelerometru a gyroskopu, můžeme získat velmi přesné informace o orientaci senzoru.

3osý gyroskop MPU-6050 se skládá ze 3osého gyroskopu, který dokáže detekovat rychlost otáčení podél osy x, y, z pomocí technologie mikroelektronického systému (MEMS). Když se senzor otáčí podél libovolné osy, dochází k vibracím způsobeným Coriolisovým efektem, který je detekován MEMS. 16bitový ADC se používá k digitalizaci napětí pro vzorkování každé osy. +/- 250, +/- 500, +/- Plný rozsah výstupu je 1000, +/- 2000. Úhlová rychlost se měří podél každé osy v jednotkách stupně za sekundu.

Užitečný odkaz: …………….

Deska Arduino: ………..

MPU6050 IMU ……………

Krok 1: Modul MPU-6050

Modul MPU-6050 má 8 pinů,

INT: Přerušte pin digitálního výstupu.

AD0: Pin LSB slave adresy I2C. Toto je 0. bit v 7bitové podřízené adrese zařízení. Pokud je připojen k VCC, pak se čte jako logická jednička a mění se adresa slave.

XCL: Pomocný sériový hodinový kolík. Tento pin se používá k připojení dalších SCL pinů s rozhraním I2C k MPU-6050.

XDA: Pin pomocných sériových dat. Tento pin slouží k připojení dalších senzorů s rozhraním SDA s rozhraním I2C k MPU-6050.

SCL: Pin sériových hodin. Připojte tento pin k pinu SCL mikrokontrolérů. SDA: Sériový datový pin. Připojte tento pin k mikrořadiči SDA pin.

GND: Zemnící kolík. Připojte tento pin k uzemnění.

VCC: Kolík napájecího zdroje. Připojte tento kolík ke zdroji +5 V DC. Modul MPU-6050 má slave adresu (když AD0 = 0, tj. Není připojen k Vcc) jako, Adresa Slave pro zápis (SLA+W): 0xD0

Adresa pro čtení podřízeného zařízení (SLA+R): 0xD1

Krok 2: Výpočty

Data senzorů gyroskopu a akcelerometru modulu MPU6050 se skládají ze 16bitových nezpracovaných dat ve formě 2 doplňků.

Data teplotního senzoru modulu MPU6050 se skládají ze 16bitových dat (ne ve formě doplňku 2).

Předpokládejme, že jsme vybrali

• - Plný rozsah akcelerometru +/- 2 g s faktorem stupnice citlivosti 16, 384 LSB (počet)/g.
• - Rozsah gyroskopu v rozsahu +/- 250 °/s s faktorem měřítka citlivosti 131 LSB (počet)/°/s. pak,

Abychom získali nezpracovaná data ze senzoru, musíme nejprve provést doplnění 2 na datech ze senzorů akcelerometru a gyroskopu. Poté, co získáme surová data ze senzoru, můžeme vypočítat zrychlení a úhlovou rychlost vydělením surových dat senzoru jejich faktorem měřítka citlivosti následujícím způsobem-

Hodnoty akcelerometru v g (g síla)

• Zrychlení podél osy X = (Akcelerometr nezpracovaná data osy X/16384) g.
• Zrychlení podél osy Y = (Akcelerometr nezpracovaná data osy Y/16384) g.
• Zrychlení podél osy Z = (Akcelerometr nezpracovaná data osy Z/16384) g.

Hodnoty gyroskopu ve °/s (stupeň za sekundu)

• Úhlová rychlost podél osy X = (nezpracovaná data osy X gyroskopu/131) °/s.
• Úhlová rychlost podél osy Y = (nezpracovaná data osy Y gyroskopu/131) °/s.
• Úhlová rychlost podél osy Z = (nezpracovaná data osy Z gyroskopu/131) °/s.

Hodnota teploty ve °/c (stupně na stupně Celsia)

Teplota ve stupních C = ((údaje teplotního čidla)/340 + 36,53) °/c.

Například, Předpokládejme, že po 2 ‘komplementu dostaneme surovou hodnotu os X akcelerometru = +15454

Pak Ax = +15454/16384 = 0,94 g.

Více,

Víme tedy, že pracujeme s citlivostí +/- 2G a +/- 250deg/s, ale jak naše hodnoty odpovídají těmto zrychlením/úhlům.

Oba jsou to přímočaré grafy a můžeme z nich vypočítat, že pro 1G odečteme 16384 a pro 1 stupeň/s budeme číst 131,07 (I když.07 bude kvůli binárním souborům ignorováno), tyto hodnoty byly právě zpracovány nakreslením přímkový graf s 2G na 32767 a -2G na -32768 a 250/-250 na stejných hodnotách.

Nyní tedy víme, že naše hodnoty citlivosti (16384 a 131.07) stačí minusovat posuny od našich hodnot a poté je rozdělit podle citlivosti.

Ty budou fungovat dobře pro hodnoty X a Y, ale vzhledem k tomu, že Z byl zaznamenán při 1G a ne 0, budeme muset minus 1G (16384) oddělit, než vydělíme naší citlivostí.

Krok 3: Připojení MPU6050-Atmega328p

Stačí připojit vše podle schématu…

Připojení jsou uvedena následovně:-

MPU6050 Arduino Nano

Výstupní kolík VCC 5v

Zemnící kolík GND

SDA A4 pin // sériová data

Pin SCL A5 // sériové hodiny

Výpočet Pitch and Roll: Roll je rotace kolem osy x a pitch je rotace podél osy y.

Výsledek je v radiánech. (převést na stupně vynásobením 180 a dělením pí)

Krok 4: Kódy a vysvětlení

/*

Výukový program akcelerometru a gyroskopu Arduino a MPU6050 od Dejana, https://howtomechatronics.com */#include const int MPU = 0x68; // Plovoucí adresa MPU6050 I2C AccX, AccY, AccZ; float GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; float roll, pitch, stáčení; float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; int c = 0; neplatné nastavení () {Serial.begin (19200); Wire.begin (); // Inicializace komunikace Wire.beginTransmission (MPU); // Zahájení komunikace s MPU6050 // MPU = 0x68 Wire.write (0x6B); // Promluvte si s registrem 6B Wire.write (0x00); // Proveďte reset - vložte 0 do registru 6B Wire.endTransmission (true); // ukončení přenosu/* // Konfigurace citlivosti akcelerometru - rozsah plného rozsahu (výchozí +/- 2g) Wire.beginTransmission (MPU); Wire.write (0x1C); // Promluvte si s registrem ACCEL_CONFIG (1C hex) Wire.write (0x10); // Nastavit bity registru jako 00010000 (+/- 8 g rozsahu plného rozsahu) Wire.endTransmission (true); // Konfigurace citlivosti gyroskopu - plný rozsah stupnice (výchozí +/- 250 stupňů/s) Wire.beginTransmission (MPU); Wire.write (0x1B); // Promluvte si s registrem GYRO_CONFIG (1B hex) Wire.write (0x10); // Nastavit bity registru jako 00010000 (plné měřítko 1000deg/s) Wire.endTransmission (true); zpoždění (20); */ // Zavolejte tuto funkci, pokud potřebujete získat chybové hodnoty IMU pro váš modul count_IMU_error (); zpoždění (20); } void loop () {// === Číst data akcelerátoru === // Wire.beginTransmission (MPU); Wire.write (0x3B); // Začněte registrem 0x3B (ACCEL_XOUT_H) Wire.endTransmission (false); Wire.requestFrom (MPU, 6, true); // Přečíst celkem 6 registrů, každá hodnota osy je uložena ve 2 registrech // Pro rozsah +-2g potřebujeme rozdělit nezpracované hodnoty na 16384, podle datového listu AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; // hodnota osy X AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; // hodnota osy Y AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // hodnota osy Z // Výpočet Roll a Pitch z dat akcelerometru accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0,58; // AccErrorX ~ (0,58) Viz uživatelská funkce Calculate_IMU_error () pro více podrobností accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1,58; // AccErrorY ~ (-1,58) // === Přečíst data gyroskopu === // previousTime = currentTime; // Předchozí čas je uložen před skutečným časem čtení currentTime = millis (); // Aktuální čas skutečný čas čtení elapsedTime = (currentTime - previousTime) / 1000; // Dělením 1000 získáte sekundy Wire.beginTransmission (MPU); Wire.write (0x43); // Adresa gyro dat prvního registru 0x43 Wire.endTransmission (false); Wire.requestFrom (MPU, 6, true); // Přečíst celkem 4 registry, každá hodnota osy je uložena ve 2 registrech GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0; // Pro rozsah 250deg/ s musíme nejprve rozdělit nezpracovanou hodnotu na 131,0, podle datového listu GyroY = (Wire.read () << 8 | Wire.read ())/ 131.0; GyroZ = (Wire.read () << 8 | Wire.read ()) / 131,0; // Opravte výstupy pomocí vypočítaných chybových hodnot GyroX = GyroX + 0,56; // GyroErrorX ~ (-0,56) GyroY = GyroY - 2; // GyroErrorY ~ (2) GyroZ = GyroZ + 0,79; // GyroErrorZ ~ (-0,8) // V současné době jsou nezpracované hodnoty ve stupních za sekundu, deg/s, takže potřebujeme vynásobit sendondy (s), abychom získali úhel ve stupních gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg gyroAngleY = gyroAngleY + GyroY * elapsedTime; zatáčení = zatáčení + GyroZ * elapsedTime; // Doplňkový filtr - kombinujte hodnoty akcelerometru a úhlu gyroskopu roll = 0,96 * gyroAngleX + 0,04 * accAngleX; rozteč = 0,96 * gyroAngleY + 0,04 * accAngleY; // Vytiskněte hodnoty na sériovém monitoru Serial.print (roll); Serial.print ("/"); Serial.print (rozteč); Serial.print ("/"); Serial.println (zatáčení); } void count_IMU_error () {// Tuto funkci můžeme nazvat v sekci nastavení pro výpočet chyby akcelerometru a gyroskopu. Odtud dostaneme hodnoty chyb použité ve výše uvedených rovnicích vytištěné na sériovém monitoru. // Všimněte si, že bychom měli umístit IMU flat, abychom získali správné hodnoty, abychom pak mohli správné hodnoty // Přečíst hodnoty akcelerometru 200krát while (c <200) {Wire.beginTransmission (MPU); Wire.write (0x3B); Wire.endTransmission (false); Wire.requestFrom (MPU, 6, true); AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Součet všech 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ím součtu číslem 200 získáte chybovou hodnotu AccErrorX = AccErrorX /200; AccErrorY = AccErrorY / 200; c = 0; // Přečíst hodnoty gyroskopu 200krát 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 (); // Součet všech naměřených hodnot GyroErrorX = GyroErrorX + (GyroX / 131.0); GyroErrorY = GyroErrorY + (GyroY / 131,0); GyroErrorZ = GyroErrorZ + (GyroZ / 131,0); c ++; } // Vydělením součtu číslem 200 získáte chybovou hodnotu GyroErrorX = GyroErrorX /200; GyroErrorY = GyroErrorY / 200; GyroErrorZ = GyroErrorZ / 200; // Vytiskněte chybové hodnoty na Serial Monitor 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); } ------------------------------------------------- ---------------------------------------------- Výsledky:-X = Y = Z = --------------------------------------------- ----------------------------------------------- Důležitá poznámka: -----------------

V sekci smyčky začneme čtením dat akcelerometru. Data pro každou osu jsou uložena ve 2 bytech nebo registrech a adresy těchto registrů vidíme z datového listu snímače.

Abychom je všechny přečetli, začneme prvním registrem a pomocí funkce requiestFrom () požádáme o načtení všech 6 registrů pro osy X, Y a Z. Poté načteme data z každého registru, a protože výstupy jsou dvě doplňkové, vhodně je spojíme, abychom získali správné hodnoty.

Krok 5: Porozumění úhlu náklonu

Akcelerometr

Zemská gravitace je neustálé zrychlování, kde síla vždy směřuje dolů do středu Země.

Když je akcelerometr rovnoběžný s gravitací, naměřené zrychlení bude 1G, když je akcelerometr kolmý na gravitaci, bude měřit 0G.

Úhel náklonu lze vypočítat z naměřeného zrychlení pomocí této rovnice:

θ = sin-1 (měřené zrychlení / gravitační zrychlení)

K měření úhlové rychlosti (ω) se používá GyroGyro (aka snímač rychlosti).

Abychom získali úhel naklonění robota, musíme integrovat data z gyroskopu, jak je uvedeno v následující rovnici:

ω = dθ / dt, θ = ∫ ω dt

Fúze gyroskopu a akcelerometru Po prostudování charakteristik gyroskopu a akcelerometru víme, že mají své vlastní silné a slabé stránky. Vypočtený úhel náklonu z dat akcelerometru má pomalou dobu odezvy, zatímco integrovaný úhel náklonu z údajů gyroskopu je po určitou dobu posunut. Jinými slovy můžeme říci, že data akcelerometru jsou užitečná dlouhodobě, zatímco data gyroskopu jsou užitečná krátkodobě.

Odkaz pro lepší pochopení: Klikněte sem