Accelerometer och gyrohandledning: 3 steg

2024 Författare: John Day | [email protected]. Senast ändrad: 2024-01-30 12:48

Introduktion

Den här guiden är avsedd för alla som är intresserade av att använda accelerometrar och gyroskop samt kombinations IMU -enheter (tröghetsmätningsenhet) i sina elektronikprojekt

Vi kommer att täcka:

• Vad mäter en accelerometer?
• Vad mäter ett gyroskop (aka gyro)?
• Hur man konverterar analoga till digitala (ADC) avläsningar som man får från dessa sensorer till fysiska enheter (de skulle vara g för accelerometer, deg/s för gyroskop)
• Hur man kombinerar accelerometer- och gyroskopavläsningar för att få exakt information om enhetens lutning i förhållande till markplanet

Under hela artikeln kommer jag att försöka hålla matten till ett minimum. Om du vet vad Sine/Cosine/Tangent är bör du kunna förstå och använda dessa idéer i ditt projekt oavsett vilken plattform du använder: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC, etc.

Det finns människor där ute som tror att du behöver komplex matematik för att kunna använda en IMU-enhet (komplexa FIR- eller IIR-filter som Kalman-filter, Parks-McClellan-filter, etc). Du kan undersöka alla dessa och uppnå underbara men komplexa resultat. Mitt sätt att förklara saker kräver bara grundläggande matematik. Jag tror mycket på enkelhet. Jag tror att ett system som är enkelt är lättare att styra och övervaka, förutom att många inbäddade enheter inte har kraft och resurser för att implementera komplexa algoritmer som kräver matrisberäkningar.

Jag ska som exempel använda en ny IMU -enhet, Acc_Gyro Accelerometer + Gyro IMU. Vi använder parametrar för denna enhet i våra exempel nedan. Denna enhet är en bra enhet att börja med eftersom den består av 2 enheter:

- LIS331AL (datablad) - en triaxial 2G -accelerometer - LPR550AL (datablad) - en dubbelaxlig pitch and roll, 500 grader/sek gyroskop

Tillsammans representerar de en 5-graders frihetströghetsmätningsenhet. Nu är det ett fint namn! Men bakom det fina namnet finns en mycket användbar kombinationsenhet som vi kommer att täcka och förklara i detalj i den här guiden.

Steg 1: Accelerometer

För att förstå den här enheten börjar vi med accelerometern. När man tänker på accelerometrar är det ofta användbart att avbilda en låda i form av en kub med en boll inuti den. Du kan tänka dig något annat som en kaka eller en munk, men jag ska tänka mig en boll:

Om vi tar den här lådan på en plats utan gravitationsfält eller för den delen utan andra fält som kan påverka bollens position - kommer bollen helt enkelt att flyta i mitten av lådan. Du kan föreställa dig att rutan är i yttre rymden långt-långt bort från alla kosmiska kroppar, eller om en sådan plats är svår att hitta föreställ dig åtminstone ett rymdbåt som kretsar runt planeten där allt är i viktlöst tillstånd. På bilden ovan kan du se att vi tilldelar varje axel ett par väggar (vi tog bort väggen Y+ så att vi kan titta inuti rutan). Tänk att varje vägg är tryckkänslig. Om vi plötsligt flyttar boxen till vänster (vi accelererar den med acceleration 1g = 9,8m/s^2), kommer bollen att träffa väggen X-. Vi mäter sedan tryckkraften som kulan applicerar på väggen och matar ut ett värde på -1g på X -axeln.

Observera att accelerometern faktiskt kommer att detektera en kraft som är riktad i motsatt riktning från accelerationsvektorn. Denna kraft kallas ofta tröghetskraft eller fiktiv kraft. En sak du bör lära dig av detta är att en accelerometer mäter acceleration indirekt genom en kraft som appliceras på en av dess väggar (enligt vår modell kan det vara en fjäder eller något annat i verkliga accelerometrar). Denna kraft kan orsakas av accelerationen, men som vi kommer att se i nästa exempel orsakas den inte alltid av acceleration.

