Kurz akcelerometru a gyroskopu: 3 kroky

2024 Autor: John Day | [email protected]. Naposledy změněno: 2024-01-30 08:25

Úvod

Tato příručka je určena všem, kteří se zajímají o používání akcelerometrů a gyroskopů, jakož i kombinovaných zařízení IMU (inerciální měřicí jednotka) v jejich elektronických projektech

Pokryjeme:

• Co měří akcelerometr?
• Co měří gyroskop (aka gyroskop)?
• Jak převést hodnoty analogově-digitální (ADC), které získáte z těchto senzorů, na fyzické jednotky (to by bylo g pro akcelerometr, deg/s pro gyroskop)
• Jak kombinovat údaje z akcelerometru a gyroskopu, abyste získali přesné informace o sklonu vašeho zařízení vzhledem k základní rovině

V celém článku se pokusím omezit matematiku na minimum. Pokud víte, co jsou Sine/Cosine/Tangent, měli byste být schopni porozumět těmto myšlenkám a používat je ve svém projektu bez ohledu na to, jakou platformu používáte: Arduino, Propeller, Basic Stamp, Atmel čipy, Microchip PIC atd.

Existují lidé, kteří věří, že k použití jednotky IMU potřebujete komplexní matematiku (komplexní filtry FIR nebo IIR, jako jsou Kalmanovy filtry, filtry Parks-McClellan atd.). Můžete je všechny prozkoumat a dosáhnout nádherných, ale složitých výsledků. Můj způsob vysvětlování věcí vyžaduje pouze základní matematiku. Jsem velkým zastáncem jednoduchosti. Myslím, že systém, který je jednoduchý, se snadněji ovládá a monitoruje, kromě toho, že mnoho vestavěných zařízení nemá sílu a prostředky k implementaci složitých algoritmů vyžadujících maticové výpočty.

Jako příklad použiji novou jednotku IMU, Acc_Gyro Accelerometer + Gyro IMU. Parametry tohoto zařízení použijeme v níže uvedených příkladech. Tato jednotka je dobrým zařízením pro začátek, protože se skládá ze 2 zařízení:

- LIS331AL (datový list) - triaxiální 2G akcelerometr - LPR550AL (datový list) - dvouosý rozteč a válec, gyroskop 500 stupňů/s

Společně představují 5stupňovou jednotku inerciálního měření svobody. Nyní je to fantastické jméno! Přesto se za vymyšleným názvem skrývá velmi užitečné kombinované zařízení, kterému se v této příručce podrobně věnujeme a vysvětlíme.

Krok 1: Akcelerometr

Abychom porozuměli této jednotce, začneme akcelerometrem. Při přemýšlení o akcelerometrech je často užitečné si představit krabici ve tvaru krychle s kuličkou uvnitř. Můžete si představit něco jiného jako sušenku nebo koblihu, ale já si představím kouli:

Vezmeme -li toto pole na místo bez gravitačních polí, nebo bez dalších polí, která by mohla ovlivnit polohu míče - koule se jednoduše vznáší uprostřed pole. Dokážete si představit, že je krabice ve vesmíru daleko od jakýchkoli vesmírných těles, nebo pokud je takové místo těžké najít, představte si alespoň vesmírné plavidlo obíhající kolem planety, kde je vše v beztížném stavu. Z výše uvedeného obrázku vidíte, že každé ose přiřadíme dvojici stěn (odstranili jsme stěnu Y+, abychom se mohli podívat dovnitř krabice). Představte si, že každá zeď je citlivá na tlak. Pokud najednou přesuneme krabici doleva (zrychlíme ji zrychlením 1g = 9,8m/s^2), míč zasáhne zeď X-. Poté změříme tlakovou sílu, kterou koule působí na zeď, a na osu X vydáme hodnotu -1g.

Pamatujte, že akcelerometr ve skutečnosti detekuje sílu, která je směrována v opačném směru než vektor zrychlení. Tato síla se často nazývá setrvačná síla nebo fiktivní síla. Jedna věc, kterou byste se z toho měli naučit, je, že akcelerometr měří zrychlení nepřímo prostřednictvím síly, která je aplikována na jednu z jeho stěn (podle našeho modelu to může být pružina nebo něco jiného v akcelerometrech v reálném životě). Tato síla může být způsobena zrychlením, ale jak uvidíme v následujícím příkladu, není vždy způsobeno zrychlením.

