Kiirendusmõõtur ja güroskoopiõpetus: 3 sammu

2024 Autor: John Day | [email protected]. Viimati modifitseeritud: 2024-01-30 08:51

Sissejuhatus

See juhend on mõeldud kõigile, kes on huvitatud kiirendusmõõturite ja güroskoopide ning kombineeritud IMU -seadmete (inertsiaalse mõõtmisseadme) kasutamisest oma elektroonikaprojektides

Me katame:

• Mida mõõdab kiirendusmõõtur?
• Mida mõõdab güroskoop (teise nimega güroskoop)?
• Kuidas teisendada nendelt anduritelt saadud analoog-digitaalseid (ADC) näiteid füüsilistesse ühikutesse (need oleksid g kiirendusmõõturi puhul, deg/s güroskoobi jaoks)
• Kiirendusmõõturi ja güroskoobi näitude kombineerimine, et saada täpset teavet seadme kaldenurga suhtes maapinna suhtes

Püüan kogu artikli vältel matemaatika miinimumini viia. Kui teate, mis on Sine/Cosinus/Tangent, siis peaksite olema võimeline neid ideid oma projektis mõistma ja kasutama, olenemata sellest, millist platvormi kasutate: Arduino, Propeller, Basic Stamp, Atmel kiibid, Microchip PIC jne.

On inimesi, kes usuvad, et IMU-seadme kasutamiseks on vaja keerulist matemaatikat (keerulised FIR- või IIR-filtrid, nagu Kalmani filtrid, Parks-McClellani filtrid jne). Saate neid kõiki uurida ja saavutada suurepäraseid, kuid keerukaid tulemusi. Minu viis asju selgitada nõuab lihtsalt elementaarset matemaatikat. Ma usun väga lihtsusse. Ma arvan, et lihtsat süsteemi on lihtsam juhtida ja jälgida, pealegi pole paljudel manustatud seadmetel jõudu ja ressursse keeruliste maatriksarvutusi nõudvate algoritmide rakendamiseks.

Kasutan näitena uut IMU -seadet Acc_Gyro Accelerometer + Gyro IMU. Allpool toodud näidetes kasutame selle seadme parameetreid. See seade on alustamiseks hea seade, kuna see koosneb kahest seadmest:

- LIS331AL (andmeleht) - kolmeteljeline 2G kiirendusmõõtur - LPR550AL (andmeleht) - kaheteljeline samm ja rull, 500 g/s güroskoop

Koos esindavad nad 5-kraadise vabaduse inertsiaalse mõõtmise üksust. Nüüd on see uhke nimi! Sellegipoolest on väljamõeldud nime taga väga kasulik kombineeritud seade, mida käsitleme ja selgitame selles juhendis üksikasjalikult.

Samm: kiirendusmõõtur

Selle seadme mõistmiseks alustame kiirendusmõõturiga. Kiirendusmõõturitele mõeldes on sageli kasulik kujutada kuubikujulist kasti, mille sees on pall. Te võite ette kujutada midagi muud nagu küpsist või sõõrikut, aga mina kujutan ette palli:

Kui me võtame selle kasti kohta, kus puuduvad gravitatsiooniväljad, või siis ilma muude väljadeta, mis võivad palli positsiooni mõjutada - pall hõljub lihtsalt kasti keskel. Võite ette kujutada, et kast asub kosmoses kosmosest kaugel, kaugel kõikidest kosmilistest kehadest, või kui sellist kohta on raske leida, siis kujutlege vähemalt kosmoselaeva, mis tiirleb ümber planeedi, kus kõik on kaaluta. Ülaltoodud pildilt näete, et määrame igale teljele paar seina (eemaldasime seina Y+, et saaksime kasti sisse vaadata). Kujutage ette, et iga sein on survetundlik. Kui liigutame kasti äkki vasakule (kiirendame seda kiirendusega 1g = 9,8m/s^2), lööb pall seina X-. Seejärel mõõdame kuuli seinale rakendatavat survejõudu ja väljastame X -teljel väärtuse -1g.

Pange tähele, et kiirendusmõõtur tuvastab tegelikult jõu, mis on suunatud kiirendusvektorist vastupidises suunas. Seda jõudu nimetatakse sageli inertsiaalseks või fiktiivseks jõuks. Üks asi, mida peaksite sellest õppima, on see, et kiirendusmõõtur mõõdab kiirendust kaudselt läbi ühe seinale rakendatava jõu (meie mudeli kohaselt võib see olla vedru või midagi muud reaalses elus kiirendusmõõturites). Selle jõu võib põhjustada kiirendus, kuid nagu näeme järgmises näites, ei põhjusta seda alati kiirendus.

