Vodič za mjerenje brzine i žiroskop: 3 koraka

2024 Autor: John Day | [email protected]. Zadnja izmjena: 2024-01-30 08:10

Uvod

Ovaj vodič namijenjen je svima koji žele koristiti akcelerometre i žiroskope, kao i kombinirane IMU uređaje (inercijska mjerna jedinica) u svojim projektima elektronike

Pokrićemo:

• Šta mjeri akcelerometar?
• Šta mjeri žiroskop (zvani žiroskop)?
• Kako pretvoriti analogno-digitalna (ADC) očitanja koja dobijete sa ovih senzora u fizičke jedinice (to bi bilo g za akcelerometar, deg/s za žiroskop)
• Kako kombinirati očitanja akcelerometra i žiroskopa kako biste dobili točne informacije o nagibu vašeg uređaja u odnosu na ravninu tla

Kroz članak ću nastojati matematiku svesti na minimum. Ako znate što su Sine/Cosine/Tangent, trebali biste razumjeti i koristiti ove ideje u svom projektu bez obzira koju platformu koristite: Arduino, Propeller, Basic Stamp, Atmel čipovi, Microchip PIC itd.

Postoje ljudi koji vjeruju da vam je potrebna složena matematika da biste mogli koristiti IMU jedinicu (složeni FIR ili IIR filtri poput Kalmanovih filtera, Parks-McClellanovih filtera itd.). Možete sve to istražiti i postići prekrasne, ali složene rezultate. Moj način objašnjavanja stvari zahtijeva samo osnovnu matematiku. Veliki sam vjernik u jednostavnost. Mislim da je jednostavan sistem lakše kontrolirati i nadzirati, osim toga mnogi ugrađeni uređaji nemaju snage i resurse za implementaciju složenih algoritama koji zahtijevaju matrične proračune.

Za primjer ću koristiti novu IMU jedinicu, Acc_Gyro Accelerometer + Gyro IMU. U donjim primjerima koristit ćemo parametre ovog uređaja. Ova jedinica je dobar uređaj za početak jer se sastoji od 2 uređaja:

- LIS331AL (tehnički list) - troosni 2G akcelerometar - LPR550AL (tehnički list) - dvoosni pitch and roll, žiroskop od 500 stepeni/sek

Zajedno predstavljaju Jedinicu za inercijalno mjerenje od 5 stepeni slobode. To je otmjeno ime! Ipak, iza fensi naziva nalazi se vrlo koristan kombinirani uređaj koji ćemo pokriti i detaljno objasniti u ovom vodiču.

Korak 1: Akcelerometar

Da bismo razumjeli ovu jedinicu, počet ćemo s mjeračem ubrzanja. Kada razmišljate o akcelerometarima, često je korisno zamisliti kutiju u obliku kocke s kuglom unutar nje. Možete zamisliti nešto drugo poput kolačića ili krafne, ali ja ću zamisliti loptu:

Ako uzmemo ovu kutiju na mjesto bez gravitacionih polja ili u tom slučaju bez drugih polja koja bi mogla uticati na položaj lopte - lopta će jednostavno plutati u sredini kutije. Možete zamisliti da je kutija u svemiru daleko od bilo kojeg kosmičkog tijela, ili ako je takvo mjesto teško pronaći zamislite barem svemirski brod koji kruži oko planete gdje je sve u bestežinskom stanju. Sa gornje slike možete vidjeti da svakoj osi dodjeljujemo par zidova (uklonili smo zid Y+ tako da možemo pogledati unutar kutije). Zamislite da je svaki zid osjetljiv na pritisak. Ako kutiju odjednom pomaknemo ulijevo (ubrzavamo je ubrzanjem 1g = 9,8m/s^2), lopta će udariti u zid X-. Zatim mjerimo silu pritiska koju kugla primjenjuje na zid i izlazimo vrijednost -1g na os X.

Imajte na umu da će akcelerometar zapravo otkriti silu koja je usmjerena u suprotnom smjeru od vektora ubrzanja. Ova sila se često naziva inercijalna sila ili fiktivna sila. Jedna stvar koju biste trebali naučiti iz ovoga je da akcelerometar mjeri ubrzanje indirektno kroz silu koja se primjenjuje na jedan od njegovih zidova (prema našem modelu, to može biti opruga ili nešto drugo u akcelerometrima u stvarnom životu). Ova sila može biti uzrokovana ubrzanjem, ali kao što ćemo vidjeti u sljedećem primjeru, nije uvijek uzrokovano ubrzanjem.

