Accelerometer og Gyro -vejledning: 3 trin

2024 Forfatter: John Day | [email protected]. Sidst ændret: 2024-01-30 08:31

Introduktion

Denne vejledning er beregnet til alle, der er interesseret i at bruge acceleratorer og gyroskoper samt kombinerede IMU -enheder (inertial måleenhed) i deres elektronikprojekter

Vi dækker:

• Hvad måler et accelerometer?
• Hvad måler et gyroskop (aka gyro)?
• Sådan konverteres analog-til-digital (ADC) aflæsninger, som du får fra disse sensorer til fysiske enheder (de ville være g for accelerometer, deg/s for gyroskop)
• Sådan kombineres accelerometer- og gyroskopmålinger for at opnå præcise oplysninger om din enheds hældning i forhold til jordplanet

Gennem hele artiklen vil jeg prøve at holde matematikken på et minimum. Hvis du ved, hvad sinus/kosinus/tangent er, skal du være i stand til at forstå og bruge disse ideer i dit projekt, uanset hvilken platform du bruger: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC osv.

Der er mennesker derude, der mener, at du har brug for kompleks matematik for at kunne bruge en IMU-enhed (komplekse FIR- eller IIR-filtre, såsom Kalman-filtre, Parks-McClellan-filtre osv.). Du kan undersøge dem alle og opnå vidunderlige, men komplekse resultater. Min måde at forklare ting på kræver bare grundlæggende matematik. Jeg tror meget på enkelhed. Jeg tror, at et enkelt system er lettere at styre og overvåge, udover at mange indlejrede enheder ikke har magt og ressourcer til at implementere komplekse algoritmer, der kræver matrixberegninger.

Jeg vil som eksempel bruge en ny IMU -enhed, Acc_Gyro Accelerometer + Gyro IMU. Vi bruger parametre for denne enhed i vores eksempler herunder. Denne enhed er en god enhed til at starte med, fordi den består af 2 enheder:

- LIS331AL (datablad) - et triaksialt 2G accelerometer - LPR550AL (datablad) - en dobbeltakset pitch and roll, 500 deg/sek gyroskop

Sammen repræsenterer de en 5-graders frihedsinertial måleenhed. Nu er det et smukt navn! Ikke desto mindre er der bag det flotte navn en meget nyttig kombinationsenhed, som vi vil dække og forklare detaljeret i denne vejledning.

Trin 1: Accelerometeret

For at forstå denne enhed starter vi med accelerometeret. Når man tænker på accelerometre, er det ofte nyttigt at forestille sig en kasse i form af en terning med en kugle indeni. Du kan forestille dig noget andet som en cookie eller en doughnut, men jeg forestiller mig en bold:

Hvis vi tager denne kasse et sted uden gravitationsfelter eller for den sags skyld uden andre felter, der kan påvirke boldens position - flyder bolden simpelthen midt i feltet. Du kan forestille dig, at boksen er i det ydre rum langt-langt væk fra alle kosmiske kroppe, eller hvis et sådant sted er svært at finde forestille sig i det mindste et rumfartøj, der kredser rundt om planeten, hvor alt er i vægtløs tilstand. På billedet ovenfor kan du se, at vi tildeler hver akse et par vægge (vi fjernede væggen Y+, så vi kan se inde i boksen). Forestil dig, at hver væg er trykfølsom. Hvis vi pludselig flytter boksen til venstre (vi accelererer den med acceleration 1g = 9,8m/s^2), vil bolden ramme væggen X-. Vi måler derefter den trykstyrke, som bolden anvender på væggen, og udsender en værdi på -1g på X -aksen.

Bemærk, at accelerometeret faktisk vil registrere en kraft, der er rettet i den modsatte retning fra accelerationsvektoren. Denne kraft kaldes ofte Inertial Force eller Fictitious Force. En ting, du bør lære af dette, er, at et accelerometer måler acceleration indirekte gennem en kraft, der påføres en af dets vægge (ifølge vores model kan det være en fjeder eller noget andet i virkelige accelerationsmålere). Denne kraft kan skyldes accelerationen, men som vi vil se i det næste eksempel, er den ikke altid forårsaget af acceleration.

