Tutorial sobre acceleròmetre i giroscopi: 3 passos

2024 Autora: John Day | [email protected]. Última modificació: 2024-01-30 08:18

Introducció

Aquesta guia està dirigida a tothom que estigui interessat a utilitzar acceleròmetres i giroscopis, així com dispositius combinats IMU (Unitat de Mesura Inercial) en els seus projectes electrònics.

Cobrirem:

• Què mesura un acceleròmetre?
• Què mesura un giroscopi (també conegut com giroscopi)?
• Com convertir les lectures analògiques a digitals (ADC) que obteniu d’aquest sensor a unitats físiques (aquestes serien g per a l’acceleròmetre, deg / s per al giroscopi)
• Com combinar les lectures de l’acceleròmetre i el giroscopi per obtenir informació precisa sobre la inclinació del dispositiu respecte al pla terrestre

Al llarg de l'article intentaré que les matemàtiques siguin mínimes. Si sabeu què són Sine / Cosine / Tangent, hauríeu de ser capaços d’entendre i utilitzar aquestes idees en el vostre projecte, independentment de la plataforma que utilitzeu: Arduino, Propeller, Basic Stamp, xips Atmel, Microchip PIC, etc.

Hi ha gent que creu que necessiteu matemàtiques complexes per fer ús d’una unitat IMU (filtres FIR complexos o IIR com ara filtres Kalman, filtres Parks-McClellan, etc.). Podeu investigar-los i obtenir resultats meravellosos però complexos. La meva manera d’explicar les coses requereix només matemàtiques bàsiques. Sóc molt creient en la senzillesa. Crec que un sistema senzill és més fàcil de controlar i controlar, a més de molts dispositius incrustats no tenen la potència i els recursos per implementar algoritmes complexos que requereixen càlculs de matriu.

Utilitzaré com a exemple una nova unitat IMU, Acc_Gyro Accelerometer + Gyro IMU. Utilitzarem paràmetres d’aquest dispositiu en els nostres exemples següents. Aquesta unitat és un bon dispositiu per començar, ja que consta de 2 dispositius:

- LIS331AL (full de dades) - un acceleròmetre triaxial 2G - LPR550AL (full de dades) - gir i rotació de doble eix, giroscopi de 500 graus / segon

Junts representen una unitat de mesura inercial de 5 graus de llibertat. Ara és un nom fantàstic. No obstant això, darrere del nom de luxe hi ha un dispositiu de combinació molt útil que cobrirem i explicarem detalladament en aquesta guia.

Pas 1: l’acceleròmetre

Per entendre aquesta unitat, començarem amb l’acceleròmetre. Quan es pensa en acceleròmetres, sovint és útil imaginar una caixa en forma de cub amb una bola al seu interior. Potser us imagineu una altra cosa com una galeta o un bunyol, però imagino una bola:

Si agafem aquesta caixa en un lloc sense camps de gravitació o, en aquest cas, sense cap altre camp que pugui afectar la posició de la pilota, la pilota simplement flotarà al centre de la caixa. Us podeu imaginar que la caixa es troba a l’espai exterior, molt lluny de qualsevol cos còsmic, o si és difícil trobar un lloc així, imagineu, com a mínim, una nau espacial que orbita al voltant del planeta on tot es troba en un estat d’ingravidesa. A la imatge superior es pot veure que assignem a cada eix un parell de parets (hem eliminat la paret Y + per poder mirar dins del quadre). Imagineu que cada paret és sensible a la pressió. Si movem sobtadament el quadre cap a l’esquerra (l’accelerem amb l’acceleració 1g = 9,8m / s ^ 2), la bola impactarà contra la paret X-. A continuació, mesurem la força de pressió que la bola aplica a la paret i donem un valor de -1 g a l'eix X.

Tingueu en compte que l’acceleròmetre realment detectarà una força dirigida en la direcció oposada al vector d’acceleració. Aquesta força sovint s’anomena força inercial o força fictícia. Una cosa que hauríeu d’aprendre d’això és que un acceleròmetre mesura l’acceleració indirectament a través d’una força que s’aplica a una de les seves parets (segons el nostre model, pot ser una molla o alguna cosa més en els acceleròmetres de la vida real). Aquesta força pot ser causada per l’acceleració, però, com veurem en el següent exemple, no sempre és causada per l’acceleració.

