Kiihtyvyysmittari ja gyro -opetusohjelma: 3 vaihetta

2024 Kirjoittaja: John Day | [email protected]. Viimeksi muokattu: 2024-01-30 09:05

Johdanto

Tämä opas on tarkoitettu kaikille, jotka ovat kiinnostuneita käyttämään kiihtyvyysmittareita ja gyroskooppeja sekä yhdistelmä -IMU -laitteita (Inertial Measurement Unit) elektroniikkaprojekteissaan

Käsittelemme:

• Mitä kiihtyvyysmittari mittaa?
• Mitä gyroskooppi (aka gyro) mittaa?
• Näiden anturien saamien analogisten digitaalisten (ADC) lukujen muuntaminen fyysisiksi yksiköiksi (ne olisivat g kiihtyvyysmittarille, deg/s gyroskoopille)
• Kiihtyvyysmittarin ja gyroskoopin lukemien yhdistäminen tarkkojen tietojen saamiseksi laitteen kaltevuudesta suhteessa maatasoon

Koko artikkelin ajan yritän pitää matematiikan minimissä. Jos tiedät, mitä Sine/Cosine/Tangent ovat, sinun pitäisi pystyä ymmärtämään ja käyttämään näitä ideoita projektissasi riippumatta siitä, mitä alustaa käytät: Arduino, Potkuri, Basic Stamp, Atmel -sirut, Microchip PIC jne.

On ihmisiä, jotka uskovat, että tarvitset monimutkaista matematiikkaa voidaksesi käyttää IMU-yksikköä (monimutkaisia FIR- tai IIR-suodattimia, kuten Kalman-suodattimia, Parks-McClellan-suodattimia jne.). Voit tutkia kaikkia näitä ja saavuttaa upeita mutta monimutkaisia tuloksia. Tapani selittää asioita vaatii vain perusmatematiikkaa. Uskon hyvin yksinkertaisuuteen. Mielestäni yksinkertaista järjestelmää on helpompi hallita ja valvoa, monien sulautettujen laitteiden lisäksi ei ole voimaa ja resursseja toteuttaa monimutkaisia matriisilaskelmia vaativia algoritmeja.

Käytän esimerkkinä uutta IMU -yksikköä, Acc_Gyro Accelerometer + Gyro IMU. Käytämme tämän laitteen parametreja alla olevissa esimerkeissä. Tämä laite on hyvä laite aloittaa, koska se koostuu kahdesta laitteesta:

- LIS331AL (tietolomake) - kolmiaksiaalinen 2G -kiihtyvyysmittari - LPR550AL (tietolomake) - kaksiakselinen nousu ja rulla, 500 astetta/sek gyroskooppi

Yhdessä ne edustavat 5 asteen vapauden inertiaalista mittausyksikköä. Nyt on hieno nimi! Siitä huolimatta hienon nimen takana on erittäin hyödyllinen yhdistelmälaite, joka katetaan ja selitetään yksityiskohtaisesti tässä oppaassa.

Vaihe 1: Kiihtyvyysmittari

Tämän yksikön ymmärtämiseksi aloitamme kiihtyvyysmittarilla. Kiihtyvyysmittaria ajatellen on usein hyödyllistä kuvata kuution muotoinen laatikko, jonka sisällä on pallo. Voit kuvitella jotain muuta, kuten evästeen tai munkin, mutta minä kuvittelen pallon:

Jos otamme tämän laatikon paikkaan, jossa ei ole painovoimakenttiä, tai ilman muita kenttiä, jotka voisivat vaikuttaa pallon sijaintiin - pallo yksinkertaisesti kelluu laatikon keskellä. Voit kuvitella, että laatikko on ulkoavaruudessa kaukana kaikista kosmisista kappaleista, tai jos tällaista paikkaa on vaikea löytää, kuvitella ainakin avaruusalusta, joka kiertää planeetan ympäri ja missä kaikki on painottomassa tilassa. Yllä olevasta kuvasta näet, että annamme kullekin akselille pari seiniä (poistimme seinän Y+, jotta voimme katsoa laatikon sisälle). Kuvittele, että jokainen seinä on paineherkkä. Jos siirrämme laatikkoa yhtäkkiä vasemmalle (kiihdytämme sitä kiihtyvyydellä 1g = 9,8 m/s^2), pallo osuu seinään X-. Mittaamme sitten palloon kohdistuvan painevoiman seinään ja annamme arvon -1g X -akselilla.

Huomaa, että kiihtyvyysmittari tunnistaa itse asiassa voiman, joka on suunnattu vastakkaiseen suuntaan kuin kiihtyvyysvektori. Tätä voimaa kutsutaan usein inertiavoimaksi tai kuvitteelliseksi voimaksi. Yksi asia, jonka sinun pitäisi oppia tästä, on se, että kiihtyvyysmittari mittaa kiihtyvyyttä epäsuorasti sen seiniin kohdistetun voiman avulla (mallimme mukaan se voi olla jousi tai jotain muuta tosielämän kiihtyvyysmittaria). Tämä voima voi johtua kiihtyvyydestä, mutta kuten näemme seuraavassa esimerkissä, se ei aina johdu kiihtyvyydestä.

