Versnellingsmeter en gyro -tutoriaal: 3 stappe

2024 Outeur: John Day | [email protected]. Laas verander: 2024-01-30 07:29

Inleiding

Hierdie gids is bedoel vir almal wat belangstel in die gebruik van versnellingsmeters en gyroskope, sowel as kombinasie -IMU -toestelle (traagmeeteenheid) in hul elektroniese projekte.

Ons behandel:

• Wat meet 'n versnellingsmeter?
• Wat meet 'n gyroscoop (aka gyro)?
• Hoe om analoog-na-digitaal (ADC) lesings wat u van hierdie sensor kry, om te skakel na fisiese eenhede (dit is g vir versnellingsmeter, deg/s vir gyroscoop)
• Hoe om die versnellingsmeter en die gyroscooplesings te kombineer om akkurate inligting te kry oor die helling van u toestel relatief tot die grondvlak

Deur die hele artikel sal ek probeer om die wiskunde tot die minimum te beperk. As u weet wat Sine/Cosine/Tangent is, moet u hierdie idees in u projek kan verstaan en gebruik, ongeag watter platform u gebruik: Arduino, Propeller, Basic Stamp, Atmel -skyfies, Microchip PIC, ens.

Daar is mense wat glo dat u komplekse wiskunde nodig het om van 'n IMU-eenheid gebruik te maak (komplekse FIR- of IIR-filters, soos Kalman-filters, Parks-McClellan-filters, ens.). U kan dit alles ondersoek en wonderlike, maar komplekse resultate behaal. My manier om dinge te verduidelik, verg net basiese wiskunde. Ek is 'n groot gelowige in eenvoud. Ek dink 'n eenvoudige stelsel is makliker om te beheer en te monitor, behalwe dat baie ingeboude toestelle nie die krag en hulpbronne het om komplekse algoritmes te implementeer wat matriksberekeninge vereis nie.

Ek sal as voorbeeld 'n nuwe IMU -eenheid, die Acc_Gyro Accelerometer + Gyro IMU, gebruik. Ons gebruik parameters van hierdie toestel in ons voorbeelde hieronder. Hierdie eenheid is 'n goeie toestel om mee te begin, want dit bestaan uit 2 toestelle:

- LIS331AL (gegewensblad) - 'n triaksiale 2G -versnellingsmeter - LPR550AL (gegewensblad) - 'n dubbele as -toonhoogte en rol, 500 deg/sec gyroscoop

Saam verteenwoordig hulle 'n eenheid van 5 grade vryheids traagheid. Dis nou 'n pragtige naam! Maar agter die spoggerige naam is 'n baie nuttige kombinasie -toestel wat ons in hierdie gids in detail sal verduidelik en verduidelik.

Stap 1: die versnellingsmeter

Om hierdie eenheid te verstaan, begin ons met die versnellingsmeter. As u aan versnellingsmeters dink, is dit dikwels nuttig om 'n boks in die vorm van 'n kubus met 'n bal daarin te beeld. U kan u iets anders voorstel, soos 'n koekie of 'n doughnut, maar ek sal my 'n bal voorstel:

As ons hierdie boks inneem op 'n plek sonder gravitasievelde, of vir die saak sonder ander velde wat die posisie van die bal kan beïnvloed - sal die bal eenvoudig in die middel van die boks dryf. U kan u voorstel dat die boks in die buitenste ruimte ver-ver weg is van enige kosmiese liggame, of as dit moeilik is om so 'n plek te vind, ten minste 'n ruimtetuig wat om die planeet wentel waar alles in gewiglose toestand is. Uit die prent hierbo kan u sien dat ons 'n paar mure aan elke as toewys (ons het die muur Y+ verwyder sodat ons in die boks kan kyk). Stel jou voor dat elke muur drukgevoelig is. As ons die boks skielik na links beweeg (ons versnel dit met versnelling 1g = 9,8m/s^2), sal die bal die muur X- tref. Ons meet dan die drukkrag wat die bal op die muur uitoefen en lewer 'n waarde van -1g op die X -as.

Let daarop dat die versnellingsmeter eintlik 'n krag sal opspoor wat in die teenoorgestelde rigting van die versnellingsvektor gerig is. Hierdie krag word dikwels Inertial Force of Fictitious Force genoem. Een ding wat u hieruit moet leer, is dat 'n versnellingsmeter indirek versnelling meet deur 'n krag wat op een van die mure toegepas word (volgens ons model kan dit 'n veer wees of iets anders in die werklike versnellingsmeters). Hierdie krag kan deur die versnelling veroorsaak word, maar soos ons in die volgende voorbeeld sal sien, word dit nie altyd deur versnelling veroorsaak nie.