Si agafem el nostre model i el posem a la Terra, la pilota caurà sobre la paret Z i aplicarà una força d’1 g a la paret inferior, tal com es mostra a la imatge següent:

En aquest cas, el quadre no es mou, però encara obtenim una lectura de -1 g a l'eix Z. La pressió que la pilota ha aplicat a la paret va ser causada per una força de gravitació. En teoria, podria ser un tipus de força diferent: per exemple, si us imagineu que la nostra bola és metàl·lica, col·locar un imant al costat de la caixa pot moure la bola de manera que toqui una altra paret. Això es va dir només per demostrar que, en essència, l’acceleròmetre mesura la força i no l’acceleració. Succeeix que l’acceleració provoca una força inercial capturada pel mecanisme de detecció de força de l’acceleròmetre.

Tot i que aquest model no és exactament com es construeix un sensor MEMS, sovint és útil per resoldre problemes relacionats amb l’acceleròmetre. En realitat, hi ha sensors similars que tenen boles metàl·liques a l’interior, s’anomenen interruptors d’inclinació, però són més primitius i, en general, només poden saber si el dispositiu està inclinat o no dins de l’interval, no el grau d’inclinació.

Fins ara hem analitzat la sortida de l’acceleròmetre en un únic eix i això és tot el que obtindreu amb acceleròmetres d’un sol eix. El valor real dels acceleròmetres triaxials prové del fet que poden detectar forces inercials en els tres eixos. Tornem al nostre model de caixa i girem la caixa 45 graus cap a la dreta. La pilota tocarà 2 parets ara: Z- i X-, tal com es mostra a la imatge següent:

Els valors de 0,71 no són arbitraris, en realitat són una aproximació per a SQRT (1/2). Això quedarà més clar a mesura que presentem el nostre proper model per a l’acceleròmetre.

En el model anterior hem fixat la força de gravitació i hem girat la nostra caixa imaginària. En els darrers 2 exemples hem analitzat la sortida en 2 posicions de caixa diferents, mentre que el vector de força es va mantenir constant. Tot i que això va ser útil per entendre com l’acceleròmetre interactua amb forces externes, és més pràctic realitzar càlculs si fixem el sistema de coordenades als eixos de l’acceleròmetre i imaginem que el vector de força gira al nostre voltant.

Feu una ullada al model anterior, he conservat els colors dels eixos perquè pugueu fer una transició mental del model anterior al nou. Imagineu que cada eix del nou model és perpendicular a les cares respectives de la caixa del model anterior. El vector R és el vector de força que mesura l’acceleròmetre (pot ser la força de gravitació o la força inercial dels exemples anteriors o una combinació de tots dos). Rx, Ry, Rz són projecció del vector R en els eixos X, Y, Z. Tingueu en compte la relació següent:

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

que és bàsicament l’equivalent al teorema de Pitagòrica en 3D.

Recordeu que una mica abans us vaig dir que els valors de SQRT (1/2) ~ 0,71 no són aleatoris. Si els connecteu a la fórmula anterior, després de recordar que la nostra força de gravitació era d’1 g, podem comprovar que:

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

simplement substituint R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) a l'Eq. 1

Després d’un llarg preàmbul de teoria ens estem apropant als acceleròmetres de la vida real. Els valors Rx, Ry, Rz estan realment relacionats linealment amb els valors que generarà l’acceleròmetre de la vida real i que podeu utilitzar per realitzar diversos càlculs.

Abans d’arribar-hi, parlem una mica de la manera com els acceleròmetres ens lliuraran aquesta informació. La majoria dels acceleròmetres es dividiran en dues categories: digital i analògica. Els acceleròmetres digitals us proporcionaran informació mitjançant un protocol sèrie com I2C, SPI o USART, mentre que els acceleròmetres analògics generaran un nivell de voltatge dins d’un rang predefinit que heu de convertir a un valor digital mitjançant un mòdul ADC (convertidor analògic a digital). No entraré en molts detalls sobre com funciona l'ADC, en part perquè és un tema tan extens i en part perquè és diferent d'una plataforma a una altra. Alguns microcontroladors tindran mòduls ADC incorporats, alguns d'ells necessitaran components externs per realitzar les conversions d'ADC. Independentment del tipus de mòdul ADC que utilitzeu, obtindreu un valor en un interval determinat. Per exemple, un mòdul ADC de 10 bits generarà un valor en l'interval de 0..1023, tingueu en compte que 1023 = 2 ^ 10 -1. Un mòdul ADC de 12 bits generarà un valor en l'interval de 0..4095, tingueu en compte que 4095 = 2 ^ 12-1.