Jos otamme mallimme ja asetamme sen maahan, pallo putoaa Z-seinälle ja kohdistuu 1 g: n voima pohjaseinään, kuten alla olevassa kuvassa:

Tässä tapauksessa laatikko ei liiku, mutta silti saamme lukeman -1 g Z -akselilla. Paine, jonka pallo on kohdistanut seinään, johtui painovoimasta. Teoriassa se voi olla erilainen voima - esimerkiksi jos kuvittelet, että pallomme on metallinen, magneetin asettaminen laatikon viereen voisi siirtää pallon niin, että se osuu toiseen seinään. Tämän sanottiin vain todistamaan, että kiihtyvyysanturi mittaa olennaisesti voimaa eikä kiihtyvyyttä. Se vain tapahtuu, että kiihtyvyys aiheuttaa hitausvoiman, joka kiihtyvyysmittarin voimanilmaisumekanismin avulla vangitaan.

Vaikka tämä malli ei ole juuri sellainen kuin MEMS -anturi on rakennettu, se on usein hyödyllinen kiihtyvyysmittariin liittyvien ongelmien ratkaisemisessa. Itse asiassa on olemassa samanlaisia antureita, joissa on metallipalloja, niitä kutsutaan kallistuskytkimiksi, mutta ne ovat primitiivisempiä ja yleensä ne voivat vain kertoa, onko laite kallistettu jollakin alueella vai ei, ei kaltevuuden laajuutta.

Toistaiseksi olemme analysoineet kiihtyvyysmittarin tuoton yhdellä akselilla, ja tämä on kaikki mitä saat yhdellä akselilla kiihtyvyysanturilla. Kolmiaksiaalisten kiihtyvyysmittarien todellinen arvo tulee siitä, että ne voivat havaita hitausvoimat kaikilla kolmella akselilla. Palataan laatikkomalliimme ja käännämme laatikkoa 45 astetta oikealle. Pallo koskettaa nyt kahta seinää: Z- ja X-, kuten alla olevassa kuvassa:

Arvot 0,71 eivät ole mielivaltaisia, ne ovat itse asiassa likimääräistä SQRT: lle (1/2). Tämä tulee selvemmäksi, kun esittelemme seuraavan kiihtyvyysmittarin mallin.

Edellisessä mallissa olemme vahvistaneet painovoiman ja kiertäneet kuvitteellista laatikkoamme. Kahdessa viimeisessä esimerkissä olemme analysoineet lähtöä kahdessa eri laatikkoasennossa, kun taas voimavektori pysyi vakiona. Vaikka tämä oli hyödyllistä ymmärtää, miten kiihtyvyysmittari on vuorovaikutuksessa ulkopuolisten voimien kanssa, on käytännöllisempää suorittaa laskelmia, jos kiinnitämme koordinaattijärjestelmän kiihtyvyysmittarin akseleihin ja kuvittelemme, että voimavektori pyörii ympärillämme.

Katsokaa yllä olevaa mallia, säilytin kirveiden värit, jotta voit tehdä mentaalisen siirtymisen edellisestä mallista uuteen. Kuvittele vain, että jokainen uuden mallin akseli on kohtisuorassa edellisen mallin laatikon vastaaviin pintoihin. Vektori R on voimavektori, jota kiihtyvyysmittari mittaa (se voi olla joko yllä olevien esimerkkien painovoima tai hitausvoima tai molempien yhdistelmä). Rx, Ry, Rz ovat R -vektorin projektio X-, Y- ja Z -akseleille. Huomaa seuraava suhde:

R^2 = Rx^2 + Ry^2 + Rz^2 (yhtälö 1)

joka on pohjimmiltaan vastaava kuin Pythagoraan lause 3D: ssä.

Muista, että hieman aiemmin kerroin sinulle, että SQRT (1/2) ~ 0,71: n arvot eivät ole satunnaisia. Jos liität ne yllä olevaan kaavaan, muistamme, että painovoimamme oli 1 g, ja voimme varmistaa, että:

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

yksinkertaisesti korvaamalla R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) yhtälössä 1

Pitkän teorian johdannon jälkeen olemme pääsemässä lähemmäksi tosielämän kiihtyvyysmittaria. Arvot Rx, Ry, Rz liittyvät itse asiassa lineaarisesti arvoihin, joita tosielämän kiihtyvyysmittari antaa ja joita voit käyttää eri laskelmien tekemiseen.