Kui võtame oma mudeli ja paneme selle Maale, langeb pall Z-seinale ja rakendab alumisele 1 g jõudu, nagu on näidatud alloleval pildil:

Sel juhul kast ei liigu, kuid saame Z -teljel siiski -1g näidu. Surve, mille pall on seinale avaldanud, põhjustas gravitatsioonijõud. Teoreetiliselt võib see olla teist tüüpi jõud - näiteks kui kujutate ette, et meie pall on metallist, võib magneti paigutamine kasti kõrvale liigutada palli nii, et see lööb teise seina. Seda öeldi lihtsalt selleks, et tõestada, et sisuliselt kiirendusmõõtur mõõdab jõudu, mitte kiirendust. Lihtsalt juhtub, et kiirendus põhjustab inertsijõu, mille haarab kiirendusmõõturi jõu tuvastamise mehhanism.

Kuigi see mudel ei ole täpselt selline, nagu MEMS -andur on ehitatud, on see sageli kiirendusmõõturiga seotud probleemide lahendamisel kasulik. Tegelikult on sarnaseid andureid, mille sees on metallkuulid, neid nimetatakse kallutuslülititeks, kuid need on primitiivsemad ja tavaliselt suudavad nad ainult öelda, kas seade on teatud vahemikus või mitte, kuid mitte kaldenurk.

Siiani oleme analüüsinud kiirendusmõõturi väljundit ühel teljel ja see on kõik, mida saate ühe teljega kiirendusmõõturitega. Kolmeteljeliste kiirendusmõõturite tegelik väärtus tuleneb asjaolust, et need suudavad tuvastada inertsijõude kõigil kolmel teljel. Läheme tagasi oma karbimudeli juurde ja pöörame kasti 45 kraadi paremale. Pall puudutab nüüd kahte seina: Z- ja X-, nagu on näidatud alloleval pildil:

Väärtused 0,71 ei ole suvalised, need on tegelikult SQRT (1/2) ligikaudsed väärtused. See saab selgemaks, kui tutvustame oma järgmist kiirendusmõõturi mudelit.

Eelmises mudelis oleme fikseerinud gravitatsioonijõu ja pööranud oma kujuteldavat kasti. Viimases kahes näites oleme analüüsinud väljundit kahes erinevas kastiasendis, samal ajal kui jõuvektor jäi konstantseks. Kuigi sellest oli abi, et mõista, kuidas kiirendusmõõtur suhtleb välisjõududega, on otstarbekam teha arvutusi, kui fikseerime koordinaatsüsteemi kiirendusmõõturi telgede külge ja kujutame ette, et jõuvektor pöörleb meie ümber.

Palun vaadake ülaltoodud mudelit, säilitasin kirveste värvid, et saaksite vaimselt ülemineku eelmiselt mudelilt uuele. Kujutage vaid ette, et uue mudeli iga telg on risti eelmise mudeli kasti vastavate külgedega. Vektor R on jõuvektor, mida kiirendusmõõtur mõõdab (see võib olla kas ülaltoodud näidete gravitatsioonijõud või inertsjõud või mõlema kombinatsioon). Rx, Ry, Rz on R vektori projektsioon X, Y, Z telgedel. Pange tähele järgmist seost:

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

mis on põhimõtteliselt samaväärne Pythagorase teoreemiga 3D -s.

Pidage meeles, et natuke varem ütlesin teile, et SQRT (1/2) ~ 0,71 väärtused ei ole juhuslikud. Kui ühendate need ülaltoodud valemiga, saame pärast meelde tuletamist, et meie gravitatsioonijõud oli 1 g, veenduda, et:

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

lihtsalt asendades R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) võrrandis 1

Pärast pikka teooria preambulat jõuame lähemale reaalse elu kiirendusmõõturitele. Väärtused Rx, Ry, Rz on tegelikult lineaarselt seotud väärtustega, mida teie tegelik kiirendusmõõtur väljastab ja mida saate kasutada erinevate arvutuste tegemiseks.

