Akselerometra un žiroskopu apmācība: 3 soļi

2024 Autors: John Day | [email protected]. Pēdējoreiz modificēts: 2024-01-30 11:00

Ievads

Šī rokasgrāmata ir paredzēta ikvienam, kuru interesē elektronikas projektos izmantot akselerometrus un žiroskopus, kā arī kombinētās IMU ierīces (inerciālās mērīšanas vienība)

Mēs aptversim:

• Ko mēra akselerometrs?
• Ko mēra žiroskops (pazīstams arī kā žiroskops)?
• Kā pārvērst no šī sensora iegūtos analogo ciparu (ADC) rādījumus fiziskās vienībās (tie būtu g akselerometram, deg/s žiroskopam)
• Kā apvienot akselerometra un žiroskopa rādījumus, lai iegūtu precīzu informāciju par ierīces slīpumu attiecībā pret iezemēto plakni

Visā rakstā es centīšos samazināt matemātiku līdz minimumam. Ja jūs zināt, kas ir Sine/Cosine/Tangent, tad jums vajadzētu būt iespējai saprast un izmantot šīs idejas savā projektā neatkarīgi no izmantotās platformas: Arduino, Propeller, Basic Stamp, Atmel mikroshēmas, Microchip PIC utt.

Ir cilvēki, kuri uzskata, ka jums ir nepieciešama sarežģīta matemātika, lai izmantotu IMU vienību (sarežģīti FIR vai IIR filtri, piemēram, Kalmana filtri, Parks-McClellan filtri utt.). Jūs varat izpētīt visus šos un sasniegt brīnišķīgus, bet sarežģītus rezultātus. Mans veids, kā izskaidrot lietas, prasa tikai pamata matemātiku. Es ļoti ticu vienkāršībai. Es domāju, ka vienkāršu sistēmu ir vieglāk kontrolēt un uzraudzīt, turklāt daudzām iegultām ierīcēm nav jaudas un resursu, lai ieviestu sarežģītus algoritmus, kuriem nepieciešami matricas aprēķini.

Kā piemēru es izmantošu jaunu IMU vienību - Acc_Gyro akselerometru + žiroskopu IMU. Tālāk sniegtajos piemēros mēs izmantosim šīs ierīces parametrus. Sākumā šī ierīce ir laba ierīce, jo tā sastāv no 2 ierīcēm:

- LIS331AL (datu lapa) - triaksiāls 2G akselerometrs - LPR550AL (datu lapa) - divu asu solis un rullis, 500 gr/s žiroskops

Kopā tie ir 5 brīvības pakāpes inerciālās mērīšanas vienība. Tagad tas ir izdomāts vārds! Tomēr aiz iedomātā nosaukuma ir ļoti noderīga kombinācijas ierīce, kuru mēs šajā rokasgrāmatā apskatīsim un detalizēti paskaidrosim.

1. darbība: akselerometrs

Lai saprastu šo vienību, mēs sāksim ar akselerometru. Domājot par akselerometriem, bieži ir lietderīgi attēlot lodziņu kuba formā ar bumbiņu tās iekšpusē. Jūs varat iedomāties kaut ko citu, piemēram, cepumu vai virtuli, bet es iedomājos bumbu:

Ja mēs šo kastīti novietojam vietā, kur nav gravitācijas lauku, vai arī bez citiem laukiem, kas varētu ietekmēt bumbas stāvokli - bumba vienkārši peldēs lodziņa vidū. Jūs varat iedomāties, ka kaste atrodas kosmosā tālu prom no visiem kosmiskajiem ķermeņiem, vai, ja šādu vietu ir grūti atrast, iedomājieties vismaz kosmosa kuģi, kas riņķo ap planētu, kur viss atrodas bezsvara stāvoklī. No iepriekš redzamā attēla var redzēt, ka katrai asij mēs piešķiram sienu pāri (mēs noņēmām sienu Y+, lai mēs varētu skatīties kastes iekšpusē). Iedomājieties, ka katra siena ir jutīga pret spiedienu. Ja pēkšņi pārvietosim kastīti pa kreisi (paātrināsim to ar paātrinājumu 1g = 9,8m/s^2), bumba atsitīsies pret sienu X-. Pēc tam mēs izmērām spiediena spēku, ko bumba pieliek sienai, un izvadām vērtību -1 g uz X ass.

Lūdzu, ņemiet vērā, ka akselerometrs faktiski noteiks spēku, kas ir vērsts pretējā virzienā no paātrinājuma vektora. Šo spēku bieži sauc par inerciālu spēku vai fiktīvu spēku. Viena lieta, kas jums jāiemācās, ir tas, ka akselerometrs paātrinājumu mēra netieši, izmantojot spēku, kas tiek pielietots vienai no tā sienām (saskaņā ar mūsu modeli tas var būt atspere vai kaut kas cits reālās dzīves akselerometros). Šo spēku var izraisīt paātrinājums, bet, kā redzēsim nākamajā piemērā, to ne vienmēr izraisa paātrinājums.

