Akselerometras ir giroskopo pamoka: 3 žingsniai

2024 Autorius: John Day | [email protected]. Paskutinį kartą keistas: 2024-01-30 10:51

Įvadas

Šis vadovas skirtas visiems, norintiems elektronikos projektuose naudoti akselerometrus ir giroskopus bei kombinuotus IMU prietaisus (inercinį matavimo įrenginį)

Mes padengsime:

• Ką matuoja akselerometras?
• Ką matuoja giroskopas (dar žinomas kaip giroskopas)?
• Kaip konvertuoti analoginius į skaitmeninius (ADC) rodmenis, gautus iš šių jutiklių, į fizinius vienetus (tai būtų g pagreičio matuokliui, deg/s giroskopui)
• Kaip sujungti akselerometro ir giroskopo rodmenis, kad gautumėte tikslią informaciją apie jūsų prietaiso polinkį įžeminimo plokštumos atžvilgiu

Visame straipsnyje aš stengsiuosi, kad matematika būtų kuo mažesnė. Jei žinote, kas yra „Sine“/„Cosine“/„Tangent“, turėtumėte sugebėti suprasti ir panaudoti šias idėjas savo projekte, nesvarbu, kokią platformą naudojate: „Arduino“, „Propeller“, „Basic Stamp“, „Atmel“lustai, „Microchip PIC“ir kt.

Yra žmonių, kurie mano, kad jums reikia sudėtingos matematikos, kad galėtumėte naudotis IMU įrenginiu (sudėtingi FIR arba IIR filtrai, tokie kaip Kalmano filtrai, „Parks-McClellan“filtrai ir kt.). Galite ištirti visus šiuos dalykus ir pasiekti nuostabių, bet sudėtingų rezultatų. Mano būdas paaiškinti dalykus reikalauja tik pagrindinės matematikos. Aš labai tikiu paprastumu. Manau, kad paprastą sistemą lengviau valdyti ir stebėti, be to, daugelis įterptųjų įrenginių neturi galios ir išteklių įgyvendinti sudėtingus algoritmus, kuriems reikia matricos skaičiavimų.

Kaip pavyzdį naudosiu naują IMU įrenginį - AccceGyro akselerometrą + Gyro IMU. Toliau pateiktuose pavyzdžiuose naudosime šio įrenginio parametrus. Šis įrenginys yra geras įrenginys, kurį reikia pradėti, nes jį sudaro 2 įrenginiai:

- LIS331AL (duomenų lapas) - triašis 2G pagreičio matuoklis - LPR550AL (duomenų lapas) - dviejų ašių žingsnis ir ritinys, 500 laipsnių/sek.

Kartu jie atstovauja 5 laipsnių laisvės inercinį matavimo vienetą. Dabar tai išgalvotas vardas! Nepaisant to, po išgalvotu pavadinimu yra labai naudingas kombinuotas įrenginys, kurį šiame vadove aptarsime ir išsamiai paaiškinsime.

1 žingsnis: akselerometras

Norėdami suprasti šį įrenginį, pradėkime nuo akselerometro. Galvojant apie akselerometrus, dažnai naudinga įsivaizduoti kubo formos dėžutę, kurios viduje yra rutulys. Galite įsivaizduoti ką nors panašaus į sausainį ar spurgą, bet aš įsivaizduoju rutulį:

Jei paimsime šią dėžę vietoje, kurioje nėra gravitacijos laukų, arba be kitų laukų, galinčių turėti įtakos rutulio padėčiai - kamuolys tiesiog plūdės dėžutės viduryje. Galite įsivaizduoti, kad dėžutė yra kosmose, toli nuo bet kokių kosminių kūnų, arba jei tokią vietą sunku rasti, įsivaizduokite bent kosminį laivą, skriejantį aplink planetą, kur viskas yra nesvarioje būsenoje. Iš aukščiau esančio paveikslėlio matote, kad kiekvienai ašiai priskiriame porą sienų (pašalinome sieną Y+, kad galėtume pažvelgti į dėžutės vidų). Įsivaizduokite, kad kiekviena siena yra jautri slėgiui. Jei staiga perkelsime dėžę į kairę (pagreitinsime ją pagreičiu 1g = 9,8 m/s^2), kamuolys atsitrenks į sieną X-. Tada mes išmatuojame slėgio jėgą, kurią rutulys daro sienai, ir X ašyje išleidžiame -1g reikšmę.

Atminkite, kad akselerometras iš tikrųjų aptiks jėgą, nukreiptą priešinga pagreičio vektoriaus kryptimi. Ši jėga dažnai vadinama inercine jėga arba fiktyvia jėga. Vienas dalykas, kurį turėtumėte pasimokyti, yra tas, kad akselerometras pagreitį matuoja netiesiogiai per jėgą, kuri yra taikoma vienai iš jo sienų (pagal mūsų modelį, tai gali būti spyruoklė ar kažkas kita realaus gyvenimo akselerometruose). Šią jėgą gali sukelti pagreitis, tačiau, kaip matysime kitame pavyzdyje, tai ne visada sukelia pagreitis.