Ako uzmemo naš model i stavimo ga na Zemlju, lopta će pasti na Z-zid i primijenit će silu od 1g na donju stijenku, kao što je prikazano na donjoj slici:

U ovom slučaju okvir se ne pomiče, ali i dalje dobivamo očitanje od -1g na Z osi. Pritisak koji je kugla izvršila na zid uzrokovan je gravitacionom silom. U teoriji bi to mogla biti druga vrsta sile - na primjer, ako zamislite da je naša lopta metalna, postavljanje magneta pored kutije moglo bi pomaknuti loptu tako da udari u drugi zid. Ovo je rečeno samo da bi se dokazalo da u suštini akcelerometar mjeri silu, a ne ubrzanje. Događa se da ubrzanje uzrokuje inercijsku silu koju zahvaća mehanizam za otkrivanje sile akcelerometra.

Iako ovaj model nije baš konstruiran MEMS senzor, često je koristan u rješavanju problema vezanih za akcelerometar. Zapravo postoje slični senzori koji u sebi imaju metalne kuglice, nazivaju se prekidači za nagib, međutim oni su primitivniji i obično mogu samo reći je li uređaj nagnut unutar nekog raspona ili ne, a ne stupanj nagiba.

Do sada smo analizirali izlaz akcelerometra na jednoj osi i to je sve što ćete dobiti s jednoosnim akcelerometrima. Prava vrijednost troosnih akcelerometra dolazi iz činjenice da mogu detektirati inercijske sile na sve tri osi. Vratimo se našem modelu kutije i okrenimo okvir za 45 stepeni udesno. Lopta će sada dodirnuti 2 zida: Z- i X- kao što je prikazano na donjoj slici:

Vrijednosti 0,71 nisu proizvoljne, one su zapravo aproksimacija za SQRT (1/2). To će postati jasnije kako predstavljamo naš sljedeći model za akcelerometar.

U prethodnom smo modelu fiksirali gravitacijsku silu i rotirali zamišljenu kutiju. U posljednja 2 primjera analizirali smo izlaz u 2 različite pozicije okvira, dok je vektor sile ostao konstantan. Iako je ovo bilo korisno za razumijevanje interakcije akcelerometra s vanjskim silama, praktičnije je izvesti proračune ako fiksiramo koordinatni sustav na osi akcelerometra i zamislimo da se vektor sile rotira oko nas.

Pogledajte gornji model, sačuvao sam boje osi kako biste mogli mentalno prijeći s prethodnog na novi. Zamislite samo da je svaka osa u novom modelu okomita na odgovarajuće strane okvira u prethodnom modelu. Vektor R je vektor sile koji mjeri akcelerometar (to može biti ili gravitacijska sila ili inercijalna sila iz gornjih primjera ili kombinacija oba). Rx, Ry, Rz su projekcija R vektora na osi X, Y, Z. Obratite pažnju na sljedeću relaciju:

R^2 = Rx^2 + Ry^2 + Rz^2 (jednadžba 1)

što je u osnovi ekvivalent Pitagorine teoreme u 3D.

Zapamtite da sam vam nešto ranije rekao da vrijednosti SQRT (1/2) ~ 0,71 nisu slučajne. Ako ih uključite u gornju formulu, nakon što se prisjetimo da je naša gravitacijska sila bila 1 g, možemo provjeriti da:

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

jednostavno zamjenom R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) u jednadžbi 1

Nakon duge uvodne teorije sve smo bliži akcelerometarima u stvarnom životu. Vrijednosti Rx, Ry, Rz su zapravo linearno povezane sa vrijednostima koje će vaš akcelerometar prikazati u stvarnom životu i koje možete koristiti za obavljanje različitih proračuna.