Hvis vi tager vores model og sætter den på jorden, vil bolden falde på Z-væggen og anvende en kraft på 1g på bundvæggen, som vist på billedet herunder:

I dette tilfælde bevæger boksen sig ikke, men vi får stadig en aflæsning på -1g på Z -aksen. Det tryk, som bolden har påført væggen, blev forårsaget af en tyngdekraft. I teorien kan det være en anden form for kraft - for eksempel hvis du forestiller dig, at vores kugle er metallisk, kan en magnet ved siden af kassen flytte bolden, så den rammer en anden væg. Dette blev sagt bare for at bevise, at accelerometeret i hovedsagen måler ikke acceleration. Det sker bare, at acceleration forårsager en inertial kraft, der fanges af accelerometerets kraftdetekteringsmekanisme.

Selvom denne model ikke ligefrem er, hvordan en MEMS -sensor er konstrueret, er den ofte nyttig til at løse accelerometerrelaterede problemer. Der er faktisk lignende sensorer, der har metalliske kugler indeni, de kaldes vippekontakter, men de er mere primitive og normalt kan de kun se, om enheden er tilbøjelig inden for et eller andet område eller ej, ikke omfanget af hældning.

Indtil videre har vi analyseret accelerometerudgangen på en enkelt akse, og det er alt, hvad du får med en enkeltakse accelerometre. Den reelle værdi af triaksiale accelerometre stammer fra, at de kan registrere inertialkræfter på alle tre akser. Lad os gå tilbage til vores boksmodel, og lad os rotere boksen 45 grader til højre. Bolden vil røre ved 2 vægge nu: Z- og X- som vist på billedet herunder:

Værdierne på 0,71 er ikke vilkårlige, de er faktisk en tilnærmelse til SQRT (1/2). Dette bliver mere klart, når vi introducerer vores næste model for accelerometeret.

I den tidligere model har vi fikseret tyngdekraften og roteret vores imaginære kasse. I de sidste 2 eksempler har vi analyseret output i 2 forskellige bokspositioner, mens kraftvektoren forblev konstant. Selvom dette var nyttigt til at forstå, hvordan accelerometeret interagerer med ydre kræfter, er det mere praktisk at udføre beregninger, hvis vi fastsætter koordinatsystemet til akselerometerets akser og forestiller os, at kraftvektoren roterer omkring os.

Tag et kig på modellen ovenfor, jeg bevarede aksernes farver, så du kan foretage en mental overgang fra den tidligere model til den nye. Forestil dig, at hver akse i den nye model er vinkelret på de respektive ansigter på boksen i den tidligere model. Vektoren R er den kraftvektor, accelerometeret måler (det kan enten være tyngdekraften eller inertialkraften fra eksemplerne ovenfor eller en kombination af begge). Rx, Ry, Rz er projektion af R -vektoren på X-, Y-, Z -akserne. Bemærk venligst følgende forhold:

R^2 = Rx^2 + Ry^2 + Rz^2 (ligning 1)

hvilket stort set svarer til Pythagoras sætning i 3D.

Husk, at jeg lidt tidligere fortalte dig, at værdierne for SQRT (1/2) ~ 0,71 ikke er tilfældige. Hvis du tilslutter dem til formlen ovenfor, kan vi efter at huske på, at vores tyngdekraft var 1 g, kontrollere, at:

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

simpelthen ved at erstatte R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) i Eq.1

Efter en lang indledning til teori kommer vi tættere på virkelige accelerometre. Værdierne Rx, Ry, Rz er faktisk lineært relateret til de værdier, som dit virkelige accelerometer vil udsende, og som du kan bruge til at udføre forskellige beregninger.