Jei paimsime savo modelį ir padėsime jį ant Žemės, rutulys nukris ant Z sienos ir apatinei sienai pritaikys 1 g jėgą, kaip parodyta paveikslėlyje žemiau:

Šiuo atveju dėžutė nejuda, tačiau Z ašyje vis tiek gauname -1g rodmenį. Rutulio spaudimą sienai sukėlė gravitacijos jėga. Teoriškai tai gali būti kitokio tipo jėga - pavyzdžiui, jei įsivaizduojate, kad mūsų kamuolys yra metalinis, padėjus magnetą šalia dėžutės, kamuolys gali judėti taip, kad jis atsitrenktų į kitą sieną. Tai buvo pasakyta tik siekiant įrodyti, kad iš esmės akselerometras matuoja jėgą, o ne pagreitį. Tiesiog atsitinka taip, kad pagreitis sukelia inercinę jėgą, kurią užfiksuoja akselerometro jėgos aptikimo mechanizmas.

Nors šis modelis nėra toks, kaip sukonstruotas MEMS jutiklis, jis dažnai yra naudingas sprendžiant su akselerometru susijusias problemas. Iš tikrųjų yra panašių jutiklių, kurių viduje yra metaliniai rutuliai, jie vadinami pakreipimo jungikliais, tačiau jie yra primityvesni ir paprastai jie gali pasakyti tik tai, ar prietaisas yra pasviręs tam tikrame diapazone, ar ne, o ne nuolydžio mastas.

Iki šiol mes išanalizavome akselerometro išėjimą vienoje ašyje ir tai yra viskas, ką gausite naudodami vienos ašies akselerometrus. Tikroji triašių akselerometrų vertė kyla iš to, kad jie gali aptikti inercines jėgas visose trijose ašyse. Grįžkime prie savo dėžutės modelio ir pasukime dėžutę 45 laipsnių kampu į dešinę. Dabar kamuolys palies 2 sienas: Z- ir X-, kaip parodyta paveikslėlyje žemiau:

0,71 reikšmės nėra savavališkos, jos iš tikrųjų yra apytikslė SQRT (1/2). Tai paaiškės, kai pristatysime kitą mūsų akselerometro modelį.

Ankstesniame modelyje mes nustatėme gravitacijos jėgą ir pasukome savo įsivaizduojamą dėžutę. Paskutiniuose 2 pavyzdžiuose mes išanalizavome išvestį 2 skirtingose dėžutės padėtyse, o jėgos vektorius išliko pastovus. Nors tai buvo naudinga norint suprasti, kaip akselerometras sąveikauja su išorinėmis jėgomis, praktiškiau atlikti skaičiavimus, jei pritvirtiname koordinačių sistemą prie akselerometro ašių ir įsivaizduojame, kad jėgos vektorius sukasi aplink mus.

Prašome pažvelgti į aukščiau pateiktą modelį, aš išsaugojau kirvių spalvas, kad galėtumėte protingai pereiti nuo ankstesnio modelio prie naujo. Įsivaizduokite, kad kiekviena naujojo modelio ašis yra statmena atitinkamiems ankstesnio modelio dėžutės paviršiams. Vektorius R yra jėgos vektorius, kurį matuoja akselerometras (tai gali būti gravitacijos jėga arba inercinė jėga iš aukščiau pateiktų pavyzdžių arba abiejų derinys). Rx, Ry, Rz yra R vektoriaus projekcija X, Y, Z ašyse. Atkreipkite dėmesį į šį ryšį:

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

kuris iš esmės atitinka Pitagoro teoremą 3D.

Prisiminkite, kad šiek tiek anksčiau aš jums sakiau, kad SQRT (1/2) ~ 0,71 reikšmės nėra atsitiktinės. Jei prijungsite juos prie aukščiau pateiktos formulės, prisiminę, kad mūsų traukos jėga buvo 1 g, galime patikrinti, ar:

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

tiesiog pakeisdami R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) Eq.1

Po ilgos teorijos preambulės mes priartėjame prie realaus gyvenimo akselerometrų. Reikšmės Rx, Ry, Rz iš tikrųjų yra tiesiškai susijusios su reikšmėmis, kurias išves jūsų realaus gyvenimo akselerometras ir kurias galite naudoti atlikdami įvairius skaičiavimus.