Ennen kuin pääsemme sinne, puhutaanpa hieman siitä, miten kiihtyvyysmittarit toimittavat nämä tiedot meille. Useimmat kiihtyvyysmittarit kuuluvat kahteen luokkaan: digitaalinen ja analoginen. Digitaaliset kiihtyvyysmittarit antavat sinulle tietoja käyttämällä sarjaprotokollaa, kuten I2C, SPI tai USART, kun taas analogiset kiihtyvyysanturit antavat jännitetason ennalta määritetyllä alueella, joka sinun on muunnettava digitaaliseksi arvoksi ADC (analoginen digitaalimuunnin) -moduulin avulla. En mene kovin yksityiskohtaisesti ADC: n toimintaan, osittain siksi, että se on niin laaja aihe ja osittain siksi, että se on erilainen eri alustoilla. Joissakin mikro-ohjaimissa on sisäänrakennettu ADC-moduuli, joista osa tarvitsee ulkoisia komponentteja ADC-muunnosten suorittamiseksi. Riippumatta siitä, minkä tyyppistä ADC -moduulia käytät, päädyt arvoon tietyllä alueella. Esimerkiksi 10 -bittinen ADC -moduuli antaa arvon alueella 0..1023, huomaa, että 1023 = 2^10 -1. 12-bittinen ADC-moduuli antaa arvon välillä 0..4095, huomaa, että 4095 = 2^12-1.

Siirrymme eteenpäin harkitsemalla yksinkertaista esimerkkiä, oletetaan, että 10 -bittinen ADC -moduulimme antoi meille seuraavat arvot kolmelle kiihtyvyysmittarin kanavalle (akselille):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Jokaisella ADC -moduulilla on vertailujännite, oletetaan esimerkissämme, että se on 3,3 V. Muuntaaksesi 10 -bittisen adc -arvon jännitteeksi käytämme seuraavaa kaavaa:

VoltsRx = AdcRx * Vref / 1023

Nopea huomautus: 8 -bittisellä ADC: llä viimeinen jakaja olisi 255 = 2 ^ 8 -1 ja 12 -bittiselle ADC: lle viimeinen jakaja olisi 4095 = 2 ^ 12 -1.

Soveltamalla tätä kaavaa kaikkiin kolmeen kanavaan saamme:

VolttiaRx = 586 * 3.3V / 1023 = ~ 1.89V (pyöristetään kaikki tulokset kahden desimaalin tarkkuuteen) VolttiaRy = 630 * 3.3V / 1023 = ~ 2.03V volttiaRz = 561 * 3.3V / 1023 = ~ 1.81V

Jokaisella kiihtyvyysmittarilla on nolla-g jännitetaso, löydät sen teknisistä tiedoista, tämä on jännite, joka vastaa 0 g. Allekirjoitetun jännitearvon saamiseksi meidän on laskettava siirtymä tältä tasolta. Oletetaan, että 0 g: n jännitetasomme on VzeroG = 1,65 V. Laskemme jännitteenmuutokset nolla-g-jännitteestä seuraavasti:

DeltaVoltsRx = 1.89V - 1.65V = 0.24V DeltaVoltsRy = 2.03V - 1.65V = 0.38V DeltaVoltsRz = 1.81V - 1.65V = 0.16V

Meillä on nyt kiihtyvyysmittarin lukemat voltteina, se ei edelleenkään ole g (9,8 m/s^2). Lopullisen muunnoksen suorittamiseksi käytämme kiihtyvyysmittarin herkkyyttä, joka yleensä ilmaistaan mV/g. Sanotaan, että herkkyytemme = 478,5 mV/g = 0,4785 V/g. Herkkyysarvot löytyvät kiihtyvyysmittarin tiedoista. Lopullisten voima -arvojen ilmaisemiseksi grammoina käytämme seuraavaa kaavaa:

Rx = DeltaVoltsRx / Herkkyys

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

Voisimme tietysti yhdistää kaikki vaiheet yhteen kaavaan, mutta kävin läpi kaikki vaiheet saadakseni selväksi, miten siirryt ADC -lukemista voimavektorikomponenttiin, joka ilmaistaan g: nä.

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

Meillä on nyt kaikki kolme osaa, jotka määrittelevät inertiavoimavektorin, jos laite ei ole muiden voimien kuin painovoiman alainen, voimme olettaa, että tämä on painovoimavektorimme suunta. Jos haluat laskea laitteen kaltevuuden suhteessa maahan, voit laskea kulman tämän vektorin ja Z -akselin välillä. Jos olet kiinnostunut myös akselikohtaisesta kaltevuussuunnasta, voit jakaa tämän tuloksen kahteen osaan: X- ja Y-akselin kaltevuus, joka voidaan laskea gravitaatiovektorin ja X / Y-akselien välisenä kulmana. Näiden kulmien laskeminen on yksinkertaisempaa kuin luulisi, nyt kun olemme laskeneet arvot Rx, Ry ja Rz. Palataan viimeiseen kiihtyvyysmittarimalliimme ja teemme joitain lisämerkintöjä:

Kiinnostuneet kulmat ovat X-, Y-, Z -akselin ja voimavektorin R väliset kulmat. Määritämme nämä kulmat Axr, Ayr, Azr. Voit huomata R: n ja Rx: n muodostamasta kolmikulmasta, että:

cos (Axr) = Rx / R ja vastaavasti: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Voimme vähentää yhtälöstä 1, että R = SQRT (Rx^2 + Ry^2 + Rz^2).

Voimme löytää kulmamme nyt käyttämällä arccos () -funktiota (käänteinen cos () -funktio):

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

Olemme kulkeneet pitkän matkan selittääksesi kiihtyvyysmittarin mallin, vain päästäksemme näihin kaavoihin. Sovelluksistasi riippuen saatat haluta käyttää mitä tahansa välituotteita, jotka olemme johtaneet. Esittelemme myös gyroskooppimallin pian ja näemme kuinka kiihtyvyysmittarin ja gyroskoopin tiedot voidaan yhdistää entistä tarkempien kaltevuusarvioiden saamiseksi.

Mutta ennen kuin teemme sen, teemme muutamia hyödyllisiä merkintöjä:

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

Tätä kolmosta kutsutaan usein suunnan kosiniksi, ja se edustaa pohjimmiltaan yksikkövektoria (vektori, jonka pituus on 1), jolla on sama suunta kuin R -vektorillamme. Voit helposti tarkistaa, että:

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

Tämä on mukava ominaisuus, koska se vapauttaa meidät valvomasta R -vektorin moduulia (pituutta). Usein, jos olemme vain kiinnostuneita inertiavektorimme suunnasta, on järkevää normalisoida sen moduuli muiden laskelmien yksinkertaistamiseksi.

Vaihe 2: Gyroskooppi

Emme aio esitellä mitään vastaavaa laatikkomallia gyroskoopille, kuten kiihtyvyysmittarille, vaan siirrymme suoraan toiseen kiihtyvyysmittarimalliin ja näytämme, mitä gyroskooppi mittaa tämän mallin mukaisesti.

Jokainen gyroskooppikanava mittaa pyörimistä yhden akselin ympäri. Esimerkiksi 2-akselinen gyroskooppi mittaa pyörimistä X- ja Y-akselien ympäri (tai jotkut voivat sanoa "noin"). Kierteen ilmaisemiseksi numeroina teemme joitain merkintöjä. Määritellään ensin:

Rxz - on inertiavoimavektorin R projektio XZ -tasolle Ryz - on inertiavoimavektorin R projektio YZ -tasolle

Rxz: n ja Rz: n muodostamasta suorakulmaisesta kolmiosta Pythagoraan lauseen avulla saadaan:

Rxz^2 = Rx^2 + Rz^2 ja vastaavasti: Ryz^2 = Ry^2 + Rz^2

huomaa myös, että:

R^2 = Rxz^2 + Ry^2, tämä voidaan johtaa yhtälöstä 1 ja sitä korkeammista yhtälöistä tai se voidaan johtaa R: n ja Ryzin muodostamasta suorakulmaisesta kolmiosta R^2 = Ryz^2 + Rx^2

Emme aio käyttää näitä kaavoja tässä artikkelissa, mutta on hyödyllistä huomata kaikkien mallimme arvojen välinen suhde.

Sen sijaan määritämme Z -akselin ja Rxz, Ryz -vektorin välisen kulman seuraavasti:

Axz - on kulma Rxz: n (R: n projektio XZ -tasossa) ja Z -akselin Ayz välillä - on Ryzin (R: n projektio YZ -tasossa) ja Z -akselin välinen kulma

Nyt lähestymme gyroskoopin mittauksia. Gyroskooppi mittaa edellä määriteltyjen kulmien muutosnopeuden. Toisin sanoen se antaa arvon, joka liittyy lineaarisesti näiden kulmien muutosnopeuteen. Tämän selittämiseksi oletetaan, että olemme mitanneet akselin Y ympärillä olevan kiertokulman (joka olisi Axz -kulma) ajanhetkellä t0 ja määrittelemme sen Axz0: ksi, seuraavaksi mittaimme tämän kulman myöhemmin t1 ja se oli Axz1. Muutosnopeus lasketaan seuraavasti:

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

Jos ilmaisemme Axzin asteina ja ajan sekunteina, tämä arvo ilmaistaan asteina/s. Tätä gyroskooppi mittaa.

Käytännössä gyroskooppi (ellei kyseessä ole erityinen digitaalinen gyroskooppi) antaa harvoin arvon, joka ilmaistaan asteina/s. Sama kuin kiihtyvyysmittari, saat ADC -arvon, joka sinun on muunnettava asteiksi/s käyttäen kaavaa, joka on samanlainen kuin Eq. 2, jonka olemme määrittäneet kiihtyvyysanturille. Esittelemme ADC: n deg/s -muunnoskaavaksi gyroskoopille (oletamme, että käytämme 10 -bittistä ADC -moduulia, 8 -bittiselle ADC: lle 1023 korvataan 255: llä, 12 bitin ADC: llä 1023 4095: llä).

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

