Náklonoměr MPU6050 - pomoc s matematikou

Odpovědět
AstroMiK
Příspěvky: 523
Registrován: 08 pro 2017, 19:05
Reputation: 0

Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od AstroMiK » 03 črc 2022, 14:26

Zdravím.

Potřebuji poradit s matematikou.

Mám náklonoměr (MPU6050), ze kterého přes I2C vyčítám tíhové zrychlení ve 3 osách jako 3 nezávislá INT čísla.
Náklonoměr je v krabičce vlepený "obecně" - může tam být vtěsnaný i nějak našikmo.

Mám v plánu provést kalibraci, při které řeknu že TEĎ krabička míří svisle vzhůru (k zenitu).
Získám tím hodnoty zrychlení pro nulový náklon ze všech os ('X0', 'Y0' a 'Z0').

Když pak krabičku někam namířím, přečtu aktuální hodnoty tíhového zrychlení z každé osy ('Xa', 'Ya' a 'Za').

Jak mám těchto 6 čísel přepočítat, abych získal jen jednu hodnotu, která bude udávat odchylku ve stupních od vertikální (zkalibrované) polohy?
Odchylka bude vždycky kladná - směr mě nezajímá.

Potřebuju něco jako:

Kód: Vybrat vše

float odchylka(int X0, int Y0, int Z0, int Xa, int Ya, int Za)
  {
    odchylka = ...........;       // odchylka ve stupnich od zenitu

    return odchylka;
  }
  

Našel jsem nějaké vzorce, které ale počítají každou osu zvlášť a spoléhají na to, že je náklonoměr uložený v krabičce vodorovně.

Odkazy:
https://arduino8.webnode.cz/news/lekce- ... -mpu-6050/
https://dronebotworkshop.com/mpu-6050-level/

To ale neřeší můj požadavek.

jankop
Příspěvky: 959
Registrován: 06 zář 2017, 20:04
Reputation: 0
Bydliště: Brno
Kontaktovat uživatele:

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od jankop » 07 črc 2022, 13:37

Přes MPU6050 expert nejsem. Ale myslím, že bude třeba, abys nejprve trošku oprášil základy fyziky. Bavíme-li se o náklonoměru, pak musíš využívat gyroskopický kanál a ten snad nemá výstup ve zrychlení, nýbrž v úhlu natočení. Akcelerometr totiž nejde kalibrovat nějakým statickým polohováním senzoru. Ale v případě gyroskopu by to neměl být problém, nakalibrovat ho třeba i ve třech osách.
Minulý týden mi dva moduly MPU6050 došly, chystám s nimi také nějaký, jak se teď s oblibou říká, projekt. Je legrační, že některým modulům s MPU6050 říkají tříosé a některým šestiosé.

jankop
Příspěvky: 959
Registrován: 06 zář 2017, 20:04
Reputation: 0
Bydliště: Brno
Kontaktovat uživatele:

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od jankop » 07 črc 2022, 14:59

Teď jsem si uvědomil ještě jeden problém. ten gyroskop nemá nulovou polohu. To znamená, že minimálně po každém vypnutí je nutné jej znovu nakalibrovat. Podle tvých požadavků by nejspíš stačilo pracovat pouze s osou Z. Když má osa Z odchylku 0°, tak je krabička v horizonut, respektive jak říkáš, vršek míří k zenitu. Nebo pak počítat ve sférických souřadnicích a porovnávat změnu úhlu průvodiče. Je ovšem otázka, jestli by byl tento druhý způsob přesnější. Ovšem výpočetně je výrazně náročnější.

AstroMiK
Příspěvky: 523
Registrován: 08 pro 2017, 19:05
Reputation: 0

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od AstroMiK » 07 črc 2022, 17:12

Díky za reakce.

Myslím ale, že nemáš pravdu.

Akcelerometr měří tíhové zrychlení (gravitační zrychlení: 9,81 m/s2). Je to statická hodnota, která se mění podle toho, v jaké poloze se čidlo nachází.
Nepočítám samozřejmě s tím, že se čidlo bude nacházet v jedoucím autě, které by svým pohybem hodnoty zrychlení v některých osách ovlivňovalo.