Ja mēs ņemam savu modeli un novietojam to uz Zemes, bumba nokrīt uz Z sienas un uzliek 1 g spēku uz apakšējo sienu, kā parādīts attēlā:

Šajā gadījumā kārba nepārvietojas, bet mēs joprojām iegūstam -1 g rādījumu uz Z ass. Spiedienu, ko bumba ir izdarījusi uz sienas, izraisīja gravitācijas spēks. Teorētiski tas varētu būt cita veida spēks - piemēram, ja iedomājaties, ka mūsu bumba ir metāliska, novietojot magnētu pie kastes, tā varētu pārvietoties, lai tā atsistu pret citu sienu. Tas tika teikts tikai, lai pierādītu, ka būtībā akselerometrs mēra spēku, nevis paātrinājumu. Vienkārši gadās, ka paātrinājums izraisa inerces spēku, ko uztver akselerometra spēka noteikšanas mehānisms.

Lai gan šis modelis nav tieši tāds, kā tiek uzbūvēts MEMS sensors, tas bieži vien ir noderīgs, lai atrisinātu ar akselerometru saistītas problēmas. Patiesībā ir līdzīgi sensori, kuru iekšpusē ir metāla lodītes, tos sauc par slīpuma slēdžiem, tomēr tie ir primitīvāki un parasti tie var noteikt tikai to, vai ierīce ir slīpi noteiktā diapazonā vai nē, nevis slīpuma pakāpi.

Līdz šim mēs esam analizējuši akselerometra izeju uz vienas ass, un tas ir viss, ko jūs iegūsit ar vienas ass akselerometriem. Triaksiālo akselerometru patiesā vērtība rodas no tā, ka tie var noteikt inerces spēkus uz visām trim asīm. Atgriezīsimies pie mūsu kastes modeļa un pagriezīsim lodziņu par 45 grādiem pa labi. Bumba tagad pieskarsies 2 sienām: Z- un X-, kā parādīts attēlā:

Vērtības 0,71 nav patvaļīgas, tās faktiski ir aptuvenas SQRT (1/2). Tas kļūs skaidrāks, iepazīstinot ar mūsu nākamo akselerometra modeli.

Iepriekšējā modelī mēs esam fiksējuši gravitācijas spēku un pagriezuši mūsu iedomāto kasti. Pēdējos 2 piemēros mēs esam analizējuši izvadi 2 dažādās kastes pozīcijās, bet spēka vektors palika nemainīgs. Lai gan tas bija noderīgi, lai saprastu, kā akselerometrs mijiedarbojas ar ārējiem spēkiem, praktiskāk ir veikt aprēķinus, ja mēs piestiprinām koordinātu sistēmu pie akselerometra asīm un iedomājamies, ka spēka vektors griežas ap mums.

Lūdzu, apskatiet iepriekš minēto modeli, es saglabāju cirvju krāsas, lai jūs varētu veikt garīgu pāreju no iepriekšējā modeļa uz jauno. Iedomājieties, ka katra jaunā modeļa ass ir perpendikulāra iepriekšējā modeļa lodziņa attiecīgajām virsmām. Vektors R ir spēka vektors, ko mēra akselerometrs (tas var būt vai nu gravitācijas spēks, vai inerces spēks no iepriekš minētajiem piemēriem, vai abu kombinācija). Rx, Ry, Rz ir R vektora projekcija uz X, Y, Z asīm. Lūdzu, ievērojiet šādu saistību:

R^2 = Rx^2 + Ry^2 + Rz^2 (1. vienādojums)

kas būtībā ir ekvivalents Pitagora teorēmai 3D formātā.

Atcerieties, ka mazliet agrāk es jums teicu, ka SQRT (1/2) ~ 0,71 vērtības nav nejaušas. Ja jūs pievienojat tos iepriekšminētajai formulai, pēc atgādināšanas, ka mūsu gravitācijas spēks bija 1 g, mēs varam pārliecināties, ka:

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

vienkārši aizstājot R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) Eq.1

Pēc ilgas teorijas preambulas mēs tuvojamies reālās dzīves akselerometriem. Vērtības Rx, Ry, Rz patiesībā ir lineāri saistītas ar vērtībām, ko izdos jūsu reālās dzīves akselerometrs un kuras varat izmantot dažādu aprēķinu veikšanai.