Prieš atvykdami, šiek tiek pakalbėkime apie tai, kaip akselerometrai mums pateiks šią informaciją. Dauguma akselerometrų skirstomi į dvi kategorijas: skaitmeninius ir analoginius. Skaitmeniniai akselerometrai suteiks jums informaciją naudojant nuoseklųjį protokolą, pvz., I2C, SPI ar USART, o analoginiai akselerometrai išves įtampos lygį iš anksto nustatytame diapazone, kurį turėsite konvertuoti į skaitmeninę vertę naudodami ADC (analoginio skaitmeninio keitiklio) modulį. Nesigilinsiu į tai, kaip veikia ADC, iš dalies dėl to, kad tai yra tokia plati tema, ir iš dalies dėl to, kad skirtingose platformose ji skiriasi. Kai kuriuose mikrovaldikliuose bus įmontuoti ADC moduliai, kai kuriems iš jų reikės išorinių komponentų, kad būtų galima atlikti ADC konversijas. Nesvarbu, kokio tipo ADC modulį naudojate, turėsite tam tikro diapazono vertę. Pvz., 10 bitų ADC modulis išves reikšmę iš 0..1023, atkreipkite dėmesį, kad 1023 = 2^10 -1. 12 bitų ADC modulis išves reikšmę nuo 0..4095, atkreipkite dėmesį, kad 4095 = 2^12-1.

Eikime toliau apsvarstydami paprastą pavyzdį, tarkime, kad mūsų 10 bitų ADC modulis suteikė mums šias trijų akselerometro kanalų (ašių) vertes:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Kiekvienas ADC modulis turės atskaitos įtampą, tarkime, kad mūsų pavyzdyje jis yra 3,3 V. Norėdami konvertuoti 10 bitų adc vertę į įtampą, naudojame šią formulę:

VoltsRx = AdcRx * Vref / 1023

Greita pastaba: 8 bitų ADC paskutinis daliklis būtų 255 = 2 ^ 8 -1, o 12 bitų ADC paskutinis daliklis būtų 4095 = 2 ^ 12 -1.

Taikydami šią formulę visiems 3 kanalams, gauname:

VoltaiRx = 586 * 3.3V / 1023 = ~ 1.89V (visus rezultatus suapvaliname iki 2 skaičių po kablelio) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Kiekvienas akselerometras turi nulio g įtampos lygį, jį galite rasti specifikacijose, tai yra įtampa, atitinkanti 0 g. Norėdami gauti pasirašytą įtampos vertę, turime apskaičiuoti poslinkį nuo šio lygio. Tarkime, kad mūsų 0g įtampos lygis yra VzeroG = 1,65V. Mes apskaičiuojame įtampos poslinkius nuo nulio g įtampos taip:

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

Dabar turime akselerometro rodmenis voltais, jis vis dar nėra g (9,8 m/s^2), o galutinei konversijai atlikti taikome akselerometro jautrumą, paprastai išreikštą mV/g. Tarkime, mūsų jautrumas = 478,5 mV/g = 0,4785 V/g. Jautrumo vertes rasite akselerometro specifikacijose. Norėdami gauti galutines jėgos reikšmes, išreikštas g, naudojame šią formulę:

Rx = DeltaVoltsRx / jautrumas

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

Mes, žinoma, galėtume sujungti visus veiksmus į vieną formulę, bet aš atlikiau visus veiksmus, kad būtų aišku, kaip pereiti nuo ADC rodmenų prie jėgos vektoriaus komponento, išreikšto g.

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

Dabar turime visus 3 komponentus, kurie apibrėžia mūsų inercijos jėgos vektorių, jei prietaisas nėra veikiamas kitų jėgų, išskyrus gravitaciją, galime manyti, kad tai yra mūsų gravitacijos jėgos vektoriaus kryptis. Jei norite apskaičiuoti prietaiso nuolydį žemės atžvilgiu, galite apskaičiuoti kampą tarp šio vektoriaus ir Z ašies. Jei jus taip pat domina kiekvienos ašies pasvirimo kryptis, galite padalyti šį rezultatą į 2 komponentus: polinkį X ir Y ašyje, kurį galima apskaičiuoti kaip kampą tarp gravitacijos vektoriaus ir X / Y ašių. Šiuos kampus apskaičiuoti yra paprasčiau, nei manote, dabar, kai apskaičiavome Rx, Ry ir Rz reikšmes. Grįžkime prie paskutinio akselerometro modelio ir atlikime keletą papildomų pastabų:

Mus dominantys kampai yra kampai tarp X, Y, Z ašių ir jėgos vektoriaus R. Šiuos kampus apibrėšime kaip Axr, Ayr, Azr. Iš stačiakampio trikampio, kurį sudaro R ir Rx, galite pastebėti, kad:

cos (Axr) = Rx / R ir panašiai: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Iš Eq.1 galime atimti, kad R = SQRT (Rx^2 + Ry^2 + Rz^2).

Dabar galime rasti savo kampus naudodami funkciją arccos () (atvirkštinė cos () funkcija):

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