Enne sinna jõudmist räägime veidi sellest, kuidas kiirendusmõõturid seda teavet meile edastavad. Enamik kiirendusmõõturit jaguneb kahte kategooriasse: digitaalne ja analoog. Digitaalsed kiirendusmõõturid annavad teile teavet, kasutades jadaprotokolli, nagu I2C, SPI või USART, samas kui analoogkiirendusmõõturid väljastavad pinge taseme etteantud vahemikus, mille peate teisendama digitaalseks väärtuseks, kasutades ADC (analoog -digitaalmuundur) moodulit. Ma ei hakka ADC tööpõhimõtteid väga üksikasjalikult kirjeldama, osaliselt seetõttu, et see on nii ulatuslik teema ja osaliselt seetõttu, et see on platvormiti erinev. Mõnel mikrokontrolleril on sisseehitatud ADC-moodulid, mõned neist vajavad ADC-teisenduste tegemiseks väliseid komponente. Olenemata sellest, millist tüüpi ADC -moodulit kasutate, saate väärtuse teatud vahemikus. Näiteks 10 -bitine ADC -moodul väljastab väärtuse vahemikus 0..1023, pange tähele, et 1023 = 2^10 -1. 12-bitine ADC-moodul väljastab väärtuse vahemikus 0..4095, pange tähele, et 4095 = 2^12-1.

Jätkame lihtsa näitega, oletame, et meie 10 -bitine ADC -moodul andis meile järgmised kolm kiirendusmõõturi kanali (telje) väärtused:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Igal ADC moodulil on võrdluspinge, oletame, et meie näites on see 3,3 V. 10 -bitise adc väärtuse teisendamiseks pingeks kasutame järgmist valemit:

VoltsRx = AdcRx * Vref / 1023

Kiire märkus: 8 -bitise ADC puhul oleks viimane jagaja 255 = 2 ^ 8 -1 ja 12 -bitise ADC puhul viimane jagaja 4095 = 2 ^ 12 -1.

Rakendades seda valemit kõigile kolmele kanalile, saame:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (ümardame kõik tulemused kahe kümnendkohani) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Igal kiirendusmõõturil on null-g pingetase, selle leiate spetsifikatsioonidest, see on pinge, mis vastab 0 g-le. Allkirjastatud pinge väärtuse saamiseks peame selle taseme nihke arvutama. Oletame, et meie 0g pingetase on VzeroG = 1,65V. Arvutame pinge nihked null-pingelt järgmiselt:

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

Meil on nüüd kiirendusmõõturi näidud voltides, see pole ikka veel g (9,8 m/s^2), lõpliku teisenduse tegemiseks kasutame kiirendusmõõturi tundlikkust, tavaliselt väljendatuna mV/g. Ütleme, et meie tundlikkus = 478,5 mV/g = 0,4785V/g. Tundlikkuse väärtused leiate kiirendusmõõturi spetsifikatsioonidest. Lõppjõu väärtuste väljendamiseks g -des kasutame järgmist valemit:

Rx = DeltaVoltsRx / tundlikkus

Rx = 0.24V / 0.4785V / g = ~ 0.5g Ry = 0.38V / 0.4785V / g = ~ 0.79g Rz = 0.16V / 0.4785V / g = ~ 0.33g

Loomulikult võiksime kõik sammud ühendada ühte valemisse, kuid ma tegin kõik sammud läbi, et oleks selge, kuidas te lähete ADC näitudest jõuduvektori komponendini, väljendatuna g -des.

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

Meil on nüüd kõik kolm komponenti, mis määratlevad meie inertsijõu vektori, kui seade ei allu muudele jõududele peale gravitatsiooni, võime eeldada, et see on meie gravitatsioonijõu vektori suund. Kui soovite arvutada seadme kaldenurka maapinna suhtes, saate arvutada selle vektori ja Z -telje vahelise nurga. Kui teid huvitab ka kaldesuund telje kohta, saate selle tulemuse jagada kaheks komponendiks: kalle X- ja Y-teljel, mida saab arvutada gravitatsioonivektori ja X / Y-telje vahelise nurga all. Nende nurkade arvutamine on lihtsam kui arvate, nüüd, kui oleme arvutanud väärtused Rx, Ry ja Rz. Läheme tagasi oma viimase kiirendusmõõturi mudeli juurde ja teeme mõned täiendavad märkused:

Meid huvitavad nurgad on X, Y, Z telgede ja jõuvektori R. vahelised nurgad. Määratleme need nurgad Axr, Ayr, Azr. R ja Rx moodustatud täisnurksest kolmnurgast näete, et:

cos (Axr) = Rx / R ja sarnaselt: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Võime võrrandist 1 maha arvata, et R = SQRT (Rx^2 + Ry^2 + Rz^2).

Nüüd saame oma nurgad leida funktsiooni arccos () (pöördfunktsiooni cos ()) abil:

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

Kiirendusmõõturi mudeli selgitamiseks oleme jõudnud kaugele, et jõuda nende valemiteni. Sõltuvalt teie rakendustest võite soovida kasutada meie saadud valemeid. Tutvustame peagi ka güroskoobi mudelit ja vaatame, kuidas kiirendusmõõturi ja güroskoobi andmeid saab kombineerida, et anda veelgi täpsemaid kaldehinnanguid.