Když vezmeme náš model a umístíme jej na Zemi, koule dopadne na stěnu Z a na spodní stěnu bude působit silou 1 g, jak ukazuje obrázek níže:

V tomto případě se pole nepohybuje, ale stále získáváme hodnotu -1g na ose Z. Tlak, který koule vyvinula na zeď, byl způsoben gravitační silou. Teoreticky by to mohl být jiný typ síly - například pokud si představíte, že naše koule je kovová, umístěním magnetu vedle krabice by se koule mohla přesunout tak, aby zasáhla jinou zeď. To bylo řečeno jen proto, aby dokázal, že akcelerometr v podstatě měří sílu, nikoli zrychlení. Prostě se stane, že zrychlení způsobí setrvačnou sílu, která je zachycena mechanismem detekce síly akcelerometru.

I když tento model není přesně konstruován jako snímač MEMS, je často užitečný při řešení problémů souvisejících s akcelerometrem. Ve skutečnosti existují podobné senzory, které mají uvnitř kovové kuličky, nazývají se přepínači náklonu, nicméně jsou primitivnější a obvykle dokážou zjistit pouze to, zda je zařízení nakloněno v určitém rozsahu nebo ne, nikoli rozsah sklonu.

Doposud jsme analyzovali výstup akcelerometru na jedné ose a to je vše, co získáte s akcelerometry s jednou osou. Skutečná hodnota trojosých akcelerometrů vychází ze skutečnosti, že dokážou detekovat setrvačné síly na všech třech osách. Vraťme se k našemu modelu boxu a otočme box o 45 stupňů doprava. Míč se nyní dotkne 2 stěn: Z- a X-, jak ukazuje obrázek níže:

Hodnoty 0,71 nejsou libovolné, jsou ve skutečnosti aproximací pro SQRT (1/2). To bude jasnější, když představíme náš další model pro akcelerometr.

V předchozím modelu jsme fixovali gravitační sílu a otočili naši imaginární krabici. V posledních 2 příkladech jsme analyzovali výstup ve 2 různých polohách boxu, zatímco vektor síly zůstal konstantní. I když to bylo užitečné pro pochopení toho, jak akcelerometr interaguje s vnějšími silami, je praktičtější provádět výpočty, pokud zafixujeme souřadnicový systém k osám akcelerometru a představíme si, že silový vektor rotuje kolem nás.

Podívejte se prosím na výše uvedený model, zachoval jsem barvy os, abyste mohli mentálně přejít z předchozího modelu na nový. Představte si, že každá osa v novém modelu je kolmá k příslušným plochám pole v předchozím modelu. Vektor R je vektor síly, který měří akcelerometr (může to být buď gravitační síla nebo setrvačná síla z výše uvedených příkladů nebo kombinace obou). Rx, Ry, Rz jsou projekce R vektoru na osách X, Y, Z. Všimněte si následujícího vztahu:

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

což je v podstatě ekvivalent Pythagorovy věty ve 3D.

Pamatujte, že o něco dříve jsem vám řekl, že hodnoty SQRT (1/2) ~ 0,71 nejsou náhodné. Pokud je zapojíte do výše uvedeného vzorce, po připomenutí, že naše gravitační síla byla 1 g, můžeme ověřit, že:

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

jednoduše nahrazením R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) v Eq.1

Po dlouhé preambuli teorie se dostáváme blíže k akcelerometrům v reálném životě. Hodnoty Rx, Ry, Rz jsou ve skutečnosti lineárně vztaženy k hodnotám, které bude produkovat váš skutečný akcelerometr a které můžete použít k provádění různých výpočtů.

Než se tam dostaneme, promluvme si trochu o způsobu, jakým nám akcelerometry tyto informace dodají. Většina akcelerometrů bude spadat do dvou kategorií: digitální a analogové. Digitální akcelerometry vám poskytnou informace pomocí sériového protokolu, jako je I2C, SPI nebo USART, zatímco analogové akcelerometry budou vydávat úroveň napětí v předdefinovaném rozsahu, který musíte převést na digitální hodnotu pomocí modulu ADC (analogový převodník na digitální převodník). Nebudu se příliš rozepisovat o tom, jak ADC funguje, jednak proto, že je to tak rozsáhlé téma, jednak proto, že se liší od jedné platformy k druhé. Některé mikrokontroléry budou mít vestavěné moduly ADC, některé z nich budou potřebovat externí komponenty, aby mohly provádět převody ADC. Bez ohledu na to, jaký typ modulu ADC používáte, skončíte s hodnotou v určitém rozsahu. Například 10bitový modul ADC vydá hodnotu v rozsahu 0..1023, všimněte si, že 1023 = 2^10 -1. 12bitový modul ADC vydá hodnotu v rozsahu 0..4095, všimněte si, že 4095 = 2^12-1.