Continuem considerant un exemple senzill, suposem que el nostre mòdul ADC de 10 bits ens va donar els següents valors per als tres canals (eixos) de l’acceleròmetre:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Cada mòdul ADC tindrà una tensió de referència, suposem que en el nostre exemple és de 3,3 V. Per convertir un valor adc de 10 bits a voltatge, fem servir la fórmula següent:

VoltsRx = AdcRx * Vref / 1023

Una nota ràpida aquí: que per a l'ADC de 8 bits l'últim divisor seria 255 = 2 ^ 8 -1, i per a l'últim divisor de l'ADC de 12 bits seria 4095 = 2 ^ 12 -1.

Aplicant aquesta fórmula als 3 canals obtindrem:

VoltsRx = 586 * 3,3V / 1023 = ~ 1,89V (arrodonim tots els resultats a 2 punts decimals) VoltsRy = 630 * 3,3V / 1023 = ~ 2,03V VoltsRz = 561 * 3,3V / 1023 = ~ 1,81V

Cada acceleròmetre té un nivell de voltatge zero-g, el podeu trobar a les especificacions, aquest és el voltatge que correspon a 0g. Per obtenir un valor de tensió signat hem de calcular el desplaçament des d’aquest nivell. Diguem que el nostre nivell de voltatge 0g és VzeroG = 1,65V. Calculem els canvis de tensió de zero-g de la següent manera:

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

Ara tenim les lectures de l’acceleròmetre en volts, encara no està en g (9,8 m / s ^ 2), per fer la conversió final apliquem la sensibilitat de l’acceleròmetre, normalment expressada en mV / g. Diguem que la nostra sensibilitat = 478,5 mV / g = 0,4785V / g. Els valors de sensibilitat es poden trobar a les especificacions de l’acceleròmetre. Per obtenir els valors de força finals expressats en g utilitzem la següent fórmula:

Rx = DeltaVoltsRx / Sensibilitat

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

Per descomptat, podríem combinar tots els passos en una fórmula, però he seguit tots els passos per deixar clar com es passa de les lectures ADC a un component de vector de força expressat en g.

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

Ara tenim els 3 components que defineixen el nostre vector de força inercial, si el dispositiu no està subjecte a altres forces diferents de la gravitació, podem suposar que aquesta és la direcció del nostre vector de força de gravitació. Si voleu calcular la inclinació del dispositiu respecte al terra podeu calcular l'angle entre aquest vector i l'eix Z. Si també us interessa la direcció d’inclinació per eix, podeu dividir aquest resultat en 2 components: inclinació a l’eix X i Y que es pot calcular com l’angle entre el vector de gravitació i els eixos X / Y. Calcular aquests angles és més senzill del que es podria pensar, ara que hem calculat els valors de Rx, Ry i Rz. Tornem al nostre darrer model d’acceleròmetres i fem algunes notacions addicionals:

Els angles que ens interessen són els angles entre els eixos X, Y, Z i el vector de força R. Definirem aquests angles com Axr, Ayr, Azr. Podeu notar pel triangle d'angle recte format per R i Rx que:

cos (Axr) = Rx / R, i de manera similar: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Podem deduir de l’Eq.1 que R = SQRT (Rx ^ 2 + Ry ^ 2 + Rz ^ 2).

Ara podem trobar els nostres angles mitjançant la funció arccos () (la funció cos () inversa):

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

Hem recorregut un llarg camí per explicar el model de l’acceleròmetre, només per arribar a aquestes fórmules. En funció de les vostres aplicacions, és possible que vulgueu utilitzar qualsevol fórmula intermèdia que hàgim derivat. Aviat també introduirem el model de giroscopi i veurem com es poden combinar les dades de l’acceleròmetre i el giroscopi per proporcionar estimacions d’inclinació encara més precises.