Kuid enne seda teeme mõned kasulikud märkused:

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

Seda kolmikut nimetatakse sageli suuna kosinuseks ja see kujutab põhimõtteliselt ühikvektorit (pikkus 1), millel on sama suund kui meie R vektoril. Saate hõlpsasti kontrollida, kas:

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

See on kena omadus, kuna see vabastab meid R vektori mooduli (pikkuse) jälgimisest. Sageli, kui meid huvitab ainult meie inertsiaalvektori suund, on mõttekas selle moodulit normaliseerida, et lihtsustada muid arvutusi.

Samm: güroskoop

Me ei tutvusta güroskoobi jaoks ühtegi samaväärset kasti mudelit nagu kiirendusmõõturi puhul, selle asemel liigume otse teise kiirendusmõõturi mudeli juurde ja näitame, mida güroskoop selle mudeli järgi mõõdab.

Iga güroskoobi kanal mõõdab pöörlemist ühe telje ümber. Näiteks mõõdab kahe teljega güroskoop X ja Y telje ümber pöörlemist (või mõned võivad öelda "umbes"). Selle pöörlemise arvudes väljendamiseks teeme mõned märkused. Kõigepealt määratleme:

Rxz - on inertsjõu vektori R projektsioon XZ tasapinnal Ryz - on inertsjõu vektori R projektsioon YZ tasapinnal

Rxz ja Rz moodustatud täisnurksest kolmnurgast saame Pythagorase teoreemi kasutades:

Rxz^2 = Rx^2 + Rz^2 ja sarnaselt: Ryz^2 = Ry^2 + Rz^2

pange tähele ka järgmist:

R^2 = Rxz^2 + Ry^2, selle saab tuletada võrranditest 1 ja üle selle või tuletada täisnurksest kolmnurgast, mille moodustavad R ja Ryz R^2 = Ryz^2 + Rx^2

Me ei kasuta neid valemeid selles artiklis, kuid on kasulik märkida seost kõigi meie mudeli väärtuste vahel.

Selle asemel määratleme nurga Z -telje ja Rxz, Ryzi vektorite vahel järgmiselt:

Axz - on nurk Rxz (R projektsioon XZ tasapinnal) ja Z telje Ayz vahel - on nurk Ryzi (R projektsioon YZ tasapinnal) ja Z telje vahel

Nüüd jõuame lähemale sellele, mida güroskoop mõõdab. Güroskoop mõõdab ülalnimetatud nurkade muutuste kiirust. Teisisõnu annab see väärtuse, mis on lineaarselt seotud nende nurkade muutumiskiirusega. Selle selgitamiseks oletame, et oleme mõõtnud pöördenurka ümber telje Y (see oleks Axzi nurk) ajal t0 ja määratleme selle kui Axz0, seejärel mõõtsime seda nurka hiljem t1 ja see oli Axz1. Muutuste määra arvutatakse järgmiselt:

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

Kui me väljendame Axzi kraadides ja aega sekundites, väljendatakse seda väärtust kraadides/s. Seda mõõdab güroskoop.

Praktikas annab güroskoop (kui see pole spetsiaalne digitaalne güroskoop) teile harva väärtuse, väljendatuna kraadides/s. Sama mis kiirendusmõõturi puhul, saate ADC väärtuse, mille peate teisendama astmeks/s, kasutades valemit, mis sarnaneb võrrandile. 2, mille oleme kiirendusmõõturi jaoks määratlenud. Tutvustame güroskoobi jaoks ADC -de teisendamise valemit (eeldame, et kasutame 10 -bitist ADC -moodulit, 8 -bitise ADC puhul asendage 1023 255 -ga, 12 -bitise ADC -ga asendage 1023 4095 -ga).

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

AdcGyroXZ, AdcGyroYZ - saadakse meie adc moodulist ja need esindavad kanaleid, mis mõõdavad R -vektori projektsiooni pöörlemist vastavalt XZ -s vastavalt YZ -tasapindadele, mis on samaväärne sellega, kui öeldakse, et pöörlemine toimus vastavalt Y- ja X -telje ümber.

