Tutorial accelerometru și giroscop: 3 pași

2024 Autor: John Day | [email protected]. Modificat ultima dată: 2024-01-30 11:47

Introducere

Acest ghid este destinat tuturor celor interesați de utilizarea accelerometrelor și giroscopului, precum și a dispozitivelor combinate IMU (Inertial Measurement Unit) în proiectele lor electronice.

Vom acoperi:

• Ce măsoară un accelerometru?
• Ce măsoară un giroscop (aka giroscop)?
• Cum se convertesc citirile analog-digitale (ADC) pe care le obțineți de la acest senzor în unități fizice (acestea ar fi g pentru accelerometru, deg / s pentru giroscop)
• Cum să combinați citirile accelerometrului și giroscopului pentru a obține informații exacte despre înclinația dispozitivului dvs. față de planul de sol

De-a lungul articolului voi încerca să mențin matematica la minimum. Dacă știți ce sunt Sine / Cosine / Tangent, atunci ar trebui să puteți înțelege și utiliza aceste idei în proiectul dvs., indiferent de platforma pe care o utilizați: Arduino, Elice, Basic Stamp, chips-uri Atmel, Microchip PIC etc.

Există oameni care cred că aveți nevoie de matematică complexă pentru a utiliza o unitate IMU (filtre complexe FIR sau IIR, cum ar fi filtre Kalman, filtre Parks-McClellan etc.). Puteți cerceta toate acestea și puteți obține rezultate minunate, dar complexe. Modul meu de a explica lucrurile necesită doar matematică de bază. Sunt un mare credincios în simplitate. Cred că un sistem simplu este mai ușor de controlat și monitorizat, pe lângă faptul că multe dispozitive încorporate nu au puterea și resursele necesare pentru a implementa algoritmi complexi care necesită calcule matriciale.

Voi folosi ca exemplu o nouă unitate IMU, Acc_Gyro Accelerometer + Gyro IMU. Vom folosi parametrii acestui dispozitiv în exemplele noastre de mai jos. Această unitate este un dispozitiv bun pentru a începe, deoarece constă din 2 dispozitive:

- LIS331AL (foaie tehnică) - un accelerometru triaxial 2G - LPR550AL (foaie tehnică) - un pas și rulare pe două axe, giroscop 500 deg / sec

Împreună reprezintă o unitate de măsurare inerțială a 5 grade de libertate. Acum este un nume de lux! Cu toate acestea, în spatele numelui de lux este un dispozitiv de combinație foarte util pe care îl vom acoperi și explica în detaliu în acest ghid.

Pasul 1: Accelerometrul

Pentru a înțelege această unitate vom începe cu accelerometrul. Când vă gândiți la accelerometre, este adesea util să imaginați o cutie în formă de cub cu o bilă înăuntru. S-ar putea să vă imaginați altceva, cum ar fi un cookie sau o gogoasă, dar îmi voi imagina o minge:

Dacă luăm această cutie într-un loc fără câmpuri de gravitație sau, de altfel, fără alte câmpuri care ar putea afecta poziția mingii - mingea va pluti pur și simplu în mijlocul cutiei. Vă puteți imagina căsuța se află în spațiul cosmic, departe de orice corpuri cosmice sau, dacă un astfel de loc este greu de găsit, imaginați-vă cel puțin o navă spațială care orbitează în jurul planetei, unde totul este în stare de greutate. Din imaginea de mai sus puteți vedea că atribuim fiecărei axe câte o pereche de pereți (am eliminat peretele Y + pentru a putea privi în interiorul cutiei). Imaginați-vă că fiecare perete este sensibil la presiune. Dacă deplasăm brusc caseta spre stânga (o accelerăm cu accelerația 1g = 9.8m / s ^ 2), mingea va atinge peretele X-. Apoi măsurăm forța de presiune pe care mingea o aplică peretelui și obținem o valoare de -1g pe axa X.

Vă rugăm să rețineți că accelerometrul va detecta de fapt o forță care este direcționată în direcția opusă vectorului de accelerație. Această forță este adesea numită Forță Inerțială sau Forță Fictivă. Un lucru pe care ar trebui să-l învățați este că un accelerometru măsoară indirect accelerația printr-o forță care este aplicată pe unul dintre pereții săi (conform modelului nostru, ar putea fi un arc sau altceva în accelerometrele din viața reală). Această forță poate fi cauzată de accelerație, dar așa cum vom vedea în exemplul următor, nu este întotdeauna cauzată de accelerație.

Dacă luăm modelul nostru și îl punem pe Pământ, mingea va cădea pe peretele Z și va aplica o forță de 1g pe peretele de jos, așa cum se arată în imaginea de mai jos:

În acest caz, caseta nu se mișcă, dar avem totuși o citire de -1g pe axa Z. Presiunea pe care mingea a aplicat-o pe perete a fost cauzată de o forță de gravitație. În teorie, ar putea fi un alt tip de forță - de exemplu, dacă vă imaginați că bila noastră este metalică, plasarea unui magnet lângă cutie ar putea muta mingea astfel încât să lovească un alt perete. Acest lucru a fost spus doar pentru a demonstra că, în esență, accelerometrul măsoară forța, nu accelerarea. Se întâmplă doar că accelerația provoacă o forță inerțială care este capturată de mecanismul de detectare a forței accelerometrului.

Deși acest model nu este exact modul în care este construit un senzor MEMS, acesta este adesea util în rezolvarea problemelor legate de accelerometru. Există, de fapt, senzori similari care au bile metalice în interior, sunt numiți comutatori de înclinare, totuși sunt mai primitivi și, de obicei, pot spune doar dacă dispozitivul este înclinat într-un anumit interval sau nu, nu și gradul de înclinare.

Până acum am analizat ieșirea accelerometrului pe o singură axă și asta este tot ce veți obține cu accelerometre cu o singură axă. Valoarea reală a accelerometrelor triaxiale provine din faptul că pot detecta forțe inerțiale pe toate cele trei axe. Să ne întoarcem la modelul nostru de cutie și să rotim cutia cu 45 de grade spre dreapta. Mingea va atinge acum 2 pereți: Z- și X- așa cum se arată în imaginea de mai jos:

Valorile de 0,71 nu sunt arbitrare, ele sunt de fapt o aproximare pentru SQRT (1/2). Acest lucru va deveni mai clar pe măsură ce vom introduce următorul nostru model pentru accelerometru.

În modelul anterior am fixat forța de gravitație și ne-am rotit cutia imaginară. În ultimele 2 exemple am analizat ieșirea în 2 poziții diferite ale cutiei, în timp ce vectorul de forță a rămas constant. Deși acest lucru a fost util în înțelegerea modului în care accelerometrul interacționează cu forțele exterioare, este mai practic să efectuăm calcule dacă fixăm sistemul de coordonate pe axele accelerometrului și ne imaginăm că vectorul de forță se rotește în jurul nostru.

Vă rugăm să aruncați o privire la modelul de mai sus, am păstrat culorile axelor, astfel încât să puteți face o tranziție mentală de la modelul anterior la cel nou. Imaginați-vă că fiecare axă din noul model este perpendiculară pe fețele respective ale casetei din modelul anterior. Vectorul R este vectorul de forță pe care îl măsoară accelerometrul (ar putea fi fie forța de gravitație, fie forța de inerție din exemplele de mai sus sau o combinație a ambelor). Rx, Ry, Rz sunt proiecția vectorului R pe axele X, Y, Z. Vă rugăm să observați următoarea relație:

R ^ 2 = Rx ^ 2 + Ry ^ 2 + Rz ^ 2 (ec. 1)

care este practic echivalentul teoremei lui Pitagora în 3D.

Amintiți-vă că puțin mai devreme v-am spus că valorile SQRT (1/2) ~ 0,71 nu sunt aleatorii. Dacă le introduceți în formula de mai sus, după ce ne amintim că forța noastră de gravitație a fost de 1 g, putem verifica că:

1 ^ 2 = (-SQRT (1/2)) ^ 2 + 0 ^ 2 + (-SQRT (1/2)) ^ 2

pur și simplu prin înlocuirea R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) în Eq.1

După un lung preambul de teorie, ne apropiem de accelerometrele din viața reală. Valorile Rx, Ry, Rz sunt de fapt liniar legate de valorile pe care le va emite accelerometrul din viața reală și pe care le puteți utiliza pentru efectuarea diferitelor calcule.

Înainte de a ajunge acolo, să vorbim puțin despre modul în care accelerometrele ne vor furniza aceste informații. Majoritatea accelerometrelor se vor încadra în două categorii: digitală și analogică. Accelerometrele digitale vă vor oferi informații folosind un protocol serial precum I2C, SPI sau USART, în timp ce accelerometrele analogice vor emite un nivel de tensiune într-un interval predefinit pe care trebuie să îl convertiți la o valoare digitală utilizând un modul ADC (analogic la convertor digital). Nu voi intra în prea multe detalii despre modul în care funcționează ADC, parțial pentru că este un subiect atât de extins și parțial pentru că este diferit de la o platformă la alta. Unele microcontrolere vor avea un modul ADC încorporat, unele dintre ele vor avea nevoie de componente externe pentru a efectua conversiile ADC. Indiferent ce tip de modul ADC utilizați, veți avea o valoare într-un anumit interval. De exemplu, un modul ADC de 10 biți va genera o valoare în intervalul 0..1023, rețineți că 1023 = 2 ^ 10 -1. Un modul ADC pe 12 biți va genera o valoare în intervalul 0..4095, rețineți că 4095 = 2 ^ 12-1.