Pirms mēs tur nokļūsim, nedaudz parunāsim par to, kā akselerometri mums sniegs šo informāciju. Lielākā daļa akselerometru ietilpst divās kategorijās: digitālie un analogie. Digitālie akselerometri sniegs jums informāciju, izmantojot seriālo protokolu, piemēram, I2C, SPI vai USART, savukārt analogie akselerometri izvadīs sprieguma līmeni iepriekš noteiktā diapazonā, kas jums jāpārvērš par ciparu vērtību, izmantojot ADC (analogo ciparu pārveidotāju) moduli. Es neiedziļināšos daudzās detaļās par to, kā darbojas ADC, daļēji tāpēc, ka tā ir tik plaša tēma, un daļēji tāpēc, ka tā dažādās platformās atšķiras. Dažiem mikrokontrolleriem būs iebūvēti ADC moduļi, dažiem no tiem būs nepieciešami ārēji komponenti, lai veiktu ADC konversijas. Neatkarīgi no izmantotā ADC moduļa veida jūs iegūsit vērtību noteiktā diapazonā. Piemēram, 10 bitu ADC modulis izvadīs vērtību diapazonā no 0..1023, ņemiet vērā, ka 1023 = 2^10 -1. 12 bitu ADC modulis izvadīs vērtību diapazonā no 0..4095, ņemiet vērā, ka 4095 = 2^12-1.

Turpināsim, apsverot vienkāršu piemēru, pieņemsim, ka mūsu 10 bitu ADC modulis mums sniedza šādas trīs akselerometra kanālu (asu) vērtības:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Katram ADC modulim būs atsauces spriegums, pieņemsim, ka mūsu piemērā tas ir 3.3V. Lai pārvērstu 10 bitu adc vērtību spriegumā, mēs izmantojam šādu formulu:

VoltsRx = AdcRx * Vref / 1023

Ātra piezīme: 8 bitu ADC pēdējais dalītājs būtu 255 = 2 ^ 8 -1, bet 12 bitu ADC pēdējais dalītājs būtu 4095 = 2 ^ 12 -1.

Piemērojot šo formulu visiem 3 kanāliem, mēs iegūstam:

VoltiRx = 586 * 3.3V / 1023 = ~ 1.89V (visus rezultātus noapaļojam līdz 2 zīmēm aiz komata) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V voltiRz = 561 * 3.3V / 1023 = ~ 1.81V

Katram akselerometram ir nulles g sprieguma līmenis, to var atrast specifikācijās, tas ir spriegums, kas atbilst 0 g. Lai iegūtu parakstītu sprieguma vērtību, mums jāaprēķina nobīde no šī līmeņa. Pieņemsim, ka mūsu 0g sprieguma līmenis ir VzeroG = 1,65V. Mēs aprēķinām sprieguma nobīdi no nulles g sprieguma šādi:

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

Tagad mūsu akselerometra rādījumi ir volti, tas joprojām nav g (9,8 m/s^2), lai veiktu galīgo konversiju, mēs izmantojam akselerometra jutību, parasti izteiktu mV/g. Pieņemsim, ka mūsu jutība = 478,5 mV/g = 0,4785V/g. Jutīguma vērtības var atrast akselerometra specifikācijās. Lai iegūtu galīgās spēka vērtības, kas izteiktas g, mēs izmantojam šādu formulu:

Rx = DeltaVoltsRx / jutība

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

Mēs, protams, varētu apvienot visus soļus vienā formulā, bet es veicu visas darbības, lai būtu skaidrs, kā jūs pārejat no ADC rādījumiem uz spēka vektora komponentu, kas izteikts g.

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

Tagad mums ir visas 3 sastāvdaļas, kas nosaka mūsu inerces spēka vektoru, ja ierīce nav pakļauta citiem spēkiem, izņemot gravitāciju, mēs varam pieņemt, ka tas ir mūsu gravitācijas spēka vektora virziens. Ja vēlaties aprēķināt ierīces slīpumu attiecībā pret zemi, varat aprēķināt leņķi starp šo vektoru un Z asi. Ja jūs interesē arī slīpuma virziens pa asi, varat sadalīt šo rezultātu divās sastāvdaļās: slīpums uz X un Y ass, ko var aprēķināt kā leņķi starp gravitācijas vektoru un X / Y asīm. Šo leņķu aprēķināšana ir vienkāršāka, nekā jūs varētu domāt, tagad, kad esam aprēķinājuši Rx, Ry un Rz vērtības. Atgriezīsimies pie mūsu pēdējā akselerometra modeļa un izdarīsim dažus papildu pierakstus:

Mūs interesējošie leņķi ir leņķi starp X, Y, Z asīm un spēka vektoru R. Mēs definēsim šos leņķus kā Axr, Ayr, Azr. No taisnleņķa trīsstūra, ko veido R un Rx, varat pamanīt, ka:

cos (Axr) = Rx / R un līdzīgi: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Mēs varam no Eq.1 atskaitīt, ka R = SQRT (Rx^2 + Ry^2 + Rz^2).

Tagad mēs varam atrast savus leņķus, izmantojot funkciju arccos () (apgrieztā cos () funkcija):

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

Mēs esam gājuši garu ceļu, lai izskaidrotu akselerometra modeli, lai nonāktu pie šīm formulām. Atkarībā no lietojumprogrammām, iespējams, vēlēsities izmantot visas mūsu iegūtās starpposma formulas. Drīz mēs arī iepazīstināsim ar žiroskopa modeli, un redzēsim, kā akselerometra un žiroskopa datus var apvienot, lai nodrošinātu vēl precīzākus slīpuma aprēķinus.

