Akselerometer og gyroopplæring: 3 trinn

2024 Forfatter: John Day | [email protected]. Sist endret: 2024-01-30 11:26

Introduksjon

Denne veiledningen er beregnet for alle som er interessert i å bruke akselerometre og gyroskoper, så vel som kombinasjon av IMU -enheter (treghetsmåleenhet) i sine elektroniske prosjekter

Vi dekker:

• Hva måler et akselerometer?
• Hva måler et gyroskop (aka gyro)?
• Hvordan konvertere analog-til-digital (ADC) avlesninger du får fra disse sensorene til fysiske enheter (de ville være g for akselerometer, deg/s for gyroskop)
• Slik kombinerer du målinger av akselerometer og gyroskop for å få nøyaktig informasjon om enhetens skråning i forhold til bakken

Gjennom artikkelen vil jeg prøve å holde matematikken til et minimum. Hvis du vet hva Sine/Cosine/Tangent er, bør du kunne forstå og bruke disse ideene i prosjektet ditt, uansett hvilken plattform du bruker: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC, etc.

Det er mennesker der ute som tror at du trenger kompleks matematikk for å kunne bruke en IMU-enhet (komplekse FIR- eller IIR-filtre som Kalman-filtre, Parks-McClellan-filtre osv.). Du kan forske på alle disse og oppnå fantastiske, men komplekse resultater. Min måte å forklare ting på krever bare grunnleggende matematikk. Jeg tror veldig på enkelhet. Jeg tror et enkelt system er lettere å kontrollere og overvåke, i tillegg til at mange innebygde enheter ikke har kraft og ressurser til å implementere komplekse algoritmer som krever matriseberegninger.

Jeg vil som eksempel bruke en ny IMU -enhet, Acc_Gyro Accelerometer + Gyro IMU. Vi bruker parametere for denne enheten i eksemplene våre nedenfor. Denne enheten er en god enhet til å begynne med fordi den består av 2 enheter:

- LIS331AL (datablad) - et triaksialt 2G akselerometer - LPR550AL (datablad) - en to -akset pitch and roll, 500 grader/sek gyroskop

Sammen representerer de en 5-graders frihet treghetsmåleenhet. Nå er det et fancy navn! Bak det flotte navnet er det imidlertid en veldig nyttig kombinasjonsenhet som vi dekker og forklarer i detalj i denne guiden.

Trinn 1: Akselerometeret

For å forstå denne enheten starter vi med akselerometeret. Når du tenker på akselerometre, er det ofte nyttig å bilde en boks i form av en terning med en ball inne i den. Du kan tenke deg noe annet som en kake eller en smultring, men jeg skal tenke meg en ball:

Hvis vi tar denne boksen på et sted uten gravitasjonsfelt eller for den saks skyld uten andre felt som kan påvirke ballens posisjon - vil ballen ganske enkelt flyte midt i feltet. Du kan forestille deg at boksen er i verdensrommet langt-langt unna noen kosmiske kropper, eller hvis et slikt sted er vanskelig å finne, tenk deg i det minste et romfartøy som kretser rundt planeten der alt er i vektløs tilstand. Fra bildet ovenfor kan du se at vi tilordner et par vegger til hver akse (vi fjernet veggen Y+ slik at vi kan se inne i boksen). Tenk deg at hver vegg er trykkfølsom. Hvis vi plutselig beveger boksen til venstre (vi akselererer den med akselerasjon 1g = 9,8m/s^2), vil ballen treffe veggen X-. Vi måler deretter trykkraften som ballen bruker på veggen og gir ut en verdi på -1g på X -aksen.

Vær oppmerksom på at akselerometeret faktisk vil oppdage en kraft som er rettet i motsatt retning fra akselerasjonsvektoren. Denne styrken kalles ofte Inertial Force eller Fictitious Force. En ting du bør lære av dette er at et akselerometer måler akselerasjon indirekte gjennom en kraft som påføres en av veggene (ifølge vår modell kan det være en fjær eller noe annet i virkelighetsakselometre). Denne kraften kan skyldes akselerasjonen, men som vi ser i neste eksempel, er den ikke alltid forårsaket av akselerasjon.