Prije nego što stignemo tamo, razgovarajmo malo o načinu na koji će nam akcelerometri dostavljati ove informacije. Većina mjerača ubrzanja bit će podijeljena u dvije kategorije: digitalne i analogne. Digitalni mjerači ubrzanja dat će vam informacije pomoću serijskog protokola poput I2C, SPI ili USART, dok će analogni mjerači ubrzanja izlaziti naponski nivo unutar unaprijed definiranog raspona koji morate pretvoriti u digitalnu vrijednost pomoću modula ADC (analogno -digitalni pretvarač). Neću ulaziti u detalje o tome kako ADC radi, dijelom zato što je to opsežna tema, a dijelom zato što se razlikuje od platforme do platforme. Neki mikrokontroleri će imati ugrađene ADC module, nekima od njih će trebati vanjske komponente kako bi se izvršile ADC konverzije. Bez obzira koju vrstu ADC modula koristite, dobit ćete vrijednost u određenom rasponu. Na primjer, 10 -bitni ADC modul će ispisati vrijednost u rasponu 0..1023, imajte na umu da je 1023 = 2^10 -1. 12-bitni ADC modul će ispisati vrijednost u rasponu 0..4095, imajte na umu da je 4095 = 2^12-1.

Idemo dalje razmatrajući jednostavan primjer, pretpostavimo da nam je naš 10 -bitni ADC modul dao sljedeće vrijednosti za tri kanala (osi) akcelerometra:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Svaki ADC modul će imati referentni napon, pretpostavimo da je u našem primjeru 3.3V. Za pretvaranje 10 -bitne adc vrijednosti u napon koristimo sljedeću formulu:

VoltsRx = AdcRx * Vref / 1023

Evo kratke napomene: za 8 -bitni ADC posljednji djelitelj bi bio 255 = 2 ^ 8 -1, a za 12 -bitni ADC posljednji djelitelj bi bio 4095 = 2 ^ 12 -1.

Primjenom ove formule na sva 3 kanala dobivamo:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (sve rezultate zaokružujemo na 2 decimalna mjesta) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Svaki mjerač ubrzanja ima naponski nivo od 0 g, možete ga pronaći u specifikacijama, to je napon koji odgovara 0 g. Da bismo dobili vrijednost napona sa predznakom, moramo izračunati pomak s ovog nivoa. Recimo da je naš naponski nivo 0 g VzeroG = 1,65 V. Pomake napona izračunavamo od napona n-g na sljedeći način:

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

Sada imamo očitavanja akcelerometra u voltima, još uvijek nije u g (9,8 m/s^2). Za konačnu konverziju primjenjujemo osjetljivost akcelerometra, obično izraženu u mV/g. Recimo da je naša osjetljivost = 478,5mV/g = 0,4785V/g. Vrijednosti osjetljivosti mogu se pronaći u specifikacijama akcelerometra. Da bismo dobili konačne vrijednosti sile izražene u g, koristimo sljedeću formulu:

Rx = DeltaVoltsRx / Osetljivost

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

Naravno, mogli bismo kombinirati sve korake u jednoj formuli, ali prošao sam sve korake kako bih vam pojasnio kako idete od očitanja ADC -a do komponente vektora sile izražene u g.

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

Sada imamo sve 3 komponente koje definiraju naš vektor inercijske sile, ako uređaj nije podložan drugim silama osim gravitacije, možemo pretpostaviti da je to smjer našeg vektora gravitacijske sile. Ako želite izračunati nagib uređaja u odnosu na tlo, možete izračunati kut između ovog vektora i osi Z. Ako vas zanima i smjer nagiba po osi, ovaj rezultat možete podijeliti na 2 komponente: nagib na osi X i Y koji se može izračunati kao kut između vektora gravitacije i osi X / Y. Izračunavanje ovih kutova jednostavnije je nego što mislite, sada kada smo izračunali vrijednosti za Rx, Ry i Rz. Vratimo se na naš posljednji model akcelerometra i napravimo neke dodatne oznake:

Uglovi koji nas zanimaju su kutovi između osi X, Y, Z i vektora sile R. Ove kutove definirat ćemo kao Axr, Ayr, Azr. Iz pravokutnog trokuta koji čine R i Rx možete primijetiti da:

cos (Axr) = Rx / R, i slično: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Iz jednadžbe 1 možemo zaključiti da je R = SQRT (Rx^2 + Ry^2 + Rz^2).

Sada možemo pronaći naše kutove pomoću funkcije arccos () (inverzna cos () funkcija):

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

Prešli smo dug put da bismo objasnili model akcelerometra, samo da bismo došli do ovih formula. Ovisno o vašim aplikacijama, možda ćete htjeti koristiti bilo koje srednje formule koje smo izveli. Uskoro ćemo predstaviti i model žiroskopa, pa ćemo vidjeti kako se podaci akcelerometra i žiroskopa mogu kombinirati kako bi se dobile još preciznije procjene nagiba.