Tady jsou reálné příklady hodnot při naklonění ve dvou polohách:

Kód: Vybrat vše

  hodnoty zrychlení pro zenit   : AXz= 7760   AYz= 12581   AZz= -4166
  hodnoty zrychlení pro horizont: AXh= 4408   AYh= 6034    AZh= 18643

Naproti tomu gyroskopy měří rychlost otáčení (dynamickou změnu - pohyb).
Když je čidlo v klidu, tak dávají gyroskopy v podstatě nulové hodnoty (nějaké nepřesnosti tam jsou vždycky).

Kód: Vybrat vše

Ustálené gyroskopy v poloze zenit:      GXz= -375  GYz= 158  GZz= -275
Ustálené gyroskopy v poloze horizont:   GXh= -379  GYh= 159  GZh= -276

Objevil jsem, že čidlo má několik registrů, pomocí kterých se dá nastavit offset.
Dá se tím provést částečná korekce polohy čidla vůči krabičce tak, že ta nulová (zenitová) poloha přesně souhlasí.
Problém ale pak nastává v horizontální poloze. Tam už dostávám vypočtený úhel třeba o 15° špatně.
Čím přesněji se podaří umístit čidlo vůči osám krabičky, tím ta chyba klesá.

Zkusil jsem i následující věc, ale to byla vyloženě slepá ulička.
Zjistil jsem která z os se při náklonu mezi horizontem a zenitem mění nejvíc a pak jsem její aktuální hodnotu zkusil přemapovat na 0° až 90° pro získané krajní hodnoty.
Kraje mi pak sice souhlasily přesně, ale třeba 45° bylo úplně mimo.
Tam je problém, že se nesmí používat lineární přemapování, ale nějaké obloukové funkce (SIN, COS ...).

jankop
Příspěvky: 959
Registrován: 06 zář 2017, 20:04
Reputation: 0
Bydliště: Brno
Kontaktovat uživatele:

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od jankop » 07 črc 2022, 21:51

Máš částečně pravdu. To tíhové zrychlení vytváří statickou odchylku od nuly, podle natočení os s ohledem na vektor tíhového zrychlení. Ale rozhodně to nefunguje staticky. To by nebyl akcelerometr. Pokud s modulem pojedeš v autě, tak se hodnoty budou měnit pouze když budeš zrychlovat, brzdit nebo zatáčet.
Tenhle článek je zajímavý, hlavně příklad 2.
https://navody.dratek.cz/navody-k-produ ... gL4hPD_BwE

AstroMiK
Příspěvky: 523
Registrován: 08 pro 2017, 19:05
Reputation: 0

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od AstroMiK » 10 črc 2022, 17:01

S tím gyroskopem to není dobrý nápad.

Je tam několik problémů:

- Hrozně rychle se musí číst FIFO buffer, do kterého se ukládají všechny změny natočení. Arduino si musí neustále udržovat přehled, v jakém aktuálním úhlu je čidlo natočeno.
Když dojde k nějakému zpoždění a buffer přeteče, jsou veškeré úhly blbě.
Arduino jede na 100% výkonu a nemá čas téměř na nic jiného.

- Před každým použitím by se musela provést kalibrace základní polohy.

- Celkově je to takové nějaké nestabilní. Když několikrát pohnu čidlem a vrátím se na původní polohu, tak málokdy získám původní úhlel - odchylky bývají několik stupňů, ale stalo se, že to ulítlo třeba i o 20°.


-----

Opustil jsem řešení s gyroskopy a vrátil se zpátky k akcelerometrům.

Níže přikládám první neučesaný, ale funkční nástřel. I když je čidlo záměrně vychýlené vůči krabičce (viz přiložené fotky),
dosahuji maximální chyby 2°. Ve většině rozsahu dokonce pod 1°. To mi úplně stačí.

Když se podaří vlepit čidlo vůči krabičce přesněji, chyba se ještě sníží.


Princip je následující.

V prvním kroku dojde k nastavení offsetových registrů čidla - tím se docela přesně srovná jedna poloha.
Registry je nutné nastavit zkusmo ručně. Dá to trochu práce, ale dělá se to jen jednou. Zjištěné hodnoty by se pak zapsaly někam do EEPROM a příště už by se jen načetly.