Hvis vi tar modellen vår og legger den på jorden, vil ballen falle på Z-veggen og bruke en kraft på 1g på bunnveggen, som vist på bildet nedenfor:

I dette tilfellet beveger boksen seg ikke, men vi får fortsatt en avlesning på -1g på Z -aksen. Trykket som ballen har påført veggen ble forårsaket av en gravitasjonskraft. I teorien kan det være en annen type kraft - for eksempel hvis du forestiller deg at ballen vår er metallisk, kan plassering av en magnet ved siden av boksen flytte ballen slik at den treffer en annen vegg. Dette ble sagt bare for å bevise at i hovedsak måler akselerometer ikke akselerasjon. Det skjer bare at akselerasjon forårsaker en treghetskraft som fanges opp av akselerometerets kraftdeteksjonsmekanisme.

Selv om denne modellen ikke akkurat er hvordan en MEMS -sensor er konstruert, er den ofte nyttig for å løse akselerometerrelaterte problemer. Det er faktisk lignende sensorer som har metallkuler inni, de kalles vippebrytere, men de er mer primitive og vanligvis kan de bare se om enheten er tilbøyelig innenfor et eller annet område eller ikke, ikke omfanget av helling.

Så langt har vi analysert akselerometerutgangen på en enkelt akse, og dette er alt du får med en akselakselometer. Den reelle verdien av triaksiale akselerometre kommer fra det faktum at de kan oppdage treghetskrefter på alle tre aksene. La oss gå tilbake til boksmodellen vår, og la oss rotere boksen 45 grader til høyre. Ballen vil berøre 2 vegger nå: Z- og X- som vist på bildet nedenfor:

Verdiene på 0,71 er ikke vilkårlige, de er faktisk en tilnærming for SQRT (1/2). Dette vil bli mer tydelig når vi introduserer vår neste modell for akselerometeret.

I den forrige modellen har vi fikset gravitasjonskraften og rotert den imaginære boksen. I de to siste eksemplene har vi analysert utgangen i 2 forskjellige boksposisjoner, mens kraftvektoren forble konstant. Selv om dette var nyttig for å forstå hvordan akselerometeret samhandler med ytre krefter, er det mer praktisk å utføre beregninger hvis vi fikser koordinatsystemet til akslene til akselerometeret og forestiller oss at kraftvektoren roterer rundt oss.

Ta en titt på modellen ovenfor, jeg beholdt fargene på aksene, slik at du kan gjøre en mental overgang fra den forrige modellen til den nye. Tenk deg at hver akse i den nye modellen er vinkelrett på de respektive sidene på esken i den forrige modellen. Vektoren R er kraftvektoren som akselerometeret måler (det kan enten være gravitasjonskraften eller treghetskraften fra eksemplene ovenfor eller en kombinasjon av begge). Rx, Ry, Rz er projeksjon av R -vektoren på X-, Y-, Z -aksene. Vær oppmerksom på følgende forhold:

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

som i utgangspunktet tilsvarer Pythagoras teorem i 3D.

Husk at jeg litt tidligere fortalte deg at verdiene til SQRT (1/2) ~ 0,71 ikke er tilfeldige. Hvis du kobler dem til formelen ovenfor, kan vi etter å ha husket at gravitasjonskraften vår var 1 g verifisere at:

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

ganske enkelt ved å erstatte R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) i Eq.1

Etter en lang innledning til teori nærmer vi oss det virkelige livets akselerometre. Verdiene Rx, Ry, Rz er faktisk lineært relatert til verdiene som ditt virkelige akselerometer vil levere og som du kan bruke til å utføre forskjellige beregninger.