As ons ons model neem en dit op die aarde plaas, val die bal op die Z-muur en sal 'n krag van 1g op die onderste muur uitoefen, soos in die prentjie hieronder getoon:

In hierdie geval beweeg die boks nie, maar ons kry steeds 'n lesing van -1g op die Z -as. Die druk wat die bal op die muur uitgeoefen het, is veroorsaak deur 'n gravitasiekrag. In teorie kan dit 'n ander soort krag wees - as u byvoorbeeld dink dat ons bal metaal is, kan die bal 'n magneet langs die boks plaas sodat dit teen 'n ander muur raak. Dit is gesê net om te bewys dat versnellingsmeters in wese nie versnelling dwing nie. Dit gebeur net dat versnelling 'n traagheidskrag veroorsaak wat deur die kragopsporingsmeganisme van die versnellingsmeter vasgevang word.

Alhoewel hierdie model nie presies 'n MEMS -sensor is nie, is dit dikwels nuttig om probleme met versnellingsmeters op te los. Daar is eintlik soortgelyke sensors wat metaalbolletjies binne het, dit word kantelknoppies genoem, maar dit is meer primitief en gewoonlik kan hulle net sien of die toestel binne 'n sekere rigting neig of nie, nie die omvang van die helling nie.

Tot dusver het ons die versnellingsmeteruitset op 'n enkele as ontleed, en dit is alles wat u met 'n enkele as versnellingsmeter kry. Die werklike waarde van triaksiale versnellingsmeters kom daaruit dat hulle traagheidskragte op al drie asse kan opspoor. Kom ons gaan terug na ons boksmodel, en laat ons die boks 45 grade na regs draai. Die bal raak nou aan 2 mure: Z- en X- soos in die prentjie hieronder getoon:

Die waardes van 0,71 is nie willekeurig nie; dit is eintlik 'n benadering vir SQRT (1/2). Dit sal duideliker word namate ons ons volgende model vir die versnellingsmeter bekendstel.

In die vorige model het ons die gravitasiekrag reggestel en ons denkbeeldige boks gedraai. In die laaste twee voorbeelde het ons die uitset in 2 verskillende boksposisies ontleed, terwyl die kragvektor konstant gebly het. Alhoewel dit nuttig was om te verstaan hoe die versnellingsmeter met eksterne kragte in wisselwerking is, is dit meer prakties om berekeninge uit te voer as ons die koördinaatstelsel aan die asse van die versnellingsmeter vasstel en ons verbeel ons dat die kragvektor om ons draai.

Kyk na die model hierbo. Ek het die kleure van die asse behou, sodat u 'n geestelike oorgang kan maak van die vorige model na die nuwe. Stel jou voor dat elke as in die nuwe model loodreg op die onderskeie vlakke van die boks in die vorige model is. Die vektor R is die kragvektor wat die versnellingsmeter meet (dit kan óf die gravitasiekrag óf die traagheidskrag uit die voorbeelde hierbo wees, óf 'n kombinasie van albei). Rx, Ry, Rz is projeksie van die R -vektor op die X-, Y-, Z -as. Let asseblief op die volgende verband:

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

wat basies die ekwivalent is van die stelling van Pythagoras in 3D.

Onthou dat ek 'n bietjie vroeër vir u gesê het dat die waardes van SQRT (1/2) ~ 0.71 nie lukraak is nie. As u die bogenoemde formule inprop, kan u, nadat ons onthou dat ons gravitasiekrag 1 g was, verifieer dat:

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

eenvoudig deur R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) in vergelyking 1 te vervang

Na 'n lang aanhef van teorie, kom ons nader aan die werklike versnellingsmeters. Die waardes Rx, Ry, Rz hou eintlik lineêr verband met die waardes wat u werklike versnellingsmeter sal lewer en wat u kan gebruik om verskillende berekeninge uit te voer.