AdcGyroXZ, AdcGyroYZ - saadaan adc -moduulistamme ja ne edustavat kanavia, jotka mittaavat R -vektorin projektion pyörimistä XZ: ssä vastaavasti YZ -tasoilla, mikä vastaa sanomista, että kierto tehtiin Y- ja X -akselien ympäri.

Vref - on ADC: n vertailujännite, jota käytämme 3.3 V: n alla olevassa esimerkissä VzeroRate - on nollanopeusjännite, toisin sanoen jännite, jonka gyroskooppi antaa, kun sitä ei pyöritä, Acc_Gyro -kortille se on esimerkiksi 1,23 V (nämä arvot löytyvät teknisistä tiedoista) Herkkyys - on gyroskoopin herkkyys, joka ilmaistaan mV / (deg / s), usein kirjoitettuna mV / deg / s, se kertoo periaatteessa kuinka monta mV gyroskoopin lähtö kasvaa, jos lisäät pyörimisnopeutta yhdellä asteella/s. Acc_Gyro -levyn herkkyys on esimerkiksi 2 mV/deg/s tai 0,002 V/deg/s

Otetaan esimerkki, oletetaan, että ADC -moduulimme palautti seuraavat arvot:

AdcGyroXZ = 571 AdcGyroXZ = 323

Käyttämällä yllä olevaa kaavaa ja käyttämällä Acc_Gyro -levyn teknisiä parametreja, saamme:

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

Toisin sanoen laite pyörii Y -akselin ympäri (tai voimme sanoa, että se pyörii XZ -tasossa) nopeudella 306 astetta/s ja X -akselin ympäri (tai voimme sanoa, että se pyörii YZ -tasossa) nopeudella - 94 astetta/s. Huomaa, että negatiivinen merkki tarkoittaa, että laite pyörii vastakkaiseen suuntaan kuin tavanomainen positiivinen suunta. Sopimuksen mukaan yksi pyörimissuunta on positiivinen. Hyvä gyroskoopin tekninen taulukko näyttää, mikä suunta on positiivinen, muuten sinun on löydettävä se kokeilemalla laitetta ja huomioimalla, mikä pyörimissuunta johtaa jännitteen nousuun. Tämä on parasta tehdä oskilloskoopilla, koska heti kun lopetat pyörimisen, jännite laskee takaisin nollatasolle. Jos käytät yleismittaria, sinun on pidettävä vakio pyörimisnopeus vähintään muutaman sekunnin ajan ja kirjattava jännite tämän pyörimisen aikana ja vertailtava sitä nollanopeusjännitteeseen. Jos se on suurempi kuin nollanopeusjännite, se tarkoittaa, että pyörimissuunta on positiivinen.

Vaihe 3: Yhdistä kiihtyvyysmittari ja gyro

Yhdistä kaikki - yhdistämällä kiihtyvyysmittarin ja gyroskoopin tiedot

Jos luet tätä artikkelia, olet todennäköisesti hankkinut tai aiot hankkia IMU -laitteen tai luultavasti aiot rakentaa sen erillisistä kiihtyvyysanturista ja gyroskooppilaitteista.

Ensimmäinen askel kiihtyvyysanturin ja gyroskoopin yhdistävän IMU -yhdistelmälaitteen käyttämisessä on niiden koordinaattijärjestelmien kohdistaminen. Helpoin tapa tehdä se on valita kiihtyvyysmittarin koordinaattijärjestelmä vertailukoordinaattijärjestelmäksi. Useimmat kiihtyvyysmittarin tietolomakkeet näyttävät X-, Y-, Z -akselin suunnan suhteessa fyysisen sirun tai laitteen kuvaan. Tässä ovat esimerkiksi X-, Y-, Z -akseleiden suunnat, kuten on esitetty Acc_Gyro -kortin teknisissä tiedoissa:

Seuraavat vaiheet ovat:

Määritä gyroskoopin lähdöt, jotka vastaavat edellä käsiteltyjä RateAxz-, RateAyz -arvoja

Älä oleta, että jos gyroskoopin ulostulo on merkitty X: llä tai Y: llä, se vastaa mitä tahansa kiihtyvyysmittarin koordinaatiston akselia, vaikka tämä lähtö olisi osa IMU -yksikköä. Paras tapa on testata se. Olettaen, että olet vahvistanut gyroskoopin asennon suhteessa kiihtyvyysmittariin. Oletetaan, että gyroskoopin ja kiihtyvyysmittarin reunat ovat yhdensuuntaiset toistensa kanssa, eli sijoitat gyron 90 asteen kulmaan, joka on moninkertainen kiihtyvyysmittarin siruun nähden. Jos olet hankkinut IMU -levyn, on todennäköistä, että ne on jo kohdistettu tällä tavalla. Emme aio keskustella tässä artikkelissa malleista, joissa gyroskooppi on sijoitettu epäsäännölliseen kulmaan suhteessa kiihtyvyysmittariin (sanotaan 45 tai 30 astetta), vaikka tämä saattaa olla hyödyllistä joissakin sovelluksissa.