Om vi tar vår modell och lägger den på jorden faller bollen på Z-väggen och applicerar en kraft på 1g på bottenväggen, som visas på bilden nedan:

I det här fallet rör sig inte rutan men vi får fortfarande en avläsning på -1g på Z -axeln. Trycket som kulan har applicerat på väggen orsakades av en gravitationskraft. I teorin kan det vara en annan typ av kraft - om du till exempel föreställer dig att vår boll är metallisk, kan en magnet bredvid lådan flytta bollen så att den träffar en annan vägg. Detta sades bara för att bevisa att i huvudsak accelerometer mäter kraft inte acceleration. Det händer bara att acceleration orsakar en tröghetskraft som fångas upp av accelerometerns kraftdetekteringsmekanism.

Även om denna modell inte exakt är hur en MEMS -sensor är konstruerad är den ofta användbar för att lösa accelerometerrelaterade problem. Det finns faktiskt liknande sensorer som har metallkulor inuti, de kallas lutningsomkopplare, men de är mer primitiva och vanligtvis kan de bara se om enheten lutar inom något område eller inte, inte omfattningen av lutning.

Hittills har vi analyserat accelerometerutmatningen på en enda axel och det här är allt du får med en enda axelaccelerometrar. Det verkliga värdet av triaxiella accelerometrar kommer från det faktum att de kan detektera tröghetskrafter på alla tre axlarna. Låt oss gå tillbaka till vår lådmodell, och låt oss rotera lådan 45 grader åt höger. Bollen kommer att röra 2 väggar nu: Z- och X- som visas på bilden nedan:

Värdena på 0,71 är inte godtyckliga, de är faktiskt en approximation för SQRT (1/2). Detta kommer att bli tydligare när vi presenterar vår nästa modell för accelerometern.

I den tidigare modellen har vi fixerat gravitationskraften och roterat vår imaginära låda. I de två senaste exemplen har vi analyserat utsignalen i 2 olika lådpositioner, medan kraftvektorn förblev konstant. Även om detta var användbart för att förstå hur accelerometern interagerar med yttre krafter, är det mer praktiskt att utföra beräkningar om vi fixar koordinatsystemet till accelerometerns axlar och föreställer oss att kraftvektorn roterar runt oss.

Ta en titt på modellen ovan, jag bevarade axlarnas färger så att du kan göra en mental övergång från den tidigare modellen till den nya. Tänk dig att varje axel i den nya modellen är vinkelrät mot respektive ytor på lådan i den tidigare modellen. Vektorn R är kraftvektorn som accelerometern mäter (det kan antingen vara gravitationskraften eller tröghetskraften från exemplen ovan eller en kombination av båda). Rx, Ry, Rz är projektion av R -vektorn på X-, Y-, Z -axlarna. Observera följande relation:

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

vilket i princip motsvarar Pythagoras sats i 3D.

Kom ihåg att lite tidigare sa jag att värdena för SQRT (1/2) ~ 0,71 inte är slumpmässiga. Om du ansluter dem till formeln ovan kan vi efter att ha erinrat oss om att vår gravitationskraft var 1 g verifiera att:

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

helt enkelt genom att ersätta R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) i ekv.1

Efter en lång inledning av teori kommer vi närmare verkliga accelerometrar. Värdena Rx, Ry, Rz är faktiskt linjärt relaterade till de värden som din verkliga accelerometer kommer att mata ut och som du kan använda för att utföra olika beräkningar.