Abans de fer-ho, anem a fer algunes notacions més útils:

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

Aquest triplet sovint s’anomena cosinus de direcció i representa bàsicament el vector unitari (vector de longitud 1) que té la mateixa direcció que el nostre vector R. Podeu verificar fàcilment que:

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

Aquesta és una bona propietat, ja que ens eximeix de controlar el mòdul (longitud) del vector R. Sovint, si només ens interessa la direcció del nostre vector inercial, té sentit normalitzar el mòdul per simplificar altres càlculs.

Pas 2: Giroscopi

No introduirem cap model de caixa equivalent per al giroscopi com ho vam fer per l’acceleròmetre, sinó que saltarem directament al segon model d’acceleròmetre i mostrarem què mesura el giroscopi segons aquest model.

Cada canal de giroscopi mesura la rotació al voltant d’un dels eixos. Per exemple, un giroscopi de 2 eixos mesurarà la rotació al voltant (o alguns poden dir "sobre") els eixos X i Y. Per expressar aquesta rotació en nombres fem algunes notacions. Primer definim:

Rxz - és la projecció del vector de força inercial R al pla XZ Ryz - és la projecció del vector de força inercial R al pla YZ

A partir del triangle d'angle recte format per Rxz i Rz, utilitzant el teorema de Pitàgores, obtenim:

Rxz ^ 2 = Rx ^ 2 + Rz ^ 2, i de manera similar: Ryz ^ 2 = Ry ^ 2 + Rz ^ 2

també tingueu en compte que:

R ^ 2 = Rxz ^ 2 + Ry ^ 2, es pot derivar de les equacions 1 i superiors, o bé es pot derivar del triangle d'angle recte format per R i Ryz R ^ 2 = Ryz ^ 2 + Rx ^ 2

No utilitzarem aquestes fórmules en aquest article, però és útil tenir en compte la relació entre tots els valors del nostre model.

En lloc d'això, definirem l'angle entre l'eix Z i els vectors Rxz, Ryz de la següent manera:

Axz - és l'angle entre l'eix Rxz (projecció de R en el pla XZ) i l'eix Z Ayz - és l'angle entre l'eix Ryz (projecció de R en el pla YZ) i l'eix Z

Ara ens acostem al que mesura el giroscopi. El giroscopi mesura la velocitat de canvis dels angles definits anteriorment. En altres paraules, generarà un valor que està relacionat linealment amb la velocitat de canvi d'aquests angles. Per explicar-ho suposem que hem mesurat l'angle de rotació al voltant de l'eix Y (que seria angle d'Axz) en el moment t0, i el definim com Axz0, a continuació, hem mesurat aquest angle en un moment posterior t1 i va ser Axz1. La taxa de variació es calcularà de la següent manera:

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

Si expressem Axz en graus i el temps en segons, aquest valor s’expressarà en deg / s. Això és el que mesura un giroscopi.

A la pràctica, un giroscopi (tret que sigui un giroscopi digital especial) poques vegades us donarà un valor expressat en deg / s. Igual que per a l’acceleròmetre, obtindreu un valor ADC que haureu de convertir a deg / s mitjançant una fórmula similar a l’equació. 2 que hem definit per a l’acceleròmetre. Presentem la fórmula de conversió ADC a deg / s per giroscopi (suposem que estem utilitzant un mòdul ADC de 10 bits, per ADC de 8 bits substituir 1023 per 255, per ADC de 12 bits substituir 1023 per 4095).

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

AdcGyroXZ, AdcGyroYZ: s’obtenen del nostre mòdul adc i representen els canals que mesuren la rotació de projecció del vector R en XZ respectivament en plans YZ, que equival a dir que la rotació es va fer al voltant dels eixos Y i X respectivament.