Før vi kommer dit, la oss snakke litt om hvordan akselerometre vil levere denne informasjonen til oss. De fleste akselerometre vil falle i to kategorier: digital og analog. Digitale akselerometre gir deg informasjon ved hjelp av en seriell protokoll som I2C, SPI eller USART, mens analoge akselerometre sender ut et spenningsnivå innenfor et forhåndsdefinert område som du må konvertere til en digital verdi ved hjelp av en ADC -modul (analog til digital omformer). Jeg skal ikke gå så mye inn på hvordan ADC fungerer, dels fordi det er et så omfattende tema og dels fordi det er forskjellig fra en plattform til en annen. Noen mikrokontroller vil ha en innebygd ADC-modul. Noen av dem trenger eksterne komponenter for å utføre ADC-konverteringene. Uansett hvilken type ADC -modul du bruker, vil du ende opp med en verdi i et bestemt område. For eksempel sender en 10 -biters ADC -modul ut en verdi i området 0..1023, merk at 1023 = 2^10 -1. En 12-biters ADC-modul sender ut en verdi i området 0..4095, vær oppmerksom på at 4095 = 2^12-1.

La oss gå videre ved å vurdere et enkelt eksempel, anta at vår 10bit ADC -modul ga oss følgende verdier for de tre akselerometerkanalene (aksene):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Hver ADC -modul vil ha en referansespenning, la oss anta at det i vårt eksempel er 3,3V. For å konvertere en 10bit adc -verdi til spenning bruker vi følgende formel:

VoltsRx = AdcRx * Vref / 1023

Et raskt notat her: at for 8bit ADC vil den siste divideren være 255 = 2 ^ 8 -1, og for 12bit ADC vil siste divider være 4095 = 2 ^ 12 -1.

Ved å bruke denne formelen på alle tre kanalene får vi:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (vi avrunder alle resultatene til 2 desimaler) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Hvert akselerometer har et null-g spenningsnivå, du finner det i spesifikasjoner, dette er spenningen som tilsvarer 0g. For å få en signert spenningsverdi må vi beregne skiftet fra dette nivået. La oss si at vårt 0g spenningsnivå er VzeroG = 1,65V. Vi beregner spenningsskiftene fra null-g spenning som følger::

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

Vi har nå våre akselerometeravlesninger i volt, det er fortsatt ikke i g (9,8 m/s^2), for å gjøre den siste konverteringen bruker vi akselerometerfølsomheten, vanligvis uttrykt i mV/g. La oss si vår følsomhet = 478.5mV/g = 0.4785V/g. Følsomhetsverdier finnes i spesifikasjonene for akselerometer. For å få de endelige kraftverdiene uttrykt i g bruker vi følgende formel:

Rx = DeltaVoltsRx / Sensitivity

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

Vi kunne selvfølgelig kombinere alle trinnene i en formel, men jeg gikk gjennom alle trinnene for å gjøre det klart hvordan du går fra ADC -avlesninger til en kraftvektorkomponent uttrykt i g.

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

Vi har nå alle tre komponentene som definerer vår treghetskraftvektor. Hvis enheten ikke er utsatt for andre krefter enn gravitasjon, kan vi anta at dette er retningen til gravitasjonskraftvektoren vår. Hvis du vil beregne enhetens helning i forhold til bakken, kan du beregne vinkelen mellom denne vektoren og Z -aksen. Hvis du også er interessert i hellingsretning per akse, kan du dele dette resultatet i 2 komponenter: helning på X- og Y-aksen som kan beregnes som vinkelen mellom gravitasjonsvektor og X / Y-akser. Å beregne disse vinklene er enklere enn du kanskje tror, nå som vi har beregnet verdiene for Rx, Ry og Rz. La oss gå tilbake til vår siste akselerometer -modell og gjøre noen ekstra notater:

Vinklene vi er interessert i er vinklene mellom X-, Y-, Z -akser og kraftvektoren R. Vi definerer disse vinklene som Axr, Ayr, Azr. Du kan merke fra den rettvinklede trekanten dannet av R og Rx at:

cos (Axr) = Rx / R, og tilsvarende: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Vi kan trekke fra Eq.1 at R = SQRT (Rx^2 + Ry^2 + Rz^2).

Vi kan nå finne vinklene våre ved å bruke arccos () -funksjonen (invers cos () -funksjonen):

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

Vi har gått langt for å forklare akselerometermodellen, bare for å komme opp til disse formlene. Avhengig av applikasjonene dine kan det være lurt å bruke mellomformler som vi har utledet. Vi vil også introdusere gyroskopmodellen snart, og vi får se hvordan akselerometer- og gyroskopdata kan kombineres for å gi enda mer nøyaktige helningsestimater.