No prije nego to učinimo, napravimo još neke korisne zapise:

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

Ovaj triplet se često naziva smjer kosinus i u osnovi predstavlja jedinični vektor (vektor dužine 1) koji ima isti smjer kao i naš R vektor. Lako možete provjeriti da:

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

Ovo je lijepo svojstvo jer nas oslobađa praćenja modula (dužine) R vektora. Često, ako nas samo zanima smjer našeg inercijalnog vektora, ima smisla normalizirati njegov modul kako bismo pojednostavili druge izračune.

Korak 2: Žiroskop

Nećemo uvesti bilo koji ekvivalentni model kutije za žiroskop kao što smo učinili za akcelerometar, umjesto toga ćemo prijeći direktno na drugi model akcelerometra i pokazat ćemo što mjeri žiroskop prema ovom modelu.

Svaki kanal žiroskopa mjeri rotaciju oko jedne od osi. Na primjer, dvoosni žiroskop mjerit će rotaciju oko (ili neki mogu reći "oko") osi X i Y. Da izrazimo ovu rotaciju u brojevima, napravimo neke oznake. Prvo da definišemo:

Rxz - je projekcija vektora inercijalne sile R na ravninu XZ Ryz - je projekcija vektora inercijalne sile R na ravninu YZ

Iz pravokutnog trokuta koji čine Rxz i Rz, koristeći Pitagorinu teoremu dobivamo:

Rxz^2 = Rx^2 + Rz^2, i slično: Ryz^2 = Ry^2 + Rz^2

takođe imajte na umu da:

R^2 = Rxz^2 + Ry^2, ovo se može izvesti iz jednadžbe 1 i gornjih jednadžbi, ili se može izvesti iz pravokutnog trokuta koji čine R i Ryz R^2 = Ryz^2 + Rx^2

Ove formule nećemo koristiti u ovom članku, ali je korisno primijetiti odnos između svih vrijednosti u našem modelu.

Umjesto toga, definirat ćemo kut između osi Z i Rxz, Ryz vektora na sljedeći način:

Axz - je kut između Rxz (projekcija R na ravnini XZ) i osi Z Ayz - je kut između Ryz (projekcija R na ravnini YZ) i osi Z

Sada smo sve bliže mjerenju žiroskopa. Žiroskop mjeri brzinu promjena gore navedenih kutova. Drugim riječima, on će ispisati vrijednost koja je linearno povezana sa brzinom promjene ovih uglova. Da bismo to objasnili, pretpostavimo da smo izmjerili kut rotacije oko osi Y (to bi bio Axz kut) u trenutku t0, te ga definiramo kao Axz0, zatim smo izmjerili ovaj kut u kasnijem trenutku t1 i to je bilo Axz1. Stopa promjene izračunat će se na sljedeći način:

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

Ako izrazimo Axz u stupnjevima, a vrijeme u sekundama, tada će ta vrijednost biti izražena u stupnjevima/s. To mjeri žiroskop.

U praksi će vam žiroskop (osim ako se radi o posebnom digitalnom žiroskopu) rijetko dati vrijednost izraženu u stepenima/s. Isto kao i za akcelerometar, dobit ćete ADC vrijednost koju ćete morati pretvoriti u stepen/s koristeći formulu sličnu jednadžbi. 2 koju smo definirali za akcelerometar. Uvedimo ADC u formulu konverzije u deg/s za žiroskop (pretpostavljamo da koristimo 10 -bitni ADC modul, za 8 -bitni ADC zamijenite 1023 sa 255, za 12 -bitni ADC zamijenite 1023 sa 4095).

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

AdcGyroXZ, AdcGyroYZ - dobiveni su iz našeg adc modula i predstavljaju kanale koji mjere rotaciju projekcije R vektora u XZ u YZ ravninama, što je ekvivalentno rećanju da je rotacija izvršena oko osi Y i X respektivno.