Voordat ons daar aankom, laat ons 'n bietjie praat oor die manier waarop versnellingsmeters hierdie inligting aan ons sal verskaf. Die meeste versnellingsmeters sal in twee kategorieë ingedeel word: digitaal en analoog. Digitale versnellingsmeters gee u inligting met behulp van 'n seriële protokol soos I2C, SPI of USART, terwyl analoog versnellingsmeters 'n spanningsvlak lewer binne 'n voorafbepaalde omvang wat u met 'n ADC -module (analoog na digitaal omskakelaar) moet omskakel. Ek gaan nie in detail gaan oor hoe ADC werk nie, deels omdat dit so 'n uitgebreide onderwerp is en deels omdat dit van een platform na 'n ander verskil. Sommige mikrobeheerders het 'n ingeboude ADC-module, waarvan sommige eksterne komponente benodig om die ADC-omskakelings uit te voer. Maak nie saak watter tipe ADC -module u gebruik nie, u kry 'n waarde in 'n sekere reeks. 'N 10 -bis ADC -module lewer byvoorbeeld 'n waarde in die reeks 0..1023, let op dat 1023 = 2^10 -1. 'N 12-bis ADC-module lewer 'n waarde in die reeks 0..4095, let op dat 4095 = 2^12-1.

Kom ons gaan voort met 'n eenvoudige voorbeeld, veronderstel ons 10bit ADC -module het ons die volgende waardes gegee vir die drie versnellingsmeterkanale (asse):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Elke ADC -module sal 'n verwysingspanning hê, kom ons neem aan dat dit in ons voorbeeld 3.3V is. Om 'n 10bit adc -waarde na spanning om te skakel, gebruik ons die volgende formule:

VoltsRx = AdcRx * Vref / 1023

'N Vinnige opmerking hier: dat die laaste verdeler vir 8bit ADC 255 = 2 ^ 8 -1 sou wees, en vir die 12bit ADC die laaste verdeler 4095 = 2 ^ 12 -1.

Deur hierdie formule op al drie kanale toe te pas, kry ons:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (ons rond alle resultate af tot 2 desimale punte) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Elke versnellingsmeter het 'n nul-g spanningsvlak; u kan dit in spesifikasies vind, dit is die spanning wat ooreenstem met 0g. Om 'n getekende spanningswaarde te kry, moet ons die verskuiwing vanaf hierdie vlak bereken. Kom ons sê ons 0g spanningsvlak is VzeroG = 1.65V. Ons bereken die spanningsverskuiwings van nul-g spanning soos volg::

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

Ons het nou ons versnellingsmeterlesings in Volt, dit is nog steeds nie in g (9,8 m/s^2) nie. Om die finale omskakeling te doen, pas ons die versnellingsmeter sensitiwiteit toe, gewoonlik uitgedruk in mV/g. Kom ons sê ons sensitiwiteit = 478.5mV/g = 0.4785V/g. Gevoeligheidswaardes kan gevind word in die versnellingsmeter spesifikasies. Om die finale kragwaardes uitgedruk in g te kry, gebruik ons die volgende formule:

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

Ons kon natuurlik alle stappe in een formule kombineer, maar ek het deur al die stappe gegaan om dit duidelik te maak hoe u van ADC -metings na 'n kragvektorkomponent uitgedruk in g gaan.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Gevoeligheid (eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Gevoeligheid Rz = (AdcRz * Vref / 1023 - VzeroG) / Sensitiwiteit

Ons het nou al drie komponente wat ons traagheidskragvektor definieer; as die toestel nie onderworpe is aan ander kragte as gravitasie nie, kan ons aanvaar dat dit die rigting van ons gravitasiekragvektor is. As u die helling van die toestel relatief tot die grond wil bereken, kan u die hoek tussen hierdie vektor en die Z -as bereken. As u ook geïnteresseerd is in die hellingsrichting per as, kan u die resultaat in twee komponente verdeel: helling op die X- en Y-as, wat bereken kan word as die hoek tussen gravitasievektor en X / Y-as. Die berekening van hierdie hoeke is eenvoudiger as wat u dink, noudat ons die waardes vir Rx, Ry en Rz bereken het. Kom ons gaan terug na ons laaste versnellingsmetermodel en maak 'n paar ekstra notasies:

Die hoeke waarin ons belangstel, is die hoeke tussen X-, Y-, Z -asse en die kragvektor R. Ons definieer hierdie hoeke as Axr, Ayr, Azr. U kan sien uit die reghoekige driehoek gevorm deur R en Rx dat:

cos (Axr) = Rx / R, en soortgelyk: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Ons kan van vergelyking 1 aflei dat R = SQRT (Rx^2 + Ry^2 + Rz^2).

Ons kan nou ons hoeke vind deur die funksie arccos () (die inverse cos () -funksie) te gebruik:

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