Mes nuėjome ilgą kelią, kad paaiškintume akselerometro modelį, kad galėtume rasti šias formules. Priklausomai nuo jūsų programų, galbūt norėsite naudoti bet kokias tarpines formules, kurias gavome. Taip pat netrukus pristatysime giroskopo modelį ir pamatysime, kaip galima sujungti akselerometro ir giroskopo duomenis, kad būtų dar tikslesni nuolydžio įvertinimai.

Tačiau prieš tai darykime keletą naudingesnių pastabų:

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

Šis tripletas dažnai vadinamas krypties kosinusu ir iš esmės reiškia vieneto vektorių (1 ilgio vektorių), kurio kryptis tokia pati kaip ir mūsų R vektoriaus. Galite lengvai patikrinti, ar:

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

Tai puiki savybė, nes ji atleidžia mus nuo R vektoriaus modulio (ilgio) stebėjimo. Dažnai, jei mus domina tik mūsų inercinio vektoriaus kryptis, prasminga normalizuoti jo modulį, kad būtų supaprastinti kiti skaičiavimai.

2 žingsnis: giroskopas

Mes nesiimsime jokio panašaus giroskopo dėžutės modelio, kaip tai darėme akselerometrui, o pereisime tiesiai prie antrojo akselerometro modelio ir parodysime, ką pagal šį modelį matuoja giroskopas.

Kiekvienas giroskopo kanalas matuoja sukimąsi aplink vieną ašį. Pavyzdžiui, 2 ašių giroskopas išmatuos X ir Y ašių sukimąsi (arba kai kurie gali pasakyti „apie“). Norėdami išreikšti šį sukimąsi skaičiais, atlikime keletą pastabų. Pirmiausia apibrėžkime:

Rxz - yra inercinės jėgos vektoriaus R projekcija XZ plokštumoje Ryz - yra inercinės jėgos vektoriaus R projekcija YZ plokštumoje

Iš stačiojo trikampio, kurį sudaro Rxz ir Rz, naudojant Pitagoro teoremą, gauname:

Rxz^2 = Rx^2 + Rz^2 ir panašiai: Ryz^2 = Ry^2 + Rz^2

taip pat atkreipkite dėmesį, kad:

R^2 = Rxz^2 + Ry^2, tai galima išvesti iš 1 ir aukštesnių lygčių arba išvesti iš stačiojo trikampio, kurį sudaro R ir Ryz R^2 = Ryz^2 + Rx^2

Šiame straipsnyje mes nenaudosime šių formulių, tačiau naudinga atkreipti dėmesį į visų mūsų modelio verčių ryšį.

Vietoj to mes nustatysime kampą tarp Z ašies ir Rxz, Ryz vektorių taip:

Axz - yra kampas tarp Rxz (R projekcija XZ plokštumoje) ir Z ašies Ayz - yra kampas tarp Ryz (R projekcija YZ plokštumoje) ir Z ašies

Dabar mes artėjame prie to, ką matuoja giroskopas. Giroskopas matuoja aukščiau apibrėžtų kampų pokyčių greitį. Kitaip tariant, ji išves vertę, kuri yra tiesiškai susijusi su šių kampų kitimo greičiu. Norėdami tai paaiškinti, tarkime, kad mes matavome sukimosi kampą aplink ašį Y (tai būtų Axz kampas) momentu t0 ir apibrėžiame jį kaip Axz0, toliau matavome šį kampą vėliau t1 ir tai buvo Axz1. Pakeitimo greitis bus apskaičiuojamas taip:

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

Jei Axz išreiškiame laipsniais, o laiką - sekundėmis, tada ši vertė bus išreikšta laipsniais/s. Tai matuoja giroskopas.