Men før vi gjør det, la oss gjøre noen flere nyttige notater:

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

Denne trillingen kalles ofte Direction Cosine, og den representerer i utgangspunktet enhetsvektoren (vektor med lengde 1) som har samme retning som vår R -vektor. Du kan enkelt bekrefte at:

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

Dette er en fin egenskap siden den fritar oss fra å overvåke modulen (lengden) til R -vektoren. Ofte er det fornuftig å normalisere modulen for å forenkle andre beregninger hvis vi bare er interessert i retningen til vår treghetsvektor.

Trinn 2: Gyroskop

Vi kommer ikke til å introdusere en tilsvarende boksmodell for gyroskopet som vi gjorde for akselerometer, i stedet skal vi hoppe rett til den andre akselerometermodellen, og vi vil vise hva som gyroskopet måler i henhold til denne modellen.

Hver gyroskopkanal måler rotasjonen rundt en av aksene. For eksempel vil et 2-akset gyroskop måle rotasjonen rundt (eller noen kan si "om") X- og Y-aksene. For å uttrykke denne rotasjonen i tall, la oss gjøre noen notasjoner. La oss først definere:

Rxz - er projeksjonen av treghetskraftvektoren R på XZ -planet Ryz - er projeksjonen av treghetskraftvektoren R på YZ -planet

Fra den rettvinklede trekanten dannet av Rxz og Rz, ved hjelp av Pythagoras teorem får vi:

Rxz^2 = Rx^2 + Rz^2, og tilsvarende: Ryz^2 = Ry^2 + Rz^2

Vær også oppmerksom på at:

R^2 = Rxz^2 + Ry^2, dette kan avledes fra ligninger 1 og over, eller det kan avledes fra en rettvinklet trekant dannet av R og Ryz R^2 = Ryz^2 + Rx^2

Vi kommer ikke til å bruke disse formlene i denne artikkelen, men det er nyttig å merke seg forholdet mellom alle verdiene i modellen vår.

I stedet skal vi definere vinkelen mellom Z -aksen og Rxz, Ryz -vektorer som følger:

Axz - er vinkelen mellom Rxz (projeksjon av R på XZ -planet) og Z -aksen Ayz - er vinkelen mellom Ryz (projeksjon av R på YZ -plan) og Z -aksen

Nå kommer vi nærmere hva gyroskopet måler. Gyroskop måler hastigheten på endringer av vinklene definert ovenfor. Med andre ord vil den sende ut en verdi som er lineært relatert til endringshastigheten til disse vinklene. For å forklare dette la oss anta at vi har målt rotasjonsvinkelen rundt akse Y (det ville være Axz -vinkel) på tidspunktet t0, og vi definerer det som Axz0, deretter målte vi denne vinkelen på et senere tidspunkt t1 og det var Axz1. Endringshastigheten beregnes som følger:

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

Hvis vi uttrykker Axz i grader, og tiden i sekunder, vil denne verdien bli uttrykt i deg/s. Dette er hva et gyroskop måler.

I praksis vil et gyroskop (med mindre det er et spesielt digitalt gyroskop) sjelden gi deg en verdi uttrykt i deg/s. På samme måte som for akselerometer får du en ADC -verdi som du må konvertere til deg/s ved å bruke en formel som ligner Eq. 2 som vi har definert for akselerometer. La oss introdusere ADC til deg/s konverteringsformel for gyroskop (vi antar at vi bruker en 10 bit ADC modul, for 8 bit ADC erstatt 1023 med 255, for 12 bit ADC erstatt 1023 med 4095).

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

AdcGyroXZ, AdcGyroYZ - hentes fra vår adc -modul, og de representerer kanalene som måler rotasjonen av projeksjonen av R -vektoren i henholdsvis XZ i YZ -fly, noe som tilsvarer å si at rotasjonen ble utført rundt henholdsvis Y- og X -akser.