Z takto nastaveného čidla se z aktuálního zrychlení všech tří os (50 zprůměrovaných hodnot do proměnných 'Xp', 'Yp' a 'Zp') přes ATAN2() spočte úhel.

Kód: Vybrat vše

angle_zz = atan2(Zp, sqrt(square(Xp) + square(Yp))  )/(PI/180);
Ve druhém kroku se krabička položí horizontálně a z hodnot zrychlení se stejným způsobem spočte horizontální úhel.
Tady už se nastavení offsetu neprovádí.

Kód: Vybrat vše

angle_zh = atan2(Zp, sqrt(square(Xp) + square(Yp))  )/(PI/180); 




Tyto dva úhly se zapamatují a budou použity jako krajní hodnoty pro přemapování.
V příkladu mi vyšly úhly angle_zz = 89,85° pro zenit a angle_zh = 4,55° pro horizont.

Pak už se jen aktuální hodnoty zrychlení ze všech os přepočítávají na nepřesný úhel a přemapovávají se na 0 až 90°.
orientace.jpg

kalibrace.jpg


Ukázkový program:

Kód: Vybrat vše

#include <Wire.h>

int addr = 0x68;                                  // I2C adresa cidla
int Xp, Yp, Zp;                                   // zprumerovane tihove zrychleni v kazde ose


int offset_x, offset_y, offset_z;                 // rucne zadane hodnoty pro kalibraci nulove polohy

float angle_z;                                    // pomocne vypocty uhlu nakloneni bez premapovani
float angle_zh, angle_zz;                         // krajni vypoctene uhly, ktre se maji premapovat na 0 az 90 stupnu

//-------------------------------------
void setup()
  {
    Serial.begin(9600);

    Wire.begin ();

    Wire.beginTransmission (addr);                // pocatecni reset
    Wire.write (0x6B);
    Wire.write (0x00);
    Wire.endTransmission (true);

    Wire.beginTransmission (addr);                // nastaveni nejcitlivejsiho rozsahu zrychleni (+/- 2g)
    Wire.write (0x1C);
    Wire.write(0x00);
    Wire.endTransmission (true);




    Serial.println("Kalibrace: nastav 'ZENIT'");
    
    offset();                                             // rucni nastavovani takovych hodnot, aby na vystupu jednotlivych kanalu byla cisla 0 , 0 a 16384

    Serial.println("Kalibrace: nastav 'HORIZONT'");

    Serial.println(">>");
    while (Serial.available() == 0)  delay(100);          // ceka na odklepnuti horizontove polohy
    while(Serial.available()) Serial.read();

    
    prumer50();                                           // do promennych Xp, Xp a Zp ulozi prumernou hodnotu zrychleni z 50 mereni


    // angle_xh = atan2(Xp, sqrt(square(Yp) + square(Zp))      )/(PI/180);     // pro odlisne orientovane cidlo se prepocty provadi pro jine osy
    // angle_yh = atan2(Yp, sqrt(square(Xp) + square(Zp))      )/(PI/180);
    angle_zh = atan2(Zp, sqrt(square(Xp) + square(Yp))      )/(PI/180);  // horizontovy uhel bude slouzit k premapovani druheho koncoveho bodu

   




    Serial.println("Zkalibrovano.");
    Serial.println("Tyhle hodnoty by se mely poznamenat nekam do EEPROM, aby se priste uz nemuselo znova kalibrovat:");
    Serial.println("Po resetu by pak stacilo offsety z EEPROM odeslat primo do MPU6050 a krajni uhly nacist zpatky do promennych");
    Serial.print("offset_x: ");
    Serial.println(offset_x);
    Serial.print("offset_y: ");
    Serial.println(offset_y);
    Serial.print("offset_z: ");
    Serial.println(offset_z);
    Serial.print("angle_zz: ");
    Serial.println(angle_zz);
    Serial.print("angle_zh: ");
    Serial.println(angle_zh);

    Serial.println(">>");
    while (Serial.available() == 0)  delay(100);          //  cekani na odklepnuti
    while(Serial.available()) Serial.read() ;
  }