Innan vi kommer dit ska vi prata lite om hur accelerometrar kommer att leverera denna information till oss. De flesta accelerometrar kommer att delas in i två kategorier: digital och analog. Digitala accelerometrar ger dig information med hjälp av ett seriellt protokoll som I2C, SPI eller USART, medan analoga accelerometrar kommer att mata ut en spänningsnivå inom ett fördefinierat intervall som du måste konvertera till ett digitalt värde med en ADC -modul (analog till digital omvandlare). Jag kommer inte att gå in på detaljer om hur ADC fungerar, dels för att det är ett så omfattande ämne och dels för att det är olika från en plattform till en annan. Vissa mikrokontroller kommer att ha en inbyggd ADC-modul, några av dem kommer att behöva externa komponenter för att utföra ADC-konverteringarna. Oavsett vilken typ av ADC -modul du använder kommer du att få ett värde i ett visst intervall. Till exempel kommer en 10 -bitars ADC -modul att mata ut ett värde i intervallet 0..1023, observera att 1023 = 2^10 -1. En 12-bitars ADC-modul kommer att mata ut ett värde i intervallet 0..4095, observera att 4095 = 2^12-1.

Låt oss gå vidare genom att överväga ett enkelt exempel, anta att vår 10bit ADC -modul gav oss följande värden för de tre accelerometerkanalerna (axlarna):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Varje ADC -modul kommer att ha en referensspänning, låt oss anta att det i vårt exempel är 3,3V. För att konvertera ett 10bit adc -värde till spänning använder vi följande formel:

VoltsRx = AdcRx * Vref / 1023

En snabb notering här: att för 8 -bitars ADC skulle den sista avdelaren vara 255 = 2 ^ 8 -1, och för 12 -bitars ADC skulle den sista avdelaren vara 4095 = 2 ^ 12 -1.

Genom att tillämpa denna formel på alla tre kanaler får vi:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (vi avrundar alla resultat till 2 decimaler) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Varje accelerometer har en noll-g spänningsnivå, du kan hitta den i specifikationer, det här är spänningen som motsvarar 0g. För att få ett signerat spänningsvärde måste vi beräkna skiftet från denna nivå. Låt oss säga att vår 0g spänningsnivå är VzeroG = 1,65V. Vi beräknar spänningsförskjutningarna från noll-g-spänning enligt följande::

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

Vi har nu våra accelerometeravläsningar i volt, det är fortfarande inte i g (9,8 m/s^2), för att göra den slutliga konverteringen använder vi accelerometerkänsligheten, vanligtvis uttryckt i mV/g. Låt oss säga att vår känslighet = 478.5mV/g = 0.4785V/g. Känslighetsvärden finns i accelerometerns specifikationer. För att få de slutliga kraftvärdena uttryckta i g använder vi följande formel:

Rx = DeltaVoltsRx / känslighet

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 kunde naturligtvis kombinera alla steg i en formel, men jag gick igenom alla steg för att göra det klart hur du går från ADC -avläsningar till en kraftvektorkomponent uttryckt i g.

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

Vi har nu alla tre komponenter som definierar vår tröghetskraftsvektor, om enheten inte utsätts för andra krafter än gravitation kan vi anta att detta är riktningen för vår gravitationskraftvektor. Om du vill beräkna enhetens lutning i förhållande till marken kan du beräkna vinkeln mellan denna vektor och Z -axeln. Om du också är intresserad av lutningsriktningen per axel kan du dela upp detta resultat i 2 komponenter: lutning på X- och Y-axeln som kan beräknas som vinkeln mellan gravitationvektorn och X / Y-axlarna. Att beräkna dessa vinklar är enklare än du kanske tror, nu när vi har beräknat värdena för Rx, Ry och Rz. Låt oss gå tillbaka till vår senaste accelerometermodell och göra några ytterligare noteringar:

Vinklarna som vi är intresserade av är vinklarna mellan X, Y, Z axlar och kraftvektorn R. Vi definierar dessa vinklar som Axr, Ayr, Azr. Du kan märka från den rätvinkliga triangeln som bildas av R och Rx att:

cos (Axr) = Rx / R och liknande: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Vi kan dra från ekv.1 att R = SQRT (Rx^2 + Ry^2 + Rz^2).

Vi kan nu hitta våra vinklar med hjälp av funktionen arccos () (funktionen invers cos ()):

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