Să mergem mai departe luând în considerare un exemplu simplu, să presupunem că modulul nostru ADC pe 10 biți ne-a dat următoarele valori pentru cele trei canale (axe) accelerometrului:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Fiecare modul ADC va avea o tensiune de referință, să presupunem că în exemplul nostru este de 3,3V. Pentru a converti o valoare ADC de 10 biți în tensiune, utilizăm următoarea formulă:

VoltsRx = AdcRx * Vref / 1023

O notă rapidă aici: că pentru 8bit ADC ultimul divizor ar fi 255 = 2 ^ 8 -1, iar pentru 12bit ADC ultimul divizor ar fi 4095 = 2 ^ 12 -1.

Aplicând această formulă tuturor celor 3 canale, obținem:

VolțiRx = 586 * 3,3V / 1023 = ~ 1,89V (rotunjim toate rezultatele la 2 puncte zecimale) VolțiRy = 630 * 3,3V / 1023 = ~ 2,03V VolțiRz = 561 * 3,3V / 1023 = ~ 1,81V

Fiecare accelerometru are un nivel de tensiune zero-g, îl puteți găsi în specificații, aceasta este tensiunea care corespunde la 0g. Pentru a obține o valoare de tensiune semnată, trebuie să calculăm schimbarea de la acest nivel. Să presupunem că nivelul nostru de tensiune 0g este VzeroG = 1,65V. Calculăm schimbările de tensiune de la tensiunea zero-g după cum urmează:

DeltaVoltsRx = 1,89V - 1,65V = 0,24V DeltaVoltsRy = 2,03V - 1,65V = 0,38V DeltaVoltsRz = 1,81V - 1,65V = 0,16V

Acum avem citirile accelerometrului în Volți, tot nu este în g (9,8 m / s ^ 2), pentru a face conversia finală aplicăm sensibilitatea accelerometrului, de obicei exprimată în mV / g. Să spunem Sensibilitatea noastră = 478,5mV / g = 0,4785V / g. Valorile sensibilității pot fi găsite în specificațiile accelerometrului. Pentru a obține valorile finale ale forței exprimate în g folosim următoarea formulă:

Rx = DeltaVoltsRx / Sensibilitate

Rx = 0.24V / 0.4785V / g = ~ 0.5g Ry = 0.38V / 0.4785V / g = ~ 0.79g Rz = 0.16V / 0.4785V / g = ~ 0.33g