Inden vi når dertil, lad os tale lidt om, hvordan accelerometre vil levere disse oplysninger til os. De fleste accelerometre vil falde i to kategorier: digital og analog. Digitale accelerometre giver dig oplysninger ved hjælp af en seriel protokol som I2C, SPI eller USART, mens analoge accelerometre udsender et spændingsniveau inden for et foruddefineret område, som du skal konvertere til en digital værdi ved hjælp af et ADC (analog til digital converter) modul. Jeg vil ikke gå nærmere ind på, hvordan ADC fungerer, dels fordi det er så omfattende et emne og dels fordi det er forskelligt fra en platform til en anden. Nogle mikrokontroller vil have en indbygget ADC moduler nogle af dem har brug for eksterne komponenter for at udføre ADC konverteringer. Uanset hvilken type ADC -modul du bruger, ender du med en værdi i et bestemt område. For eksempel udsender et 10 -bit ADC -modul en værdi i området 0..1023, bemærk at 1023 = 2^10 -1. Et 12-bit ADC-modul udsender en værdi i området 0..4095, bemærk at 4095 = 2^12-1.

Lad os gå videre ved at overveje et simpelt eksempel, antag at vores 10bit ADC -modul gav os følgende værdier for de tre accelerometerkanaler (akser):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Hvert ADC -modul vil have en referencespænding, lad os antage, at det i vores eksempel er 3,3V. For at konvertere en 10bit adc -værdi til spænding bruger vi følgende formel:

VoltsRx = AdcRx * Vref / 1023

En hurtig bemærkning her: at for 8bit ADC den sidste divider ville være 255 = 2 ^ 8 -1, og for 12bit ADC sidste divider ville være 4095 = 2 ^ 12 -1.

Ved at anvende denne formel på alle 3 kanaler får vi:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (vi afrunder alle resultater til 2 decimaler) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Hvert accelerometer har et nul-g spændingsniveau, du kan finde det i specifikationer, dette er spændingen, der svarer til 0g. For at få en signeret spændingsværdi skal vi beregne skiftet fra dette niveau. Lad os sige, at vores 0g spændingsniveau er VzeroG = 1,65V. Vi beregner spændingsforskydningerne fra nul-g spænding 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 nu vores accelerometeraflæsninger i volt, det er stadig ikke i g (9,8 m/s^2), for at udføre den sidste konvertering anvender vi accelerometerets følsomhed, normalt udtrykt i mV/g. Lad os sige vores følsomhed = 478.5mV/g = 0.4785V/g. Følsomhedsværdier findes i specifikationer for accelerometer. For at få de endelige kraftværdier udtrykt i g bruger vi følgende formel:

Rx = DeltaVoltsRx / Følsomhed

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 naturligvis kombinere alle trin i en formel, men jeg gennemgik alle trinene for at gøre det klart, hvordan du går fra ADC -aflæsninger til en kraftvektorkomponent udtrykt i g.

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

Vi har nu alle 3 komponenter, der definerer vores inertial kraftvektor, hvis enheden ikke er udsat for andre kræfter end gravitation, kan vi antage, at dette er retningen af vores gravitationskraftvektor. Hvis du vil beregne enhedens hældning i forhold til jorden, kan du beregne vinklen mellem denne vektor og Z -aksen. Hvis du også er interesseret i hældningsretningen pr. Akse, kan du opdele dette resultat i 2 komponenter: hældning på X- og Y-aksen, der kan beregnes som vinklen mellem tyngdevektor og X / Y-akser. Beregning af disse vinkler er mere enkel, end du måske tror, nu hvor vi har beregnet værdierne for Rx, Ry og Rz. Lad os gå tilbage til vores sidste accelerometer -model og gøre nogle ekstra notationer:

De vinkler, vi er interesserede i, er vinklerne mellem X, Y, Z akser og kraftvektoren R. Vi definerer disse vinkler som Axr, Ayr, Azr. Du kan bemærke fra den retvinklede trekant dannet af R og Rx, at:

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

Vi kan fra Eq.1 trække, at R = SQRT (Rx^2 + Ry^2 + Rz^2).