Bet pirms mēs to darīsim, darīsim dažus noderīgākus apzīmējumus:

cosX = cos (Axr) = Rx / R mājīgs = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Šo tripletu bieži sauc par virziena kosinusu, un tas būtībā attēlo vienības vektoru (vektors ar garumu 1), kuram ir tāds pats virziens kā mūsu R vektoram. Jūs varat viegli pārbaudīt, vai:

SQRT (cosX^2 + mājīgs^2 + cosZ^2) = 1

Tas ir jauks īpašums, jo tas atbrīvo mūs no R vektora moduļa (garuma) uzraudzības. Bieži vien, ja mūs interesē tikai mūsu inerces vektora virziens, ir jēga normalizēt tā moduli, lai vienkāršotu citus aprēķinus.

2. solis: žiroskops

Mēs neieviesīsim žiroskopam līdzvērtīgu kārbas modeli, kā mēs to darījām akselerometram, tā vietā mēs pāriesim tieši uz otro akselerometra modeli un parādīsim, ko žiroskops mēra saskaņā ar šo modeli.

Katrs žiroskopa kanāls mēra rotāciju ap vienu no asīm. Piemēram, divu asu žiroskops mērīs rotāciju ap (un daži var teikt "par") X un Y asīm. Lai izteiktu šo rotāciju skaitļos, izdarīsim dažus apzīmējumus. Vispirms definēsim:

Rxz - ir inerces spēka vektora R projekcija XZ plaknē Ryz - ir inerces spēka vektora R projekcija YZ plaknē

No taisnleņķa trīsstūra, ko veido Rxz un Rz, izmantojot Pitagora teorēmu, iegūstam:

Rxz^2 = Rx^2 + Rz^2 un līdzīgi: Ryz^2 = Ry^2 + Rz^2

ņemiet vērā arī to, ka:

R^2 = Rxz^2 + Ry^2, to var iegūt no vienādojuma 1 un augstākiem vienādojumiem, vai arī to var iegūt no taisnleņķa trīsstūra, ko veido R un Ryz R^2 = Ryz^2 + Rx^2

Mēs šajā rakstā neizmantosim šīs formulas, taču ir lietderīgi atzīmēt saikni starp visām mūsu modeļa vērtībām.

Tā vietā mēs definēsim leņķi starp Z asi un Rxz, Ryz vektoriem šādi:

Axz - ir leņķis starp Rxz (R projekcija XZ plaknē) un Z asi Ayz - ir leņķis starp Ryz (R projekcija YZ plaknē) un Z asi

Tagad mēs tuvojamies tam, ko mēra žiroskops. Žiroskops mēra iepriekš definēto leņķu izmaiņu ātrumu. Citiem vārdiem sakot, tiks parādīta vērtība, kas ir lineāri saistīta ar šo leņķu izmaiņu ātrumu. Lai to izskaidrotu, pieņemsim, ka mēs esam izmērījuši rotācijas leņķi ap asi Y (tas būtu Axz leņķis) laikā t0, un mēs to definējam kā Axz0, pēc tam mēs izmērījām šo leņķi vēlāk t1 un tas bija Axz1. Izmaiņu likme tiks aprēķināta šādi:

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

Ja mēs izteiksim Axz grādos un laiku sekundēs, tad šī vērtība tiks izteikta grādos/s. Tas ir tas, ko mēra žiroskops.

Praksē žiroskops (ja vien tas nav īpašs digitālais žiroskops) reti sniegs jums vērtību, kas izteikta grādos/s. Tāpat kā akselerometram, jūs iegūsit ADC vērtību, kas jums būs jāpārvērš par deg/s, izmantojot formulu, kas līdzīga vienādojumam. 2, ko esam definējuši akselerometram. Iepazīstināsim ADC ar deg/s konversijas formulu žiroskopam (mēs pieņemam, ka mēs izmantojam 10 bitu ADC moduli, 8 bitu ADC vietā 1023 aizstājiet ar 255, 12 bitu ADC aizstājiet 1023 ar 4095).

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

AdcGyroXZ, AdcGyroYZ - tiek iegūti no mūsu adc moduļa, un tie attēlo kanālus, kas mēra R vektora projekcijas rotāciju attiecīgi XZ YZ plaknēs, kas ir līdzvērtīgi sakot, ka rotācija tika veikta attiecīgi ap Y un X asīm.

Vref - vai ADC atskaites spriegums, ko izmantosim 3.3V zemāk redzamajā piemērā VzeroRate - ir nulles ātruma spriegums, citiem vārdiem sakot, spriegums, ko žiroskops izvada, kad tas nav pakļauts nekādai rotācijai, Acc_Gyro plāksnei tas ir piemēram, 1.23V (šīs vērtības varat atrast specifikācijās) žiroskopa izeja palielinās, ja rotācijas ātrumu palielināsiet par vienu deg/s. Piemēram, Acc_Gyro plāksnes jutība ir 2 mV/deg/s vai 0,002 V/deg/s