Ons het baie moeite gedoen om die versnellingsmetermodel te verduidelik, net om hierdie formules te bereik. Afhangende van u toepassings, wil u moontlik enige formules wat ons verkry het, gebruik. Ons stel ook binnekort die gyroscoopmodel bekend, en ons sal sien hoe versnellingsmeter- en gyroscoopdata gekombineer kan word om nog meer akkurate hellingsberamings te bied.

Maar voordat ons dit doen, laat ons nog 'n paar nuttige notas doen:

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

Hierdie drieling word dikwels Direction Cosine genoem, en dit verteenwoordig basies die eenheidsvektor (vektor met lengte 1) wat dieselfde rigting as ons R -vektor het. U kan maklik verifieer dat:

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

Dit is 'n goeie eienskap, aangesien dit ons daarvan weerhou om die modulus (lengte) van R -vektor te monitor. As ons net belangstel in die rigting van ons traagheidsvektor, is dit sinvol om die modulus te normaliseer om ander berekeninge te vereenvoudig.

Stap 2: Gyroscoop

Ons gaan nie 'n ekwivalente model vir die gyroscoop bekendstel soos met die versnellingsmeter nie, maar ons spring reguit na die tweede versnellingsmeter en ons sal wys wat die gyroscoop volgens hierdie model meet.

Elke gyroscoopkanaal meet die rotasie om een van die asse. Byvoorbeeld, 'n 2-assige gyroscoop meet die rotasie rondom (of sommige kan "ongeveer" sê) oor die X- en Y-as. Om hierdie rotasie in getalle uit te druk, laat ons 'n paar notasies doen. Laat ons eers definieer:

Rxz - is die projeksie van die traagkragvektor R op die XZ -vlak Ryz - is die projeksie van die traagkragvektor R op die YZ -vlak

Uit die reghoekige driehoek gevorm deur Rxz en Rz, met behulp van die stelling van Pythagoras, kry ons:

Rxz^2 = Rx^2 + Rz^2, en soortgelyk: Ryz^2 = Ry^2 + Rz^2

let ook op dat:

R^2 = Rxz^2 + Ry^2, dit kan afgelei word van vergelyking 1 en hoër, of dit kan afgelei word van 'n reghoekige driehoek gevorm deur R en Ryz R^2 = Ryz^2 + Rx^2

Ons gaan nie hierdie formules in hierdie artikel gebruik nie, maar dit is handig om die verband tussen al die waardes in ons model op te let.

In plaas daarvan gaan ons die hoek tussen die Z -as en Rxz, Ryz -vektore soos volg definieer:

Axz - is die hoek tussen die Rxz (projeksie van R op XZ -vlak) en Z -as Ayz - is die hoek tussen die Ryz (projeksie van R op YZ -vlak) en Z -as

Nou kom ons nader aan wat die gyroscoop meet. Gyroscoop meet die tempo van veranderinge van die hoeke wat hierbo gedefinieer is. Met ander woorde, dit sal 'n waarde lewer wat lineêr verband hou met die tempo van verandering van hierdie hoeke. Om dit te verduidelik, neem ons aan dat ons die rotasiehoek rondom as Y (dit sou Axz -hoek) op tyd t0 gemeet word, en ons definieer dit as Axz0, en daarna het ons hierdie hoek op 'n later tydstip t1 gemeet en dit was Axz1. Die tempo van verandering word soos volg bereken:

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

As ons Axz in grade en tyd in sekondes uitdruk, sal hierdie waarde uitgedruk word in deg/s. Dit is wat 'n gyroscoop meet.

In die praktyk sal 'n gyroscoop (tensy dit 'n spesiale digitale gyroscoop is) selde 'n waarde gee wat uitgedruk word in deg/s. Net soos vir die versnellingsmeter, kry u 'n ADC -waarde wat u na deg/s moet omskakel met 'n formule soortgelyk aan Eq. 2 wat ons vir versnellingsmeter gedefinieer het. Kom ons stel die ADC na deg/s -omskakelingsformule vir die gyroscoop bekend (ons neem aan dat ons 'n 10bit ADC -module gebruik, vir 8bit ADC vervang 1023 met 255, vir 12bit ADC vervang 1023 met 4095).

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

AdcGyroXZ, AdcGyroYZ - word verkry uit ons adc -module en dit verteenwoordig die kanale wat die rotasie van die projeksie van R -vektor in XZ onderskeidelik in YZ -vliegtuie meet, wat gelykstaande is aan rotasie onderskeidelik rondom Y- en X -as.