Podívejme se na jednoduchý příklad, předpokládejme, že náš 10bitový modul ADC nám poskytl následující hodnoty pro tři kanály (osy) akcelerometru:

AdcRx = 586 AdcRy = 630 AdcRz = 561

Každý modul ADC bude mít referenční napětí, předpokládejme, že v našem příkladu je 3,3V. K převodu hodnoty 10bit adc na napětí použijeme následující vzorec:

VoltsRx = AdcRx * Vref / 1023

Rychlá poznámka zde: že pro 8bitový ADC bude poslední dělič 255 = 2 ^ 8 -1 a pro 12bit ADC bude poslední dělič 4095 = 2 ^ 12 -1.

Použitím tohoto vzorce na všechny 3 kanály získáme:

VoltsRx = 586 * 3,3V / 1023 = ~ 1,89V (zaokrouhlíme všechny výsledky na 2 desetinná místa) VoltsRy = 630 * 3,3V / 1023 = ~ 2,03V VoltsRz = 561 * 3,3V / 1023 = ~ 1,81V

Každý akcelerometr má úroveň napětí nula g, najdete ho ve specifikacích, toto je napětí, které odpovídá 0 g. Abychom získali podepsanou hodnotu napětí, musíme vypočítat posun z této úrovně. Řekněme, že naše úroveň napětí 0g je VzeroG = 1,65V. Posuny napětí z napětí nulové g vypočítáme následovně:

DeltaVoltsRx = 1,89V - 1,65V = 0,24V DeltaVoltsRy = 2,03V - 1,65V = 0,38V DeltaVoltsRz = 1,81V - 1,65V = 0,16V

Nyní máme naměřené hodnoty akcelerometru ve voltech, stále to není v g (9,8 m/s^2), pro konečný převod použijeme citlivost akcelerometru, obvykle vyjádřenou v mV/g. Řekněme, že naše citlivost = 478,5 mV/g = 0,4785 V/g. Hodnoty citlivosti lze nalézt ve specifikacích akcelerometru. K získání konečných hodnot síly vyjádřených v g použijeme následující vzorec:

Rx = DeltaVoltsRx / Citlivost

Rx = 0,24 V / 0,4785 V / g = ~ 0,5 g Ry = 0,38 V / 0,4785 V / g = ~ 0,79 g Rz = 0,16 V / 0,4785 V / g = ~ 0,33 g