Vref - és el voltatge de referència ADC que utilitzarem a 3,3V a l'exemple següent VzeroRate - és el voltatge de taxa zero, és a dir, el voltatge que el giroscopi emet quan no està sotmès a cap rotació, per a la placa Acc_Gyro és per exemple, 1,23 V (podeu trobar aquests valors a les especificacions) Sensibilitat: és la sensibilitat del giroscopi que s’expressa en mV / (deg / s) sovint escrita com mV / deg / s, bàsicament us indica quants mV la sortida del giroscopi augmenta si augmenta la velocitat de rotació un grau / s. La sensibilitat de la placa Acc_Gyro és, per exemple, de 2 mV / deg / s o 0,002 V / deg / s

Posem un exemple, suposem que el nostre mòdul ADC ha retornat els valors següents:

AdcGyroXZ = 571 AdcGyroXZ = 323

Utilitzant la fórmula anterior i utilitzant els paràmetres de les especificacions del tauler Acc_Gyro obtindrem:

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 graus / s

En altres paraules, el dispositiu gira al voltant de l'eix Y (o podem dir que gira en pla XZ) amb una velocitat de 306 graus / s i al voltant de l'eix X (o podem dir que gira en pla YZ) amb una velocitat de - 94 graus / s. Tingueu en compte que el signe negatiu significa que el dispositiu gira en sentit contrari a la direcció positiva convencional. Per convenció, un sentit de rotació és positiu. Un bon full d’especificacions del giroscopi us mostrarà quina direcció és positiva; en cas contrari, haureu de trobar-la experimentant amb el dispositiu i assenyalant quina direcció de rotació resulta en augmentar la tensió al pin de sortida. Es fa millor mitjançant un oscil·loscopi, ja que tan bon punt atureu la rotació, el voltatge tornarà a baixar al nivell de taxa zero. Si utilitzeu un multímetre, hauríeu de mantenir una velocitat de rotació constant durant almenys uns segons i fixar-vos en la tensió durant aquesta rotació, i compareu-la amb la tensió de velocitat zero. Si és superior a la tensió zero, significa que el sentit de gir és positiu.

Pas 3: Combinació de l’acceleròmetre i del giroscopi

Unir-ho tot: combinar dades de l’acceleròmetre i del giroscopi

Si esteu llegint aquest article, probablement heu adquirit un dispositiu IMU o teniu previst adquirir-lo, o probablement en teniu la intenció de construir-lo a partir de dispositius separats d’acceleròmetres i giroscopis.

El primer pas per utilitzar un dispositiu IMU combinat que combina un acceleròmetre i un giroscopi és alinear els seus sistemes de coordenades. La forma més senzilla de fer-ho és triar el sistema de coordenades de l’acceleròmetre com a sistema de coordenades de referència. La majoria de fulls de dades de l’acceleròmetre mostraran la direcció dels eixos X, Y, Z en relació amb la imatge del xip o dispositiu físic. Per exemple, aquí teniu les direccions dels eixos X, Y, Z tal com es mostra a les especificacions de la placa Acc_Gyro:

Els passos següents són:

Identifiqueu les sortides del giroscopi que corresponen als valors de RateAxz, RateAyz comentats anteriorment. Determineu si cal invertir aquestes sortides a causa de la posició física del giroscopi en relació amb l’acceleròmetre

No suposeu que si un giroscopi té una sortida marcada X o Y, correspondrà a qualsevol eix del sistema de coordenades de l’acceleròmetre, fins i tot si aquesta sortida forma part d’una unitat IMU. La millor manera és provar-ho. Suposant que heu fixat la posició del giroscopi en relació amb l’acceleròmetre. Se suposa que les fronteres del giroscopi i l’acceleròmetre són paral·leles entre si, és a dir, col·loqueu el giroscòpic en un angle múltiple de 90 graus respecte al xip de l’acceleròmetre. Si heu adquirit un tauler IMU, és probable que ja estiguin alineats d'aquesta manera. No parlarem en aquest article de models en què el giroscopi es col·loca en un angle irregular respecte a l’acceleròmetre (diguem que 45 o 30 graus), tot i que això pot ser útil en algunes aplicacions.

Aquí teniu una seqüència de mostra per determinar quina sortida del giroscopi correspon al valor RateAxz comentat anteriorment.

- Comenceu posant el dispositiu en posició horitzontal. Tant les sortides X com Y de l’acceleròmetre produirien la tensió zero-g (per exemple, per a la placa Acc_Gyro és 1,65 V)