Vref - is die ADC -verwysingspanning wat ons in die voorbeeld hieronder VzeroRate sal gebruik 3.3V byvoorbeeld 1.23V (u kan hierdie waardes in die spesifikasies vind) Gevoeligheid - is die sensitiwiteit van u gyroscoop, dit word uitgedruk in mV / (deg / s), dikwels geskryf as mV / deg / s, dit vertel u basies hoeveel mV sal die uitset van die gyroscoop neem toe as u die rotasiesnelheid met een graad/s verhoog. Die sensitiwiteit van Acc_Gyro -bord is byvoorbeeld 2mV/deg/s of 0.002V/deg/s

Kom ons neem 'n voorbeeld, veronderstel dat ons ADC -module die volgende waardes teruggee:

AdcGyroXZ = 571 AdcGyroXZ = 323

Deur die bogenoemde formule te gebruik en die spesifikasies van die Acc_Gyro -bord te gebruik, kry ons:

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

Met ander woorde, die toestel draai om die Y -as (of ons kan sê dat dit in 'n XZ -vlak draai) met 'n snelheid van 306 °/s en om die X -as (of ons kan sê dat dit in 'n YZ -vlak draai) met 'n snelheid van - 94 grade/sek. Let daarop dat die negatiewe teken beteken dat die toestel in die teenoorgestelde rigting draai as die konvensionele positiewe rigting. Volgens konvensie is een rotasie rigting positief. 'N Goeie spesifikasieblad van die gyroscoop sal u wys watter rigting positief is, anders moet u dit vind deur met die toestel te eksperimenteer en op te let watter draairigting die spanning op die uitsetpen verhoog. Dit word die beste gedoen met behulp van 'n ossilloskoop, want sodra u die rotasie stop, sal die spanning terugsak na die nul-tempo. As u 'n multimeter gebruik, moet u ten minste 'n paar sekondes 'n konstante rotasiesnelheid handhaaf en die spanning tydens hierdie rotasie noteer, dan vergelyk dit met die nulsnelheidsspanning. As dit groter is as die nulsnelheidspanning, beteken dit dat die rotasie rigting positief is.

Stap 3: Kombineer die versnellingsmeter en die gyro

Alles saamvoeg - Kombineer versnellingsmeter- en gyroscoopdata

As u hierdie artikel lees, het u waarskynlik 'n IMU -toestel bekom of beplan om dit te bou, of is u van plan om dit te bou uit 'n aparte versnellingsmeter en gyroscoop.

Die eerste stap in die gebruik van 'n kombinasie IMU -toestel wat 'n versnellingsmeter en 'n gyroscoop kombineer, is om hul koördinaatstelsels in lyn te bring. Die maklikste manier om dit te doen is om die koördinaatstelsel van die versnellingsmeter as u verwysingskoördinaatstelsel te kies. Die meeste versnellingsmetergegewensblaaie sal die rigting van die X-, Y-, Z -as vertoon relatief tot die beeld van die fisiese chip of toestel. Hier is byvoorbeeld die rigtings van die X-, Y-, Z -as, soos aangedui in die spesifikasies vir die Acc_Gyro -bord:

Volgende stappe is:

Identifiseer die gyroscoop -uitsette wat ooreenstem met RateAxz, RateAyz -waardes hierbo bespreek. Bepaal of hierdie uitsette omgedraai moet word as gevolg van die fisiese posisie van die gyroscoop relatief tot die versnellingsmeter

Moenie aanneem dat as 'n gyroscoop 'n uitset met X of Y het nie, dit ooreenstem met enige as in die versnellingsmeterkoördinaatstelsel, selfs al is hierdie uitset deel van 'n IMU -eenheid. Die beste manier is om dit te toets. Gestel jy het die posisie van die gyroscoop relatief tot die versnellingsmeter bepaal. Daar word aanvaar dat die gyro- en versnellingsmetergrense ewewydig aan mekaar is, dit wil sê dat u die gyro onder 'n hoekmeervoud van 90deg in verhouding tot die versnellingsmeterskyfie plaas. As u 'n IMU -bord gekry het, is die kans goed dat hulle reeds op hierdie manier in lyn is. Ons gaan nie in hierdie artikel modelle bespreek waar die gyroscoop teen 'n onreëlmatige hoek teenoor die versnellingsmeter geplaas word nie (laat ons sê 45 of 30 grade), hoewel dit in sommige toepassings nuttig kan wees.

Hier is 'n voorbeeldreeks om vas te stel watter uitset van die gyroscoop ooreenstem met die RateAxz -waarde wat hierbo bespreek is.

- begin deur die toestel in horisontale posisie te plaas. Beide X- en Y-uitsette van die versnellingsmeter sal die nul-g-spanning lewer (byvoorbeeld, vir Acc_Gyro-bord is dit 1,65V)

- begin dan om die toestel om die Y -as te draai, 'n ander manier om dit te sê, is dat u die toestel in 'n XZ -vlak draai, sodat die uitsette van die X- en Z -versnellingsmeter verander en die Y -uitset konstant bly. - terwyl die toestel met 'n konstante snelheid gedraai word, let op watter gyroscoopuitset verander, moet die ander gyroscoopuitsette konstant bly - die gyroscoopuitset wat verander het tydens die rotasie om die Y -as (rotasie in XZ -vlak) sal die invoerwaarde vir AdcGyroXZ verskaf, waaruit ons bereken RateAxz - die laaste stap is om te verseker dat die rotasie rigting ooreenstem met ons model; in sommige gevalle moet u die RateAxz -waarde omkeer as gevolg van die fisiese posisie van die gyroscoop relatief tot die versnellingsmeter - voer weer die bogenoemde toets uit en draai die toestel om die Y -as, monitor hierdie keer die X -uitset van die versnellingsmeter (AdcRx in ons model). As AdcRx groei (die eerste 90 grade rotasie vanaf horisontale posisie), behoort AdcGyroXZ ook te groei. Andersins moet u RateAxz omkeer, u kan dit bereik deur 'n tekenfaktor in vergelyking 3 soos volg in te stel:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Gevoeligheid, waar InvertAxz 1 of -1 is

dieselfde toetsriet kan vir RateAyz gedoen word, deur die toestel om die X -as te draai, en u kan identifiseer watter uitset van die gyroscoop ooreenstem met RateAyz, en as dit omgedraai moet word. Sodra u die waarde vir InvertAyz het, moet u die volgende formule gebruik om RateAyz te bereken:

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

As u hierdie toetse op die Acc_Gyro -bord sou doen, kry u die volgende resultate:

- die uitvoerpen vir RateAxz is GX4 en InvertAxz = -1. - die uitvoerpen vir RateAyz is GY4 en InvertAyz = -1

Vanaf hierdie punt sal ons oorweeg dat u u IMU so opgestel het dat u die korrekte waardes vir Axr, Ayr, Azr (soos gedefinieer in deel 1. Accelerometer) en RateAxz, RateAyz (soos gedefinieer in deel 2. Gyroscope) kan bereken.). Vervolgens sal ons die verwantskappe tussen hierdie waardes ontleed, wat nuttig is om 'n meer akkurate skatting van die helling van die toestel in verhouding tot die grondvlak te verkry.

U vra uself dalk op hierdie punt af, as die versnellingsmetermodel ons al die hellingshoeke van Axr, Ayr, Azr gegee het, waarom sou ons dan moeite wil doen met die gyroscoopdata? Die antwoord is eenvoudig: versnellingsmeterdata kan nie altyd 100%vertrou word nie. Daar is verskeie redes, onthou dat die versnellingsmeter traagheid meet, so 'n krag kan veroorsaak word deur gravitasie (en verkieslik slegs deur gravitasie), maar dit kan ook veroorsaak word deur versnelling (beweging) van die toestel. Selfs al is die versnellingsmeter in 'n relatief stabiele toestand, is dit steeds baie sensitief vir vibrasie en meganiese geraas in die algemeen. Dit is die belangrikste rede waarom die meeste IMU -stelsels 'n gyroscoop gebruik om foute in die versnellingsmeter uit te skakel. Maar hoe word dit gedoen? En is die gyroscoop vry van geraas?

Die gyroscoop is nie vry van geraas nie, maar omdat dit die rotasie meet, is dit minder sensitief vir lineêre meganiese bewegings, die tipe geraas waaraan die versnellingsmeter ly, maar gyroskope het ander probleme, soos byvoorbeeld drift (kom nie terug na nulkoers nie) wanneer rotasie stop). Tog kan ons, deur die gemiddelde data van die versnellingsmeter en die gyroscoop te bereken, 'n relatiewe beter skatting van die huidige helling van die toestel verkry as wat ons sou verkry deur die versnellingsmeterdata alleen te gebruik.