Mohli bychom samozřejmě kombinovat všechny kroky v jednom vzorci, ale prošel jsem všemi kroky, aby bylo jasné, jak přejdete od čtení ADC ke složce vektoru síly vyjádřené v g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Citlivost (Rovnice 2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Citlivost Rz = (AdcRz * Vref / 1023 - VzeroG) / Citlivost

Nyní máme všechny 3 složky, které definují náš vektor setrvačné síly, pokud zařízení není vystaveno jiným silám kromě gravitace, můžeme předpokládat, že toto je směr našeho vektoru gravitační síly. Pokud chcete vypočítat sklon zařízení vzhledem k zemi, můžete vypočítat úhel mezi tímto vektorem a osou Z. Pokud vás také zajímá směr náklonu na osu, můžete tento výsledek rozdělit na 2 složky: sklon na ose X a Y, který lze vypočítat jako úhel mezi gravitačním vektorem a osami X / Y. Výpočet těchto úhlů je jednodušší, než byste si mohli myslet, když jsme nyní vypočítali hodnoty pro Rx, Ry a Rz. Vraťme se k našemu poslednímu modelu akcelerometru a proveďte několik dalších zápisů:

Úhly, které nás zajímají, jsou úhly mezi osami X, Y, Z a silovým vektorem R. Tyto úhly definujeme jako Axr, Ayr, Azr. Z pravoúhlého trojúhelníku tvořeného R a Rx si můžete všimnout, že:

cos (Axr) = Rx / R a podobně: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Z rovnice 1 můžeme odečíst, že R = SQRT (Rx^2 + Ry^2 + Rz^2).

Nyní můžeme najít své úhly pomocí funkce arccos () (inverzní funkce cos ()):

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

Ušli jsme dlouhou cestu k vysvětlení modelu akcelerometru, abychom se dostali k těmto vzorcům. V závislosti na vašich aplikacích možná budete chtít použít jakékoli mezilehlé vzorce, které jsme odvodili. Brzy také představíme model gyroskopu a uvidíme, jak lze data akcelerometru a gyroskopu kombinovat, aby poskytovaly ještě přesnější odhady sklonu.

Ale než to uděláme, udělejme si další užitečné zápisy:

cosX = cos (Axr) = Rx / R útulné = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Tento triplet se často nazývá Direction Cosine a v zásadě představuje jednotkový vektor (vektor s délkou 1), který má stejný směr jako náš R vektor. Můžete si snadno ověřit, že:

SQRT (cosX^2 + útulný^2 + cosZ^2) = 1

To je pěkná vlastnost, protože nás to zbavuje monitorování modulu (délky) R vektoru. Pokud nás často zajímá směr našeho setrvačného vektoru, má smysl normalizovat jeho modul, aby se zjednodušily další výpočty.

Krok 2: Gyroskop

Nebudeme představovat žádný ekvivalentní krabicový model pro gyroskop jako pro akcelerometr, místo toho přejdeme rovnou k druhému modelu akcelerometru a ukážeme si, co gyroskop měří podle tohoto modelu.

Každý kanál gyroskopu měří rotaci kolem jedné z os. Například dvouosý gyroskop bude měřit rotaci kolem (nebo někteří mohou říkat „o“) osách X a Y. Abychom tuto rotaci vyjádřili v číslech, udělejme několik zápisů. Nejprve definujme:

Rxz - je projekce vektoru setrvačné síly R na rovinu XZ Ryz - je projekce vektoru setrvačné síly R na rovinu YZ

Z pravoúhlého trojúhelníku tvořeného Rxz a Rz pomocí Pythagorovy věty získáme:

Rxz^2 = Rx^2 + Rz^2 a podobně: Ryz^2 = Ry^2 + Rz^2

také si všimněte, že:

R^2 = Rxz^2 + Ry^2, to lze odvodit z rovnic rovnic 1 a výše, nebo to lze odvodit z pravoúhlého trojúhelníku tvořeného R a Ryz R^2 = Ryz^2 + Rx^2

V tomto článku nebudeme používat tyto vzorce, ale je užitečné si všimnout vztahu mezi všemi hodnotami v našem modelu.

Místo toho budeme definovat úhel mezi osou Z a Rxz, Ryz vektory následujícím způsobem:

Axz - je úhel mezi Rxz (průmět R na rovinu XZ) a osou A Ayz - je úhel mezi Ryz (projekce R na rovinu YZ) a osou Z

Nyní se dostáváme blíže k tomu, co měří gyroskop. Gyroskop měří rychlost změn výše definovaných úhlů. Jinými slovy, vygeneruje hodnotu, která je lineárně úměrná rychlosti změny těchto úhlů. Abychom to vysvětlili, předpokládejme, že jsme změřili úhel otočení kolem osy Y (to by byl úhel Axz) v čase t0 a definujeme jej jako Axz0, dále jsme tento úhel změřili v pozdější době t1 a byl to Axz1. Rychlost změny bude vypočítána následovně:

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

Pokud vyjádříme Axz ve stupních a čas v sekundách, pak bude tato hodnota vyjádřena v deg/s. To měří gyroskop.

V praxi vám gyroskop (pokud nejde o speciální digitální gyroskop) zřídka poskytne hodnotu vyjádřenou v deg/s. Stejně jako pro akcelerometr získáte hodnotu ADC, kterou budete muset převést na deg/s pomocí vzorce podobného Eq. 2, které jsme definovali pro akcelerometr. Představme vzorec převodu ADC na deg/s pro gyroskop (předpokládáme, že používáme 10bitový ADC modul, u 8bitového ADC nahraďte 1023 255, u 12bitového ADC nahraďte 1023 4095).

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

AdcGyroXZ, AdcGyroYZ - jsou získány z našeho modulu adc a představují kanály, které měří rotaci projekce R vektoru v XZ respektive v rovinách YZ, což je ekvivalentní tomu, že rotace byla provedena kolem os Y a X.

Vref - je referenční napětí ADC, které použijeme 3,3 V v příkladu níže VzeroRate - je napětí s nulovou rychlostí, jinými slovy napětí, které gyroskop vydává, když není předmětem žádného otáčení, pro desku Acc_Gyro je například 1,23 V (tyto hodnoty najdete ve specifikacích) Citlivost - je citlivost vašeho gyroskopu, která je vyjádřena v mV / (deg / s) často psaná jako mV / deg / s, v podstatě vám řekne, kolik mV bude výkon gyroskopu se zvýší, pokud zvýšíte rychlost otáčení o jeden deg/s. Citlivost desky Acc_Gyro je například 2 mV/deg/s nebo 0,002 V/deg/s

Vezměme si příklad, předpokládejme, že náš modul ADC vrátil následující hodnoty:

AdcGyroXZ = 571 AdcGyroXZ = 323

Pomocí výše uvedeného vzorce a pomocí parametrů specifikace desky Acc_Gyro získáme:

RateAxz = (571 * 3,3 V/1023 - 1,23 V)/(0,002 V/deg/s) = ~ 306 °/s RateAyz = (323 * 3,3 V/1023 - 1,23 V)/(0,002 V/deg/s) = ~ -94 stupňů/s

Jinými slovy, zařízení se otáčí kolem osy Y (nebo můžeme říci, že se otáčí v rovině XZ) rychlostí 306 stupňů/s a kolem osy X (nebo můžeme říci, že se otáčí v rovině YZ) rychlostí - 94 stupňů/s. Upozorňujeme, že záporné znaménko znamená, že se zařízení otáčí opačným směrem než konvenční kladný směr. Podle konvence je jeden směr otáčení kladný. Dobrý list se specifikací gyroskopu vám ukáže, který směr je kladný, v opačném případě ho budete muset najít experimentováním se zařízením a poznamenáním, který směr otáčení má za následek zvýšení napětí na výstupním pinu. To se nejlépe provádí pomocí osciloskopu, protože jakmile zastavíte otáčení, napětí klesne zpět na úroveň nulové rychlosti. Pokud používáte multimetr, budete muset udržovat konstantní rychlost otáčení po dobu alespoň několika sekund a zaznamenat napětí během tohoto otáčení, poté jej porovnat s napětím s nulovou rychlostí. Pokud je větší než napětí s nulovou rychlostí, znamená to, že směr otáčení je kladný.

Krok 3: Kombinace akcelerometru a gyroskopu

Dát vše dohromady - kombinace dat akcelerometru a gyroskopu

Pokud čtete tento článek, pravděpodobně jste získali nebo plánujete pořídit zařízení IMU, nebo pravděpodobně plánujete jeho vybudování ze samostatných zařízení pro akcelerometr a gyroskop.

Prvním krokem při použití kombinovaného zařízení IMU, které kombinuje akcelerometr a gyroskop, je sladit jejich souřadnicové systémy. Nejjednodušší způsob, jak to udělat, je vybrat souřadnicový systém akcelerometru jako referenční souřadnicový systém. Většina datových listů akcelerometru bude zobrazovat směr os X, Y, Z vzhledem k obrazu fyzického čipu nebo zařízení. Zde jsou například směry os X, Y, Z, jak je uvedeno ve specifikacích pro desku Acc_Gyro:

Další kroky jsou:

Identifikujte výstupy gyroskopu, které odpovídají hodnotám RateAxz, RateAyz diskutovaným výše. Zjistěte, zda je třeba tyto výstupy invertovat kvůli fyzické poloze gyroskopu vzhledem k akcelerometru

Nepředpokládejte, že pokud má gyroskop výstup označený X nebo Y, bude odpovídat jakékoli ose v souřadnicovém systému akcelerometru, i když je tento výstup součástí jednotky IMU. Nejlepší způsob je otestovat to. Za předpokladu, že jste fixovali polohu gyroskopu vzhledem k akcelerometru. Předpokládá se, že okraje gyroskopu a akcelerometru jsou navzájem rovnoběžné, tj. Umístíte gyroskop pod úhlem násobkem 90 stupňů vzhledem k čipu akcelerometru. Pokud jste získali desku IMU, je pravděpodobné, že jsou již zarovnány tímto způsobem. V tomto článku nebudeme diskutovat o modelech, kde je gyroskop umístěn v nepravidelném úhlu vzhledem k akcelerometru (řekněme 45 nebo 30 stupňů), i když to může být v některých aplikacích užitečné.

Zde je ukázková sekvence, která určuje, který výstup gyroskopu odpovídá hodnotě RateAxz diskutované výše.

- začněte od umístění zařízení do vodorovné polohy. Výstupy akcelerometru X i Y by vydávaly napětí nulové g (například u desky Acc_Gyro je to 1,65 V)

- příště začněte otáčet zařízením kolem osy Y, což je další způsob, jak to říci, že zařízení otáčíte v rovině XZ, takže výstupy akcelerometru X a Z se mění a výstup Y zůstává konstantní. - při otáčení zařízení konstantní rychlostí, který výstup gyroskopu se mění, by ostatní výstupy gyroskopu měly zůstat konstantní - výstup gyroskopu, který se změnil během otáčení kolem osy Y (otáčení v rovině XZ), poskytne vstupní hodnotu pro AdcGyroXZ, ze kterého vypočítáme RateAxz - posledním krokem je zajistit, aby směr otáčení odpovídal našemu modelu, v některých případech budete muset hodnotu RateAxz převrátit kvůli fyzické poloze gyroskopu vzhledem k akcelerometru - proveďte znovu výše uvedený test a otočte zařízení kolem osa Y, tentokrát monitoruje výstup X akcelerometru (v našem modelu AdcRx). Pokud AdcRx roste (prvních 90 stupňů rotace z horizontální polohy), pak by měl růst i AdcGyroXZ. V opačném případě musíte invertovat RateAxz, můžete toho dosáhnout zavedením znakového faktoru v Eq.3, a to následovně:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Citlivost, kde InvertAxz je 1 nebo -1

pro RateAyz proveďte stejný test, otočením zařízení kolem osy X a můžete zjistit, který výstup gyroskopu odpovídá RateAyz a zda je třeba jej převrátit. Jakmile budete mít hodnotu pro InvertAyz, měli byste pro výpočet RateAyz použít následující vzorec:

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

Pokud byste provedli tyto testy na desce Acc_Gyro, získali byste následující výsledky:

- výstupní pin pro RateAxz je GX4 a InvertAxz = -1. - výstupní pin pro RateAyz je GY4 a InvertAyz = -1

Od této chvíle budeme mít za to, že jste svou IMU nastavili tak, že můžete vypočítat správné hodnoty pro Axr, Ayr, Azr (podle definice v části 1. Akcelerometr) a RateAxz, RateAyz (podle definice v části 2. Gyroskop). Dále budeme analyzovat vztahy mezi těmito hodnotami, které se ukázaly být užitečné při získávání přesnějších odhadů sklonu zařízení vzhledem k zemské rovině.

V tomto bodě se možná ptáte, pokud nám model akcelerometru již poskytl úhly sklonu Axr, Ayr, Azr, proč bychom se chtěli obtěžovat s daty gyroskopu? Odpověď je jednoduchá: datům akcelerometru nelze vždy 100%věřit. Existuje několik důvodů, pamatujte, že akcelerometr měří setrvačnou sílu, taková síla může být způsobena gravitací (a ideálně pouze gravitací), ale může to být také způsobeno zrychlením (pohybem) zařízení. Výsledkem je, že i když je akcelerometr v relativně stabilním stavu, je stále velmi citlivý na vibrace a mechanický hluk obecně. To je hlavní důvod, proč většina systémů IMU používá k vyhlazení všech chyb akcelerometru gyroskop. Ale jak se to dělá? A je gyroskop bez šumu?

Gyroskop není bez šumu, ale protože měří rotaci, je méně citlivý na lineární mechanické pohyby, což je typ hluku, kterým trpí akcelerometr, ale gyroskopy mají jiné typy problémů, jako je například drift (nevrací se na hodnotu nulové rychlosti) když se rotace zastaví). Přesto zprůměrováním dat, která pocházejí z akcelerometru a gyroskopu, můžeme získat relativně lepší odhad aktuálního sklonu zařízení, než jaký bychom získali použitím samotných dat akcelerometru.