Vi kan nu finde vores vinkler ved at bruge funktionen arccos () (funktionen inverse cos ()):

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

Vi er gået langt med at forklare accelerometermodellen, bare for at komme op til disse formler. Afhængigt af dine applikationer vil du måske bruge alle mellemformler, som vi har udledt. Vi vil også snart introducere gyroskopmodellen, og vi vil se, hvordan accelerometer- og gyroskopdata kan kombineres for at give endnu mere præcise hældningsestimater.

Men før vi gør det, lad os gøre nogle flere nyttige notationer:

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

Denne trilling kaldes ofte Direction Cosine, og den repræsenterer dybest set den enhedsvektor (vektor med længde 1), der har samme retning som vores R -vektor. Du kan nemt kontrollere, at:

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

Dette er en dejlig ejendom, da den fritager os fra at overvåge modulet (længden) af R -vektoren. Ofte hvis vi bare er interesseret i retning af vores inertialvektor, er det fornuftigt at normalisere dets modul for at forenkle andre beregninger.

Trin 2: Gyroskop

Vi vil ikke introducere nogen tilsvarende boksemodel til gyroskopet, som vi gjorde for accelerometer, i stedet går vi direkte til den anden accelerometermodel, og vi viser, hvad gyroskopet måler i henhold til denne model.

Hver gyroskopkanal måler rotationen omkring en af akserne. For eksempel vil et 2-akset gyroskop måle rotationen omkring (eller nogle kan sige "om") X- og Y-akserne. For at udtrykke denne rotation i tal, lad os lave nogle notationer. Lad os først definere:

Rxz - er projektionen af inertialkraftvektoren R på XZ -planet Ryz - er projektionen af inertialkraftvektoren R på YZ -planet

Fra den retvinklede trekant dannet af Rxz og Rz får vi ved hjælp af Pythagoras sætning:

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

bemærk også, at:

R^2 = Rxz^2 + Ry^2, dette kan afledes af ligninger 1 og derover, eller det kan afledes af en retvinklet trekant dannet af R og Ryz R^2 = Ryz^2 + Rx^2

Vi kommer ikke til at bruge disse formler i denne artikel, men det er nyttigt at notere forholdet mellem alle værdierne i vores model.

I stedet vil vi definere vinklen mellem Z -aksen og Rxz, Ryz -vektorer som følger:

Axz - er vinklen mellem Rxz (projektion af R på XZ -plan) og Z -akse Ayz - er vinklen mellem Ryz (projektion af R på YZ -plan) og Z -aksen

Nu kommer vi tættere på, hvad gyroskopet måler. Gyroskop måler hastigheden af ændringer af vinklerne defineret ovenfor. Med andre ord vil det udsende en værdi, der er lineært relateret til ændringshastigheden for disse vinkler. For at forklare dette lad os antage, at vi har målt rotationsvinklen omkring akse Y (det ville være Axz -vinkel) på tidspunktet t0, og vi definerer det som Axz0, derefter målte vi denne vinkel på et senere tidspunkt t1, og det var Axz1. Ændringshastigheden beregnes som følger:

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

Hvis vi udtrykker Axz i grader, og tiden i sekunder, vil denne værdi blive udtrykt i deg/s. Dette er, hvad et gyroskop måler.

I praksis vil et gyroskop (medmindre det er et specielt digitalt gyroskop) sjældent give dig en værdi udtrykt i deg/s. Samme som for accelerometer får du en ADC -værdi, som du skal konvertere til deg/s ved hjælp af en formel, der ligner Eq. 2, som vi har defineret for accelerometer. Lad os introducere ADC til deg/s konverteringsformel for gyroskop (vi antager, at vi bruger et 10bit ADC -modul, for 8bit ADC udskift 1023 med 255, for 12bit ADC erstat 1023 med 4095).

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

AdcGyroXZ, AdcGyroYZ - hentes fra vores adc -modul, og de repræsenterer de kanaler, der måler rotation af projektion af R -vektor i henholdsvis XZ i YZ -fly, hvilket svarer til at sige, at rotation blev udført omkring henholdsvis Y- og X -akser.