Tässä on esimerkkisekvenssi sen määrittämiseksi, mikä gyroskoopin lähtö vastaa edellä käsiteltyä RateAxz -arvoa.

- aloita laitteen asettamisesta vaakasuoraan asentoon. Sekä kiihtyvyysanturin X- että Y-lähdöt antavat nolla-g jännitteen (esimerkiksi Acc_Gyro-kortille tämä on 1,65 V)

- Aloita seuraavaksi laitteen kiertäminen Y -akselin ympäri, toinen tapa sanoa se on, että käännät laitetta XZ -tasossa niin, että X- ja Z -kiihtyvyysanturin lähdöt muuttuvat ja Y -ulostulo pysyy vakiona. - kun laitetta pyöritetään vakionopeudella, mikä gyroskoopin ulostulo muuttuu, muiden gyroskooppilähtöjen tulee pysyä vakioina - gyroskoopin ulostulo, joka muuttui pyörimisen aikana Y -akselin ympäri (kierto XZ -tasossa), antaa syöttöarvon AdcGyroXZ: lle, josta laskemme RateAxzin - viimeinen vaihe on varmistaa, että pyörimissuunta vastaa malliamme, joissakin tapauksissa saatat joutua kääntämään RateAxz -arvon gyroskoopin fyysisen sijainnin vuoksi suhteessa kiihtyvyysmittariin - suorita yllä oleva testi uudelleen kiertämällä laitetta ympäri Y -akselilla, tällä kertaa seurata kiihtyvyysmittarin X -lähtöä (AdcRx mallissamme). Jos AdcRx kasvaa (ensimmäiset 90 asteen kierto vaakasuorasta asennosta), myös AdcGyroXZ: n pitäisi kasvaa. Muussa tapauksessa sinun on käännettävä RateAxz, voit saavuttaa tämän lisäämällä merkkitekijän Eq.3: een seuraavasti:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Herkkyys, jossa InvertAxz on 1 tai -1

sama testikeppi voidaan tehdä RateAyzille kiertämällä laitetta X -akselin ympäri ja voit tunnistaa, mikä gyroskoopin lähtö vastaa RateAyzia, ja onko se käännettävä. Kun olet määrittänyt InvertAyz -arvon, sinun on käytettävä seuraavaa kaavaa RateAyzin laskemiseen:

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

Jos teet nämä testit Acc_Gyro -levyllä, saat seuraavat tulokset:

- RateAxz -lähdön nasta on GX4 ja InvertAxz = -1. - RateAyzin lähtötappi on GY4 ja InvertAyz = -1

Tästä lähtien katsomme, että olet asettanut IMU: n siten, että voit laskea oikeat arvot Axr, Ayr, Azr (kuten määritelty osassa 1. Kiihtyvyysmittari) ja RateAxz, RateAyz (kuten määritelty osassa 2. Gyroskooppi)). Seuraavaksi analysoimme näiden arvojen välisiä suhteita, jotka osoittautuvat hyödyllisiksi saadakseen tarkemman arvion laitteen kaltevuudesta suhteessa maatasoon.

Saatat kysyä itseltäsi tässä vaiheessa, jos kiihtyvyysmittarin malli jo antoi meille Axrin, Ayrin, Azrin kaltevuuskulmat, miksi haluaisimme vaivautua gyroskoopin tietojen kanssa? Vastaus on yksinkertainen: kiihtyvyysmittarin tietoja ei voi aina luottaa 100%. On useita syitä, muista, että kiihtyvyysmittari mittaa hitausvoimaa, tällainen voima voi johtua painovoimasta (ja mieluiten vain painovoimasta), mutta se voi johtua myös laitteen kiihtyvyydestä (liikkeestä). Tämän seurauksena, vaikka kiihtyvyysmittari on suhteellisen vakaassa tilassa, se on silti erittäin herkkä tärinälle ja mekaaniselle melulle yleensä. Tämä on tärkein syy siihen, miksi useimmat IMU -järjestelmät käyttävät gyroskooppia tasoittaakseen kiihtyvyysmittarin virheet. Mutta miten tämä tehdään? Ja onko gyroskooppi vapaa melusta?

Gyroskooppi ei kuitenkaan ole meluisa, mutta koska se mittaa pyörimistä, se on vähemmän herkkä lineaarisille mekaanisille liikkeille, kiihtyvyysmittarin kärsimälle melulle, mutta gyroskooppeihin liittyy muunlaisia ongelmia, kuten ajelehtiminen (ei palaudu nollanopeusarvoon) kun kierto pysähtyy). Kuitenkin keskiarvoistamalla kiihtyvyysmittarista ja gyroskoopista saadut tiedot voimme saada suhteellisen paremman arvion laitteen nykyisestä kaltevuudesta kuin mitä saisimme käyttämällä vain kiihtyvyysmittarin tietoja.