V dalších krocích představím algoritmus, který byl inspirován některými nápady používanými v Kalmanově filtru, ale je mnohem jednodušší a snadněji implementovatelný na vestavěných zařízeních. Předtím se nejprve podívejme, co chceme, aby náš algoritmus vypočítal. Je to směr vektoru gravitační síly R = [Rx, Ry, Rz], ze kterého můžeme odvodit další hodnoty jako Axr, Ayr, Azr nebo cosX, útulný, cosZ, což nám poskytne představu o sklonu našeho zařízení vzhledem k základní rovině diskutujeme vztah mezi těmito hodnotami v části 1. Dalo by se říci - nemáme již tyto hodnoty Rx, Ry, Rz z rovnice 2 v části 1? Ano, ale pamatujte si, že tyto hodnoty jsou odvozeny pouze z dat akcelerometru, takže pokud byste je chtěli použít přímo ve své aplikaci, můžete dostat více hluku, než může vaše aplikace tolerovat. Abychom se vyhnuli dalším nejasnostem, znovu definujeme měření akcelerometru následovně:

Racc - je vektor setrvačné síly měřený akcelerometrem, který se skládá z následujících složek (projekce na osy X, Y, Z):

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

Zatím máme soubor naměřených hodnot, které můžeme získat čistě z hodnot ADC akcelerometru. Tuto sadu dat budeme nazývat „vektor“a použijeme následující notaci.