Vref - er ADC -referansespenningen vi skal bruke 3.3V i eksemplet under VzeroRate - er nullhastighetsspenningen, med andre ord spenningen som gyroskopet sender ut når det ikke er utsatt for noen rotasjon, for Acc_Gyro -kortet er det for eksempel 1,23V (du kan finne disse verdiene i spesifikasjonene) Følsomhet - er følsomheten til gyroskopet ditt uttrykt i mV / (deg / s) ofte skrevet som mV / deg / s, det forteller deg i utgangspunktet hvor mange mV som vil gyroskopets utgang øker, hvis du øker rotasjonshastigheten med en grad/s. Følsomheten til Acc_Gyro -kortet er for eksempel 2mV/deg/s eller 0.002V/deg/s

La oss ta et eksempel, anta at ADC -modulen returnerte følgende verdier:

AdcGyroXZ = 571 AdcGyroXZ = 323

Ved å bruke formelen ovenfor og spesifikasjonsparametrene til Acc_Gyro -kortet får vi:

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

Med andre ord roterer enheten rundt Y -aksen (eller vi kan si at den roterer i XZ -plan) med en hastighet på 306 grader/s og rundt X -aksen (eller vi kan si at den roterer i YZ -planet) med en hastighet på - 94 grader/s. Vær oppmerksom på at det negative tegnet betyr at enheten roterer i motsatt retning fra den konvensjonelle positive retningen. Etter konvensjon er en rotasjonsretning positiv. Et godt gyroskopspesifikasjonsark viser deg hvilken retning som er positiv, ellers må du finne den ved å eksperimentere med enheten og merke hvilken rotasjonsretning som resulterer i økende spenning på utgangspinnen. Dette gjøres best ved hjelp av et oscilloskop, siden spenningen vil falle tilbake til nullhastighetsnivået så snart du stopper rotasjonen. Hvis du bruker et multimeter, må du opprettholde en konstant rotasjonshastighet i minst noen sekunder og notere spenningen under denne rotasjonen, og sammenligne den med nullhastighetsspenningen. Hvis den er større enn nullhastighetsspenningen betyr det at rotasjonsretningen er positiv.

Trinn 3: Kombinere akselerometer og gyro

Setter alt sammen - Kombinerer akselerometer- og gyroskopdata

Hvis du leser denne artikkelen, har du sannsynligvis kjøpt eller planlegger å kjøpe en IMU -enhet, eller sannsynligvis planlegger du å bygge en fra separate akselerometer- og gyroskopenheter.

Det første trinnet i å bruke en kombinasjons IMU -enhet som kombinerer et akselerometer og et gyroskop er å justere koordinatsystemene deres. Den enkleste måten å gjøre det på er å velge akselerometerets koordinatsystem som referansekoordinatsystem. De fleste akselerometerdatablad vil vise retningen til X-, Y-, Z -aksene i forhold til bildet av den fysiske brikken eller enheten. Her er for eksempel instruksjonene til X-, Y-, Z -aksene som vist i spesifikasjonene for Acc_Gyro -kortet:

Neste trinn er:

Identifiser gyroskoputgangene som tilsvarer RateAxz, RateAyz -verdiene som er diskutert ovenfor. Bestem om disse utgangene må inverteres på grunn av gyroskopets fysiske posisjon i forhold til akselerometeret

Ikke anta at hvis et gyroskop har en utgang merket X eller Y, vil det tilsvare enhver akse i akselerometerets koordinatsystem, selv om denne utgangen er en del av en IMU -enhet. Den beste måten er å teste det. Forutsatt at du har bestemt posisjonen til gyroskopet i forhold til akselerometeret. Det antas at gyro- og akselerometergrensene er parallelle med hverandre, det vil si at du plasserer gyroen i en vinkel på 90 grader i forhold til akselerometerbrikken. Hvis du skaffet deg et IMU -kort, er sjansen stor for at de allerede er justert på denne måten. Vi kommer ikke til å diskutere i denne artikkelen modeller hvor gyroskop er plassert i en uregelmessig vinkel i forhold til akselerometer (la oss si 45 eller 30 grader), selv om dette kan være nyttig i noen applikasjoner.

Her er en prøvesekvens for å bestemme hvilken utgang av gyroskop som tilsvarer RateAxz -verdien som er diskutert ovenfor.