Seuraavissa vaiheissa esitän algoritmin, joka on saanut inspiraationsa joistakin Kalman -suodattimessa käytetyistä ideoista, mutta se on paljon yksinkertaisempi ja helpompi toteuttaa sulautetuilla laitteilla. Ennen sitä katsotaan ensin, mitä haluamme algoritmimme laskevan. No, gravitaatiovoiman suunta R = [Rx, Ry, Rz], josta voimme johtaa muita arvoja, kuten Axr, Ayr, Azr tai cosX, cosy, cosZ, antaa meille käsityksen laitteen kaltevuudesta suhteessa maatasoon, keskustelemme näiden arvojen välisestä suhteesta osassa 1. Voitaisiin sanoa - eikö meillä ole jo näitä arvoja Rx, Ry, Rz osan 1 ekv.2: stä? No kyllä, mutta muista, että nämä arvot on johdettu vain kiihtyvyysmittarin tiedoista, joten jos käytät niitä suoraan sovelluksessasi, saatat saada enemmän melua kuin sovelluksesi voi sietää. Sekaannusten välttämiseksi määritellään kiihtyvyysmittarin mittaukset uudelleen seuraavasti:

Racc - on kiihtyvyysmittarilla mitattu hitausvoimavektori, joka koostuu seuraavista komponenteista (projektiot X-, Y- ja Z -akseleille):

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

Toistaiseksi meillä on joukko mitattuja arvoja, jotka voimme saada puhtaasti kiihtyvyysmittarin ADC -arvoista. Kutsumme tätä datajoukkoa "vektoriksi" ja käytämme seuraavia merkintöjä.

Racc = [RxAcc, RyAcc, RzAcc]

Koska nämä Raccin komponentit voidaan saada kiihtyvyysmittarin tiedoista, voimme pitää sitä panoksena algoritmillemme.

Huomaa, että koska Racc mittaa gravitaatiovoimaa, olet oikeassa, jos oletat, että tämän vektorin pituus, joka on määritelty seuraavasti, on yhtä suuri tai lähellä 1 g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Varmuuden vuoksi on kuitenkin järkevää päivittää tämä vektori seuraavasti:

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

Tämä varmistaa, että normalisoidun Racc -vektorin pituus on aina 1.

Seuraavaksi esittelemme uuden vektorin ja kutsumme sitä

Lepo = [RxEst, RyEst, RzEst]

Tämä on algoritmimme tulos, nämä ovat korjattuja arvoja, jotka perustuvat gyroskooppitietoihin ja aiempien arvioitujen tietojen perusteella.

Meidän algoritmimme tekee tämän: - kiihtyvyysmittari kertoo meille: "Olet nyt asemassa Racc" - sanomme "Kiitos, mutta anna minun tarkistaa", - korjaa sitten nämä tiedot gyroskooppitiedoilla sekä aiemmilla lepo- ja annamme uuden arvioidun vektorin Rest. - pidämme lepoa "parhaana panoksena" laitteen nykyisen sijainnin suhteen.

Katsotaanpa, miten voimme saada sen toimimaan.

Aloitamme sarjamme luottaen kiihtyvyysmittarimme ja määrittämällä:

Lepo (0) = Racc (0)

Muista muistaa Rest ja Racc ovat vektoreita, joten yllä oleva yhtälö on yksinkertainen tapa kirjoittaa 3 yhtälösarjaa ja välttää toistoa:

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

Seuraavaksi teemme säännöllisiä mittauksia tasaisin aikavälein T sekuntia ja saamme uusia mittauksia, jotka määritellään Racc (1), Racc (2), Racc (3) ja niin edelleen. Annamme myös uusia arvioita jokaisella aikavälillä Lepo (1), Lepo (2), Lepo (3) ja niin edelleen.

Oletetaan, että olemme vaiheessa n. Meillä on kaksi tunnettua arvoryhmää, joita haluamme käyttää:

Lepo (n -1) - edellinen arvio, jossa lepo (0) = Racc (0) Racc (n) - nykyinen kiihtyvyysmittarimme

Ennen kuin voimme laskea lepoa (n), otamme käyttöön uuden mittausarvon, jonka voimme saada gyroskoopiltamme ja aiemman arvion.

Kutsumme sitä Rgyroksi, ja se on myös vektori, joka koostuu kolmesta komponentista:

Rgyro = [RxGyro, RyGyro, RzGyro]

Laskemme tämän vektorin yksi komponentti kerrallaan. Aloitamme RxGyrolla.

Aloitetaan tarkkailemalla gyroskooppimallimme seuraavaa suhdetta, Rz: n ja Rxz: n muodostamasta suorakulmaisesta kolmiosta voimme päätellä, että:

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