Racc = [RxAcc, RyAcc, RzAcc]

Protože tyto komponenty Racc lze získat z dat akcelerometru, můžeme to považovat za vstup do našeho algoritmu.

Vezměte prosím na vědomí, že protože Racc měří gravitační sílu, budete správný, pokud předpokládáte, že délka tohoto vektoru definovaného níže je stejná nebo blízká 1 g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Aby však bylo jisté, že má smysl aktualizovat tento vektor následujícím způsobem:

Racc (normalizováno) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

Tím zajistíte, že délka vašeho normalizovaného Racc vektoru bude vždy 1.

Dále představíme nový vektor a nazveme jej

Zbytek = [RxEst, RyEst, RzEst]

Toto bude výstup našeho algoritmu, jedná se o opravené hodnoty založené na datech gyroskopu a na základě předchozích odhadovaných údajů.

Náš algoritmus udělá toto: - akcelerometr nám říká: „Nyní jste na pozici Racc“- říkáme „Děkuji, ale nechám zkontrolovat“, - poté tyto informace opravte pomocí údajů gyroskopu i předchozích dat Rest a vydáme nový odhadovaný vektorový odpočinek. - Rest považujeme za naši „nejlepší sázku“, pokud jde o aktuální polohu zařízení.

Podívejme se, jak to můžeme zajistit.