Vi har gått långt för att förklara accelerometermodellen, bara för att komma fram till dessa formler. Beroende på dina applikationer kanske du vill använda alla mellanformler som vi har härlett. Vi kommer också att introducera gyroskopmodellen snart, och vi får se hur accelerometer- och gyroskopdata kan kombineras för att ge ännu mer exakta lutningsuppskattningar.

Men innan vi gör det, låt oss göra några mer användbara noteringar:

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

Denna triplett kallas ofta Direction Cosine, och den representerar i princip enhetsvektorn (vektor med längd 1) som har samma riktning som vår R -vektor. Du kan enkelt verifiera att:

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

Detta är en trevlig egenskap eftersom den fritar oss från att övervaka modulen (längden) för R -vektorn. Ofta om vi bara är intresserade av riktningen för vår tröghetsvektor är det vettigt att normalisera modulen för att förenkla andra beräkningar.

Steg 2: Gyroskop

Vi kommer inte att introducera någon likvärdig boxmodell för gyroskopet som vi gjorde för accelerometer, istället hoppar vi direkt till den andra accelerometermodellen och vi visar vad mäter gyroskopet enligt denna modell.

Varje gyroskopkanal mäter rotationen runt en av axlarna. Till exempel kommer ett 2-axligt gyroskop att mäta rotationen runt (eller några kan säga "ungefär") X- och Y-axlarna. För att uttrycka denna rotation i siffror, låt oss göra några notationer. Låt oss först definiera:

Rxz - är projektionen av tröghetskraftsvektorn R på XZ -planet Ryz - är projektionen av tröghetskraftsvektorn R på YZ -planet

Från den rätvinkliga triangeln som bildas av Rxz och Rz får vi med hjälp av Pythagoras sats:

Rxz^2 = Rx^2 + Rz^2 och liknande: Ryz^2 = Ry^2 + Rz^2

notera också att:

R^2 = Rxz^2 + Ry^2, detta kan härledas från ekvationerna 1 och ovan, eller det kan härledas från en rätvinklig triangel bildad av R och Ryz R^2 = Ryz^2 + Rx^2

Vi kommer inte att använda dessa formler i den här artikeln, men det är användbart att notera sambandet mellan alla värden i vår modell.

Istället ska vi definiera vinkeln mellan Z -axeln och Rxz, Ryz -vektorer enligt följande:

Axz - är vinkeln mellan Rxz (projektion av R på XZ -planet) och Z -axeln Ayz - är vinkeln mellan Ryz (projektion av R på YZ -planet) och Z -axeln

Nu kommer vi närmare vad gyroskopet mäter. Gyroskop mäter hastigheten för förändringar av vinklarna definierade ovan. Med andra ord kommer det att mata ut ett värde som är linjärt relaterat till förändringstakten för dessa vinklar. För att förklara detta antar vi att vi har mätt rotationsvinkeln runt axeln Y (det skulle vara Axz -vinkel) vid tiden t0, och vi definierar det som Axz0, sedan mätte vi denna vinkel vid en senare tidpunkt t1 och det var Axz1. Ändringstakten beräknas enligt följande:

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

Om vi uttrycker Axz i grader och tiden i sekunder, kommer detta värde att uttryckas i deg/s. Detta är vad ett gyroskop mäter.

I praktiken ger ett gyroskop (om det inte är ett speciellt digitalt gyroskop) sällan ett värde uttryckt i deg/s. Samma som för accelerometer får du ett ADC -värde som du måste konvertera till deg/s med en formel som liknar ekv. 2 som vi har definierat för accelerometer. Låt oss introducera ADC till deg/s konverteringsformel för gyroskop (vi antar att vi använder en 10bit ADC -modul, för 8bit ADC ersätt 1023 med 255, för 12bit ADC ersätt 1023 med 4095).

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

AdcGyroXZ, AdcGyroYZ - erhålls från vår adc -modul och de representerar kanalerna som mäter projektionsrotationen av R -vektorn i XZ respektive i YZ -plan, vilket motsvarar att rotationen gjordes runt Y respektive X -axlar.