Desigur, am putea combina toți pașii într-o singură formulă, dar am parcurs toți pașii pentru a clarifica modul în care treceți de la citirile ADC la o componentă vector forță exprimată în g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Sensibilitate (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Sensibilitate Rz = (AdcRz * Vref / 1023 - VzeroG) / Sensibilitate

Acum avem toate cele 3 componente care definesc vectorul nostru de forță inerțială, dacă dispozitivul nu este supus altor forțe decât gravitația, putem presupune că aceasta este direcția vectorului nostru de forță de gravitație. Dacă doriți să calculați înclinația dispozitivului față de sol, puteți calcula unghiul dintre acest vector și axa Z. Dacă sunteți, de asemenea, interesat de direcția de înclinare pe axă, puteți împărți acest rezultat în 2 componente: înclinarea pe axa X și Y care poate fi calculată ca unghiul dintre vectorul de gravitație și axele X / Y. Calculul acestor unghiuri este mai simplu decât ați putea crede, acum că am calculat valorile pentru Rx, Ry și Rz. Să ne întoarcem la ultimul nostru model de accelerometru și să facem câteva notații suplimentare:

Unghiurile care ne interesează sunt unghiurile dintre axele X, Y, Z și vectorul de forță R. Vom defini aceste unghiuri ca Axr, Ayr, Azr. Puteți observa din triunghiul unghi drept format din R și Rx că:

cos (Axr) = Rx / R și în mod similar: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Din Eq.1 putem deduce că R = SQRT (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).

Putem găsi acum unghiurile noastre folosind funcția arccos () (funcția inversă cos ()):

Axr = arccos (Rx / R) Ayr = arccos (Ry / R) Azr = arccos (Rz / R)

Am parcurs un drum lung pentru a explica modelul accelerometrului, doar pentru a ajunge la aceste formule. În funcție de aplicațiile dvs., ați putea dori să utilizați orice formule intermediare pe care le-am obținut. De asemenea, vom introduce în curând modelul giroscopului și vom vedea cum datele accelerometrului și giroscopului pot fi combinate pentru a oferi estimări ale înclinației și mai precise.

Dar înainte de a face asta, să facem câteva notații mai utile:

cosX = cos (Axr) = Rx / R confortabil = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Acest triplet este adesea numit Cosinus de direcție și reprezintă practic vectorul unitar (vector cu lungimea 1) care are aceeași direcție ca vectorul nostru R. Puteți verifica cu ușurință că:

SQRT (cosX ^ 2 + cozy ^ 2 + cosZ ^ 2) = 1

Aceasta este o proprietate plăcută, deoarece ne scutește de monitorizarea modulului (lungimea) vectorului R. De multe ori, dacă suntem doar interesați de direcția vectorului nostru inerțial, are sens să normalizăm modulul pentru a simplifica alte calcule.

Pasul 2: Giroscop

Nu vom introduce niciun model de cutie echivalent pentru giroscop, așa cum am făcut pentru accelerometru, în schimb vom sări direct la al doilea model de accelerometru și vom arăta ce măsoară giroscopul în funcție de acest model.

Fiecare canal de giroscop măsoară rotația în jurul uneia dintre axe. De exemplu, un giroscop cu 2 axe va măsura rotația în jurul (sau unii ar putea spune „despre”) axele X și Y. Pentru a exprima această rotație în numere, să facem câteva notații. Mai întâi să definim:

Rxz - este proiecția vectorului forței inerțiale R pe planul XZ Ryz - este proiecția vectorului forței inerțiale R pe planul YZ

Din triunghiul unghi drept format din Rxz și Rz, folosind teorema lui Pitagora obținem:

Rxz ^ 2 = Rx ^ 2 + Rz ^ 2 și în mod similar: Ryz ^ 2 = Ry ^ 2 + Rz ^ 2

de asemenea, rețineți că:

R ^ 2 = Rxz ^ 2 + Ry ^ 2, aceasta poate fi derivată din ecuațiile 1 și mai sus, sau poate fi derivată din triunghiul unghi drept format din R și Ryz R ^ 2 = Ryz ^ 2 + Rx ^ 2

Nu vom folosi aceste formule în acest articol, dar este util să observăm relația dintre toate valorile din modelul nostru.

În schimb, vom defini unghiul dintre axa Z și vectorii Rxz, Ryz după cum urmează:

Axz - este unghiul dintre axa Rxz (proiecția lui R pe planul XZ) și axa Z Ayz - este unghiul dintre axa Ryz (proiecția lui R pe planul YZ) și axa Z

Acum ne apropiem de ceea ce măsoară giroscopul. Giroscopul măsoară rata modificărilor unghiurilor definite mai sus. Cu alte cuvinte, va genera o valoare care este liniar legată de rata de schimbare a acestor unghiuri. Pentru a explica acest lucru, să presupunem că am măsurat unghiul de rotație în jurul axei Y (care ar fi unghiul Axz) la momentul t0 și îl definim ca Axz0, apoi am măsurat acest unghi la un moment ulterior t1 și a fost Axz1. Rata de schimbare va fi calculată după cum urmează:

RateAxz = (Axz1 - Axz0) / (t1 - t0).

Dacă exprimăm Axz în grade și timpul în secunde, atunci această valoare va fi exprimată în deg / s. Iată ce măsoară un giroscop.

În practică, un giroscop (cu excepția cazului în care este un giroscop digital special) vă va oferi rareori o valoare exprimată în deg / s. La fel ca pentru accelerometru, veți obține o valoare ADC pe care va trebui să o convertiți în deg / s folosind o formulă similară cu Eq. 2 pe care l-am definit pentru accelerometru. Să introducem formula de conversie ADC în deg / s pentru giroscop (presupunem că folosim un modul ADC de 10 biți, pentru ADC de 8 biți înlocuiți 1023 cu 255, pentru ADC de 12 biți înlocuiți 1023 cu 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensibilitate Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Sensibilitate

AdcGyroXZ, AdcGyroYZ - sunt obținute din modulul nostru adc și reprezintă canalele care măsoară rotația proiecției vectorului R în XZ, respectiv în planurile YZ, care este echivalentul cu care se spune că rotația s-a făcut în jurul axelor Y și, respectiv, a X.

Vref - este tensiunea de referință ADC pe care o vom folosi de 3,3V în exemplul de mai jos VzeroRate - este tensiunea de rată zero, cu alte cuvinte tensiunea pe care o emite giroscopul atunci când nu este supusă niciunei rotații, pentru placa Acc_Gyro este de exemplu 1.23V (puteți găsi aceste valori în specificații) Sensibilitate - este sensibilitatea giroscopului dvs. este exprimată în mV / (deg / s) adesea scrisă ca mV / deg / s, practic vă spune câte mV vor ieșirea giroscopului crește, dacă măriți viteza de rotație cu un deg / s. Sensibilitatea plăcii Acc_Gyro este de exemplu 2mV / deg / s sau 0,002V / deg / s

Să luăm un exemplu, să presupunem că modulul nostru ADC a returnat următoarele valori:

AdcGyroXZ = 571 AdcGyroXZ = 323

Folosind formula de mai sus și utilizând parametrii specificațiilor plăcii Acc_Gyro vom obține:

RateAxz = (571 * 3,3V / 1023 - 1,23V) / (0,002V / deg / s) = ~ 306 deg / s RateAyz = (323 * 3,3V / 1023 - 1,23V) / (0,002V / deg / s) = ~ -94 grade / s

Cu alte cuvinte, dispozitivul se rotește în jurul axei Y (sau putem spune că se rotește în plan XZ) cu o viteză de 306 deg / s și în jurul axei X (sau putem spune că se rotește în plan YZ) cu o viteză de - 94 grade / s. Vă rugăm să rețineți că semnul negativ înseamnă că dispozitivul se rotește în direcția opusă față de direcția pozitivă convențională. Prin convenție, o direcție de rotație este pozitivă. O foaie bună de specificații a giroscopului vă va arăta ce direcție este pozitivă, altfel va trebui să o găsiți experimentând cu dispozitivul și notând ce direcție de rotație duce la creșterea tensiunii pe pinul de ieșire. Acest lucru se face cel mai bine folosind un osciloscop, deoarece de îndată ce opriți rotația, tensiunea va scădea înapoi la nivelul zero. Dacă utilizați un multimetru, va trebui să mențineți o rată de rotație constantă timp de cel puțin câteva secunde și să notați tensiunea în timpul acestei rotații, apoi comparați-o cu tensiunea de rată zero. Dacă este mai mare decât tensiunea zero, înseamnă că sensul de rotație este pozitiv.

Pasul 3: Combinați accelerometrul și giroscopul

Reunind totul - Combinând date accelerometru și giroscop

Dacă citiți acest articol, probabil că ați achiziționat sau intenționați să achiziționați un dispozitiv IMU sau probabil intenționați să construiți unul din accelerometru și dispozitive giroscopice separate.

Primul pas în utilizarea unui dispozitiv IMU combinat care combină un accelerometru și un giroscop este alinierea sistemelor lor de coordonate. Cel mai simplu mod de a face acest lucru este să alegeți sistemul de coordonate al accelerometrului ca sistem de coordonate de referință. Majoritatea fișelor de date ale accelerometrului vor afișa direcția axelor X, Y, Z în raport cu imaginea cipului fizic sau a dispozitivului. De exemplu, aici sunt direcțiile axelor X, Y, Z așa cum se arată în specificațiile pentru placa Acc_Gyro:

Pașii următori sunt:

Identificați ieșirile giroscopului care corespund valorilor RateAxz, RateAyz discutate mai sus. Determinați dacă aceste ieșiri trebuie inversate datorită poziției fizice a giroscopului față de accelerometru

Nu presupuneți că dacă un giroscop are o ieșire marcată X sau Y, aceasta va corespunde oricărei axe din sistemul de coordonate al accelerometrului, chiar dacă această ieșire face parte dintr-o unitate IMU. Cel mai bun mod este să-l testați. Presupunând că ați fixat poziția giroscopului față de accelerometru. Se presupune că limitele giroscopului și accelerometrului sunt paralele una cu cealaltă, adică plasezi giroscopul la un unghi multiplu de 90 de grade față de cipul accelerometrului. Dacă ați achiziționat o placă IMU, șansele sunt că acestea sunt deja aliniate în acest fel. Nu vom discuta în acest articol modele în care giroscopul este plasat la un unghi neregulat față de accelerometru (să presupunem 45 sau 30 de grade), deși acest lucru ar putea fi util în unele aplicații.

Iată o secvență de probă pentru a determina care ieșire a giroscopului corespunde valorii RateAxz discutate mai sus.

- începeți de la plasarea dispozitivului în poziție orizontală. Atât ieșirile X, cât și Y ale accelerometrului ar genera tensiunea zero-g (de exemplu pentru placa Acc_Gyro aceasta este 1,65V)

- apoi începeți să rotiți dispozitivul în jurul axei Y, un alt mod de a spune că rotiți dispozitivul în plan XZ, astfel încât ieșirile accelerometrului X și Z să se schimbe și ieșirea Y să rămână constantă. - în timp ce rotiți dispozitivul cu o viteză constantă, care se modifică ieșirea giroscopului, celelalte ieșiri giroscopice ar trebui să rămână constante - ieșirea giroscopului care s-a modificat în timpul rotației în jurul axei Y (rotație în plan XZ) va furniza valoarea de intrare pentru AdcGyroXZ, din care calculăm RateAxz - ultimul pas este să ne asigurăm că direcția de rotație corespunde modelului nostru, în unele cazuri poate fi necesar să inversați valoarea RateAxz datorită poziției fizice a giroscopului față de accelerometru - efectuați din nou testul de mai sus, rotind dispozitivul în jurul pe axa Y, de această dată monitorizați ieșirea X a accelerometrului (AdcRx în modelul nostru). Dacă AdcRx crește (primele 90 de grade de rotație din poziția orizontală), atunci AdcGyroXZ ar trebui să crească și el. În caz contrar, trebuie să inversați RateAxz, puteți realiza acest lucru introducând un factor de semn în Eq.3, după cum urmează:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensibilitate, unde InvertAxz este 1 sau -1

același baston de testare se face pentru RateAyz, prin rotirea dispozitivului în jurul axei X și puteți identifica ce ieșire giroscopică corespunde RateAyz și dacă trebuie inversat. Odată ce aveți valoarea pentru InvertAyz, ar trebui să utilizați următoarea formulă pentru a calcula RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Sensibilitate

Dacă ați face aceste teste pe placa Acc_Gyro, veți obține următoarele rezultate:

- pinul de ieșire pentru RateAxz este GX4 și InvertAxz = -1. - pinul de ieșire pentru RateAyz este GY4 și InvertAyz = -1

Din acest moment vom considera că ați configurat IMU în așa fel încât să puteți calcula valorile corecte pentru Axr, Ayr, Azr (așa cum este definit partea 1. Accelerometru) și RateAxz, RateAyz (așa cum este definit în partea 2. Giroscop). În continuare vom analiza relațiile dintre aceste valori care se dovedesc utile pentru a obține o estimare mai precisă a înclinației dispozitivului față de planul de sol.

S-ar putea să vă întrebați până acum, dacă modelul accelerometrului ne-a dat deja unghiuri de înclinare ale lui Axr, Ayr, Azr, de ce ne-am dori să ne deranjăm cu datele giroscopului? Răspunsul este simplu: datele accelerometrului nu pot fi întotdeauna de încredere 100%. Există mai multe motive, amintiți-vă că accelerometrul măsoară forța inerțială, o astfel de forță poate fi cauzată de gravitație (și în mod ideal numai prin gravitație), dar ar putea fi cauzată și de accelerația (mișcarea) dispozitivului. Prin urmare, chiar dacă accelerometrul este într-o stare relativ stabilă, este încă foarte sensibil la vibrații și la zgomotul mecanic în general. Acesta este principalul motiv pentru care majoritatea sistemelor IMU folosesc un giroscop pentru a netezi orice erori ale accelerometrului. Dar cum se face asta? Și giroscopul este liber de zgomot?

Giroscopul nu este liber de zgomot, totuși, deoarece măsoară rotația, este mai puțin sensibil la mișcările mecanice liniare, tipul de zgomot de care suferă accelerometrul, totuși giroscopele au alte tipuri de probleme, cum ar fi, de exemplu, deriva (nu revine la valoarea zero-rate când rotația se oprește). Cu toate acestea, prin medierea datelor care provin de la accelerometru și giroscop, putem obține o estimare relativ mai bună a înclinației curente a dispozitivului decât am obține utilizând numai datele accelerometrului.

În următorii pași voi introduce un algoritm care a fost inspirat de unele idei folosite în filtrul Kalman, cu toate acestea este mult mai simplu și mai ușor de implementat pe dispozitivele încorporate. Înainte de asta, să vedem mai întâi ce vrem să calculeze algoritmul nostru. Ei bine, este direcția vectorului forței de gravitație R = [Rx, Ry, Rz] din care putem obține alte valori precum Axr, Ayr, Azr sau cosX, confortabil, cosZ care ne va oferi o idee despre înclinația dispozitivului nostru în raport cu planul de la sol, discutăm relația dintre aceste valori în partea 1. S-ar putea spune - nu avem deja aceste valori Rx, Ry, Rz din ecuația 2 în partea 1? Ei bine, da, dar amintiți-vă că aceste valori sunt derivate numai din datele accelerometrului, așa că, dacă doriți să le utilizați direct în aplicația dvs., puteți obține mai mult zgomot decât poate tolera aplicația dvs. Pentru a evita confuzii suplimentare, să redefinim măsurătorile accelerometrului după cum urmează:

Racc - este vectorul forței inerțiale măsurat prin accelerometru, care constă din următoarele componente (proiecții pe axele X, Y, Z):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Sensibilitate RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Sensibilitate RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Sensibilitate

Până acum avem un set de valori măsurate pe care le putem obține pur din valorile ADC ale accelerometrului. Vom numi acest set de date „vector” și vom folosi următoarea notație.

Racc = [RxAcc, RyAcc, RzAcc]

Deoarece aceste componente ale Racc pot fi obținute din datele accelerometrului, îl putem considera o intrare în algoritmul nostru.

Vă rugăm să rețineți că, deoarece Racc măsoară forța de gravitație, veți fi corect dacă presupuneți că lungimea acestui vector definită după cum urmează este egală sau apropiată de 1g.

| Racc | = SQRT (RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2), Cu toate acestea, pentru a fi siguri, este logic să actualizați acest vector după cum urmează:

Racc (normalizat) = [RxAcc / | Racc |, RyAcc / | Racc |, RzAcc / | Racc |].

Acest lucru vă va asigura că lungimea vectorului Racc normalizat este întotdeauna 1.

Apoi vom introduce un nou vector și îl vom numi

Rest = [RxEst, RyEst, RzEst]

Acesta va fi rezultatul algoritmului nostru, acestea sunt valori corectate pe baza datelor giroscopice și pe baza datelor estimate din trecut.

Iată ce va face algoritmul nostru: - accelerometrul ne spune: „Acum sunteți în poziția Racc” - spunem „Mulțumesc, dar permiteți-mi să verific”, - apoi corectați aceste informații cu datele giroscopului, precum și cu datele de odihnă din trecut și scoatem un nou vector estimat Rest. - considerăm odihna ca „cel mai bun pariu” al nostru în ceea ce privește poziția actuală a dispozitivului.

Să vedem cum o putem face să funcționeze.

Vom începe secvența încredându-ne în accelerometru și atribuind:

Rest (0) = Racc (0)

Apropo, amintiți-vă Rest și Racc sunt vectori, deci ecuația de mai sus este doar o modalitate simplă de a scrie 3 seturi de ecuații și de a evita repetarea:

RxEst (0) = RxAcc (0) RyEst (0) = RyAcc (0) RzEst (0) = RzAcc (0)

Apoi vom face măsurători regulate la intervale de timp egale de T secunde și vom obține noi măsurători pe care le vom defini ca Racc (1), Racc (2), Racc (3) și așa mai departe. De asemenea, vom emite noi estimări la fiecare interval de timp Rest (1), Rest (2), Rest (3) și așa mai departe.

Să presupunem că suntem la pasul n. Avem două seturi de valori cunoscute pe care am dori să le folosim:

Rest (n-1) - estimarea noastră anterioară, cu Rest (0) = Racc (0) Racc (n) - măsurarea curentă a accelerometrului

Înainte de a putea calcula Rest (n), să introducem o nouă valoare măsurată, pe care o putem obține din giroscopul nostru și o estimare anterioară.

Îl vom numi Rgyro și este, de asemenea, un vector format din 3 componente:

Rgyro = [RxGyro, RyGyro, RzGyro]

Vom calcula acest vector câte o componentă la un moment dat. Vom începe cu RxGyro.

Să începem prin a observa următoarea relație în modelul nostru de giroscop, din triunghiul unghi drept format din Rz și Rxz putem deduce că:

tan (Axz) = Rx / Rz => Axz = atan2 (Rx, Rz)

Atan2 ar putea fi o funcție pe care nu ați mai folosit-o până acum, este similară cu atan, cu excepția faptului că returnează valori în intervalul de (-PI, PI) spre deosebire de (-PI / 2, PI / 2) așa cum a fost returnat de atan și este nevoie de 2 argumente în loc de unul. Ne permite să convertim cele două valori ale Rx, Rz în unghiuri în domeniul complet de 360 de grade (-PI la PI). Puteți citi mai multe despre atan2 aici.

Deci, cunoscând RxEst (n-1) și RzEst (n-1) putem găsi:

Axz (n-1) = atan2 (RxEst (n-1), RzEst (n-1)).

Amintiți-vă că giroscopul măsoară rata de schimbare a unghiului Axz. Deci, putem estima noul unghi Axz (n) după cum urmează:

Axz (n) = Axz (n-1) + RateAxz (n) * T

Amintiți-vă că RateAxz poate fi obținut din citirile noastre ADC prin giroscop. O formulă mai precisă poate utiliza o rată medie de rotație calculată după cum urmează:

RateAxzAvg = (RateAxz (n) + RateAxz (n-1)) / 2 Axz (n) = Axz (n-1) + RateAxzAvg * T

În același mod în care putem găsi:

Ayz (n) = Ayz (n-1) + RateAyz (n) * T

Ok, acum avem Axz (n) și Ayz (n). Unde mergem de aici pentru a deduce RxGyro / RyGyro? Din ec. 1 putem scrie lungimea vectorului Rgyro după cum urmează:

| Rgyro | = SQRT (RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2)

De asemenea, pentru că ne-am normalizat vectorul Racc, putem presupune că lungimea acestuia este 1 și nu s-a schimbat după rotație, deci este relativ sigur să scrieți:

| Rgyro | = 1

Să adoptăm o notație temporară mai scurtă pentru calculele de mai jos:

x = RxGyro, y = RyGyro, z = RzGyro

Folosind relațiile de mai sus putem scrie:

x = x / 1 = x / SQRT (x ^ 2 + y ^ 2 + z ^ 2)

Să împărțim numărătorul și numitorul fracției cu SQRT (x ^ 2 + z ^ 2)

x = (x / SQRT (x ^ 2 + z ^ 2)) / SQRT ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))