Praktiškai giroskopas (nebent tai yra specialus skaitmeninis giroskopas) retai duos jums vertę, išreikštą laipsniais/s. Kaip ir akselerometro atveju, gausite ADC vertę, kurią turėsite konvertuoti į deg/s, naudodami formulę, panašią į Eq. 2, kurį nustatėme akselerometrui. Įveskime ADC į deg/s konversijos formulę giroskopui (darome prielaidą, kad naudojame 10 bitų ADC modulį, 8 bitų ADC pakeiskite 1023 255, 12 bitų ADC 1023 pakeiskite 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Jautrumo lygtis 3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Jautrumas

„AdcGyroXZ“, „AdcGyroYZ“- gaunami iš mūsų „adc“modulio ir atspindi kanalus, kurie matuoja R vektoriaus projekcijos sukimąsi XZ atitinkamai YZ plokštumose, o tai prilygsta sakymui, kad sukimasis buvo atliktas atitinkamai aplink Y ir X ašis.

Vref - tai ADC etaloninė įtampa, kurią naudosime 3.3 V pavyzdyje žemiau „VzeroRate“- yra nulinės srovės įtampa, kitaip tariant, įtampa, kurią giroskopas išleidžia, kai jis nėra sukamas, „Acc_Gyro“plokštėje Pavyzdžiui, 1,23 V (šias vertes galite rasti specifikacijose) Jautrumas - ar jūsų giroskopo jautrumas jis išreiškiamas mV / (deg / s), dažnai užrašomas kaip mV / deg / s, tai iš esmės nurodo, kiek mV bus padidėja giroskopo išėjimas, jei sukimosi greitį padidinate vienu laipsniu per sekundę. Pavyzdžiui, „Acc_Gyro“plokštės jautrumas yra 2 mV/deg/s arba 0,002 V/deg/s

Paimkime pavyzdį, tarkime, kad mūsų ADC modulis grąžino šias vertes:

AdcGyroXZ = 571 AdcGyroXZ = 323

Naudodami aukščiau pateiktą formulę ir naudodami „Acc_Gyro“plokštės specifikacijų parametrus, gausime:

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

Kitaip tariant, prietaisas sukasi aplink Y ašį (arba galime sakyti, kad sukasi XZ plokštumoje) 306 laipsnių per sekundę greičiu ir aplink X ašį (arba galime pasakyti, kad sukasi YZ plokštumoje) greičiu - 94 laipsniai per sekundę. Atminkite, kad neigiamas ženklas reiškia, kad prietaisas sukasi priešinga kryptimi nei įprasta teigiama kryptis. Pagal susitarimą viena sukimosi kryptis yra teigiama. Geras giroskopo specifikacijų lapas parodys, kuri kryptis yra teigiama, kitaip turėsite ją rasti eksperimentuodami su įrenginiu ir pažymėdami, kokia sukimosi kryptimi padidėja išėjimo kaiščio įtampa. Tai geriausia padaryti naudojant osciloskopą, nes kai tik sustabdysite sukimąsi, įtampa nukris iki nulio lygio. Jei naudojate multimetrą, turėsite palaikyti pastovų sukimosi greitį bent kelias sekundes ir atkreipti dėmesį į įtampą šio sukimosi metu, tada palyginkite jį su nulinio greičio įtampa. Jei ji yra didesnė už nulinės įtampos įtampą, tai reiškia, kad sukimosi kryptis yra teigiama.

3 žingsnis: akselerometro ir giroskopo derinimas

Viską sujungti - sujungiami akselerometro ir giroskopo duomenys

Jei skaitote šį straipsnį, tikriausiai įsigijote ar ketinate įsigyti IMU įrenginį, arba tikriausiai planuojate jį sukurti iš atskirų akselerometro ir giroskopo prietaisų.

Pirmasis žingsnis naudojant kombinuotą IMU įrenginį, kuriame yra akselerometras ir giroskopas, yra suderinti jų koordinačių sistemas. Lengviausias būdas tai padaryti yra pasirinkti akcelerometro koordinačių sistemą kaip atskaitos koordinačių sistemą. Dauguma akselerometro duomenų lapų parodys X, Y, Z ašių kryptį, palyginti su fizinio lusto ar įrenginio vaizdu. Pavyzdžiui, čia yra X, Y, Z ašių kryptys, kaip parodyta „Acc_Gyro“plokštės specifikacijose:

Kiti veiksmai yra šie:

Nustatykite giroskopo išvestis, kurios atitinka aukščiau aptartas RateAxz, RateAyz reikšmes. Nustatykite, ar šiuos išėjimus reikia apversti dėl fizinės giroskopo padėties pagreičio matuoklio atžvilgiu

Nemanykite, kad jei giroskopo išėjimas yra pažymėtas X arba Y, jis atitiks bet kurią akselerometro koordinačių sistemos ašį, net jei šis išėjimas yra IMU įrenginio dalis. Geriausias būdas tai išbandyti. Darant prielaidą, kad nustatėte giroskopo padėtį akselerometro atžvilgiu. Daroma prielaida, kad giroskopo ir akselerometro kraštinės yra lygiagrečios viena kitai, t. Y. Giroskopą statote 90 laipsnių kampu, palyginti su akselerometro lustu. Jei įsigijote IMU plokštę, yra tikimybė, kad jie jau yra suderinti tokiu būdu. Šiame straipsnyje nekalbėsime apie modelius, kuriuose giroskopas yra pastatytas netaisyklingu kampu, palyginti su akselerometru (tarkime, 45 ar 30 laipsnių), nors tai gali būti naudinga kai kuriose programose.

Čia yra pavyzdinė seka, skirta nustatyti, kuri giroskopo išvestis atitinka aukščiau aptartą „RateAxz“vertę.

- pradėkite nuo prietaiso pastatymo horizontalioje padėtyje. Akselerometro X ir Y išėjimai išvestų nulinę g įtampą (pavyzdžiui, „Acc_Gyro“plokštėje tai yra 1,65 V)

- kitą kartą pradėkite sukti prietaisą aplink Y ašį, dar vienas būdas pasakyti, kad pasukate prietaisą XZ plokštumoje, kad pasikeistų X ir Z akselerometro išėjimai, o Y išvestis išliktų pastovi. - sukant prietaisą pastoviu greičiu, kuris keičia giroskopo išvestį, kiti giroskopo išėjimai turėtų likti pastovūs - giroskopo išėjimas, kuris pasikeitė sukimosi aplink Y ašį metu (sukimasis XZ plokštumoje), suteiks „AdcGyroXZ“įvesties vertę. mes apskaičiuojame „RateAxz“- paskutinis žingsnis yra užtikrinti, kad sukimosi kryptis atitiktų mūsų modelį, kai kuriais atvejais gali tekti apversti „RateAxz“vertę dėl giroskopo fizinės padėties pagreičio matuoklio atžvilgiu - dar kartą atlikite aukščiau pateiktą bandymą, sukdami prietaisą aplink Y ašį, šį kartą stebėkite akselerometro X išėjimą (mūsų modelyje „AdcRx“). Jei „AdcRx“auga (pirmieji 90 laipsnių sukimosi iš horizontalios padėties), tada „AdcGyroXZ“taip pat turėtų augti. Priešingu atveju jums reikia apversti „RateAxz“, tai galite pasiekti įvesdami ženklų koeficientą į Eq.3, taip:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / jautrumas, kai InvertAxz yra 1 arba -1

tą patį bandomąjį cukranendrį galima atlikti naudojant „RateAyz“, sukant prietaisą aplink X ašį, ir galite nustatyti, kuris giroskopo išėjimas atitinka „RateAyz“, ir ar jį reikia apversti. Turėdami „InvertAyz“vertę, turėtumėte naudoti šią formulę, kad apskaičiuotumėte „RateAyz“:

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

Jei atliktumėte šiuos bandymus „Acc_Gyro“plokštėje, gautumėte šiuos rezultatus:

- „RateAxz“išvesties kaištis yra GX4, o „InvertAxz“- -1. - „RateAyz“išvesties kaištis yra GY4, o „InvertAyz“- -1

Nuo tada mes manysime, kad savo IMU nustatėte taip, kad galėtumėte apskaičiuoti teisingas Axr, Ayr, Azr (kaip apibrėžta 1 dalyje. Akselerometras) ir RateAxz, RateAyz (kaip apibrėžta 2 dalyje) vertes.). Toliau analizuosime ryšius tarp šių verčių, kurios yra naudingos norint tiksliau įvertinti prietaiso nuolydį, palyginti su įžeminimo plokštuma.