Vref - är den ADC -referensspänning som vi kommer att använda 3.3V i exemplet nedan VzeroRate - är nollhastighetsspänningen, med andra ord spänningen som gyroskopet matar ut när det inte utsätts för någon rotation, för Acc_Gyro -kortet är det till exempel 1,23V (du kan hitta dessa värden i specifikationerna) Känslighet - är känsligheten för ditt gyroskop det uttrycks i mV / (deg / s) ofta skrivet som mV / deg / s, det berättar i princip hur många mV kommer gyroskopets utmatning ökar om du ökar rotationshastigheten med en grad/s. Känsligheten hos Acc_Gyro -kortet är till exempel 2mV/deg/s eller 0.002V/deg/s

Låt oss ta ett exempel, anta att vår ADC -modul returnerade följande värden:

AdcGyroXZ = 571 AdcGyroXZ = 323

Med hjälp av ovanstående formel och med specifikationerna för 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 andra ord roterar enheten runt Y -axeln (eller vi kan säga att den roterar i XZ -plan) med en hastighet på 306 grader/s och runt X -axeln (eller vi kan säga att den roterar i YZ -planet) med en hastighet av - 94 grader/s. Observera att det negativa tecknet betyder att enheten roterar i motsatt riktning från den konventionella positiva riktningen. Enligt konvention är en rotationsriktning positiv. Ett bra gyroskopspecifikationsblad visar vilken riktning som är positiv, annars måste du hitta den genom att experimentera med enheten och notera vilken rotationsriktning som resulterar i ökad spänning på utgångsstiften. Detta görs bäst med ett oscilloskop eftersom så snart du stoppar rotationen kommer spänningen att sjunka tillbaka till nollhastighetsnivån. Om du använder en multimeter måste du behålla en konstant rotationshastighet i minst några sekunder och notera spänningen under denna rotation, jämför sedan den med nollhastighetsspänningen. Om den är större än nollhastighetsspänningen betyder det att rotationsriktningen är positiv.

Steg 3: Kombinera accelerometer och gyro

Att sätta ihop allt - Kombinera accelerometer- och gyroskopdata

Om du läser den här artikeln har du förmodligen förvärvat eller planerar att skaffa en IMU -enhet, eller förmodligen planerar du att bygga en från separata accelerometer- och gyroskopanordningar.

Det första steget i att använda en kombinations IMU -enhet som kombinerar en accelerometer och ett gyroskop är att anpassa sina koordinatsystem. Det enklaste sättet att göra det är att välja accelerometerns koordinatsystem som ditt referenskoordinatsystem. De flesta accelerometerdatablad visar X, Y, Z -axlarnas riktning i förhållande till bilden av det fysiska chipet eller enheten. Till exempel här är riktningarna för X-, Y-, Z -axlar som visas i specifikationerna för Acc_Gyro -kortet:

Nästa steg är:

Identifiera gyroskoputgångarna som motsvarar RateAxz, RateAyz -värden som diskuterats ovan. Bestäm om dessa utgångar behöver inverteras på grund av gyroskopets fysiska position i förhållande till accelerometern

Antag inte att om ett gyroskop har en utgång märkt X eller Y, kommer det att motsvara vilken axel som helst i accelerometerns koordinatsystem, även om denna utgång är en del av en IMU -enhet. Det bästa sättet är att testa det. Förutsatt att du har fixerat gyroskopets position i förhållande till accelerometern. Det antas att gyro- och accelerometergränserna är parallella med varandra, det vill säga att du placerar gyro i en vinkelmultipel på 90 grader i förhållande till accelerometerchipet. Om du fick en IMU -kort är chansen att de redan är anpassade på detta sätt. Vi kommer inte att diskutera i denna artikel modeller där gyroskop placeras i en oregelbunden vinkel i förhållande till accelerometern (låt oss säga 45 eller 30 grader), även om det kan vara användbart i vissa applikationer.

Här är en provsekvens för att avgöra vilken utmatning av gyroskop som motsvarar RateAxz -värdet som diskuterats ovan.