- Comenceu a girar el dispositiu al voltant de l'eix Y, una altra manera de dir-ho és que gireu el dispositiu en pla XZ, de manera que les sortides de l'acceleròmetre X i Z canvien i la sortida Y es mantingui constant. - mentre gireu el dispositiu a una velocitat constant, observeu quina sortida de giroscopi canvia, les altres sortides de giroscopi haurien de romandre constants; la sortida de giroscopi que va canviar durant la rotació al voltant de l’eix Y (rotació en pla XZ) proporcionarà el valor d’entrada per a AdcGyroXZ, a partir del qual calculem RateAxz: l'últim pas és assegurar-nos que la direcció de rotació correspon al nostre model, en alguns casos és possible que hagueu d'invertir el valor de RateAxz a causa de la posició física del giroscopi en relació amb l'acceleròmetre. Torneu a realitzar la prova anterior, girant el dispositiu al voltant a l’eix Y, aquesta vegada supervisem la sortida X de l’acceleròmetre (AdcRx al nostre model). Si AdcRx creix (els primers 90 graus de rotació des de la posició horitzontal), llavors AdcGyroXZ també hauria de créixer. En cas contrari, cal invertir RateAxz, per aconseguir-ho introduint un factor de signe a l'Eq.3, de la següent manera:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensibilitat, on InvertAxz és 1 o -1

es fa la mateixa canya de prova per a RateAyz, girant el dispositiu al voltant de l'eix X i podeu identificar quina sortida de giroscopi correspon a RateAyz i si cal invertir-la. Un cop tingueu el valor d’InvertAyz, heu d’utilitzar la fórmula següent per calcular RateAyz:

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

Si feu aquestes proves al tauler Acc_Gyro obtindreu els següents resultats:

- El pin de sortida per a RateAxz és GX4 i InvertAxz = -1. - El pin de sortida per a RateAyz és GY4 i InvertAyz = -1

A partir d’aquest moment considerarem que heu configurat l’IMU de manera que pugueu calcular els valors correctes per Axr, Ayr, Azr (tal com es defineix a la part 1. Acceleròmetre) i RateAxz, RateAyz (segons es defineix a la part 2. Giroscopi). A continuació, analitzarem les relacions entre aquests valors que resulten útils per obtenir una estimació més precisa de la inclinació del dispositiu respecte al pla terrestre.

És possible que us pregunteu a vosaltres mateixos en aquest punt, si el model de l’acceleròmetre ja ens donava angles d’inclinació d’Axr, Ayr, Azr, per què ens voldríem molestar amb les dades del giroscopi? La resposta és senzilla: les dades de l’acceleròmetre no sempre es poden confiar al 100%. Hi ha diversos motius, recordeu que l’acceleròmetre mesura la força inercial, tal força pot ser causada per la gravitació (i idealment només per gravitació), però també pot ser causada per l’acceleració (moviment) del dispositiu. Com a resultat, fins i tot si l’acceleròmetre es troba en un estat relativament estable, encara és molt sensible a les vibracions i al soroll mecànic en general. Aquesta és la raó principal per la qual la majoria dels sistemes IMU utilitzen un giroscopi per suavitzar els errors de l’acceleròmetre. Però, com es fa això? I el giroscopi està lliure de sorolls?

El giroscopi no està lliure de soroll, però, ja que mesura la rotació, és menys sensible als moviments mecànics lineals, el tipus de soroll que pateix l’acceleròmetre, tot i que els giroscopis tenen altres tipus de problemes com, per exemple, la deriva (no tornar al valor zero). quan s’atura la rotació). Tanmateix, mitjançant la mitjana de dades que provenen de l’acceleròmetre i el giroscopi, podem obtenir una estimació relativament millor de la inclinació del dispositiu actual que obtindríem utilitzant només les dades de l’acceleròmetre.