- begynn med å plassere enheten i horisontal posisjon. Både X- og Y-utgangene til akselerometeret gir null-g-spenningen (for eksempel for Acc_Gyro-kortet er dette 1,65V)

- neste start roterer enheten rundt Y -aksen, en annen måte å si det på er at du roterer enheten i XZ -plan, slik at X- og Z -akselerometerutgangene endres og Y -utgangen forblir konstant. - mens enheten roteres med konstant hastighet og noterer hvilken gyroskoputgang som endres, bør de andre gyroskoputgangene forbli konstant - gyroskoputgangen som endret seg under rotasjonen rundt Y -aksen (rotasjon i XZ -planet) vil gi inngangsverdien for AdcGyroXZ, hvorfra vi beregner RateAxz - det siste trinnet er å sikre at rotasjonsretningen tilsvarer modellen vår. I noen tilfeller må du kanskje invertere RateAxz -verdien på grunn av gyroskopets fysiske posisjon i forhold til akselerometeret - utfør testen ovenfor igjen, roter enheten rundt Y -aksen, denne gangen overvåker du X -utgangen til akselerometeret (AdcRx i vår modell). Hvis AdcRx vokser (de første 90 rotasjonsgradene fra horisontal posisjon), bør AdcGyroXZ også vokse. Ellers må du invertere RateAxz, du kan oppnå dette ved å introdusere en tegnfaktor i Eq.3, som følger:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity, hvor InvertAxz er 1 eller -1

samme testrør gjøres for RateAyz, ved å rotere enheten rundt X -aksen, og du kan identifisere hvilken gyroskoputgang som tilsvarer RateAyz, og om den må inverteres. Når du har verdien for InvertAyz, bør du bruke følgende formel for å beregne RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Følsomhet

Hvis du ville gjøre disse testene på Acc_Gyro -kortet, ville du få følgende resultater:

- utgangspinnen for RateAxz er GX4 og InvertAxz = -1. - utgangspinnen for RateAyz er GY4 og InvertAyz = -1

Fra dette tidspunktet vil vi vurdere at du har konfigurert IMU på en slik måte at du kan beregne riktige verdier for Axr, Ayr, Azr (som definert del 1. Akselerometer) og RateAxz, RateAyz (som definert i del 2. Gyroskop)). Deretter analyserer vi forholdet mellom disse verdiene som viser seg å være nyttige for å få en mer nøyaktig estimering av enhetens helning i forhold til grunnplanet.

Du kan spørre deg selv på dette tidspunktet, hvis akselerometermodellen allerede ga oss hellingsvinklene til Axr, Ayr, Azr, hvorfor skulle vi bry oss om gyroskopdataene? Svaret er enkelt: Akselerometerdata kan ikke alltid stole 100%på. Det er flere grunner, husk at akselerometer måler treghetskraft, en slik kraft kan være forårsaket av gravitasjon (og ideelt sett bare av gravitasjon), men det kan også være forårsaket av akselerasjon (bevegelse) av enheten. Som et resultat, selv om akselerometeret er i en relativt stabil tilstand, er det fortsatt veldig følsomt for vibrasjoner og mekanisk støy generelt. Dette er hovedårsaken til at de fleste IMU -systemer bruker et gyroskop for å utjevne eventuelle akselerometerfeil. Men hvordan gjøres dette? Og er gyroskopet fritt for støy?

Gyroskopet er ikke fritt for støy, men fordi det måler rotasjon er det mindre følsomt for lineære mekaniske bevegelser, typen støy som akselerometeret lider av, men gyroskoper har andre typer problemer som for eksempel drift (kommer ikke tilbake til nullhastighetsverdi når rotasjonen stopper). Likevel kan vi ved å beregne data som kommer fra akselerometer og gyroskop få et relativt bedre estimat av gjeldende enhetshelling enn vi ville oppnå ved å bruke akselerometerdataene alene.