In die volgende stappe sal ek 'n algoritme bekendstel wat geïnspireer is deur 'n paar idees wat in die Kalman -filter gebruik is, maar dit is baie eenvoudiger en makliker om op ingeboude toestelle te implementeer. Laat ons eers kyk wat ons wil hê ons algoritme moet bereken. Wel, dit is die rigting van gravitasiekragvektor R = [Rx, Ry, Rz] waaruit ons ander waardes soos Axr, Ayr, Azr of cosX, cosy, cosZ kan aflei, wat ons 'n idee sal gee van die neiging van ons toestel met betrekking tot die grondvlak, bespreek ons die verband tussen hierdie waardes in Deel 1. 'n Mens kan sê - het ons nie reeds hierdie waardes Rx, Ry, Rz van vergelyking 2 in deel 1 nie? Wel, maar onthou dat hierdie waardes slegs afkomstig is van versnellingsmeterdata, dus as u dit direk in u toepassing sou gebruik, kan u meer geraas kry as wat u toepassing kan verdra. Om verdere verwarring te voorkom, laat ons die metings van die versnellingsmeter soos volg herdefinieer:

Racc - is die traagheidskragvektor gemeet deur 'n versnellingsmeter, wat bestaan uit die volgende komponente (projeksies op X-, Y-, Z -asse):

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