En els següents passos introduiré un algorisme que es va inspirar en algunes idees utilitzades en el filtre de Kalman, però és molt més senzill i fàcil d’implementar en dispositius incrustats. Abans vegem primer què volem que calculi el nostre algorisme. Bé, és la direcció de la força de gravitació vector R = [Rx, Ry, Rz] de la qual podem derivar altres valors com Axr, Ayr, Azr o cosX, acollidor, cosZ el que ens farà una idea de la inclinació del nostre dispositiu en relació amb el pla de terra, discutim la relació entre aquests valors a la part 1. Es podria dir: no tenim ja aquests valors Rx, Ry, Rz de l'Eq.2 a la part 1? Doncs sí, però recordeu que aquests valors es deriven només de les dades de l’acceleròmetre, de manera que si voleu utilitzar-los directament a la vostra aplicació, podríeu obtenir més soroll del que la vostra aplicació pot tolerar. Per evitar més confusions, tornem a definir les mesures de l’acceleròmetre de la següent manera:

Racc - és el vector de força inercial mesurat per acceleròmetre, que consisteix en els components següents (projeccions en els eixos X, Y, Z):

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

Fins ara tenim un conjunt de valors mesurats que podem obtenir exclusivament a partir dels valors ADC de l’acceleròmetre. Anomenarem aquest conjunt de dades "vector" i utilitzarem la següent notació.

Racc = [RxAcc, RyAcc, RzAcc]

Com que aquests components de Racc es poden obtenir a partir de dades de l’acceleròmetre, podem considerar que és una entrada al nostre algorisme.

Tingueu en compte que, com que Racc mesura la força de gravitació, tindreu raó si assumiu que la longitud d’aquest vector definida de la següent manera és igual o propera a 1 g.

| Racc | = SQRT (RxAcc ^ 2 + RyAcc ^ 2 + RzAcc ^ 2), Tanmateix, per assegurar-vos que té sentit actualitzar aquest vector de la següent manera:

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

Això assegurarà que la longitud del vector Racc normalitzat sigui sempre 1.

A continuació, introduirem un nou vector i l'anomenarem

Rest = [RxEst, RyEst, RzEst]

Aquesta serà la sortida del nostre algorisme, es tracta de valors corregits basats en dades de giroscopi i basats en dades estimades passades.

Això és el que farà el nostre algoritme: - l'acceleròmetre ens diu: "Ara esteu a la posició Racc" - diem "Gràcies, però deixeu-me comprovar", - després corregiu aquesta informació amb les dades del giroscopi, així com amb les dades de descans anteriors i generem un nou vector estimat Rest. - considerem que la resta és la nostra "millor aposta" pel que fa a la posició actual del dispositiu.

Vegem com podem fer que funcioni.

Començarem la nostra seqüència confiant en el nostre acceleròmetre i assignant:

Rest (0) = Racc (0)

Per cert, recordeu que Rest i Racc són vectors, de manera que l’equació anterior és només una forma senzilla d’escriure 3 conjunts d’equacions i evitar la repetició:

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

A continuació, farem mesures regulars a intervals de temps iguals de T segons i obtindrem noves mesures que definirem com Racc (1), Racc (2), Racc (3), etc. També emetrem estimacions noves a cada interval de temps Rest (1), Rest (2), Rest (3), etc.

Suposem que som al pas n. Tenim dos conjunts de valors coneguts que ens agradaria utilitzar:

Rest (n-1): la nostra estimació anterior, amb Rest (0) = Racc (0) Racc (n): la nostra mesura actual de l’acceleròmetre

Abans de poder calcular Rest (n), introduïm un nou valor mesurat, que podem obtenir del nostre giroscopi i una estimació prèvia.

L’anomenarem Rgyro, i també és un vector format per 3 components:

Rgyro = [RxGyro, RyGyro, RzGyro]

Calculem aquest vector d’un component a la vegada. Començarem per RxGyro.

Comencem per observar la següent relació en el nostre model de giroscopi, a partir del triangle d'angle recte format per Rz i Rxz podem derivar que:

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

Atan2 pot ser una funció que mai no havíeu utilitzat abans, és similar a atan, excepte que retorna valors en el rang de (-PI, PI) en lloc de (-PI / 2, PI / 2) tal com els ha retornat atan, i es necessita 2 arguments en lloc d’un. Ens permet convertir els dos valors de Rx, Rz a angles en el rang complet de 360 graus (-PI a PI). Podeu llegir més sobre atan2 aquí.