Vref - er den ADC -referencespænding, vi vil bruge 3.3V i eksemplet nedenfor VzeroRate - er nulhastighedsspændingen, med andre ord spændingen, som gyroskopet udsender, når det ikke udsættes for nogen rotation, for Acc_Gyro -kortet er det for eksempel 1,23V (du kan finde disse værdier i specifikationerne) Følsomhed - er følsomheden af dit gyroskop det udtrykkes i mV / (deg / s) ofte skrevet som mV / deg / s, det fortæller dig dybest set, hvor mange mV vil gyroskopets output stiger, hvis du øger rotationshastigheden med en grad/s. Følsomheden af Acc_Gyro -kortet er f.eks. 2mV/deg/s eller 0.002V/deg/s

Lad os tage et eksempel, antag at vores ADC -modul returnerede følgende værdier:

AdcGyroXZ = 571 AdcGyroXZ = 323

Ved hjælp af ovenstående formel og ved hjælp af specifikationsparametrene for Acc_Gyro -bord 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 enheden omkring Y -aksen (eller vi kan sige, at den roterer i XZ -plan) med en hastighed på 306 deg/s og omkring X -aksen (eller vi kan sige, at den roterer i YZ -plan) med en hastighed på - 94 grader/s. Bemærk, at det negative tegn betyder, at enheden roterer i den modsatte retning fra den konventionelle positive retning. Ifølge konventionen er en rotationsretning positiv. Et godt gyroskop -specifikationsark viser dig, hvilken retning der er positiv, ellers bliver du nødt til at finde den ved at eksperimentere med enheden og notere hvilken rotationsretning, der resulterer i stigende spænding på udgangsstiften. Dette gøres bedst ved hjælp af et oscilloskop, da spændingen falder tilbage til nulhastighedsniveauet, så snart du stopper rotationen. Hvis du bruger et multimeter, skal du opretholde en konstant rotationshastighed i mindst få sekunder og notere spændingen under denne rotation og derefter sammenligne den med nulhastighedsspændingen. Hvis den er større end nulhastighedsspændingen, betyder det, at rotationsretningen er positiv.

Trin 3: Kombination af accelerometer og gyro

Sætter det hele sammen - Kombination af accelerometer- og gyroskopdata

Hvis du læser denne artikel, har du sandsynligvis erhvervet eller planlægger at erhverve en IMU -enhed, eller sandsynligvis planlægger du at bygge en fra separate accelerometer- og gyroskopanordninger.

Det første trin i at bruge en kombinations -IMU -enhed, der kombinerer et accelerometer og et gyroskop, er at justere deres koordinatsystemer. Den nemmeste måde at gøre det på er at vælge accelerometerets koordinatsystem som dit referencekoordinatsystem. De fleste accelerometer -datablade viser retningen for X-, Y-, Z -akser i forhold til billedet af den fysiske chip eller enhed. Her er f.eks. Anvisningerne for X-, Y-, Z -akserne som vist i specifikationerne for Acc_Gyro -kortet:

Næste trin er:

Identificer gyroskopudgange, der svarer til RateAxz, RateAyz -værdier diskuteret ovenfor. Bestem, om disse output skal vendes på grund af gyroskopets fysiske position i forhold til accelerometeret

Antag ikke, at hvis et gyroskop har et output mærket X eller Y, vil det svare til enhver akse i accelerometerets koordinatsystem, selvom denne output er en del af en IMU -enhed. Den bedste måde er at teste det. Forudsat at du har fastlagt gyroskopets position i forhold til accelerometeret. Det antages, at gyro- og accelerometerets grænser er parallelle med hinanden, det vil sige, at du placerer gyroen i et vinkelmultiple på 90 grader i forhold til accelerometerchippen. Hvis du har erhvervet et IMU -kort, er chancerne for, at de allerede er justeret på denne måde. Vi vil ikke i denne artikel diskutere modeller, hvor gyroskop er placeret i en uregelmæssig vinkel i forhold til accelerometer (lad os sige 45 eller 30 grader), selvom dette kan være nyttigt i nogle applikationer.