I de neste trinnene vil jeg introdusere en algoritme som ble inspirert av noen ideer som ble brukt i Kalman -filteret, men den er langt enklere og enklere å implementere på innebygde enheter. Før det skal vi først se hva vi vil at algoritmen vår skal beregne. Vel, det er retningen for gravitasjonskraftvektoren R = [Rx, Ry, Rz] hvorfra vi kan utlede andre verdier som Axr, Ayr, Azr eller cosX, koselig, cosZ som vil gi oss en ide om helningen til enheten vår i forhold til grunnplanet, diskuterer vi forholdet mellom disse verdiene i del 1. Man kan si - har vi ikke allerede disse verdiene Rx, Ry, Rz fra Eq.2 i del 1? Ja, men husk at disse verdiene bare er avledet fra akselerometerdata, så hvis du ville bruke dem direkte i applikasjonen din, kan du få mer støy enn appen din tåler. For å unngå ytterligere forvirring, la oss definere akselerometermålingene på nytt som følger:

Racc - er treghetskraftvektoren målt ved akselerometer, som består av følgende komponenter (anslag på X-, Y-, Z -akser):

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

Så langt har vi et sett med måleverdier som vi kan få rent fra ADC -verdier for akselerometer. Vi kaller dette datasettet en "vektor", og vi bruker følgende notasjon.

Racc = [RxAcc, RyAcc, RzAcc]

Fordi disse komponentene i Racc kan hentes fra akselerometerdata, kan vi betrakte det som en inngang til algoritmen vår.

Vær oppmerksom på at fordi Racc måler gravitasjonskraften, vil du ha rett hvis du antar at lengden på denne vektoren definert som følger er lik eller nær 1g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Men for å være sikker er det fornuftig å oppdatere denne vektoren som følger:

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

Dette vil sikre at lengden på din normaliserte Racc -vektor alltid er 1.

Deretter introduserer vi en ny vektor, og vi kaller den

Rest = [RxEst, RyEst, RzEst]

Dette vil være resultatet av algoritmen vår, disse er korrigerte verdier basert på gyroskopdata og basert på tidligere estimerte data.

Her er hva algoritmen vår vil gjøre: - akselerometer forteller oss: "Du er nå i posisjon Racc" - vi sier "Takk, men la meg sjekke", - korriger deretter denne informasjonen med gyroskopdata så vel som med tidligere hviledata og vi sender ut en ny estimert vektor Rest. - vi anser hvile som vår "beste innsats" når det gjelder enhetens nåværende posisjon.

La oss se hvordan vi kan få det til å fungere.

Vi starter sekvensen vår med å stole på akselerometeret vårt og tildele:

Hvile (0) = Racc (0)

Husk forresten hvile og Racc er vektorer, så ligningen ovenfor er bare en enkel måte å skrive 3 sett med ligninger på, og unngå gjentagelse:

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

Deretter utfører vi regelmessige målinger med like lange tidsintervaller på T sekunder, og vi får nye målinger som vi definerer som Racc (1), Racc (2), Racc (3) og så videre. Vi vil også utstede nye estimater for hver tidsintervall Rest (1), Rest (2), Rest (3) og så videre.

Anta at vi er på trinn n. Vi har to kjente sett med verdier som vi ønsker å bruke:

Rest (n -1) - vårt tidligere estimat, med Rest (0) = Racc (0) Racc (n) - vår nåværende akselerometermåling

Før vi kan beregne hvile (n), la oss introdusere en ny måleverdi som vi kan få fra vårt gyroskop og et tidligere estimat.

Vi kaller det Rgyro, og det er også en vektor som består av 3 komponenter:

Rgyro = [RxGyro, RyGyro, RzGyro]

Vi beregner denne vektoren en komponent om gangen. Vi starter med RxGyro.

La oss starte med å observere følgende forhold i vår gyroskopmodell, fra den rettvinklede trekanten dannet av Rz og Rxz kan vi utlede det:

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

Atan2 kan være en funksjon du aldri har brukt før, den ligner på atan, bortsett fra at den returnerer verdier i området (-PI, PI) i motsetning til (-PI/2, PI/2) som returnert av atan, og den tar 2 argumenter i stedet for ett. Det lar oss konvertere de to verdiene til Rx, Rz til vinkler i hele området 360 grader (-PI til PI). Du kan lese mer om atan2 her.