Atan2 saattaa olla toiminto, jota et ole koskaan käyttänyt, se on samanlainen kuin atan, paitsi että se palauttaa arvot alueella (-PI, PI) toisin kuin (-PI/2, PI/2), jonka atan palauttaa, ja se vie 2 argumenttia yhden sijasta. Sen avulla voimme muuntaa kaksi arvoa Rx, Rz kulmiksi koko 360 asteen alueella (-PI-PI). Voit lukea lisää atan2: sta täältä.

Joten tietäen RxEst (n-1) ja RzEst (n-1) voimme löytää:

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

Muista, että gyroskooppi mittaa Axz -kulman muutosnopeuden. Joten voimme arvioida uuden kulman Axz (n) seuraavasti:

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

Muista, että RateAxz on saatavana gyroskoopin ADC -lukemistamme. Tarkempi kaava voi käyttää keskimääräistä kiertonopeutta, joka lasketaan seuraavasti:

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

Samalla tavalla voimme löytää:

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

Ok, nyt meillä on Axz (n) ja Ayz (n). Mihin mennään täältä vähentämään RxGyro/RyGyro? Lähteestä Eq 1 voimme kirjoittaa vektorin Rgyro pituuden seuraavasti:

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

Myös koska olemme normalisoineet Racc -vektorin, voimme olettaa, että sen pituus on 1 eikä se ole muuttunut pyörimisen jälkeen, joten on suhteellisen turvallista kirjoittaa:

| Rgyro | = 1

Otetaan väliaikainen lyhyempi merkintä alla oleviin laskelmiin:

x = RxGyro, y = RyGyro, z = RzGyro

Käyttämällä yllä olevia suhteita voimme kirjoittaa:

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

Jaetaan murto -osan lukija ja nimittäjä SQRT: llä (x^2 + z^2)

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

Huomaa, että x / SQRT (x^2 + z^2) = sin (Axz), joten:

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

Kerro nyt SQRT: n murto -osien lukija ja nimittäjä z^2: lla

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

Huomaa, että z / SQRT (x^2 + z^2) = cos (Axz) ja y / z = tan (Ayz), joten lopuksi:

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

Palatessamme muistiinpanoihimme saamme:

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

samalla tavalla löydämme sen

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

Lopulta löydämme:

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

Missä merkki (RzGyro) = 1, kun RzGyro> = 0, ja merkki (RzGyro) = -1, kun RzGyro <0.

Yksi yksinkertainen tapa arvioida tämä on ottaa:

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

Käytännössä ole varovainen, kun RzEst (n-1) on lähellä 0. Tässä tapauksessa voit ohittaa gyro-vaiheen kokonaan ja määrittää: Rgyro = Lepo (n-1). Rz: tä käytetään vertailukohtana Axz- ja Ayz -kulmien laskemiseen, ja kun se on lähellä nollaa, arvot voivat ylivuotoa ja laukaista huonoja tuloksia. Olet liukulukuisten suurten lukujen alueella, jossa tan () / atan () -funktion toteutukset saattavat puuttua tarkkuudesta.

Joten toistetaan mitä meillä on tähän mennessä, olemme algoritmimme vaiheessa n ja olemme laskeneet seuraavat arvot:

Racc - kiihtyvyysmittarimme Rgyro -virran lukemat - saatu lepo- (n -1) ja gyroskoopin nykyisistä lukemista

Mitä arvoja käytämme laskettaessa päivitetty arvio Rest (n)? Luultavasti arvasit, että käytämme molempia. Käytämme painotettua keskiarvoa, jotta:

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

Voimme yksinkertaistaa tätä kaavaa jakamalla sekä murtoluvun lukijan että nimittäjän w1: llä.

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

ja korvaamisen jälkeen w2/w1 = wGyro saamme:

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

Yllä olevassa foorumissa wGyro kertoo meille, kuinka paljon luotamme gyroomme verrattuna kiihtyvyysmittariin. Tämä arvo voidaan valita kokeellisesti, yleensä arvot välillä 5..20 laukaisevat hyviä tuloksia.

Tämän algoritmin tärkein ero Kalman -suodattimesta on, että tämä paino on suhteellisen kiinteä, kun taas Kalman -suodattimessa painot päivitetään jatkuvasti kiihtyvyysmittarin lukemien mitatun melun perusteella. Kalman -suodatin keskittyy antamaan sinulle "parhaat" teoreettiset tulokset, kun taas tämä algoritmi voi antaa sinulle "riittävän hyviä" tuloksia käytännön sovellukseesi. Voit ottaa käyttöön algoritmin, joka säätää wGyroa joidenkin mittaamiesi kohinatekijöiden mukaan, mutta kiinteät arvot toimivat hyvin useimmissa sovelluksissa.

Olemme yhden askeleen päässä päivitettyjen arvioitujen arvojen saamisesta:

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

Nyt normalisoidaan tämä vektori uudelleen:

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

Ja olemme valmiita toistamaan kiertomme uudelleen.

Tämä opas ilmestyi alun perin starlino.com-sivustolla. Olen tehnyt muutaman pienen muokkauksen ja lähettänyt sen uudelleen luvalla. Kiitos Starlino!