Vodič za mjerenje brzine i žiroskop: 3 koraka

2024 Autor: John Day | [email protected]. Zadnja promjena: 2024-01-30 09:39

Uvod

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

Pokriti ćemo:

• Što mjeri akcelerometar?
• Što mjeri žiroskop (zvani žiroskop)?
• Kako pretvoriti analogno-digitalna (ADC) očitanja koja dobijete s 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 podatke o nagibu vašeg uređaja u odnosu na ravninu tla

U cijelom članku pokušat ću matematiku svesti na minimum. Ako znate što su Sinus/Kosinus/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.). Sve to možete 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 jednostavniji sustav lakše kontrolirati i nadzirati, osim toga mnogi ugrađeni uređaji nemaju snage i resursa za implementaciju složenih algoritama koji zahtijevaju matrične izračune.

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

- LIS331AL (podatkovni list) - troosni 2G akcelerometar - LPR550AL (podatkovni list) - dvoosni korak i rola, žiroskop od 500 stupnjeva/s

Zajedno predstavljaju jedinicu za mjerenje inercijalnog mjerenja od 5 stupnjeva slobode. To je sada otmjeno ime! Ipak, iza maštovitog imena 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 mjeračima ubrzanja, često je korisno zamisliti kutiju u obliku kocke s kuglicom u njoj. Možete zamisliti nešto drugo poput kolačića ili krafne, ali ja ću zamisliti loptu:

Ako uzmemo ovu kutiju na mjesto bez gravitacijskih polja ili bez drugih polja koja bi mogla utjecati na položaj lopte - lopta će jednostavno lebdjeti u sredini kutije. Možete zamisliti da je kutija u svemiru daleko od bilo kakvih kozmičkih tijela, ili ako je takvo mjesto teško pronaći zamislite barem svemirski brod koji kruži oko planeta gdje je sve u bestežinskom stanju. Sa gornje slike možete vidjeti da svakoj osi dodjeljujemo par zidova (uklonili smo zid Y+ kako bismo mogli 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. Ta se sila često naziva inercijska sila ili fiktivna sila. Iz ovoga biste trebali naučiti jednu stvar: akcelerometar mjeri ubrzanje neizravno putem sile koja se primjenjuje na jedan od njegovih zidova (prema našem modelu to bi moglo 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 osi Z. Pritisak koji je kugla izvršila na zid uzrokovan je gravitacionom silom. U teoriji bi to mogla biti druga vrsta sile - na primjer, zamislite li da je naša lopta metalna, postavljanjem magneta uz kutiju mogla bi se pomaknuti lopta tako da udari u drugi zid. To 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 točno način na koji je MEMS senzor konstruiran, č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 reći samo 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 stupnjeva 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 kada predstavimo naš sljedeći model za akcelerometar.

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

Molimo pogledajte gornji model, sačuvao sam boje osi kako biste mogli mentalno prijeći s prethodnog na novi model. Zamislite samo da je svaka os u novom modelu okomita na odgovarajuća lica okvira u prethodnom modelu. Vektor R je vektor sile koji mjeri akcelerometar (to može biti ili gravitacijska sila ili inercijska sila iz gornjih primjera ili njihova kombinacija). Rx, Ry, Rz su projekcija vektora R 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 Pitagorinog teorema u 3D.

Upamtite 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 preambule teorije sve smo bliže akcelerometrima u stvarnom životu. Vrijednosti Rx, Ry, Rz zapravo su linearno povezane s vrijednostima koje će vaš akcelerometar u stvarnom životu ispisati i koje možete koristiti za izvođenje različitih izračuna.

Prije nego što stignemo, razgovarajmo malo o načinu na koji će nam akcelerometri dostavljati te podatke. 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 razinu napona 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 funkcionira ADC, 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 od 0..1023, imajte na umu da je 1023 = 2^10 -1. 12-bitni ADC modul ispisat će 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 imat će referentni napon, pretpostavimo da je u našem primjeru 3.3V. Za pretvaranje 10 -bitne vrijednosti ADC -a 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 razinu napona od 0 g, možete ga pronaći u specifikacijama, to je napon koji odgovara 0 g. Da bismo dobili potpisanu vrijednost napona, moramo izračunati pomak s ove razine. Recimo da je naša razina napona 0 g VzeroG = 1,65 V. Izračunavamo pomake napona 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čitanja 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. Za dobivanje konačnih vrijednosti sila izraženih u g koristimo sljedeću formulu:

Rx = DeltaVoltsRx / Osjetljivost

Rx = 0,24 V / 0,4785 V / g = ~ 0,5 g Ry = 0,38 V / 0,4785 V / g = ~ 0,79 g Rz = 0,16 V / 0,4785 V / g = ~ 0,33 g

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:

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

cos (Axr) = Rx / R, a 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)

Daleko smo 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š točnije 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 trojac se često naziva smjer kosinus i u osnovi predstavlja jedinični vektor (vektor s duljinom 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 (duljine) 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, nego ćemo prijeći ravno na drugi model akcelerometra i pokazat ćemo što žiroskop mjeri prema ovom modelu.

Svaki kanal žiroskopa mjeri rotaciju oko jedne od osi. Na primjer, dvoosni žiroskop mjerit će rotaciju oko (i neki mogu reći "oko") osi X i Y. Da bismo izrazili ovu rotaciju u brojevima, napravimo neke zapise. Prvo definirajmo:

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

Iz pravokutnog trokuta koji tvore Rxz i Rz, koristeći Pitagorin teorem dobivamo:

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

također imajte na umu da:

R^2 = Rxz^2 + Ry^2, to se može izvesti iz jednadžbe 1 i iznad 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 napomenuti 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, ispisat će vrijednost koja je linearno povezana sa brzinom promjene ovih kutova. 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 bio 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 stupnjevima/s. Isto kao za akcelerometar dobit ćete ADC vrijednost koju ćete morati pretvoriti u stupnjeve/s pomoću formule slične jednadžbi. 2 koju smo definirali za akcelerometar. Uvedimo ADC u formulu pretvorbe u stupnjevima/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 - 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 ploču Acc_Gyro to je na primjer 1,23 V (ove vrijednosti možete pronaći u specifikacijama) Osjetljivost - osjetljivost vašeg žiroskopa izražena je 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,3 V/1023 - 1,23 V)/(0,002 V/stepeni/s) = ~ 306 stupnjeva/s RateAyz = (323 * 3,3 V/1023 - 1,23 V)/(0,002 V/stepeni/s) = ~ -94 stepeni/s

Drugim riječima, uređaj se okreće oko osi Y (ili možemo reći da se rotira u ravnini XZ) brzinom od 306 stupnjeva/s i oko osi X (ili možemo reći da se okreće u ravnini YZ) brzinom od - 94 stupnjeva/s. Imajte na umu da negativni znak znači da se uređaj okreće u suprotnom smjeru od uobičajenog pozitivnog smjera. Prema dogovoru, 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 s rotacijom napon pasti natrag na nultu razinu. Ako koristite multimetar, morali biste održavati konstantnu brzinu vrtnje barem nekoliko sekundi i zabilježiti napon tijekom ove rotacije, a zatim ga usporediti s nultom brzinom. Ako je veći od nultog napona, to znači da je smjer rotacije pozitivan.

Korak 3: Kombiniranje akcelerometra i žiroskopa

Spajanje svega - Kombiniranje podataka akcelerometra i žiroskopa

Ako čitate ovaj članak, vjerojatno ste kupili ili planirate nabaviti IMU uređaj ili ga namjeravate izgraditi 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 sustava. Najlakši način za to je odabir koordinatnog sustava akcelerometra kao referentnog koordinatnog sustava. 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 ploču Acc_Gyro:

Sljedeći koraci su:

Identificirajte izlaze žiroskopa koji odgovaraju gore navedenim vrijednostima RateAxz, RateAyz. Odredite treba li ove izlaze preokrenuti zbog fizičkog položaja žiroskopa u odnosu na akcelerometar

Nemojte pretpostavljati da će, ako žiroskop ima izlaz označen s X ili Y, odgovarati bilo kojoj osi u koordinatnom sustavu akcelerometra, čak i ako je taj 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 stupnjeva u odnosu na čip akcelerometra. Ako ste kupili IMU ploču, 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.

Ovdje je uzorak slijeda 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 emitirat će nulti napon (na primjer za ploču Acc_Gyro ovo je 1,65 V)

- sljedeći put zakrenite 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, napominjte 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 promijeniti vrijednost RateAxz zbog fizičkog položaja žiroskopa u odnosu na mjerač ubrzanja - ponovite gornji test, okrećući uređaj oko osi Y, 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 predznaka u jednadžbu 3, kako slijedi:

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

isti testni štap treba se obaviti za RateAyz, zakretanjem uređaja oko osi X, 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 ove točke 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 se pokazuju korisnima za dobivanje točnije 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, imajte na umu da akcelerometar mjeri inercijsku 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 općenito na mehaničku buku. To je glavni razlog zašto većina IMU sustava koristi žiroskop kako bi izgladila sve pogreš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 trpi, međutim žiroskopi imaju i druge vrste problema, na primjer, pomak (ne vraća se na vrijednost nulte stope) kad 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 samo pomoću 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 pogledajmo što ž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 dat će nam ideju o nagibu našeg uređaja u odnosu na ravninu tla, raspravljamo o odnosu ovih vrijednosti u 1. dijelu. Moglo bi se reći - nemamo li već ove vrijednosti Rx, Ry, Rz iz jednadžbe 2 u 1. dijelu? Pa da, ali imajte na umu da su te 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. Kako bismo izbjegli daljnju zabunu, redefinirajmo mjerenja akcelerometra na sljedeći način:

Racc - je vektor inercijske 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. Taj ćemo skup podataka nazvati "vektorom" i upotrijebit ćemo sljedeću notaciju.

Racc = [RxAcc, RyAcc, RzAcc]

Budući da se ove komponente Racca 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 duljina 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, ima smisla ažurirati ovaj vektor na sljedeći način:

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

To će osigurati da duljina vašeg normaliziranog Racc vektora uvijek bude 1.

Zatim ćemo uvesti novi vektor i nazvat ćemo ga

Odmor = [RxEst, RyEst, RzEst]

To će biti rezultat našeg algoritma, to su ispravljene vrijednosti na temelju podataka žiroskopa i na temelju prošlih 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 prošlim podacima o mirovanju i izlazimo novi procijenjeni vektor Rest. - smatramo da je odmor naš "najbolji ulog" u pogledu trenutnog položaja uređaja.

Pogledajmo kako to možemo učiniti.

Započet ćemo naš slijed vjerujući svom mjeraču ubrzanja 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 redovita 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, s 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.

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

Rgyro = [RxGyro, RyGyro, RzGyro]

Izračunavat ćemo ovaj vektor jednu po jednu komponentu. Počet ćemo s RxGyroom.

Započnimo promatranjem sljedeće relacije u našem modelu žiroskopa, iz pravokutnog trokuta koji čine Rz i Rxz možemo izvesti da:

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 umjesto jednog. Omogućuje nam pretvaranje dviju vrijednosti Rx, Rz u kutove u cijelom 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 kuta Axz. Dakle, novi kut 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 naših očitanja ADC -a u žiroskopu. 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 duljinu 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 duljina 1 i da se nije promijenila nakon rotacije, pa je relativno sigurno napisati:

| Rgyro | = 1

Usvojimo privremeni kraći zapis za sljedeće izračune:

x = RxGyro, y = RyGyro, z = RzGyro

Koristeći gornje 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)))

Imajte na umu 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šem zapisu dobivamo:

RxGiro = 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 fazu žiroskopa i dodijeliti: Rgyro = Odmor (n-1). Rz se koristi kao referenca za izračunavanje Axz i Ayz kutova, a kada je blizu 0, vrijednosti se mogu prelijevati i izazvati loše rezultate. Bit ćete u domeni 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 s našeg akcelerometra Rgyro - dobivena iz odmora (n -1) i trenutnih očitanja žiroskopa

Koje vrijednosti koristimo za izračun 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 s 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 navedenoj forumu wGyro nam govori koliko vjerujemo svom žiroskopu u odnosu na akcelerometar. Ova se vrijednost može odabrati eksperimentalno, obično vrijednosti između 5..20 potaknut će 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 temelju izmjerene buke očitanja akcelerometra. Kalmanov filter usmjeren je na pružanje "najboljih" teoretskih 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 + wGiro)

Ajmo normalizirati ovaj vektor opet:

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č izvorno se pojavio na starlino.com, napravio sam nekoliko lakih izmjena i ponovno ga objavio s dopuštenjem. Hvala Starlino!