Galbūt šiuo metu savęs klausiate, jei akselerometro modelis mums jau davė Axr, Ayr, Azr polinkio kampus, kodėl mes norėtume nerimauti dėl giroskopo duomenų? Atsakymas paprastas: akselerometro duomenimis ne visada galima pasitikėti 100%. Yra keletas priežasčių, atminkite, kad akselerometras matuoja inercinę jėgą, tokią jėgą gali sukelti gravitacija (o idealiu atveju - tik gravitacija), bet taip pat gali atsirasti dėl prietaiso pagreičio (judėjimo). Dėl to, net jei akselerometras yra gana stabilios būklės, jis vis tiek yra labai jautrus vibracijai ir mechaniniam triukšmui apskritai. Tai yra pagrindinė priežastis, kodėl dauguma IMU sistemų naudoja giroskopą, kad išlygintų visas akselerometro klaidas. Bet kaip tai daroma? Ir ar giroskopas neturi triukšmo?

Tačiau giroskopas neturi triukšmo, tačiau matuoja sukimąsi, jis yra mažiau jautrus tiesiniams mechaniniams judesiams, triukšmo tipui, kurį patiria akselerometras, tačiau giroskopai turi kitų problemų, tokių kaip, pavyzdžiui, dreifas (negrįžta prie nulinės normos vertės) kai sukimasis sustoja). Nepaisant to, vidutiniškai apskaičiuodami duomenis, gautus iš pagreičio matuoklio ir giroskopo, galime gauti santykinai geresnį dabartinio prietaiso polinkio įvertinimą, nei gautume naudodami tik akselerometro duomenis.