Rețineți că x / SQRT (x ^ 2 + z ^ 2) = sin (Axz), deci:

x = sin (Axz) / SQRT (1 + y ^ 2 / (x ^ 2 + z ^ 2))

Acum înmulțiți numărătorul și numitorul fracției din interiorul SQRT cu z ^ 2

x = sin (Axz) / SQRT (1 + y ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))

Rețineți că z / SQRT (x ^ 2 + z ^ 2) = cos (Axz) și y / z = tan (Ayz), deci în cele din urmă:

x = sin (Axz) / SQRT (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)

Revenind la notația noastră, obținem:

RxGyro = sin (Axz (n)) / SQRT (1 + cos (Axz (n)) ^ 2 * tan (Ayz (n)) ^ 2)

în același mod găsim că

RyGyro = sin (Ayz (n)) / SQRT (1 + cos (Ayz (n)) ^ 2 * tan (Axz (n)) ^ 2)

Acum, în cele din urmă putem găsi:

RzGyro = Sign (RzGyro) * SQRT (1 - RxGyro ^ 2 - RyGyro ^ 2).

Unde semn (RzGyro) = 1 când RzGyro> = 0 și semn (RzGyro) = -1 când RzGyro <0.

O modalitate simplă de a estima acest lucru este de a lua:

Sign (RzGyro) = Sign (RzEst (n-1))

În practică, aveți grijă când RzEst (n-1) este aproape de 0. Puteți sări peste faza giroscopică cu totul în acest caz și să atribuiți: Rgyro = Rest (n-1). Rz este folosit ca referință pentru calcularea unghiurilor Axz și Ayz și atunci când este aproape de 0, valorile pot să curgă și să provoace rezultate proaste. Veți fi în domeniul numerelor mari în virgulă mobilă în care implementările funcției tan () / atan () pot lipsi de precizie.

Deci, să recapitulăm ceea ce avem până acum, suntem la pasul n al algoritmului nostru și am calculat următoarele valori:

Racc - citiri curente din accelerometrul nostru Rgyro - obținut din repaus (n-1) și citiri giroscopice curente

Ce valori folosim pentru a calcula estimarea actualizată Rest (n)? Probabil ați ghicit că vom folosi ambele. Vom folosi o medie ponderată, astfel încât:

Rest (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Putem simplifica această formulă împărțind atât numărătorul, cât și numitorul fracției la w1.

Rest (n) = (Racc * w1 / w1 + Rgyro * w2 / w1) / (w1 / w1 + w2 / w1)

iar după substituirea w2 / w1 = wGyro obținem:

Rest (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

În forumula de mai sus wGyro ne spune cât de mult avem încredere în giroscopul nostru în comparație cu accelerometrul nostru. Această valoare poate fi aleasă experimental, de obicei, valorile cuprinse între 5..20 vor declanșa rezultate bune.

Principala diferență a acestui algoritm față de filtrul Kalman este că această greutate este relativ fixă, în timp ce în filtrul Kalman greutățile sunt actualizate permanent pe baza zgomotului măsurat al citirilor accelerometrului. Filtrul Kalman este concentrat să vă ofere „cele mai bune” rezultate teoretice, în timp ce acest algoritm vă poate oferi rezultate „suficient de bune” pentru aplicația dvs. practică. Puteți implementa un algoritm care ajustează wGyro în funcție de unii factori de zgomot pe care îi măsurați, dar valorile fixe vor funcționa bine pentru majoritatea aplicațiilor.

Suntem la un pas să obținem valorile estimate actualizate:

RxEst (n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro) RyEst (n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro) RzEst (n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

Acum să normalizăm din nou acest vector:

R = SQRT (RxEst (n) ^ 2 + RyEst (n) ^ 2 + RzEst (n) ^ 2)

RxEst (n) = RxEst (n) / R RyEst (n) = RyEst (n) / R RzEst (n) = RzEst (n) / R

Și suntem gata să ne repetăm bucla din nou.

Acest ghid a apărut inițial pe starlino.com, am făcut câteva modificări ușoare și l-am postat din nou cu permisiunea. Mulțumesc Starlino!