- börja med att placera enheten i horisontellt läge. Både X- och Y-utgångar från accelerometern skulle mata ut noll-g-spänningen (till exempel för Acc_Gyro-kortet är detta 1,65V)

- nästa gång du roterar enheten runt Y -axeln, ett annat sätt att säga det är att du roterar enheten i XZ -planet, så att X- och Z -accelerometerutgångarna ändras och Y -utgången förblir konstant. - medan enheten roteras med konstant hastighet och noterar vilken gyroskoputgång som ändras, bör de andra gyroskoputgångarna förbli konstanta - gyroskoputgången som förändrades under rotationen runt Y -axeln (rotation i XZ -planet) kommer att tillhandahålla ingångsvärdet för AdcGyroXZ, från vilket vi beräknar RateAxz - det sista steget är att se till att rotationsriktningen motsvarar vår modell, i vissa fall kan du behöva invertera RateAxz -värdet på grund av gyroskopets fysiska position i förhållande till accelerometern - utför igen ovanstående test, rotera enheten runt Y -axeln, övervakar den här gången accelerationsmätarens X -utgång (AdcRx i vår modell). Om AdcRx växer (de första 90 graderna av rotation från horisontellt läge) bör AdcGyroXZ också växa. Annars måste du invertera RateAxz, du kan uppnå detta genom att införa en teckenfaktor i ekv.3, enligt följande:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Känslighet, där InvertAxz är 1 eller -1

samma teströr görs för RateAyz, genom att rotera enheten runt X -axeln, och du kan identifiera vilken gyroskoputgång som motsvarar RateAyz, och om den behöver inverteras. När du har värdet för InvertAyz bör du använda följande formel för att beräkna RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Känslighet

Om du skulle göra dessa tester på Acc_Gyro -kortet skulle du få följande resultat:

- utgångsstiften för RateAxz är GX4 och InvertAxz = -1. - utgångsstiften för RateAyz är GY4 och InvertAyz = -1

Från och med nu anser vi att du har konfigurerat din IMU på ett sådant sätt att du kan beräkna korrekta värden för Axr, Ayr, Azr (enligt definitionen i del 1. Accelerometer) och RateAxz, RateAyz (enligt definitionen i del 2. Gyroskop). Därefter analyserar vi relationerna mellan dessa värden som visar sig vara användbara för att få en mer exakt uppskattning av enhetens lutning i förhållande till markplanet.

Du kanske frågar dig själv vid det här laget, om accelerometermodellen redan gav oss lutningsvinklarna för Axr, Ayr, Azr, varför skulle vi vilja störa med gyroskopdata? Svaret är enkelt: accelerometerdata går inte alltid att lita på till 100%. Det finns flera anledningar, kom ihåg att accelerometern mäter tröghetskraft, en sådan kraft kan orsakas av gravitation (och helst bara av gravitation), men den kan också orsakas av acceleration (rörelse) hos enheten. Som ett resultat, även om accelerometern är i ett relativt stabilt tillstånd, är den fortfarande mycket känslig för vibrationer och mekaniskt brus i allmänhet. Detta är huvudorsaken till att de flesta IMU -system använder ett gyroskop för att utjämna eventuella accelerometerfel. Men hur görs detta? Och är gyroskopet fritt från buller?

Gyroskopet är inte fritt från buller, men eftersom det mäter rotation är det mindre känsligt för linjära mekaniska rörelser, den typ av buller som accelerometern lider av, men gyroskop har andra typer av problem som till exempel drift (kommer inte tillbaka till nollhastighetsvärde när rotationen stannar). Ändå kan vi genom medelvärdesdata som kommer från accelerometer och gyroskop få en relativt bättre uppskattning av den aktuella enhetens lutning än vi skulle få genom att använda accelerometerdata ensam.