Naši sekvenci zahájíme důvěřováním našemu akcelerometru a přiřazením:

Zbytek (0) = Racc (0)

Mimochodem, Rest a Racc jsou vektory, takže výše uvedená rovnice je jen jednoduchý způsob, jak napsat 3 sady rovnic a vyhnout se opakování:

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

Dále provedeme pravidelná měření ve stejných časových intervalech T sekund a získáme nová měření, která definujeme jako Racc (1), Racc (2), Racc (3) a tak dále. Rovněž vydáme nové odhady v každém časovém intervalu Odpočinek (1), Odpočinek (2), Odpočinek (3) atd.

Předpokládejme, že jsme v kroku n. Máme dvě známé sady hodnot, které bychom chtěli použít:

Zbytek (n -1) - náš předchozí odhad, se zbytkem (0) = Racc (0) Racc (n) - naše aktuální měření akcelerometru

Než budeme moci vypočítat Rest (n), představme si novou naměřenou hodnotu, kterou můžeme získat z našeho gyroskopu a předchozího odhadu.

Budeme tomu říkat Rgyro a je to také vektor skládající se ze 3 složek:

Rgyro = [RxGyro, RyGyro, RzGyro]

Tento vektor vypočítáme po jedné složce. Začneme RxGyro.

Začněme pozorováním následujícího vztahu v našem modelu gyroskopu, z pravoúhlého trojúhelníku tvořeného Rz a Rxz můžeme odvodit, že:

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

Atan2 může být funkce, kterou jste nikdy předtím nepoužili, je podobná atan, kromě toho, že vrací hodnoty v rozsahu (-PI, PI) na rozdíl od (-PI/2, PI/2), jak je vráceno atanem, a to trvá 2 argumenty místo jednoho. Umožňuje nám převést dvě hodnoty Rx, Rz na úhly v celém rozsahu 360 stupňů (-PI na PI). Více o atan2 si můžete přečíst zde.