Vref - je referentni napon ADC -a koji ćemo koristiti 3,3 V u primjeru ispod VzeroRate - je nulti napon, drugim riječima napon koji žiroskop emitira kada nije podložan nikakvoj rotaciji, za Acc_Gyro ploču to je na primjer 1,23 V (ove vrijednosti možete pronaći u specifikacijama) Osjetljivost - je osjetljivost vašeg žiroskopa izražena u mV / (deg / s) koja se često piše kao mV / deg / s, u osnovi vam govori koliko će mV povećanje izlaza žiroskopa ako povećate brzinu rotacije za jedan stepen/s. Osjetljivost Acc_Gyro ploče je na primjer 2mV/deg/s ili 0,002V/deg/s

Uzmimo primjer, pretpostavimo da je naš ADC modul vratio sljedeće vrijednosti:

AdcGyroXZ = 571 AdcGyroXZ = 323

Koristeći gornju formulu i koristeći parametre specifikacija Acc_Gyro ploče dobit ćemo:

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

Drugim riječima, uređaj se rotira oko Y osi (ili možemo reći da se rotira u ravnini XZ) brzinom od 306 stepeni/s i oko osi X (ili možemo reći da se rotira u ravnini YZ) brzinom od - 94 stepeni/s. Imajte na umu da negativni znak znači da se uređaj okreće u suprotnom smjeru od uobičajenog pozitivnog smjera. Prema konvenciji, jedan smjer rotacije je pozitivan. Dobar list sa specifikacijama žiroskopa pokazat će vam koji je smjer pozitivan, inače ćete ga morati pronaći eksperimentiranjem s uređajem i primijetiti koji smjer rotacije dovodi do povećanja napona na izlaznom pinu. To je najbolje učiniti pomoću osciloskopa jer će čim prestanete okretati napon pasti natrag na nultu razinu. Ako koristite multimetar, morali biste održavati konstantnu brzinu rotacije najmanje nekoliko sekundi i zabilježiti napon tijekom ove rotacije, a zatim ga uporediti s naponom nulte brzine. Ako je veći od nultog napona, to znači da je smjer rotacije pozitivan.

Korak 3: Kombiniranje akcelerometra i žiroskopa

Sve to zajedno - Kombinacija podataka akcelerometra i žiroskopa

Ako čitate ovaj članak, vjerovatno ste kupili ili planirate nabaviti IMU uređaj, ili vjerovatno planirate da ga napravite od zasebnih uređaja za akcelerometar i žiroskop.

Prvi korak u korištenju kombiniranog IMU uređaja koji kombinira akcelerometar i žiroskop je usklađivanje njihovih koordinatnih sistema. Najlakši način za to je da odaberete koordinatni sistem akcelerometra kao referentni koordinatni sistem. Većina listova podataka akcelerometra prikazat će smjer osi X, Y, Z u odnosu na sliku fizičkog čipa ili uređaja. Na primjer, ovdje su smjerovi osi X, Y, Z kako je prikazano u specifikacijama za Acc_Gyro ploču:

Sljedeći koraci su:

Identificirajte izlaze žiroskopa koji odgovaraju RateAxz, RateAyz vrijednostima o kojima smo gore govorili. Odredite treba li ove izlaze obrnuti zbog fizičkog položaja žiroskopa u odnosu na akcelerometar

Nemojte pretpostavljati da će, ako žiroskop ima izlaz označen sa X ili Y, odgovarati bilo kojoj osi u koordinatnom sistemu akcelerometra, čak i ako je ovaj izlaz dio IMU jedinice. Najbolji način je da ga testirate. Pod pretpostavkom da ste fiksirali položaj žiroskopa u odnosu na akcelerometar. Pretpostavlja se da su granice žiroskopa i akcelerometra paralelne jedna s drugom, odnosno postavljate žiroskop pod kutom većim od 90 stepeni u odnosu na čip akcelerometra. Ako ste kupili ploču IMU, velike su šanse da su već usklađene na ovaj način. U ovom članku nećemo raspravljati o modelima gdje je žiroskop postavljen pod nepravilnim kutom u odnosu na akcelerometar (recimo 45 ili 30 stupnjeva), iako bi to moglo biti korisno u nekim aplikacijama.

Evo niza uzoraka za određivanje koji izlaz žiroskopa odgovara RateAxz vrijednosti o kojoj smo gore govorili.

- počnite s postavljanjem uređaja u vodoravni položaj. I X i Y izlazi akcelerometra bi dali nulti napon (na primjer za Acc_Gyro ploču ovo je 1,65 V)