Vref - on ADC võrdluspinge, mida kasutame allpool toodud näites VzeroRate 3.3V - on nullkiirusega pinge, teisisõnu pinge, mille güroskoop väljastab, kui see ei pöörle, Acc_Gyro plaadi jaoks näiteks 1,23 V (need väärtused leiate spetsifikatsioonidest) Tundlikkus - kas teie güroskoobi tundlikkus on väljendatud mV / (deg / s), sageli kirjutatakse kui mV / deg / s, see ütleb teile põhimõtteliselt, mitu mV güroskoobi väljund suureneb, kui suurendate pöörlemiskiirust ühe kraadi/s võrra. Acc_Gyro plaadi tundlikkus on näiteks 2mV/deg/s või 0,002V/deg/s

Võtame näite, oletame, et meie ADC -moodul tagastas järgmised väärtused:

AdcGyroXZ = 571 AdcGyroXZ = 323

Kasutades ülaltoodud valemit ja kasutades Acc_Gyro plaadi spetsifikatsiooniparameetreid, saame:

RateAxz = (571 * 3.3V/1023 - 1.23V)/(0.002V/deg/s) = ~ 306 °/s RateAyz = (323 * 3.3V/1023 - 1.23V)/(0.002V/deg/s) = ~ -94 kraadi/s

Teisisõnu, seade pöörleb ümber Y -telje (või võime öelda, et pöörleb XZ -tasapinnas) kiirusega 306 °/s ja ümber X -telje (või võib öelda, et pöörleb YZ -tasapinnas) kiirusega - 94 kraadi/s. Pange tähele, et negatiivne märk tähendab, et seade pöörleb tavapärasest positiivsest suunast vastupidises suunas. Kokkuleppe kohaselt on üks pöörlemissuund positiivne. Hea güroskoobi spetsifikatsioonileht näitab teile, milline suund on positiivne, vastasel juhul peate selle leidma, katsetades seadet ja märkides, millise pöörlemissuuna tõttu suureneb väljundpinge pinge. Seda on kõige parem teha ostsilloskoobi abil, kuna niipea, kui pöörlemise peatate, langeb pinge tagasi nullmäära tasemele. Kui kasutate multimeetrit, peate vähemalt paar sekundit hoidma konstantset pöörlemiskiirust ja märkima selle pöörlemise ajal pinget, seejärel võrrelge seda nullpingega. Kui see on suurem kui nullpinge, tähendab see, et pöörlemissuund on positiivne.

Samm: kiirendusmõõturi ja güroskoopi kombineerimine

Kõike kokku pannes - kiirendusmõõturi ja güroskoobi andmete ühendamine

Kui loete seda artiklit, omandasite tõenäoliselt või kavatsete soetada IMU -seadme või plaanite selle ehitada eraldi kiirendusmõõturi ja güroskoobi seadmetest.

Esimene samm kiirendusmõõturit ja güroskoopi kombineeriva IMU -seadme kasutamisel on nende koordinaatsüsteemide joondamine. Lihtsaim viis seda teha on valida võrdluskoordinaadiks kiirendusmõõturi koordinaatsüsteem. Enamik kiirendusmõõturi andmelehti kuvab X-, Y-, Z -telje suuna füüsilise kiibi või seadme kujutise suhtes. Näiteks siin on X, Y, Z telgede suunad, nagu on näidatud Acc_Gyro plaadi spetsifikatsioonides:

Järgmised sammud on järgmised:

Tuvastage güroskoobi väljundid, mis vastavad eespool käsitletud väärtustele RateAxz, RateAyz. Määrake, kas need väljundid tuleb güroskoobi füüsilise asukoha tõttu kiirendusmõõturi suhtes ümber pöörata

Ärge arvake, et kui güroskoobi väljund on tähistatud X või Y, vastab see kiirendusmõõturi koordinaatsüsteemi mis tahes teljele, isegi kui see väljund on osa IMU -seadmest. Parim viis on seda testida. Eeldusel, et olete güroskoobi asukoha kiirendusmõõturi suhtes fikseerinud. Eeldatakse, et güroskoop ja kiirendusmõõturi piirid on üksteisega paralleelsed, st asetate güroskoobi kiirendusmõõturi kiibi suhtes 90 -kraadise nurga all. Kui olete omandanud IMU -plaadi, on tõenäoline, et need on juba sel viisil joondatud. Selles artiklis me ei aruta mudeleid, kus güroskoop on kiirendusmõõturi suhtes ebaregulaarse nurga all (oletame, et see on 45 või 30 kraadi), kuigi see võib mõnes rakenduses kasulik olla.

Siin on näidisjärjestus, et teha kindlaks, milline güroskoobi väljund vastab eespool käsitletud RateAxz väärtusele.

- alustage seadme asetamisest horisontaalsesse asendisse. Nii kiirendusmõõturi X kui ka Y väljund väljastavad null-g pinge (näiteks Acc_Gyro plaadi puhul on see 1,65 V)