//--------------------------------------------------
void loop()
  {

    prumer50();                // do promennych Xp, Yp a Zp ulozi prumernou hodnotu zrychleni z 50 mereni

    // angle_x = atan2(Xp, sqrt(square(Yp) + square(Zp))      )/(PI/180);     // pro odlisne orientovane cidlo se prepocty provadi pro jine osy
    // angle_y = atan2(Yp, sqrt(square(Xp) + square(Zp))      )/(PI/180);
    angle_z = atan2(Zp, sqrt(square(Xp) + square(Yp))      )/(PI/180);
    

    float z_map = mapfloat(angle_z, angle_zh , angle_zz , 90 , 0);          // premapovani: Zenit = 0, horizont = 90

    Serial.print("premapovany uhel 'z' = ");
    Serial.println(z_map);

  }


//--------------------------------------------------
// rucni nastaveni offsetu vsech 3 os pro zenitovou polohu 
void offset(void)
  {
    Serial.println("Prenastav offsety tak, aby se X a Y priblizilo nule a Z se priblizilo cislu 16384");
    Serial.println("POZOR: plati to jen pro situaci, ze v zenitu napis na cidle smeruje nahoru (alespon zhruba).");    
    Serial.println("Pro jinak orientovane cidlo se musi osy prohodit, nebo se dokonce kalibruje tak, ze se nastavuje zaporne cislo -16384");    
    Serial.println("Pro nastaveni pouzivej znaky 'X', 'Y' a 'Z' nasledovane INT cislem. (Priklad: 'Y547', nebo 'Z-2145')");
    Serial.println("Ukonceni offsetove kalibrace znakem 'K'");

    Serial.println(">>");
    while (Serial.available() == 0)  delay(100);          // ceka na odklepnuti zenitove polohy
    while(Serial.available()) Serial.read();

    while (true)
      {
        if (Serial.available())
          {
            char znak = Serial.read();

            if (znak == 'X')
              {
                offset_x = Serial.parseInt();
                Wire.beginTransmission (addr); 
                Wire.write (6);
                Wire.write(offset_x / 256);
                Wire.endTransmission (true);
                delay(20);
                Wire.beginTransmission (addr); 
                Wire.write (7);
                Wire.write(offset_x % 256);
                Wire.endTransmission (true);
                delay(20);            
              }
            if (znak == 'Y')
              {
                offset_y = Serial.parseInt();
                Wire.beginTransmission (addr); 
                Wire.write (8);
                Wire.write(offset_y / 256);
                Wire.endTransmission (true);
                delay(20);
                Wire.beginTransmission (addr); 
                Wire.write (9);
                Wire.write(offset_y % 256);
                Wire.endTransmission (true);
                delay(20);            
              }
    
            if (znak == 'Z')
              {
                offset_z = Serial.parseInt();
                Wire.beginTransmission (addr); 
                Wire.write (10);
                Wire.write(offset_z / 256);
                Wire.endTransmission (true);
                delay(20);
                Wire.beginTransmission (addr); 
                Wire.write (11);
                Wire.write(offset_z % 256);
                Wire.endTransmission (true);
                delay(20);            
              }

            if (znak == 'K')
              {
                
                // angle_zx = atan2(Xp, sqrt(square(Yp) + square(Zp))      )/(PI/180);     // pro odlisne orientovane cidlo se prepocty provadi pro jine osy
                // angle_zy = atan2(Yp, sqrt(square(Xp) + square(Zp))      )/(PI/180);
                angle_zz = atan2(Zp, sqrt(square(Xp) + square(Yp))      )/(PI/180);    // zenitovy uhel bude slouzit k premapovani prvniho koncoveho bodu
                return;
              }

          }

        prumer50();                // do promennych Xp, Yp a Zp ulozi prumernou hodnotu zrychleni z 50 mereni
        Serial.print("\t\tX= ");
        Serial.print(Xp);
        Serial.print("\t\tY= ");
        Serial.print(Yp);
        Serial.print("\t\tZ= ");
        Serial.println(Zp);

      
      }
  }