Kituose žingsniuose pristatysiu algoritmą, kurį įkvėpė kai kurios Kalmano filtro idėjos, tačiau jis yra daug paprastesnis ir lengviau įdiegtas įterptiniuose įrenginiuose. Prieš tai pažiūrėkime, ką norime apskaičiuoti mūsų algoritmui. Na, tai yra gravitacijos jėgos vektoriaus R = [Rx, Ry, Rz] kryptis, iš kurios galime išvesti kitas vertes, tokias kaip „Axr“, „Ayr“, „Azr“arba „cosX“, jaukus, „cosZ“, kurios suteiks mums idėją apie mūsų prietaiso polinkį palyginti su įžeminimo plokštuma, šių dalių santykį aptariame 1 dalyje. Galima sakyti - ar mes jau neturime šių reikšmių Rx, Ry, Rz iš 1 dalies Eq.2? Na taip, bet atminkite, kad šios vertės yra gautos tik iš akselerometro duomenų, taigi, jei jas naudosite tiesiogiai savo programoje, galite sukelti daugiau triukšmo, nei jūsų programa gali toleruoti. Kad išvengtumėte papildomos painiavos, iš naujo apibrėžkime akselerometro matavimus taip:

Racc - yra inercinės jėgos vektorius, matuojamas akselerometru, kurį sudaro šie komponentai (iškyšos X, Y, Z ašyse):

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

Iki šiol turime išmatuotų verčių rinkinį, kurį galime gauti vien iš akselerometro ADC verčių. Šį duomenų rinkinį vadinsime „vektoriu“ir naudosime toliau nurodytą žymėjimą.

Racc = [RxAcc, RyAcc, RzAcc]

Kadangi šiuos „Racc“komponentus galima gauti iš akselerometro duomenų, galime tai laikyti įvestimi į mūsų algoritmą.

Atminkite, kad kadangi „Racc“matuoja gravitacijos jėgą, būsite teisūs, jei manysite, kad šio vektoriaus, apibrėžto taip, ilgis yra lygus arba artimas 1 g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Tačiau norėdami būti tikri, tikslinga atnaujinti šį vektorių taip:

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

Tai užtikrins, kad normalizuoto „Racc“vektoriaus ilgis visada yra 1.

Toliau pristatysime naują vektorių ir jį pavadinsime

Poilsis = [RxEst, RyEst, RzEst]

Tai bus mūsų algoritmo išvestis, tai yra pataisytos vertės, pagrįstos giroskopo duomenimis ir ankstesniais apskaičiuotais duomenimis.

Štai ką padarys mūsų algoritmas: - pagreičio matuoklis mums sako: „Jūs dabar esate Racc padėtyje“- mes sakome „ačiū, bet leiskite man patikrinti“, - tada pataisykite šią informaciją naudodami giroskopo duomenis ir ankstesnius poilsio duomenis išvedame naują apskaičiuotą vektorių „Rest. - „Rest“laikome „geriausiu pasirinkimu“, susijusiu su dabartine prietaiso padėtimi.

Pažiūrėkime, kaip mes galime tai padaryti.

Savo seką pradėsime pasitikėdami savo akselerometru ir priskirdami:

Poilsis (0) = Racc (0)

Beje, atminkite, kad „Rest“ir „Racc“yra vektoriai, todėl aukščiau pateikta lygtis yra tik paprastas būdas parašyti 3 lygčių rinkinius ir išvengti kartojimo:

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

Toliau atliksime reguliarius matavimus vienodais laiko intervalais T sekundžių ir gausime naujus matavimus, kuriuos apibrėžsime kaip Racc (1), Racc (2), Racc (3) ir pan. Taip pat kiekvieną kartą paskelbsime naujus įvertinimus Poilsis (1), Poilsis (2), Poilsis (3) ir pan.

Tarkime, kad esame n žingsnyje. Turime žinomus du verčių rinkinius, kuriuos norėtume naudoti:

Poilsis (n -1) - mūsų ankstesnis įvertinimas, kai poilsis (0) = Racc (0) Racc (n) - dabartinis mūsų akselerometro matavimas

Prieš apskaičiuodami poilsį (n), pristatykime naują išmatuotą vertę, kurią galime gauti iš savo giroskopo ir ankstesnio įvertinimo.

Pavadinsime jį „Rgyro“, jis taip pat yra vektorius, susidedantis iš 3 komponentų:

Rgyro = [RxGyro, RyGyro, RzGyro]

Mes apskaičiuosime šį vektorių po vieną komponentą. Pradėsime nuo „RxGyro“.

Pradėkime stebėdami šį mūsų giroskopo modelio ryšį, iš stačiakampio trikampio, kurį sudaro Rz ir Rxz, galime tai padaryti:

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

„Atan2“gali būti funkcija, kurios niekada nenaudojote, ji yra panaši į „atan“, išskyrus tai, kad pateikia reikšmes diapazone (-PI, PI), o ne (-PI/2, PI/2), kaip nurodė atanas. 2 argumentai vietoj vieno. Tai leidžia mums konvertuoti dvi Rx, Rz reikšmes į kampus visame 360 laipsnių diapazone (nuo -PI iki PI). Daugiau apie atan2 galite paskaityti čia.

Taigi žinodami RxEst (n-1) ir RzEst (n-1) galime rasti:

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