Així, coneixent RxEst (n-1) i RzEst (n-1) podem trobar:

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

Recordeu que el giroscopi mesura la velocitat de canvi de l'angle d'Axz. Així, podem estimar el nou angle Axz (n) de la següent manera:

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

Recordeu que RateAxz es pot obtenir a partir de les nostres lectures de giroscopi ADC. Una fórmula més precisa pot utilitzar una taxa de rotació mitjana calculada de la manera següent:

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

De la mateixa manera que podem trobar:

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

D'acord, ara tenim Axz (n) i Ayz (n). On anem d'aquí per deduir RxGyro / RyGyro? Des de l'Eq. 1 podem escriure la longitud del vector Rgyro de la següent manera:

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

A més, perquè hem normalitzat el nostre vector Racc, podem suposar que la seva longitud és 1 i que no ha canviat després de la rotació, de manera que és relativament segur escriure:

| Rgyro | = 1

Adoptem una notació temporal més curta per als càlculs següents:

x = RxGyro, y = RyGyro, z = RzGyro

Utilitzant les relacions anteriors podem escriure:

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

Dividim el numerador i el denominador de la fracció per SQRT (x ^ 2 + z ^ 2)

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

Tingueu en compte que x / SQRT (x ^ 2 + z ^ 2) = sin (Axz), de manera que:

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

Ara multipliqueu el numerador i el denominador de la fracció dins SQRT per z ^ 2

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

Tingueu en compte que z / SQRT (x ^ 2 + z ^ 2) = cos (Axz) i y / z = tan (Ayz), així que finalment:

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

Tornant a la nostra notació, obtenim:

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

de la mateixa manera que ho trobem

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

Ara, finalment, podem trobar:

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

On Sign (RzGyro) = 1 quan RzGyro> = 0, i Sign (RzGyro) = -1 quan RzGyro <0.

Una manera senzilla d’estimar-ho consisteix en:

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

A la pràctica, tingueu precaució quan RzEst (n-1) és a prop de 0. Podeu ometre la fase giroscòpica en aquest cas i assignar: Rgyro = Rest (n-1). Rz s’utilitza com a referència per al càlcul d’angles Axz i Ayz i, quan s’acosta a 0, els valors poden superar-se i provocar mals resultats. Estareu al domini de grans nombres de coma flotant on les implementacions de la funció tan () / atan () poden no tenir precisió.

Doncs, resumim el que tenim fins ara, som al pas n del nostre algorisme i hem calculat els valors següents:

Racc - lectures actuals del nostre acceleròmetre Rgyro - obtingudes de Rest (n-1) i lectures de giroscopi actuals

Quins valors fem servir per calcular l'estimació actualitzada Rest (n)? Probablement heu endevinat que farem servir tots dos. Utilitzarem una mitjana ponderada, de manera que:

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

Podem simplificar aquesta fórmula dividint tant el numerador com el denominador de la fracció per w1.

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

i després de substituir w2 / w1 = wGyro obtenim:

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

En el fòrum anterior, wGyro ens indica quant confiem en el nostre giroscopi en comparació amb el nostre acceleròmetre. Aquest valor es pot triar experimentalment, normalment els valors entre 5..20 generaran bons resultats.

La diferència principal d’aquest algorisme respecte al filtre de Kalman és que aquest pes és relativament fix, mentre que al filtre de Kalman els pesos s’actualitzen permanentment en funció del soroll mesurat de les lectures de l’acceleròmetre. El filtre Kalman està enfocat a oferir-vos "els millors" resultats teòrics, mentre que aquest algorisme us pot donar resultats "prou bons" per a la vostra aplicació pràctica. Podeu implementar un algorisme que ajusta wGyro en funció d'alguns factors de soroll que mesureu, però els valors fixos funcionaran bé per a la majoria d'aplicacions.

Estem a un pas d'aconseguir els nostres valors estimats actualitzats:

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

Ara tornem a normalitzar aquest 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 estem preparats per repetir el bucle de nou.

Aquesta guia va aparèixer originalment a starlino.com, he fet algunes modificacions lleugeres i l'he tornat a publicar amb permís. Gràcies Starlino!