Tot dusver het ons 'n stel meetwaardes wat ons suiwer kan verkry uit die versnellingsmeter ADC -waardes. Ons noem hierdie stel data 'n 'vektor' en gebruik die volgende notasie.

Racc = [RxAcc, RyAcc, RzAcc]

Omdat hierdie komponente van Racc verkry kan word uit versnellingsmeterdata, kan ons dit as 'n inset tot ons algoritme beskou.

Let daarop dat, omdat Racc die gravitasiekrag meet, u korrek sal wees as u aanneem dat die lengte van hierdie vektor soos volg gelyk of gelyk is aan 1g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Om seker te wees, is dit egter sinvol om hierdie vektor soos volg op te dateer:

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

Dit sal verseker dat die lengte van u genormaliseerde Racc -vektor altyd 1 is.

Vervolgens stel ons 'n nuwe vektor voor en noem dit

Rus = [RxEst, RyEst, RzEst]

Dit is die uitset van ons algoritme; dit is gekorrigeerde waardes gebaseer op gyroscoopdata en gebaseer op geraamde data uit die verlede.

Dit is wat ons algoritme sal doen: - versnellingsmeter vertel ons: "U is nou op posisie Racc" - ons sê "Dankie, maar laat ek kyk", - korrigeer dan hierdie inligting met gyroscoopdata sowel as met vorige rusdata en ons lewer 'n nuwe geskatte vektorrus uit. - ons beskou rus as ons 'beste weddenskap' oor die huidige posisie van die toestel.

Kom ons kyk hoe ons dit kan laat werk.

Ons begin ons volgorde deur ons versnellingsmeter te vertrou en toe te ken:

Rus (0) = Racc (0)

Terloops, onthou dat rus en racc vektore is, dus is die bogenoemde vergelyking net 'n eenvoudige manier om 3 stelle vergelykings te skryf en herhaling te vermy:

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

Vervolgens doen ons gereelde metings met gelyke tydsintervalle van T sekondes, en kry ons nuwe metings wat ons sal definieer as Racc (1), Racc (2), Racc (3) ensovoorts. Ons gee ook nuwe ramings met elke tydsinterval Rus (1), Rus (2), Rus (3) ensovoorts.

Gestel ons is by stap n. Ons het twee bekende stelle waardes wat ons wil gebruik:

Rus (n -1) - ons vorige skatting, met rus (0) = Racc (0) Racc (n) - ons huidige versnellingsmetermeting

Voordat ons Rest (n) kan bereken, laat ons 'n nuwe meetwaarde bekendstel wat ons van ons gyroscoop en 'n vorige skatting kan verkry.

Ons sal dit Rgyro noem, en dit is ook 'n vektor wat uit 3 komponente bestaan:

Rgyro = [RxGyro, RyGyro, RzGyro]

Ons sal hierdie vektor een komponent op 'n slag bereken. Ons begin met RxGyro.

Kom ons begin deur die volgende verband in ons gyroscoopmodel waar te neem, uit die reghoekige driehoek gevorm deur Rz en Rxz kan ons dit aflei:

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

Atan2 is moontlik 'n funksie wat u nog nooit gebruik het nie; dit is soortgelyk aan atan, behalwe dat dit waardes in die omvang van (-PI, PI) oplewer, in teenstelling met (-PI/2, PI/2) soos teruggestuur deur atan, en dit neem 2 argumente in plaas van een. Dit stel ons in staat om die twee waardes van Rx, Rz in hoeke in die volle omvang van 360 grade (-PI na PI) om te skakel. U kan hier meer lees oor atan2.