Ņemsim piemēru, pieņemsim, ka mūsu ADC modulis ir atgriezis šādas vērtības:

AdcGyroXZ = 571 AdcGyroXZ = 323

Izmantojot iepriekš minēto formulu un izmantojot Acc_Gyro plates specifikācijas parametrus, mēs iegūsim:

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

Citiem vārdiem sakot, ierīce griežas ap Y asi (vai mēs varam teikt, ka tā rotē XZ plaknē) ar ātrumu 306 gr/s un ap X asi (vai mēs varam teikt, ka tā rotē YZ plaknē) ar ātrumu - 94 grādi/s. Lūdzu, ņemiet vērā, ka negatīvā zīme nozīmē, ka ierīce griežas pretēji parastajam pozitīvajam virzienam. Pēc vienošanās viens rotācijas virziens ir pozitīvs. Laba žiroskopa specifikācijas lapa parādīs, kurš virziens ir pozitīvs, pretējā gadījumā jums tas būs jāatrod, eksperimentējot ar ierīci un atzīmējot, kurš griešanās virziens palielina spriegumu izejas tapā. To vislabāk var izdarīt, izmantojot osciloskopu, jo, tiklīdz jūs pārtraucat rotāciju, spriegums samazināsies līdz nulles likmes līmenim. Ja izmantojat multimetru, jums vismaz dažas sekundes jāuztur nemainīgs rotācijas ātrums un jāatzīmē spriegums šīs rotācijas laikā, pēc tam salīdziniet to ar nulles ātruma spriegumu. Ja tas ir lielāks par nulles ātruma spriegumu, tas nozīmē, ka rotācijas virziens ir pozitīvs.

3. darbība: akselerometra un žiroskopu apvienošana

Visu saliekot kopā - apvienojot akselerometru un žiroskopa datus

Ja lasāt šo rakstu, jūs, iespējams, iegādājāties vai plānojat iegādāties IMU ierīci, vai, iespējams, plānojat to izveidot no atsevišķām akselerometra un žiroskopa ierīcēm.

Pirmais solis kombinētās IMU ierīces izmantošanā, kas apvieno akselerometru un žiroskopu, ir to koordinātu sistēmu izlīdzināšana. Vienkāršākais veids, kā to izdarīt, ir izvēlēties akselerometra koordinātu sistēmu kā atsauces koordinātu sistēmu. Lielākajā daļā akselerometra datu lapu tiks parādīts X, Y, Z asu virziens attiecībā pret fiziskās mikroshēmas vai ierīces attēlu. Piemēram, šeit ir X, Y, Z asu virzieni, kā parādīts Acc_Gyro plates specifikācijās:

Nākamās darbības ir šādas:

Identificējiet žiroskopa izejas, kas atbilst iepriekš aplūkotajām RateAxz, RateAyz vērtībām. Nosakiet, vai šīs izejas ir jāapgriež otrādi, ņemot vērā žiroskopa fizisko stāvokli attiecībā pret akselerometru

Neuzņemieties, ka, ja žiroskopam ir izeja, kas apzīmēta ar X vai Y, tas atbilst jebkurai akselerometra koordinātu sistēmas asij, pat ja šī izeja ir daļa no IMU vienības. Labākais veids ir to pārbaudīt. Pieņemot, ka esat fiksējis žiroskopa stāvokli attiecībā pret akselerometru. Tiek pieņemts, ka žiroskopa un akselerometra apmales ir paralēlas viena otrai, t.i., jūs novietojat žiroskopu 90 grādu leņķī attiecībā pret akselerometra mikroshēmu. Ja esat iegādājies IMU plati, iespējams, ka tā jau ir izlīdzināta šādā veidā. Šajā rakstā mēs nerunāsim par modeļiem, kuros žiroskops ir novietots neregulārā leņķī attiecībā pret akselerometru (teiksim, 45 vai 30 grādi), lai gan tas var būt noderīgi dažos lietojumos.

Šeit ir parauga secība, lai noteiktu, kura žiroskopa izeja atbilst iepriekš apspriestajai RateAxz vērtībai.

- sāciet no ierīces novietošanas horizontālā stāvoklī. Akselerometra X un Y izejas izvadīs nulles g spriegumu (piemēram, Acc_Gyro plates gadījumā tas ir 1,65 V)