Atminkite, kad giroskopas matuoja Axz kampo kitimo greitį. Taigi naują kampą Axz (n) galime įvertinti taip:

Axz (n) = Axz (n-1) + dažnisAxz (n) * T.

Atminkite, kad „RateAxz“galima gauti iš mūsų giroskopo ADC rodmenų. Tikslesnėje formulėje galima naudoti vidutinį sukimosi greitį, apskaičiuotą taip:

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

Panašiai galime rasti:

Ayz (n) = Ayz (n-1) + dažnisAyz (n) * T.

Gerai, dabar turime Axz (n) ir Ayz (n). Kur mes einame, kad išskaičiuotume „RxGyro“/„RyGyro“? Iš ekv. 1 galime užrašyti vektoriaus Rgyro ilgį taip:

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

Taip pat todėl, kad normalizavome savo „Racc“vektorių, galime manyti, kad jo ilgis yra 1 ir jis nepasikeitė pasukus, todėl rašyti yra gana saugu:

| Rgyro | = 1

Toliau pateiktiems skaičiavimams priimkime trumpesnį žymėjimą:

x = RxGyro, y = RyGyro, z = RzGyro

Naudodami aukščiau pateiktus santykius, galime parašyti:

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

Dalinkime trupmenos skaitiklį ir vardiklį iš SQRT (x^2 + z^2)

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

Atminkite, kad x / SQRT (x^2 + z^2) = sin (Axz), taigi:

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

Dabar padauginkite SQRT viduje esančios trupmenos skaitiklį ir vardiklį iš z^2

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

Atkreipkite dėmesį, kad z / SQRT (x^2 + z^2) = cos (Axz) ir y / z = tan (Ayz), taigi galiausiai:

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

Grįžtant prie mūsų žymėjimo, gauname:

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

tą patį randame ir mes

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

Dabar pagaliau galime rasti:

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

Kur ženklas (RzGyro) = 1, kai RzGyro> = 0, ir ženklas (RzGyro) = -1, kai RzGyro <0.

Vienas paprastas būdas tai įvertinti yra:

Ženklas (RzGyro) = Ženklas (RzEst (n-1))

Praktiškai būkite atsargūs, kai RzEst (n-1) yra artimas 0. Šiuo atveju galite visiškai praleisti giroskopo fazę ir priskirti: Rgyro = Poilsis (n-1). Rz naudojamas kaip atskaitos taškas Axz ir Ayz kampams apskaičiuoti, o kai jis artimas 0, reikšmės gali perpildyti ir sukelti blogus rezultatus. Jūs būsite didelių slankiojo kablelio skaičių srityje, kur tan () / atan () funkcijų įgyvendinimai gali būti nepakankamai tikslūs.

Taigi pakartokime, ką turėjome iki šiol, esame algoritmo n žingsnyje ir apskaičiavome šias vertes:

„Racc“- dabartiniai mūsų akselerometro „Rgyro“rodmenys, gauti iš poilsio (n -1) ir dabartinių giroskopo rodmenų

Kokias vertes naudojame apskaičiuodami atnaujintą įvertį Poilsis (n)? Tikriausiai atspėjote, kad naudosime abu. Naudosime svertinį vidurkį, kad:

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

Šią formulę galime supaprastinti padaliję trupmenos skaitiklį ir vardiklį iš w1.

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

ir pakeitus w2/w1 = wGyro gauname:

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

Aukščiau pateiktame forume „wGyro“mums sako, kiek mes pasitikime savo giroskopu, palyginti su mūsų akselerometru. Šią vertę galima pasirinkti eksperimentiškai. Paprastai vertės nuo 5 iki 20 parodys gerus rezultatus.

Pagrindinis šio algoritmo skirtumas nuo Kalmano filtro yra tas, kad šis svoris yra santykinai fiksuotas, o Kalmano filtre svoriai yra nuolat atnaujinami, atsižvelgiant į išmatuotą akselerometro rodmenų triukšmą. Kalmano filtras yra skirtas suteikti jums „geriausius“teorinius rezultatus, tuo tarpu šis algoritmas gali duoti „pakankamai gerų“rezultatų jūsų praktiniam pritaikymui. Galite įdiegti algoritmą, kuris koreguoja „wGyro“, atsižvelgiant į kai kuriuos išmatuotus triukšmo veiksnius, tačiau fiksuotos vertės puikiai tiks daugeliui programų.

Iki žingsnio iki atnaujintų apskaičiuotų verčių gavimo:

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

Dabar vėl normalizuokime šį vektorių:

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

Ir mes esame pasirengę dar kartą pakartoti savo ciklą.

Šis vadovas iš pradžių pasirodė starlino.com. Aš padariau keletą lengvų pakeitimų ir vėl paskelbiau jį su leidimu. Ačiū Starlino!