As ons RxEst (n-1) en RzEst (n-1) ken, kan ons vind:

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

Onthou dat die gyroscoop die veranderingstempo van die Axz -hoek meet. Ons kan dus die nuwe hoek Axz (n) soos volg skat:

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

Onthou dat RateAxz verkry kan word uit ons gyroscoop ADC -lesings. 'N Meer presiese formule kan 'n gemiddelde rotasiesnelheid gebruik soos volg bereken:

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

Op dieselfde manier kan ons vind:

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

Ok, nou het ons Axz (n) en Ayz (n). Waar gaan ons van hier af om RxGyro/RyGyro af te trek? Uit Vgl. 1 kan ons die lengte van vektor Rgyro soos volg skryf:

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

Omdat ons ons Racc -vektor genormaliseer het, kan ons aanneem dat die lengte 1 is en dit nie verander het na die rotasie nie, dus is dit relatief veilig om te skryf:

| Rgyro | = 1

Kom ons neem 'n tydelike korter notasie vir die onderstaande berekeninge aan:

x = RxGyro, y = RyGyro, z = RzGyro

Deur die bande hierbo te gebruik, kan ons skryf:

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

Kom ons deel teller en noemer van breuk met SQRT (x^2 + z^2)

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

Let op dat x / SQRT (x^2 + z^2) = sin (Axz), dus:

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

Vermenigvuldig nou teller en noemer van breuk binne SQRT met z^2

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

Let daarop dat z / SQRT (x^2 + z^2) = cos (Axz) en y / z = tan (Ayz), dus laastens:

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

As ons terugkeer na ons notasie, kry ons:

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

op dieselfde manier vind ons dit

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

Nou kan ons uiteindelik vind:

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

Waar teken (RzGyro) = 1 wanneer RzGyro> = 0, en teken (RzGyro) = -1 wanneer RzGyro <0.

Een eenvoudige manier om dit te skat is om te neem:

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

Wees in die praktyk versigtig as RzEst (n-1) naby 0. U kan in hierdie geval die gyrofase heeltemal oorslaan en toewys: Rgyro = Rus (n-1). Rz word gebruik as 'n verwysing vir die berekening van die hoeke van Axz en Ayz, en as dit naby 0 is, kan waardes vloei en slegte resultate veroorsaak. U sal in die domein van groot dryfpuntgetalle wees, waar die implementering van funksies van tan () / atan () moontlik nie akkuraat is nie.

Laat ons dus herhaal wat ons tot dusver het; ons is in stap n van ons algoritme en ons het die volgende waardes bereken:

Racc - huidige metings van ons versnellingsmeter Rgyro - verkry uit Rest (n -1) en huidige gyroscooplesings

Watter waardes gebruik ons om die opgedateerde skatting Rest (n) te bereken? U het waarskynlik geraai dat ons albei sal gebruik. Ons sal 'n geweegde gemiddelde gebruik, sodat:

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

Ons kan hierdie formule vereenvoudig deur beide teller en noemer van die breuk met w1 te deel.

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

en nadat ons w2/w1 = wGyro vervang het, kry ons:

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

In bogenoemde forumula vertel wGyro ons hoeveel ons ons gyro vertrou in vergelyking met ons versnellingsmeter. Hierdie waarde kan eksperimenteel gekies word, gewoonlik sal waardes tussen 5..20 goeie resultate oplewer.

Die belangrikste verskil van hierdie algoritme met die Kalman -filter is dat hierdie gewig relatief vas is, terwyl die gewigte in Kalman -filter permanent opgedateer word op grond van die gemete geraas van die versnellingsmeterlesings. Kalman -filter is daarop gefokus om u 'die beste' teoretiese resultate te gee, terwyl hierdie algoritme u 'goed genoeg' kan gee vir u praktiese toepassing. U kan 'n algoritme implementeer wat wGyro aanpas, afhangende van sommige geraasfaktore wat u meet, maar vaste waardes werk goed vir die meeste toepassings.

Ons is net 'n entjie verder om ons bygewerkte geraamde waardes te kry:

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

Laat ons hierdie vektor weer normaliseer:

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

En ons is gereed om ons lus weer te herhaal.

Hierdie gids verskyn oorspronklik op starlino.com. Ek het 'n paar ligte wysigings aangebring en dit met toestemming weer geplaas. Dankie Starlino!