- pēc tam sāciet griezt ierīci ap Y asi, vēl viens veids, kā to pateikt, ir tas, ka jūs pagriežat ierīci XZ plaknē, lai X un Z akselerometra izejas mainītos un Y izeja paliktu nemainīga. - rotējot ierīci ar nemainīgu ātrumu, atzīmējot žiroskopa izejas izmaiņas, pārējām žiroskopa izejām jāpaliek nemainīgām - žiroskopa izeja, kas mainījās rotācijas laikā ap Y asi (rotācija XZ plaknē), nodrošinās AdcGyroXZ ievades vērtību, no kuras mēs aprēķinām RateAxz - pēdējais solis ir nodrošināt rotācijas virziena atbilstību mūsu modelim, dažos gadījumos jums var nākties apgriezt RateAxz vērtību žiroskopa fiziskā stāvokļa dēļ attiecībā pret akselerometru - vēlreiz veiciet iepriekš minēto testu, pagriežot ierīci apkārt Y ass, šoreiz uzraugiet akselerometra X izeju (mūsu modelī AdcRx). Ja AdcRx aug (pirmie 90 rotācijas grādi no horizontālā stāvokļa), tad arī AdcGyroXZ vajadzētu augt. Pretējā gadījumā jums ir jāpārvērš RateAxz, to varat panākt, ieviešot zīmes koeficientu Eq.3 šādi:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Jutība, kur InvertAxz ir 1 vai -1

to pašu testa spieķi var veikt RateAyz, pagriežot ierīci ap X asi, un jūs varat noteikt, kura žiroskopa izeja atbilst RateAyz, un vai tā ir jāapgriež. Kad esat ieguvis InvertAyz vērtību, RateAyz aprēķināšanai izmantojiet šādu formulu:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Jutība

Ja jūs veiktu šos testus uz Acc_Gyro plates, jūs iegūtu šādus rezultātus:

- RateAxz izejas tapa ir GX4 un InvertAxz = -1. - RateAyz izejas tapa ir GY4 un InvertAyz = -1

No šī brīža mēs uzskatīsim, ka esat iestatījis savu IMU tā, lai jūs varētu aprēķināt pareizās vērtības Axr, Ayr, Azr (kā noteikts 1. daļā. Akselerometrs) un RateAxz, RateAyz (kā noteikts 2. daļā).). Tālāk mēs analizēsim attiecības starp šīm vērtībām, kas izrādās noderīgas, lai iegūtu precīzāku ierīces slīpuma novērtējumu attiecībā pret iezemēto plakni.

Šajā brīdī jūs varētu sev jautāt, ja akselerometra modelis mums jau ir devis Axr, Ayr, Azr slīpuma leņķus, kāpēc mēs gribētu uztraukties ar žiroskopa datiem? Atbilde ir vienkārša: akselerometra datiem ne vienmēr var uzticēties 100%. Ir vairāki iemesli, atcerieties, ka akselerometrs mēra inerces spēku, šādu spēku var izraisīt gravitācija (un ideālā gadījumā tikai gravitācija), bet to var izraisīt arī ierīces paātrinājums (kustība). Tā rezultātā, pat ja akselerometrs ir salīdzinoši stabilā stāvoklī, tas joprojām ir ļoti jutīgs pret vibrāciju un mehānisko troksni kopumā. Tas ir galvenais iemesls, kāpēc lielākā daļa IMU sistēmu izmanto žiroskopu, lai izlīdzinātu visas akselerometra kļūdas. Bet kā tas tiek darīts? Un vai žiroskopā nav trokšņa?

Žiroskopā nav trokšņu, taču, tā kā tas mēra rotāciju, tas ir mazāk jutīgs pret lineārām mehāniskām kustībām, trokšņa veidu, no kura cieš akselerometrs, tomēr žiroskopiem ir cita veida problēmas, piemēram, dreifs (neatgriežas pie nulles likmes vērtības) kad rotācija apstājas). Tomēr, vidēji aprēķinot datus, kas iegūti no akselerometra un žiroskopa, mēs varam iegūt salīdzinoši labāku pašreizējās ierīces slīpuma novērtējumu, nekā mēs iegūtu, izmantojot tikai akselerometra datus.

Nākamajos soļos es iepazīstināšu ar algoritmu, kuru iedvesmoja dažas idejas, kas izmantotas Kalmana filtrā, tomēr tas ir daudz vienkāršāk un vieglāk īstenojams iegultās ierīcēs. Pirms tam vispirms apskatīsim, ko mēs vēlamies aprēķināt mūsu algoritmam. Tas ir gravitācijas spēka vektora virziens R = [Rx, Ry, Rz], no kura mēs varam iegūt citas vērtības, piemēram, Axr, Ayr, Azr vai cosX, mājīgs, cosZ, kas mums sniegs priekšstatu par mūsu ierīces slīpumu attiecībā pret iezemēto plakni, mēs apspriežam attiecību starp šīm vērtībām 1. daļā. Varētu teikt - vai mums jau nav šīs vērtības Rx, Ry, Rz no Eq.2 1. daļā? Jā, bet atcerieties, ka šīs vērtības ir iegūtas tikai no akselerometra datiem, tādēļ, ja jūs tās izmantosit tieši savā lietojumprogrammā, iespējams, radīsit vairāk trokšņa, nekā jūsu lietojumprogramma var izturēt. Lai izvairītos no turpmākas neskaidrības, vēlreiz definēsim akselerometra mērījumus šādi:

Racc - inerces spēka vektors, ko mēra ar akselerometru, kas sastāv no šādiem komponentiem (izvirzījumi uz X, Y, Z asīm):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Jutība RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Jutība RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Jutība

Līdz šim mums ir izmērīto vērtību kopums, ko varam iegūt tikai no akselerometra ADC vērtībām. Mēs šo datu kopu sauksim par “vektoru” un izmantosim šādu apzīmējumu.

Racc = [RxAcc, RyAcc, RzAcc]

Tā kā šīs Racc sastāvdaļas var iegūt no akselerometra datiem, mēs to varam uzskatīt par mūsu algoritma ievadi.

Lūdzu, ņemiet vērā: tā kā Racc mēra gravitācijas spēku, jums būs taisnība, ja pieņemsiet, ka šī vektora garums, kas definēts šādi, ir vienāds vai tuvu 1 g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Tomēr, lai būtu pārliecināts, ka ir lietderīgi atjaunināt šo vektoru šādi:

Racc (normalizēts) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

Tas nodrošinās, ka jūsu normalizētā Racc vektora garums vienmēr ir 1.

Tālāk mēs ieviesīsim jaunu vektoru, un mēs to sauksim

Atpūta = [RxEst, RyEst, RzEst]

Tas būs mūsu algoritma rezultāts, tās ir koriģētas vērtības, kuru pamatā ir žiroskopa dati un iepriekšējie aprēķinātie dati.

Lūk, ko darīs mūsu algoritms: - akselerometrs mums saka: "Jūs tagad atrodaties Racc pozīcijā" - mēs sakām "paldies, bet ļaujiet man pārbaudīt", - pēc tam labojiet šo informāciju ar žiroskopa datiem, kā arī ar iepriekšējiem atpūtas datiem un mēs izvadām jaunu aprēķināto vektoru Rest. - mēs uzskatām atpūtu par mūsu "labāko variantu" attiecībā uz ierīces pašreizējo stāvokli.

Apskatīsim, kā mēs varam panākt, lai tas darbotos.

Mēs sāksim savu secību, uzticoties savam akselerometram un piešķirot:

Atpūta (0) = Racc (0)

Starp citu, atcerieties, ka Rest un Racc ir vektori, tāpēc iepriekš minētais vienādojums ir vienkāršs veids, kā uzrakstīt 3 vienādojumu kopas un izvairīties no atkārtošanās:

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

Tālāk mēs veiksim regulārus mērījumus ar vienādiem laika intervāliem T sekundes, un mēs iegūsim jaunus mērījumus, kurus definēsim kā Racc (1), Racc (2), Racc (3) un tā tālāk. Mēs arī izdosim jaunus aprēķinus katrā laika intervālā Atpūta (1), Atpūta (2), Atpūta (3) un tā tālāk.

Pieņemsim, ka esam n. Mums ir zināmas divas vērtību kopas, kuras mēs vēlētos izmantot:

Atpūta (n -1) - mūsu iepriekšējais aprēķins, ar atpūtu (0) = Racc (0) Racc (n) - mūsu pašreizējais akselerometra mērījums

Pirms mēs varam aprēķināt atpūtu (n), ieviesīsim jaunu izmērīto vērtību, ko varam iegūt no sava žiroskopa un iepriekšējās aplēses.

Mēs to sauksim par Rgyro, un tas ir arī vektors, kas sastāv no 3 komponentiem:

Rgyro = [RxGyro, RyGyro, RzGyro]

Mēs aprēķināsim šo vektoru pa vienam komponentam. Mēs sāksim ar RxGyro.

Sāksim, novērojot šādu attiecību mūsu žiroskopa modelī, no taisnleņķa trijstūra, ko veido Rz un Rxz, mēs varam secināt, ka:

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

Atan2 varētu būt funkcija, kuru jūs nekad iepriekš neizmantojāt, tā ir līdzīga atan, izņemot to, ka atgriež vērtības diapazonā (-PI, PI) pretstatā (-PI/2, PI/2), ko atgriež atan, un tas prasa 2 argumenti viena vietā. Tas ļauj mums pārvērst divas Rx, Rz vērtības leņķos visā 360 grādu diapazonā (no -PI līdz PI). Vairāk par atan2 varat lasīt šeit.

Tātad, zinot RxEst (n-1) un RzEst (n-1), mēs varam atrast:

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

Atcerieties, ka žiroskops mēra Axz leņķa maiņas ātrumu. Tātad mēs varam novērtēt jauno leņķi Axz (n) šādi:

Axz (n) = Axz (n-1) + likmeAxz (n) * T.

Atcerieties, ka RateAxz var iegūt no mūsu žiroskopa ADC rādījumiem. Precīzākā formula var izmantot vidējo rotācijas ātrumu, kas aprēķināts šādi:

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

Tādā pašā veidā mēs varam atrast:

Ayz (n) = Ayz (n-1) + likmeAyz (n) * T.

Labi, tagad mums ir Axz (n) un Ayz (n). Kur mēs ejam, lai atskaitītu RxGyro/RyGyro? No ekv. 1 mēs varam uzrakstīt vektora Rgyro garumu šādi:

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

Arī tāpēc, ka mēs normalizējām savu Racc vektoru, mēs varam pieņemt, ka tā garums ir 1 un pēc rotācijas tas nav mainījies, tāpēc ir salīdzinoši droši rakstīt:

| Rgyro | = 1

Tālāk esošajiem aprēķiniem pieņemsim īsāku īsāku apzīmējumu:

x = RxGyro, y = RyGyro, z = RzGyro

Izmantojot iepriekš minētās attiecības, mēs varam uzrakstīt:

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

Sadalīsim frakcijas skaitītāju un saucēju ar SQRT (x^2 + z^2)

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

Ņemiet vērā, ka x / SQRT (x^2 + z^2) = sin (Axz), tātad:

x = grēks (Axz) / SQRT (1 + y^2 / (x^2 + z^2))

Tagad reiziniet SQRT iekšējās daļas skaitītāju un saucēju ar z^2

x = grēks (Axz) / SQRT (1 + y^2 * z^2 / (z^2 * (x^2 + z^2)))

Ņemiet vērā, ka z / SQRT (x^2 + z^2) = cos (Axz) un y / z = tan (Ayz), tāpēc beidzot:

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

Atgriežoties pie mūsu apzīmējuma, mēs iegūstam:

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

tāpat mēs to atrodam

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

Tagad beidzot varam atrast:

RzGyro = zīme (RzGyro)*SQRT (1 - RxGyro^2 - RyGyro^2).

Kur zīme (RzGyro) = 1, kad RzGyro> = 0, un zīme (RzGyro) = -1, kad RzGyro <0.

Viens vienkāršs veids, kā to novērtēt, ir:

Zīme (RzGyro) = zīme (RzEst (n-1))

Praksē esiet piesardzīgs, ja RzEst (n-1) ir tuvu 0. Šajā gadījumā jūs varat pilnībā izlaist žiroskopu un piešķirt: Rgyro = atpūta (n-1). Rz tiek izmantots kā atsauce, lai aprēķinātu Axz un Ayz leņķus, un, kad tas ir tuvu 0, vērtības var pārplūst un izraisīt sliktus rezultātus. Jūs atradīsit lielu peldošā komata skaitļu domēnu, kur iedeguma () / atan () funkciju ieviešanai var nebūt precizitātes.

Atgādināsim, kas mums ir līdz šim, mēs esam algoritma n solī un esam aprēķinājuši šādas vērtības:

Racc - pašreizējie rādījumi no mūsu akselerometra Rgyro - iegūti no atpūtas (n -1) un pašreizējiem žiroskopa rādījumiem

Kādas vērtības mēs izmantojam, lai aprēķinātu atjaunināto novērtējumu Atpūta (n)? Jūs droši vien uzminējāt, ka mēs izmantosim abus. Mēs izmantosim vidējo svērto vērtību, lai:

Atpūta (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Mēs varam vienkāršot šo formulu, dalot skaitļa skaitītāju un saucēju ar w1.

Atpūta (n) = (Racc * w1/w1 + Rgyro * w2/w1)/(w1/w1 + w2/w1)

un pēc aizstāšanas w2/w1 = wGyro mēs iegūstam:

Atpūta (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

Iepriekš minētajā forumā wGyro stāsta mums, cik ļoti mēs uzticamies savam žiroskopam, salīdzinot ar akselerometru. Šo vērtību var izvēlēties eksperimentāli, parasti vērtības starp 5..20 izraisīs labus rezultātus.

Šī algoritma galvenā atšķirība no Kalmana filtra ir tā, ka šis svars ir relatīvi fiksēts, turpretī Kalmana filtrā svari tiek pastāvīgi atjaunināti, pamatojoties uz akselerometra rādījumu izmērīto troksni. Kalmana filtrs ir vērsts uz to, lai sniegtu jums "labākos" teorētiskos rezultātus, turpretī šis algoritms var dot jums "pietiekami labus" rezultātus praktiskai lietošanai. Jūs varat ieviest algoritmu, kas pielāgo wGyro atkarībā no dažiem jūsu izmērītajiem trokšņa faktoriem, bet fiksētās vērtības labi darbosies lielākajā daļā lietojumprogrammu.

Mēs esam viena soļa attālumā no atjaunināto aprēķināto vērtību iegūšanas:

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

Tagad normalizēsim šo vektoru vēlreiz:

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

Un mēs esam gatavi vēlreiz atkārtot savu cilpu.

Šī rokasgrāmata sākotnēji tika parādīta vietnē starlino.com. Esmu nedaudz rediģējis un atkārtoti ievietojis to ar atļauju. Paldies Starlino!