- sljedeći put rotirajte uređaj oko Y osi, drugi način da to kažete je da rotirate uređaj u ravnini XZ, tako da se izlazi akcelerometra X i Z mijenjaju, a izlaz Y ostaje konstantan. - dok rotirate uređaj konstantnom brzinom, primijetite koji se izlaz žiroskopa mijenja, ostali izlazi žiroskopa trebaju ostati konstantni - izlaz žiroskopa koji se promijenio tijekom rotacije oko osi Y (rotacija u ravnini XZ) pružit će ulaznu vrijednost za AdcGyroXZ, iz koje izračunavamo RateAxz - posljednji korak je osigurati da smjer rotacije odgovara našem modelu, u nekim slučajevima možda ćete morati obrnuti RateAxz vrijednost zbog fizičkog položaja žiroskopa u odnosu na akcelerometar - ponovite gornji test, okrećući uređaj oko Y osi, ovaj put pratite X izlaz akcelerometra (AdcRx u našem modelu). Ako AdcRx raste (prvih 90 stupnjeva rotacije iz vodoravnog položaja), tada bi trebao rasti i AdcGyroXZ. U protivnom morate obrnuti RateAxz, to možete postići uvođenjem znakovnog faktora u jednadžbu 3, kako slijedi:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Osjetljivost, gdje je InvertAxz 1 ili -1

isti testni štap se radi za RateAyz, rotiranjem uređaja oko X osi, i možete identificirati koji izlaz žiroskopa odgovara RateAyzu i treba li ga preokrenuti. Nakon što dobijete vrijednost za InvertAyz, trebali biste koristiti sljedeću formulu za izračun RateAyza:

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

Ako biste radili ove testove na Acc_Gyro ploči, dobili biste sljedeće rezultate:

- izlazni pin za RateAxz je GX4 i InvertAxz = -1. - izlazni pin za RateAyz je GY4 i InvertAyz = -1

Od ovog trenutka nadalje smatrat ćemo da ste IMU postavili na takav način da možete izračunati ispravne vrijednosti za Axr, Ayr, Azr (kako je definirano u dijelu 1. Akcelerometar) i RateAxz, RateAyz (kako je definirano u 2. dijelu žiroskop).). Zatim ćemo analizirati odnose između ovih vrijednosti koji su se pokazali korisnim za dobijanje preciznije procjene nagiba uređaja u odnosu na ravninu tla.

Možda ste se već zapitali, ako nam je model akcelerometra već dao kutove nagiba Axr, Ayr, Azr, zašto bismo se htjeli zamarati podacima o žiroskopu? Odgovor je jednostavan: podacima akcelerometra ne može se uvijek vjerovati 100%. Postoji nekoliko razloga, zapamtite da akcelerometar mjeri inercijalnu silu, takva sila može biti uzrokovana gravitacijom (i idealno samo gravitacijom), ali može biti uzrokovana i ubrzanjem (kretanjem) uređaja. Kao rezultat toga, čak i ako je akcelerometar u relativno stabilnom stanju, i dalje je vrlo osjetljiv na vibracije i mehaničku buku općenito. Ovo je glavni razlog zašto većina IMU sistema koristi žiroskop kako bi izgladila sve greške akcelerometra. Ali kako se to radi? Je li žiroskop bez buke?

Žiroskop nije oslobođen buke, međutim, jer mjeri rotaciju, manje je osjetljiv na linearna mehanička kretanja, vrstu buke od koje akcelerometar pati, međutim žiroskopi imaju i druge vrste problema, na primjer, drift (ne vraća se na vrijednost nulte stope) kada prestane rotacija). Ipak, prosječnim podacima koji dolaze iz akcelerometra i žiroskopa možemo dobiti relativno bolju procjenu trenutnog nagiba uređaja nego što bismo dobili korištenjem samih podataka akcelerometra.

U sljedećim koracima predstavit ću algoritam inspiriran nekim idejama koje se koriste u Kalmanovom filtru, no daleko je jednostavniji i lakši za implementaciju na ugrađenim uređajima. Prije toga, prvo da vidimo šta želimo da naš algoritam izračuna. Pa, vektor smjera gravitacijske sile R = [Rx, Ry, Rz] iz kojeg možemo izvesti druge vrijednosti poput Axr, Ayr, Azr ili cosX, cozy, cosZ će nam dati ideju o nagibu našeg uređaja u odnosu na ravninu tla, raspravljamo o odnosu između ovih vrijednosti u 1. dijelu. Moglo bi se reći - zar već nemamo te vrijednosti Rx, Ry, Rz iz jednadžbe 2 u 1. dijelu? Pa da, ali imajte na umu da su ove vrijednosti izvedene samo iz podataka akcelerometra, pa ako biste ih koristili izravno u svojoj aplikaciji, mogli biste dobiti više buke nego što vaša aplikacija može tolerirati. Da bismo izbjegli dodatnu zabunu, definirajmo mjerenja akcelerometra na sljedeći način:

Racc - je vektor inercijalne sile mjeren akcelerometrom, koji se sastoji od sljedećih komponenti (projekcije na osi X, Y, Z):

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

Do sada imamo skup izmjerenih vrijednosti koje možemo dobiti isključivo iz ADC vrijednosti akcelerometra. Ovaj skup podataka nazvat ćemo "vektorom" i upotrijebit ćemo sljedeću notaciju.

Racc = [RxAcc, RyAcc, RzAcc]

Budući da se ove komponente Racc -a mogu dobiti iz podataka akcelerometra, možemo ga smatrati ulazom u naš algoritam.

Imajte na umu da ćete, budući da Racc mjeri silu gravitacije, biti u pravu ako pretpostavite da je dužina ovog vektora definirana na sljedeći način jednaka ili blizu 1 g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Međutim, kako bismo bili sigurni da ima smisla ažurirati ovaj vektor na sljedeći način:

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

Ovo će osigurati da je dužina vašeg normaliziranog Racc vektora uvijek 1.

Zatim ćemo uvesti novi vektor i nazvat ćemo ga

Odmor = [RxEst, RyEst, RzEst]

Ovo će biti rezultat našeg algoritma, to su ispravljene vrijednosti na osnovu podataka žiroskopa i na osnovu procijenjenih podataka.

Evo što će učiniti naš algoritam: - akcelerometar nam govori: "Sada ste na položaju Racc" - kažemo "Hvala, ali dopustite mi da provjerim", - zatim ispravite ove podatke pomoću podataka žiroskopa, kao i s podacima iz prošlosti i izlazimo novi procijenjeni vektor Rest. - smatramo da je odmor naš "najbolji ulog" u pogledu trenutne pozicije uređaja.

Pogledajmo kako to možemo učiniti.

Započećemo naš niz vjerujući svom akcelerometru i dodjeljujući:

Odmor (0) = Racc (0)

Usput, zapamtite da su Rest i Racc vektori, pa je gornja jednadžba samo jednostavan način da napišete 3 skupa jednadžbi i izbjegnete ponavljanje:

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

Zatim ćemo raditi redovna mjerenja u jednakim vremenskim intervalima od T sekundi, te ćemo dobiti nova mjerenja koja ćemo definirati kao Racc (1), Racc (2), Racc (3) itd. Također ćemo izdavati nove procjene u svakom vremenskom intervalu Odmor (1), Odmor (2), Odmor (3) itd.

Pretpostavimo da smo na koraku n. Imamo dva poznata skupa vrijednosti koje bismo željeli koristiti:

Rest (n -1) - naša prethodna procjena, sa Rest (0) = Racc (0) Racc (n) - naše trenutno mjerenje akcelerometra

Prije nego što možemo izračunati Rest (n), uvedimo novu izmjerenu vrijednost koju možemo dobiti iz našeg žiroskopa i prethodnu procjenu.

Zvat ćemo ga Rgyro, a to je također vektor koji se sastoji od 3 komponente:

Rgyro = [RxGyro, RyGyro, RzGyro]

Izračunavat ćemo ovaj vektor jednu po jednu komponentu. Počećemo sa RxGyro -om.

Počnimo promatranjem sljedeće relacije u našem modelu žiroskopa, iz pravokutnog trokuta formiranog od Rz i Rxz možemo izvesti sljedeće:

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

Atan2 može biti funkcija koju nikada prije niste koristili, slična je atanu, samo što vraća vrijednosti u rasponu od (-PI, PI) za razliku od (-PI/2, PI/2) koje je vratio atan, i potrebno je 2 argumenta umesto jednog. Omogućava nam da pretvorimo dvije vrijednosti Rx, Rz u kutove u punom rasponu od 360 stupnjeva (-PI u PI). Više o atan2 možete pročitati ovdje.