//--------------------------------------------------
// prumerovani 50 vzorku zrychleni ve vsech osach
//  vysledek je ulozeny v globalnich promennych Xp, Yp a Zp
void prumer50(void)
  {
    long sumaX = 0;
    long sumaY = 0;
    long sumaZ = 0; 
    for (byte i=0; i < 50; i++)                              // prumerovani 50 hodnot nulove polohy
      {      
        Wire.beginTransmission (addr);
        Wire.write (0x3B);                                   // uvodni registr pro zrychleni v ose X (2 bajty)
        Wire.endTransmission (false);
        Wire.requestFrom (addr, 6, true);                    // celkem 6 registru (2 pro kazdou osu)
        
        sumaX = sumaX + (Wire.read() << 8 | Wire.read());    // aktualni hodnoty se nascitavaji do sumarnich promennych
        sumaY = sumaY + (Wire.read() << 8 | Wire.read());
        sumaZ = sumaZ + (Wire.read() << 8 | Wire.read());
    
      }
    
    Xp = sumaX / 50;                                         // zprumerovane hodnoty zenitovou polohu
    Yp = sumaY / 50;
    Zp = sumaZ / 50;
  }



//--------------------------------------------------
// nahrada funkce map() pro desetinna cisla
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
  {
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  }



AstroMiK
Příspěvky: 523
Registrován: 08 pro 2017, 19:05
Reputation: 0

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od AstroMiK » 12 črc 2022, 20:44

Tak se mi nakonec podařilo v krabičce najít místo, kam se destička s náklonoměrem vejde a její osy se dají nastavit dostatečně přesně vůči osám krabičky.
Takže je ve výsledku přesnost v celém rozsahu natáčení velice dobrá.

Musel jsem jen v programu z předchozího příspěvku trochu přeházet osy a směry v kalibraci nulové polohy.

Systém ruční kalibrace offsetů by možná bylo dobré nějak zautomatizovat, ale protože se dělá jen 1x, tak se na to asi vykašlu.

Celý program tady neuvádím. Využívám tam i další speciální přídavný HW (například I2C EEPROM pro ukládání kalibračních offsetových hodnot), takže by to asi nemělo význam.

Tady je trochu rozmazané video s porovnáním vodováhy v mobilu a MPU-6050 v mém zařízení.
https://youtu.be/BQFMpext0vE

Umístění a orientace destičky MPU-6050:
umisteni.jpg

AstroMiK
Příspěvky: 523
Registrován: 08 pro 2017, 19:05
Reputation: 0

Re: Náklonoměr MPU6050 - pomoc s matematikou

Příspěvek od AstroMiK » 31 črc 2022, 08:13

Vytvořil jsem autokalibrační program pro čip MPU-6050.
Je docela rychlý a zdá se být i dostatečně přesný.
Kromě knihovny pro I2C komunikaci (Wire.h) nepoužívá žádné jiné speciální knihovny.
Vyzkoušel jsem ho na dvou odlišných modulech.

Než ho ale zapracuji do mého zařízení a zveřejním postup kalibrace, potřeboval bych ho ověřit i na jiných modulech.
Když budu mít k dispozici víc výsledků, dalo by se na tom provést i nějaké statistické vyhodnocení a možná by to šlo ještě zrychlit.
Nebo naopak - pokud by se ukázalo, že to některé z čidel nezvládne, musel bych rozšířit oblast hledání offsetů a tím kalibraci o něco protáhnout.

A proto bych chtěl poprosit jestli byste mi s tím nepomohli.

Program pro Arduino přikládám níže.
Funguje na běžných Arduinech Uno, Nano ...
Je ale optimalizovaný tak, aby šel bez úprav použít i pro desky s 32-bitovými procesory STM32F103 (STMduino, BluePill ...).


Postup je velice jednoduchý:

1) Na nějakou krabičku nalepit modul se senzorem tak, aby nápis na čipu směřoval "tak nějak" vzhůru a krabičkou
se dalo otáčet tak, aby hlavní pohyb byl kolem osy Y.
POZOR! Různé moduly mohou mít tu osu Y jinak orientovanou (fotografie níže)