Takže když známe RxEst (n-1) a RzEst (n-1), můžeme najít:

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

Pamatujte, že gyroskop měří rychlost změny úhlu Axz. Nový úhel Axz (n) tedy můžeme odhadnout následovně:

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

Pamatujte, že RateAxz lze získat z naměřených hodnot ADC z gyroskopu. Přesnější vzorec může použít průměrnou rychlost rotace vypočítanou následovně:

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

Stejným způsobem můžeme najít:

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

Dobře, takže teď máme Axz (n) a Ayz (n). Kam odsud půjdeme, abychom odečetli RxGyro/RyGyro? Z rov. 1 můžeme zapsat délku vektoru Rgyro následovně:

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

Také proto, že jsme normalizovali náš Racc vektor, můžeme předpokládat, že jeho délka je 1 a po otočení se nezměnil, takže je relativně bezpečné psát:

| Rgyro | = 1

Přijměme dočasný kratší zápis pro níže uvedené výpočty:

x = RxGyro, y = RyGyro, z = RzGyro

Pomocí výše uvedených vztahů můžeme napsat:

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

Rozdělíme čitatele a jmenovatele zlomku na SQRT (x^2 + z^2)

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

Všimněte si, že x / SQRT (x^2 + z^2) = sin (Axz), takže:

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

Nyní vynásobte čitatele a jmenovatele zlomku uvnitř SQRT z^2

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

Všimněte si, že z / SQRT (x^2 + z^2) = cos (Axz) a y / z = tan (Ayz), takže nakonec:

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

Když se vrátíme k našemu zápisu, dostaneme:

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

stejným způsobem to zjistíme

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

Nyní konečně můžeme najít:

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

Where Sign (RzGyro) = 1 when RzGyro> = 0, and Sign (RzGyro) = -1 when RzGyro <0.

Jeden jednoduchý způsob, jak to odhadnout, je vzít:

Znamení (RzGyro) = Znamení (RzEst (n-1))

V praxi buďte opatrní, když se RzEst (n-1) blíží nule. V tomto případě můžete gyroskopickou fázi úplně přeskočit a přiřadit: Rgyro = Rest (n-1). Rz se používá jako reference pro výpočet úhlů Axz a Ayz a když je blízko 0, hodnoty mohou přetéct a vyvolat špatné výsledky. Budete v doméně velkých čísel s plovoucí desetinnou čárkou, kde implementacím funkcí tan () / atan () může chybět přesnost.

Zrekapitulujme si tedy, co zatím máme, jsme v kroku n našeho algoritmu a vypočítali jsme následující hodnoty:

Racc - aktuální hodnoty z našeho akcelerometru Rgyro - získané z klidových (n -1) a aktuálních naměřených hodnot gyroskopu

Jaké hodnoty používáme k výpočtu aktualizovaného odhadu Rest (n)? Pravděpodobně jste uhodli, že použijeme obojí. Použijeme vážený průměr, takže:

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

Tento vzorec můžeme zjednodušit vydělením čitatele a jmenovatele zlomku w1.

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

a po nahrazení w2/w1 = wGyro dostaneme:

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

Ve výše uvedeném fóru wGyro nám říká, do jaké míry věříme svému gyroskopu ve srovnání s naším akcelerometrem. Tato hodnota může být zvolena experimentálně, obvykle hodnoty mezi 5..20 spustí dobré výsledky.

Hlavní rozdíl tohoto algoritmu od Kalmanova filtru je v tom, že tato hmotnost je relativně pevná, zatímco v Kalmanově filtru jsou váhy trvale aktualizovány na základě naměřeného hluku naměřených hodnot akcelerometru. Kalmanův filtr je zaměřen na získání „nejlepších“teoretických výsledků, zatímco tento algoritmus vám může poskytnout výsledky „dostatečně dobré“pro vaši praktickou aplikaci. Můžete implementovat algoritmus, který upravuje wGyro v závislosti na některých hlukových faktorech, které měříte, ale pevné hodnoty budou dobře fungovat pro většinu aplikací.

Jsme jen krůček od získání našich aktualizovaných odhadovaných hodnot:

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

Nyní tento vektor opět normalizujeme:

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

A jsme připraveni znovu zopakovat naši smyčku.

Tato příručka se původně objevila na starlino.com, provedl jsem několik lehkých úprav a se svolením ji znovu zveřejnil. Díky Starlino!