I nästa steg kommer jag att introducera en algoritm som inspirerats av några idéer som används i Kalman -filtret, men det är mycket enklare och enklare att implementera på inbäddade enheter. Innan dess låt oss först se vad vi vill att vår algoritm ska beräkna. Jo, det är tyngdkraftsvektorriktningen R = [Rx, Ry, Rz] från vilken vi kan härleda andra värden som Axr, Ayr, Azr eller cosX, cosy, cosZ som ger oss en uppfattning om lutningen för vår enhet i förhållande till markplanet, diskuterar vi förhållandet mellan dessa värden i del 1. Man kan säga - har vi inte redan dessa värden Rx, Ry, Rz från ekv. 2 i del 1? Jo, men kom ihåg att dessa värden härrör endast från accelerometerdata, så om du skulle använda dem direkt i din applikation kan du få mer brus än din applikation tål. För att undvika ytterligare förvirring låt oss omdefiniera accelerometermätningarna enligt följande:

Racc - är tröghetskraftsvektorn mätt med accelerometer, som består av följande komponenter (utskott på X, Y, Z axlar):

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

Hittills har vi en uppsättning mätvärden som vi endast kan erhålla från accelerometerns ADC -värden. Vi kallar denna uppsättning data en "vektor" och vi använder följande notation.

Racc = [RxAcc, RyAcc, RzAcc]

Eftersom dessa komponenter i Racc kan erhållas från accelerometerdata kan vi betrakta det som en ingång till vår algoritm.

Observera att eftersom Racc mäter gravitationskraften har du rätt om du antar att längden på denna vektor definierad enligt följande är lika med eller nära 1g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Men för att vara säker är det vettigt att uppdatera denna vektor enligt följande:

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

Detta säkerställer att längden på din normaliserade Racc -vektor alltid är 1.

Därefter introducerar vi en ny vektor och vi kallar den

Rest = [RxEst, RyEst, RzEst]

Detta kommer att vara resultatet av vår algoritm, dessa är korrigerade värden baserade på gyroskopdata och baserade på tidigare uppskattade data.

Här är vad vår algoritm kommer att göra: - accelerometer berättar för oss: "Du är nu i position Racc" - vi säger "Tack, men låt mig kontrollera", - korrigera sedan denna information med gyroskopdata såväl som med tidigare vilodata och vi matar ut en ny uppskattad vektor Rest. - vi anser att vila är vår "bästa insats" när det gäller enhetens nuvarande position.

Låt oss se hur vi kan få det att fungera.

Vi börjar vår sekvens med att lita på vår accelerometer och tilldela:

Vila (0) = Racc (0)

Förresten, kom ihåg vila och Racc är vektorer, så ovanstående ekvation är bara ett enkelt sätt att skriva 3 uppsättningar ekvationer och undvika upprepning:

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

Därefter gör vi regelbundna mätningar med lika långa tidsintervall på T sekunder, och vi får nya mätningar som vi definierar som Racc (1), Racc (2), Racc (3) och så vidare. Vi kommer också att utfärda nya uppskattningar vid varje tidsintervall Rest (1), Rest (2), Rest (3) och så vidare.

Antag att vi är i steg n. Vi har två kända värden som vi vill använda:

Rest (n -1) - vår tidigare uppskattning, med Rest (0) = Racc (0) Racc (n) - vår aktuella accelerometermätning

Innan vi kan beräkna Rest (n), låt oss introducera ett nytt uppmätt värde, som vi kan få från vårt gyroskop och en tidigare uppskattning.

Vi kallar det Rgyro, och det är också en vektor som består av 3 komponenter:

Rgyro = [RxGyro, RyGyro, RzGyro]

Vi kommer att beräkna denna vektor en komponent i taget. Vi börjar med RxGyro.

Låt oss börja med att observera följande relation i vår gyroskopmodell, från den rätvinkliga triangeln som bildas av Rz och Rxz kan vi härleda det:

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

Atan2 kan vara en funktion du aldrig använt tidigare, den liknar atan, förutom att den returnerar värden inom (-PI, PI) i motsats till (-PI/2, PI/2) som returneras av atan, och det tar 2 argument istället för ett. Det gör att vi kan konvertera de två värdena för Rx, Rz till vinklar i hela intervallet 360 grader (-PI till PI). Du kan läsa mer om atan2 här.