- Järgmisena alustage seadme pööramist ümber Y -telje, teine võimalus seda öelda on see, et pöörate seadet XZ -tasapinnas, nii et X- ja Z -kiirendusmõõturi väljundid muutuvad ja Y -väljund jääb konstantseks. - pöörates seadet konstantsel kiirusel, märkides, milline güroskoobi väljund muutub, peaksid teised güroskoobi väljundid jääma konstantseks - güroskoobi väljund, mis muutus ümber Y -telje pöörlemise ajal (pöörlemine XZ -tasapinnas), annab AdcGyroXZ -i sisendväärtuse, millest arvutame RateAxz - viimane samm on tagada, et pöörlemissuund vastaks meie mudelile; mõnel juhul peate võib -olla RateAxz väärtuse ümber pöörama güroskoobi füüsilise asukoha tõttu kiirendusmõõturi suhtes - tehke uuesti ülaltoodud test, pöörates seadet ümber Y -teljel, seekord jälgige kiirendusmõõturi X väljundit (meie mudelis AdcRx). Kui AdcRx kasvab (esimesed 90 kraadi pöörlemist horisontaalsest asendist), siis peaks kasvama ka AdcGyroXZ. Vastasel juhul peate RateAxzi ümber pöörama, saate selle saavutada, lisades märgistusteguri ekvivalenti 3 järgmiselt:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / tundlikkus, kus InvertAxz on 1 või -1

sama katsekepi saab teha ka RateAyzi jaoks, pöörates seadet ümber X -telje, ja saate kindlaks teha, milline güroskoobi väljund vastab RateAyzile ja kas see tuleb ümber pöörata. Kui teil on InvertAyzi väärtus, peaksite RateAyzi arvutamiseks kasutama järgmist valemit:

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

Kui teeksite need testid Acc_Gyro plaadil, saaksite järgmised tulemused:

- RateAxzi väljundnõel on GX4 ja InvertAxz = -1. - RateAyzi väljundnõel on GY4 ja InvertAyz = -1

Sellest hetkest alates leiame, et olete oma IMU seadistanud nii, et saate arvutada õiged väärtused Axr, Ayr, Azr (nagu on määratletud 1. osas. Kiirendusmõõtur) ja RateAxz, RateAyz (nagu on määratletud 2. osas).). Järgmisena analüüsime nende väärtuste vahelisi seoseid, mis osutuvad kasulikuks seadme kalde ja aluspinna suhtes täpsema hinnangu saamiseks.

Võib -olla küsite endalt juba praegu, kui kiirendusmõõturi mudel andis meile juba Axri, Ayri, Azri kaldenurgad, miks me tahame güroskoobi andmetega vaeva näha? Vastus on lihtne: kiirendusmõõturi andmeid ei saa alati 100%usaldada. On mitmeid põhjuseid, pidage meeles, et kiirendusmõõtur mõõdab inertsjõudu, sellise jõu võib põhjustada gravitatsioon (ja ideaalis ainult gravitatsioon), kuid see võib olla tingitud ka seadme kiirendusest (liikumisest). Selle tulemusena, isegi kui kiirendusmõõtur on suhteliselt stabiilses olekus, on see endiselt väga tundlik vibratsiooni ja mehaanilise müra suhtes üldiselt. See on peamine põhjus, miks enamik IMU süsteeme kasutab kiirendusmõõturi vigade silumiseks güroskoopi. Aga kuidas seda tehakse? Ja kas güroskoop on mürast vaba?

Güroskoop ei ole mürast vaba, kuid kuna see mõõdab pöörlemist, on see vähem tundlik lineaarsete mehaaniliste liikumiste suhtes, kiirendusmõõturi kannatava müratüübi suhtes, kuid güroskoopidel on muud tüüpi probleeme, näiteks triiv (mitte nullmäära väärtuse taastamine) kui pöörlemine peatub). Sellegipoolest, kiirendusmõõturilt ja güroskoobilt saadud andmete keskmistamise teel saame seadme praeguse kalde suhteliselt parema hinnangu kui ainult kiirendusmõõturi andmeid kasutades.