Umístění modulu nemusí být vůbec přesné (+/- 10 stupňů nehraje žádnou roli).
Právě kvůli tomu se dělá ta kalibrace, aby se modul nemusel umisťovat přesně vůči osám krabičky.


2) Propojení s Arduinem je jen klasické I2C přes 4 dráty (nepoužívá se ani žádný interrupt):
Vcc - 5V
GND - GND
SDA - A4
SCL - A5


Při připojení k deskám s STM, který používá 3V úrovně, je připojení následující:
3V3 - 3V3 (vývod 3V3 se nachází jen na některých modulech)
GND - GND
SDA - PB7
SCL - PB6
Sériová linka: "Serial - PA9, PA10" - (tady ale záleží na nastavení parametrů kompilace)


3) Zapnout USB a nahrát program.
Spustit sériový terminál.
Pokud by v terminálu zůstaly po nahrání programu nějaké nečitelné znaky, tak okno terminálu vymazat a Arduino zresetovat.

4) V sériovém terminálu se začnou zobrazovat instrukce a spousty čísel.
Když program na něco čeká, jsou v terminálu zobrazeny 2 šipky ">>".
Po provedení příslušné instrukce odešlete do terminálu libovolný znak a tím program dostane signál, že má pokračovat.

Z pohledu "testovacího pilota" jsou instrukce následující:
Položit krabičku tak, aby bylo čidlo horizontálně (nápis na čipu vzhůru) >> odklepnout, čekat.
Otočit krabičku kolem osy Y do vertikální polohy >> odklepnout
Podložit krabičku trojúhelníkem na 45° >> odklepnout, čekat na dokončení 10 testovacích měření.
Pak celý výpis terminálu zkopírovat a odeslat (buď sem do toho vlákna, nebo emailem)


5) Pokud proběhla kalibrace až do konce, mělo by čidlo po posledním odklepnutí ">>" začít měřit náklon
kolem osy Y jako obyčejná vodováha v rozsahu 0 až 90°.
Mělo by to fungovat i mírně pod 0 (namíření pod horizont) a mírně přes 90°. Tam už ale není zaručena přesnost.
Tohle už jen vyzkoušejte jestli ukazuje správné hodnoty ve sloupci "naklon", nebo jestli tam vypisuje nesmyslné hodnoty (NaN, inf ... ).
Tenhle výpis už nepotřebuju.


Během kalibrace se může stát, že program skončí chybovým hlášením:
"Chyba kalibrace naklonu"
I tato informace je pro mne dost důležitá a předchozí výpis hodnot může napovědět, co se nepovedlo a co mám kde rozšířit.


To je všechno.

O čidlo se nemusíte bát. Přenastavené hodnoty offsetových registrů si to pamatuje jen do vypnutí napájení.
Pak se natáhnou zpátky tovární hodnoty.

V tomto testovacím programu nepoužívám ani žádné zápisy do EEPROM.
EEPROM by se použila až ve finální verzi, kde by se do ní ukládaly hodnoty zjištěné při kalibraci.
Při zapnutí by se ty hodnoty načetly z EEPROM a odeslaly přímo do offsetových registrů MPU-6050 - kalibrace by pak nebyla nutná.


Když v programu zakomentujete řádku:

Kód: Vybrat vše

  #define dbg
bude se místo výpisů čísel zobrazovat jen jednoduchý textový bargraf.
Takhle nějak bych si ve finále představoval kalibraci pro běžného uživatele.

Kód: Vybrat vše

Nastav cidlo horizontalne (napis na senzoru smeruje vzhuru)
>>
[                    |   |   |   ]
[********************|***|***|***]


Otoc cidlo kolem osy Y do svisle polohy (napis na senzoru miri do boku)
>>

Video je tady: https://youtu.be/fw0c_Ucxx8A

Program tady: http://www.astromik.org/forum/6050/kali ... pu6050.ino

První výsledky vypadají takto: http://www.astromik.org/forum/6050/

A tady nějaké fotky:
mpu6050.jpg
smer_osy_y.jpg

Odpovědět

Kdo je online

Uživatelé prohlížející si toto fórum: Žádní registrovaní uživatelé a 2 hosti