Så att känna till RxEst (n-1) och RzEst (n-1) kan vi hitta:

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

Kom ihåg att gyroskop mäter förändringstakten för Axz -vinkeln. Så vi kan uppskatta den nya vinkeln Axz (n) enligt följande:

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

Kom ihåg att RateAxz kan erhållas från våra gyroskop ADC -mätningar. En mer exakt formel kan använda en genomsnittlig rotationshastighet beräknad enligt följande:

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

På samma sätt kan vi hitta:

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

Ok så nu har vi Axz (n) och Ayz (n). Var går vi härifrån för att dra av RxGyro/RyGyro? Från ekv. 1 kan vi skriva längden på vektorn Rgyro enligt följande:

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

Eftersom vi normaliserade vår Racc -vektor kan vi anta att dess längd är 1 och att den inte har ändrats efter rotationen, så det är relativt säkert att skriva:

| Rgyro | = 1

Låt oss anta en tillfällig kortare notation för beräkningarna nedan:

x = RxGyro, y = RyGyro, z = RzGyro

Med hjälp av relationerna ovan kan vi skriva:

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

Låt oss dela täljare och nämnare 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))

Observera att x / SQRT (x^2 + z^2) = sin (Axz), så:

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

Nu multiplicera täljare och nämnare för bråk inuti SQRT med z^2

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

Observera att z / SQRT (x^2 + z^2) = cos (Axz) och y / z = tan (Ayz), så slutligen:

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

När vi går tillbaka till vår notering får vi:

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

på samma sätt som vi hittar det

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

Nu kan vi äntligen hitta:

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

Var tecken (RzGyro) = 1 när RzGyro> = 0, och tecken (RzGyro) = -1 när RzGyro <0.

Ett enkelt sätt att uppskatta detta är att ta:

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

Var i praktiken försiktig när RzEst (n-1) är nära 0. Du kan hoppa över gyrofasen helt och hållet i det här fallet och tilldela: Rgyro = Rest (n-1). Rz används som referens för att beräkna Axz- och Ayz -vinklar och när det är nära 0 kan värden flöda och utlösa dåliga resultat. Du kommer att vara i domänen för stora flytpunktsnummer där tan () / atan () funktionsimplementeringar kan sakna precision.

Så låt oss sammanfatta vad vi har hittills, vi är i steg n i vår algoritm och vi har beräknat följande värden:

Racc - strömavläsningar från vår accelerometer Rgyro - erhållna från vila (n -1) och aktuella gyroskopavläsningar

Vilka värden använder vi för att beräkna den uppdaterade uppskattningen Rest (n)? Du gissade säkert att vi kommer att använda båda. Vi använder ett vägat genomsnitt, så att:

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

Vi kan förenkla denna formel genom att dividera både täljare och nämnare för fraktionen med w1.

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

och efter att vi har ersatt w2/w1 = wGyro får vi:

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

I forumula ovan berättar wGyro hur mycket vi litar på vår gyro jämfört med vår accelerometer. Det här värdet kan väljas experimentellt vanligtvis kommer värden mellan 5..20 att utlösa bra resultat.

Huvudskillnaden mellan denna algoritm och Kalman -filtret är att vikten är relativt fast, medan kalmanfilteret uppdaterar vikterna permanent baserat på det uppmätta bruset från accelerometeravläsningarna. Kalman -filtret är inriktat på att ge dig "de bästa" teoretiska resultaten, medan denna algoritm kan ge dig resultat "tillräckligt bra" för din praktiska tillämpning. Du kan implementera en algoritm som justerar wGyro beroende på några brusfaktorer som du mäter, men fasta värden fungerar bra för de flesta applikationer.

Vi är ett steg från att få våra uppdaterade uppskattade värden:

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

Låt oss nu normalisera den här vektorn 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

Och vi är redo att upprepa vår loop igen.

Den här guiden dök ursprungligen upp på starlino.com, jag har gjort några lätta redigeringar och lagt upp den igen med tillstånd. Tack Starlino!