Järgmistes sammudes tutvustan algoritmi, mis on inspireeritud mõnest Kalmani filtris kasutatud ideest, kuid see on palju lihtsam ja hõlpsamini rakendatav manustatud seadmetes. Enne seda vaatame kõigepealt, mida me tahame, et meie algoritm arvutaks. Noh, see on gravitatsioonijõu vektori R = [Rx, Ry, Rz] suund, millest saame tuletada muid väärtusi, nagu Axr, Ayr, Azr või cosX, hubane, cosZ, mis annab meile aimu meie seadme kalde kohta võrreldes põhitasandiga, arutleme nende väärtuste vahelist seost 1. osas. Võib öelda - kas meil pole neid väärtusi Rx, Ry, Rz 1. osa ekv. Jah, aga pidage meeles, et need väärtused on tuletatud ainult kiirendusmõõturi andmetest, nii et kui kasutate neid otse oma rakenduses, võite saada rohkem müra, kui teie rakendus talub. Segaduse vältimiseks määratleme kiirendusmõõturi mõõtmised uuesti järgmiselt.

Racc - on kiirendusmõõturiga mõõdetud inertsjõu vektor, mis koosneb järgmistest komponentidest (projektsioonid X, Y, Z telgedel):

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

Siiani on meil mõõdetud väärtuste kogum, mille saame puhtalt kiirendusmõõturi ADC väärtustest. Nimetame seda andmekogumit vektoriks ja kasutame järgmist märget.

Racc = [RxAcc, RyAcc, RzAcc]

Kuna neid Racci komponente saab kiirendusmõõturi andmetest, võime seda pidada meie algoritmi sisendiks.

Pange tähele, et kuna Racc mõõdab gravitatsioonijõudu, on teil õigus, kui eeldate, et selle järgmiselt määratletud vektori pikkus on 1 g või selle lähedal.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Kuid selleks, et olla kindel, on mõttekas seda vektorit värskendada järgmiselt.

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

See tagab, et teie normaliseeritud Racc -vektori pikkus on alati 1.

Järgmisena tutvustame uut vektorit ja nimetame seda

Puhkus = [RxEst, RyEst, RzEst]

See on meie algoritmi väljund, need on parandatud väärtused, mis põhinevad güroskoobi andmetel ja varasematel hinnangulistel andmetel.

Meie algoritm teeb järgmist: - kiirendusmõõtur ütleb meile: "Olete nüüd Racc positsioonil" - ütleme "aitäh, aga lubage mul kontrollida", - seejärel parandage seda teavet nii güroskoobi kui ka varasemate puhkeandmetega väljastame uue hinnangulise vektori Rest. - peame puhkust seadme praeguse asukoha osas meie parimaks võimaluseks.

Vaatame, kuidas saame selle toimima panna.

Alustame oma järjestust, usaldades oma kiirendusmõõturit ja määrates:

Puhka (0) = Racc (0)

Muide, pidage meeles, et Rest ja Racc on vektorid, seega on ülaltoodud võrrand lihtsalt viis kolme võrrandikomplekti kirjutamiseks ja korduste vältimiseks:

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

Järgmisena teeme regulaarseid mõõtmisi võrdsete ajavahemike T -sekunditega ja saame uued mõõtmised, mille määratleme kui Racc (1), Racc (2), Racc (3) jne. Samuti anname igal ajavahemikul välja uued hinnangud Puhkus (1), Puhkus (2), Puhkus (3) ja nii edasi.

Oletame, et oleme sammus n. Meil on teada kaks väärtuste komplekti, mida sooviksime kasutada.

Puhkus (n -1) - meie eelmine hinnang, ülejäänud (0) = Racc (0) Racc (n) - meie praegune kiirendusmõõturi mõõtmine

Enne puhkuse (n) arvutamist tutvustame uut mõõdetud väärtust, mille saame oma güroskoobist ja eelmisest hinnangust.

Me nimetame seda Rgyroks ja see on ka vektor, mis koosneb kolmest komponendist:

Rgyro = [RxGyro, RyGyro, RzGyro]

Arvutame selle vektori üks komponent korraga. Alustame RxGyro'st.

Alustuseks jälgime meie güroskoobi mudelis järgmist seost, Rz ja Rxz moodustatud täisnurksest kolmnurgast võime tuletada, et:

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

Atan2 võib olla funktsioon, mida te pole kunagi varem kasutanud, see on sarnane ataniga, välja arvatud juhul, kui see tagastab väärtused vahemikus (-PI, PI), mitte (-PI/2, PI/2), nagu tagastab atan, ja see võtab aega 2 argumenti ühe asemel. See võimaldab meil teisendada kaks väärtust Rx, Rz nurkadeks 360 kraadi ulatuses (-PI kuni PI). Atan2 kohta saate rohkem lugeda siit.

Seega teades RxEst (n-1) ja RzEst (n-1), võime leida:

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

Pidage meeles, et güroskoop mõõdab Axzi nurga muutumise kiirust. Seega saame uut nurka Axz (n) hinnata järgmiselt:

Axz (n) = Axz (n-1) + määrAxz (n) * T.

Pidage meeles, et RateAxzi saab meie güroskoobi ADC näitudest. Täpsem valem võib kasutada järgmist pöörlemiskiirust, mis arvutatakse järgmiselt:

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

Samamoodi võime leida:

Ayz (n) = Ayz (n-1) + määrAyz (n) * T.

Ok, nüüd on meil Axz (n) ja Ayz (n). Kuhu me siit läheme, et RxGyro/RyGyro maha arvata? Alates ekv. 1 saame vektori Rgyro pikkuse kirjutada järgmiselt:

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

Ka seetõttu, et normaliseerisime oma Racc -vektori, võime eeldada, et selle pikkus on 1 ja see pole pärast pöörlemist muutunud, seega on suhteliselt ohutu kirjutada:

| Rgyro | = 1

Võtame järgmiste arvutuste jaoks ajutise lühema märke:

x = RxGyro, y = RyGyro, z = RzGyro

Ülaltoodud seoseid kasutades saame kirjutada:

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

Jagame murru lugeja ja nimetaja SQRT -ga (x^2 + z^2)

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

Pange tähele, et x / SQRT (x^2 + z^2) = sin (Axz), seega:

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

Nüüd korrutage SQRT -s oleva murru lugeja ja nimetaja z^2 -ga

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

Pange tähele, et z / SQRT (x^2 + z^2) = cos (Axz) ja y / z = tan (Ayz), nii et lõpuks:

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

Tulles tagasi meie märkimise juurde, saame:

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

samamoodi leiame selle

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

Nüüd võime lõpuks leida:

RzGyro = Märk (RzGyro)*SQRT (1 - RxGyro^2 - RyGyro^2).

Kus märk (RzGyro) = 1, kui RzGyro> = 0, ja märk (RzGyro) = -1, kui RzGyro <0.

Üks lihtne viis selle hindamiseks on võtta:

Märk (RzGyro) = Märk (RzEst (n-1))

Praktikas olge ettevaatlik, kui RzEst (n-1) on nullilähedane. Sellisel juhul võite güroskoopifaasi üldse vahele jätta ja määrata: Rgyro = Puhkus (n-1). Rz kasutatakse võrdlusalusena Axz ja Ayz nurkade arvutamisel ning kui see on 0 lähedal, võivad väärtused üle voolata ja vallandada halbu tulemusi. Olete suurte ujukomaarvude pärusmaa, kus tan () / atan () funktsiooni rakendused ei pruugi olla täpsed.

Võtame siis kokku selle, mis meil siiani on, oleme oma algoritmi n -n etapil ja oleme arvutanud järgmised väärtused:

Racc - meie kiirendusmõõturi Rgyro praegused näidud - saadud puhkehetkest (n -1) ja praegustest güroskoopinäitudest

Milliseid väärtusi kasutame uuendatud hinnangu Rest (n) arvutamiseks? Arvatavasti arvasite, et kasutame mõlemat. Kasutame kaalutud keskmist, et:

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

Seda valemit saame lihtsustada, jagades murdosa lugeja ja nimetaja w1 -ga.

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

ja pärast w2/w1 = wGyro asendamist saame:

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

Ülaltoodud foorumis ütleb wGyro meile, kui palju me oma kiirendusmõõturiga võrreldes oma güroskoopi usaldame. Selle väärtuse saab valida eksperimentaalselt, tavaliselt väärtused vahemikus 5–20 vallandavad häid tulemusi.

Selle algoritmi peamine erinevus Kalmani filtrist on see, et see kaal on suhteliselt fikseeritud, samas kui Kalmani filtris värskendatakse kaalusid pidevalt, lähtudes kiirendusmõõturi näitude mõõdetud mürast. Kalmani filter on suunatud teile "parimate" teoreetiliste tulemuste andmisele, samas kui see algoritm võib anda teile praktilise rakenduse jaoks piisavalt häid tulemusi. Saate rakendada algoritmi, mis reguleerib wGyro sõltuvalt mõnest mõõdetavast mürategurist, kuid püsiväärtused sobivad enamiku rakenduste jaoks hästi.

Oleme ühe sammu kaugusel uuendatud hinnanguliste väärtuste saamisest:

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

Nüüd normaliseerime selle vektori uuesti:

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

Ja me oleme valmis oma tsüklit uuesti kordama.

See juhend ilmus algselt saidil starlino.com, olen teinud mõningaid kergeid muudatusi ja loaga uuesti postitanud. Aitäh Starlino!