Her er en prøvesekvens for at bestemme, hvilket output af gyroskop, der svarer til RateAxz -værdien diskuteret ovenfor.

- start med at placere enheden i vandret position. Både X- og Y-udgange af accelerometer ville udsende nul-g-spændingen (for eksempel for Acc_Gyro-kort er dette 1,65V)

- næste gang skal du rotere enheden omkring Y -aksen, en anden måde at sige det på er, at du roterer enheden i XZ -plan, så X- og Z -accelerometerudgange ændres, og Y -output forbliver konstant. - mens enheden roteres med en konstant hastighed, hvilket gyroskopoutput ændres, bør de andre gyroskopudgange forblive konstant - gyroskopoutputtet, der ændrede sig under rotation omkring Y -aksen (rotation i XZ -plan), vil give inputværdien for AdcGyroXZ, hvorfra vi beregner RateAxz - det sidste trin er at sikre, at rotationsretningen svarer til vores model, i nogle tilfælde skal du muligvis invertere RateAxz -værdien på grund af gyroskopets fysiske position i forhold til accelerometeret - udfør ovenstående test igen, roter enheden rundt Y -aksen, overvåger denne gang accelerationen af X -output (AdcRx i vores model). Hvis AdcRx vokser (de første 90 grader af rotation fra vandret position), så bør AdcGyroXZ også vokse. Ellers skal du invertere RateAxz, du kan opnå dette ved at indføre en tegnfaktor i ligning 3 som følger:

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

samme testrør udføres for RateAyz ved at dreje enheden rundt om X -aksen, og du kan identificere, hvilket gyroskopoutput der svarer til RateAyz, og hvis det skal vendes. Når du har værdien for InvertAyz, skal du bruge følgende formel til at beregne RateAyz:

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

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

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

Fra dette tidspunkt vil vi overveje, at du har konfigureret din IMU på en sådan måde, at du kan beregne korrekte værdier for Axr, Ayr, Azr (som defineret del 1. Accelerometer) og RateAxz, RateAyz (som defineret i del 2. Gyroskop). Dernæst analyserer vi forholdet mellem disse værdier, der viser sig at være nyttige til at opnå en mere nøjagtig vurdering af enhedens hældning i forhold til jordplanet.

Du spørger måske dig selv på dette tidspunkt, hvis accelerometermodellen allerede gav os hældningsvinkler for Axr, Ayr, Azr, hvorfor skulle vi så gider med gyroskopdataene? Svaret er enkelt: accelerometerdata kan ikke altid stole 100%på. Der er flere årsager, husk at accelerometer måler inertial kraft, en sådan kraft kan være forårsaget af tyngdekraft (og ideelt set kun af tyngdekraft), men det kan også være forårsaget af acceleration (bevægelse) af enheden. Som et resultat, selvom accelerometeret er i en relativt stabil tilstand, er det stadig meget følsomt over for vibrationer og mekanisk støj generelt. Dette er hovedårsagen til, at de fleste IMU -systemer bruger et gyroskop til at udjævne eventuelle accelerometerfejl. Men hvordan gøres dette? Og er gyroskopet fri for støj?

Gyroskopet er ikke støjfrit, men fordi det måler rotation, er det mindre følsomt for lineære mekaniske bevægelser, den type støj, accelerometer lider af, men gyroskoper har andre typer problemer som f.eks. Drift (kommer ikke tilbage til nul-rate værdi når rotationen stopper). Ikke desto mindre kan vi ved hjælp af gennemsnitsdata fra accelerometer og gyroskop få et relativt bedre estimat af den aktuelle enhedshældning, end vi ville opnå ved at bruge accelerometerdataene alene.