Så å kjenne RxEst (n-1) og RzEst (n-1) kan vi finne:

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

Husk at gyroskop måler endringshastigheten til Axz -vinkelen. Så vi kan estimere den nye vinkelen Axz (n) som følger:

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

Husk at RateAxz kan fås fra våre gyroskop ADC -målinger. En mer presis formel kan bruke en gjennomsnittlig rotasjonshastighet beregnet som følger:

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

På samme måte kan vi finne:

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

Ok, så nå har vi Axz (n) og Ayz (n). Hvor går vi herfra for å trekke fra RxGyro/RyGyro? Fra ekv. 1 kan vi skrive lengden på vektoren Rgyro slik:

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

Også fordi vi normaliserte vår Racc -vektor, kan vi anta at lengden er 1 og at den ikke har endret seg etter rotasjonen, så det er relativt trygt å skrive:

| Rgyro | = 1

La oss vedta en midlertidig kortere notasjon for beregningene nedenfor:

x = RxGyro, y = RyGyro, z = RzGyro

Ved å bruke relasjonene ovenfor kan vi skrive:

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

La oss dele teller og nevner av brøk med SQRT (x^2 + z^2)

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

Vær oppmerksom på at x / SQRT (x^2 + z^2) = sin (Axz), så:

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

Nå multipliserer teller og nevner av brøk inne i SQRT med z^2

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

Legg merke til at z / SQRT (x^2 + z^2) = cos (Axz) og y / z = tan (Ayz), så til slutt:

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

Når vi går tilbake til notasjonen vår, får vi:

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

på samme måte finner vi det

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

Nå kan vi endelig finne:

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

Hvor tegn (RzGyro) = 1 når RzGyro> = 0, og tegn (RzGyro) = -1 når RzGyro <0.

En enkel måte å anslå dette på er å ta:

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

Vær i praksis forsiktig når RzEst (n-1) er nær 0. Du kan hoppe over gyrofasen helt i dette tilfellet og tildele: Rgyro = Rest (n-1). Rz brukes som referanse for beregning av Axz- og Ayz -vinkler, og når det er nær 0, kan verdier flyte ut og utløse dårlige resultater. Du vil være i domenet til store flytende tall der tan () / atan () funksjon implementeringer kan mangle presisjon.

Så la oss komme tilbake til det vi har så langt, vi er på trinn n i algoritmen vår, og vi har beregnet følgende verdier:

Racc - strømavlesninger fra vårt akselerometer Rgyro - hentet fra hvilen (n -1) og nåværende gyroskopavlesninger

Hvilke verdier bruker vi for å beregne det oppdaterte estimatet Rest (n)? Du gjettet sikkert at vi kommer til å bruke begge. Vi bruker et veid gjennomsnitt, slik at:

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

Vi kan forenkle denne formelen ved å dele både teller og nevner av brøken med w1.

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

og etter å ha byttet ut w2/w1 = wGyro får vi:

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

I forumula ovenfor forteller wGyro oss hvor mye vi stoler på gyroen vår sammenlignet med akselerometeret vårt. Denne verdien kan velges eksperimentelt. Vanligvis vil verdier mellom 5..20 utløse gode resultater.

Hovedforskjellen mellom denne algoritmen og Kalman -filteret er at denne vekten er relativt fast, mens vektene i Kalman -filteret oppdateres permanent basert på den målte støyen fra akselerometeravlesningene. Kalman -filteret er fokusert på å gi deg "de beste" teoretiske resultatene, mens denne algoritmen kan gi deg resultater "gode nok" for din praktiske anvendelse. Du kan implementere en algoritme som justerer wGyro avhengig av noen støyfaktorer du måler, men faste verdier vil fungere godt for de fleste applikasjoner.

Vi er et skritt unna å få våre oppdaterte estimerte verdier:

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

La oss nå normalisere denne vektoren igjen:

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

Og vi er klare til å gjenta løkken vår igjen.

Denne guiden dukket opprinnelig opp på starlino.com. Jeg har gjort noen lette redigeringer og lagt den ut på nytt med tillatelse. Takk Starlino!