Znajući RxEst (n-1) i RzEst (n-1) možemo pronaći:

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

Upamtite da žiroskop mjeri brzinu promjene Axz kuta. Dakle, novi ugao Axz (n) možemo procijeniti na sljedeći način:

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

Upamtite da se RateAxz može dobiti iz očitanja ADC -a žiroskopa. Preciznija formula može koristiti prosječnu brzinu rotacije izračunatu na sljedeći način:

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

Na isti način možemo pronaći:

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

U redu, sada imamo Axz (n) i Ayz (n). Gdje idemo odavde da odbijemo RxGyro/RyGyro? Iz jednadžbe 1 možemo zapisati dužinu vektora Rgyro na sljedeći način:

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

Također zato što smo normalizirali naš Racc vektor, možemo pretpostaviti da je njegova dužina 1 i da se nije promijenila nakon rotacije, pa je relativno sigurno napisati:

| Rgyro | = 1

Usvojimo privremenu kraću notaciju za donje izračune:

x = RxGyro, y = RyGyro, z = RzGyro

Koristeći gore navedene odnose možemo napisati:

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

Podijelimo brojnik i nazivnik razlomka sa SQRT (x^2 + z^2)

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

Imajte na umu da je x / SQRT (x^2 + z^2) = sin (Axz), pa:

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

Sada pomnožite brojnik i nazivnik razlomka unutar SQRT sa z^2

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

Primijetite da je z / SQRT (x^2 + z^2) = cos (Axz) i y / z = tan (Ayz), pa konačno:

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

Vraćajući se na našu notaciju dobijamo:

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

na isti način na koji to nalazimo

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

Sada konačno možemo pronaći:

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

Gdje je Sign (RzGyro) = 1 kada je RzGyro> = 0, a Sign (RzGyro) = -1 kada je RzGyro <0.

Jedan jednostavan način da to procijenite je uzeti u obzir:

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

U praksi budite oprezni kada je RzEst (n-1) blizu 0. U ovom slučaju možete potpuno preskočiti žiroskopsku fazu i dodijeliti: Rgyro = Ostatak (n-1). Rz se koristi kao referenca za izračunavanje Axz i Ayz uglova, a kada je blizu 0, vrijednosti se mogu prelijevati i izazvati loše rezultate. Bit ćete u domenu velikih brojeva s pomičnim zarezom gdje implementacije funkcije tan () / atan () možda neće imati preciznost.

Dakle, rezimirajmo ono što smo do sada imali, mi smo u koraku n našeg algoritma i izračunali smo sljedeće vrijednosti:

Racc - trenutna očitanja sa našeg akcelerometra Rgyro - dobivena iz odmora (n -1) i trenutnih očitanja žiroskopa

Koje vrijednosti koristimo za izračunavanje ažurirane procjene Rest (n)? Vjerojatno ste pogodili da ćemo koristiti oboje. Koristit ćemo ponderirani prosjek tako da:

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

Ovu formulu možemo pojednostaviti dijeljenjem i brojnika i nazivnika razlomka sa w1.

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

i nakon zamjene w2/w1 = wGyro dobivamo:

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

U gore navedenom forumu wGyro nam govori koliko vjerujemo svom žiroskopu u odnosu na akcelerometar. Ova vrijednost se može odabrati eksperimentalno, obično vrijednosti između 5..20 će potaknuti dobre rezultate.

Glavna razlika ovog algoritma od Kalmanovog filtera je u tome što je ta težina relativno fiksna, dok se u Kalmanovom filtru težine stalno ažuriraju na osnovu izmjerene buke očitanja akcelerometra. Kalmanov filter je usmjeren na pružanje "najboljih" teorijskih rezultata, dok vam ovaj algoritam može dati rezultate "dovoljno dobre" za vašu praktičnu primjenu. Možete implementirati algoritam koji prilagođava wGyro ovisno o nekim faktorima šuma koje mjerite, ali fiksne vrijednosti dobro će funkcionirati za većinu aplikacija.

Na korak smo od dobivanja ažuriranih procijenjenih vrijednosti:

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

Ajmo sada normalizirati ovaj vektor ponovo:

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 spremni smo za ponavljanje naše petlje.

Ovaj vodič se prvobitno pojavio na starlino.com, napravio sam nekoliko lakih izmjena i ponovo ga objavio s dozvolom. Hvala Starlino!