I de næste trin vil jeg introducere en algoritme, der var inspireret af nogle ideer, der blev brugt i Kalman -filteret, men det er langt mere enkelt og lettere at implementere på integrerede enheder. Før det lad os først se, hvad vi vil have vores algoritme til at beregne. Nå, det er tyngdekraftens retning vektor R = [Rx, Ry, Rz], hvorfra vi kan udlede andre værdier som Axr, Ayr, Azr eller cosX, hyggelig, cosZ, der vil give os en idé om hældningen af vores enhed i forhold til jordplanet diskuterer vi forholdet mellem disse værdier i del 1. Man kan sige - har vi ikke allerede disse værdier Rx, Ry, Rz fra ligning 2 i del 1? Nå ja, men husk, at disse værdier kun stammer fra accelerometerdata, så hvis du ville bruge dem direkte i din applikation, kan du få mere støj, end din applikation kan tåle. For at undgå yderligere forvirring lad os omdefinere accelerometermålingerne som følger:

Racc - er inertialkraftvektoren målt ved accelerometer, der består af følgende komponenter (fremspring på X, Y, Z akser):

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

Indtil videre har vi et sæt måleværdier, som vi udelukkende kan opnå fra accelerometerets ADC -værdier. Vi kalder dette datasæt en "vektor", og vi vil bruge følgende notation.

Racc = [RxAcc, RyAcc, RzAcc]

Fordi disse komponenter i Racc kan hentes fra accelerometerdata, kan vi betragte det som et input til vores algoritme.

Bemærk, at fordi Racc måler tyngdekraften, vil du have ret, hvis du antager, at længden af denne vektor defineret som følger er lig med eller tæt på 1g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Men for at være sikker er det fornuftigt at opdatere denne vektor som følger:

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

Dette sikrer, at længden af din normaliserede Racc -vektor altid er 1.

Dernæst introducerer vi en ny vektor, og vi kalder den

Rest = [RxEst, RyEst, RzEst]

Dette vil være output fra vores algoritme, disse er korrigerede værdier baseret på gyroskopdata og baseret på tidligere estimerede data.

Her er hvad vores algoritme vil gøre: - accelerometer fortæller os: "Du er nu i position Racc" - vi siger "Tak, men lad mig kontrollere", - korriger derefter disse oplysninger med gyroskopdata såvel som med tidligere hviledata og vi udsender en ny estimeret vektor Rest. - vi betragter hvile som vores "bedste bud" med hensyn til enhedens nuværende position.

Lad os se, hvordan vi kan få det til at fungere.

Vi starter vores sekvens med at stole på vores accelerometer og tildele:

Rest (0) = Racc (0)

Husk i øvrigt hvile og Racc er vektorer, så ovenstående ligning er bare en simpel måde at skrive 3 sæt ligninger på og undgå gentagelse:

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

Dernæst foretager vi regelmæssige målinger med samme 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 udsender også nye estimater for hver tidsinterval Rest (1), Rest (2), Rest (3) og så videre.

Antag, at vi er på trin n. Vi har to kendte værdisæt, som vi gerne vil bruge:

Rest (n -1) - vores tidligere estimat, med Rest (0) = Racc (0) Racc (n) - vores nuværende accelerometermåling

Inden vi kan beregne Rest (n), lad os introducere en ny måleværdi, som vi kan få fra vores gyroskop og et tidligere estimat.

Vi kalder det Rgyro, og det er også en vektor bestående af 3 komponenter:

Rgyro = [RxGyro, RyGyro, RzGyro]

Vi beregner denne vektor en komponent ad gangen. Vi starter med RxGyro.

Lad os starte med at observere følgende forhold i vores gyroskopmodel, fra den retvinklede trekant dannet af Rz og Rxz kan vi udlede det:

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

Atan2 kan være en funktion, du aldrig har brugt før, den ligner atan, bortset fra at den returnerer værdier inden for (-PI, PI) i modsætning til (-PI/2, PI/2) som returneret af atan, og det tager 2 argumenter i stedet for et. Det giver os mulighed for at konvertere de to værdier for Rx, Rz til vinkler i hele 360 grader (-PI til PI). Du kan læse mere om atan2 her.

Så ved at kende RxEst (n-1) og RzEst (n-1) kan vi finde:

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

Husk, at gyroskop måler ændringshastigheden for Axz -vinklen. Så vi kan estimere den nye vinkel Axz (n) som følger:

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

Husk, at RateAxz kan fås fra vores gyroskop ADC -aflæsninger. En mere præcis formel kan bruge en gennemsnitlig rotationshastighed beregnet som følger:

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

På samme måde kan vi finde:

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

Ok, så nu har vi Axz (n) og Ayz (n). Hvor går vi herfra for at fratrække RxGyro/RyGyro? Fra Eq. 1 kan vi skrive længden af vektor Rgyro som følger:

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

Også fordi vi normaliserede vores Racc -vektor, kan vi antage, at dens længde er 1, og at den ikke har ændret sig efter rotationen, så det er relativt sikkert at skrive:

| Rgyro | = 1

Lad os vedtage en midlertidig kortere notation til beregningerne herunder:

x = RxGyro, y = RyGyro, z = RzGyro

Ved hjælp af relationerne ovenfor kan vi skrive:

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

Lad os dele tæller og nævner af 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))

Bemærk, at x / SQRT (x^2 + z^2) = sin (Axz), så:

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

Multiplicer nu tæller og nævner for brøk inde i SQRT med z^2

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

Bemærk, at z / SQRT (x^2 + z^2) = cos (Axz) og y / z = tan (Ayz), så endelig:

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

Går vi tilbage til vores notation får vi:

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

på samme måde finder vi det

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

Nu kan vi endelig finde:

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åde at estimere dette på er at tage:

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

I praksis skal du være forsigtig, når RzEst (n-1) er tæt på 0. Du kan helt springe over gyrofasen i dette tilfælde og tildele: Rgyro = Rest (n-1). Rz bruges som reference til beregning af Axz og Ayz vinkler, og når det er tæt på 0, kan værdier flyde over og udløse dårlige resultater. Du vil være i domæne med store flydende tal, hvor tan () / atan () funktionsimplementeringer kan mangle præcision.

Så lad os opsummere, hvad vi har indtil nu, vi er på trin n i vores algoritme, og vi har beregnet følgende værdier:

Racc - aktuelle aflæsninger fra vores accelerometer Rgyro - hentet fra Rest (n -1) og aktuelle gyroskopmålinger

Hvilke værdier bruger vi til at beregne det opdaterede estimat Rest (n)? Du har sikkert gættet på, at vi vil bruge begge dele. Vi bruger et vægtet gennemsnit, så:

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

Vi kan forenkle denne formel ved at dividere både tæller og nævner af brøken med w1.

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

og efter substitution af w2/w1 = wGyro får vi:

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

I ovenstående forumula fortæller wGyro os, hvor meget vi har tillid til vores gyro sammenlignet med vores accelerometer. Denne værdi kan vælges eksperimentelt normalt vil værdier mellem 5..20 udløse gode resultater.

Hovedforskellen mellem denne algoritme og Kalman -filteret er, at denne vægt er relativt fast, hvorimod vægten i Kalman -filter opdateres permanent baseret på den målte støj fra accelerometermålingerne. Kalman filter er fokuseret på at give dig "de bedste" teoretiske resultater, hvorimod denne algoritme kan give dig resultater "gode nok" til din praktiske anvendelse. Du kan implementere en algoritme, der justerer wGyro afhængigt af nogle støjfaktorer, du måler, men faste værdier fungerer godt for de fleste applikationer.

Vi er et skridt væk fra at få vores opdaterede estimerede værdier:

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

Lad os nu normalisere denne vektor igen:

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 klar til at gentage vores loop igen.

Denne vejledning dukkede oprindeligt op på starlino.com, jeg har foretaget et par lette redigeringer og genudsendt den med tilladelse. Tak Starlino!