3D forgatás giroszkóp adatai alapján

Grafika: GUI, OpenGL

Sziasztok!

Elkezdtem egy giroszkóp IC-vel foglalkozni, ez lényegében egy szögsebességet az vissza másodpercenként 800-szor. Ha csak 1 tengely adataival dolgozok, akkor egyszerűen egy ilyen képlettel tudom az elfordulást megkapni: szög += mértSzögsebesség * érzékenység / 800.

A gond ott kezdődik, hogy ha több(3) tengely körül is elforgatom. Ilyenkor a giroszkóp tengelyei máshogy állnak mint a forgatás előtt, ezért a fenti dolog nyilván nem működik.

Gondoltam rá, hogy mátrixokkal talán meg lehetne oldani, de nem sikerült, ugyanis az IC a 3 tengely szögsebességét egyszerre adja vissza, mátrixokkal pedig csak egymás után lehet az egyes tengelyek mentén forgatni, és a sorrend számít.

Ezek után több helyen olvastam, hogy az igazi megoldás a kvaterniók használata. Ezekhez nem értek és amiket találtam leírásokat leginkább matematikai oldalról közelítették meg, és nem volt bennük, hogy hogyan is lehet ezeket forgatások leírására használni.

Nincs valami ötletetek, mivel lehet egyáltalán ilyet megoldani, merre érdemes elindulni, hol lehet ennek utánanézni?

Erről az IC-ről van szó: http://www.st.com/internet/analog/product/250373.jsp
Egy PIC32-es mikrovezérlő kapja meg az adatait. (A számítási teljesítménye nem túl nagy)

Köszönettel: raron

  • 11907 megtekintés

( uid_15426 | 2012. 08. 18., szo – 22:56 )

Lantos Bela konyveit keresd meg, azon belul is a robotiranyitassal foglalkozo kisalaku, kek konyvecsket. BME villanyon/infon bizonyos szakiranyokon tananyag.

Elkezdhetnenk egyebkent itt magyarazni a kvaterniokat, de e tema eseteben sokkal hasznosabb, ha nekiallsz szep szisztematikusan olvasni.

----------------------
while (!sleep) sheep++;

( BaT | 2012. 08. 19., v – 00:19 )

Ha jól értem az a probléma, hogy forgatáskor az ic referenciatengelye elfordul, neked viszont egy külső, rögzített referenciatengelyhez kellene az eredő elfordulást meghatározni.

Én valahogy úgy csinálnám, hogy először kiszámolnám az ic tengelyeihez az aktuális elfordulási vektort, majd ezt transzformálnám a külső referenciatengelyhez a tárolt elfordulás alapján (visszaforgatás), végül a kapott vektort hozzáadnám a tárolt elfordulás vektorhoz. Vagy ha szögeket tárolsz, akkor a kapott vektort szögekre konvertálva a kapott szögeket hozzáadnám a tárolt szögekhez.

Ebben a legbonyolultabb művelet a forgatás transzformáció, de nem hiszem, hogy másodpercenként 800-szor végrehajtva ez problémát okozna egy picnek. :)

Ilyesmivel próbálkoztam, de a mátrixos forgatásnál nem mindegy, hogy milyen sorrendben csinálod (mikor melyik tengely körül forgatsz).

Az IC azt mondja meg, hogy mindhárom tengely mentén egyszerre mennyit fordult. Mátrixszal csak sorrendben tudom forgatni, külön-külön az egyes tengelyek mentén. (Legalábbis még mással nem találkoztam.)

(A datasheet 3.1-es pontja nálam a Block diagram.)

Ha jól értelek, akkor van 3 szögsebesség vektorom, mindegyik külön-külön tengely irányába néz, az irányát jobb-kéz-szabály-ból tudjuk, és a nagysága arányos a sebességgel.
Ha ezeket vektoriálisan összeadom akkor kapok egy eredő vektort, ami a 3 tengely menti szögsebességet egybe adja vissza? (Ilyesmivel még nem próbálkoztam.)
Ezzel pontosan mit is tudok csinálni?

szerk.: lehet, hogy ez nagy hülyeség lesz, de ha ezeket a vektorokat osztom 800-al(frissítési időköz) és összegzem őket, akkor kaphatok egy vektort, aminek az iránya megad egy forgatási tengelyt a nagysága megy egy szöget? (Ezt mondjuk meg tudom etetni a glRotate(szög, tengely) -el?)

"(A datasheet 3.1-es pontja nálam a Block diagram.)"

Jogos, az application notest néztem.

Nem akartalak félrevezetni, mégis sikerült, elnézést kérek. Arról van szó, hogy annyira hangsúlyoztad az egyszerre szót, hogy olyan késői órán úgy értelmeztem, hogy egy skalár. Ezért emeltem ki, hogy egy 3 dimenziós vektort kapsz vissza, de nyilván ezt te is tudtad.

Ha a szögekkel történő számítások gyorsításáról lenne szó, akkor 360 fokot felbontanám, és mint egy iránytűt, úgy kezelném. Modulo 45 fokkal számolva kisebb számokat kellene kezelni, így gyorsulhatnak a számítások, ha másodpercenként 800 műveletre lenne igény. (csak 1ötlet, ami nem biztos, hogy ezen esetben jó) Végül egy ZXspectrum proc elég lenne:)

---
--- A gond akkor van, ha látszólag minden működik. ---
---

( uid_2716 | 2012. 08. 19., v – 15:51 )

Pontosan mi az, amit megkapsz az IC-től, és mi az, amit ebből elő szeretnél állítani? Rögzített, külső tengelyek körüli, egymástól független elfordulási rátákat kapsz? Vagy az IC 3 db egydimenziós szögsebességet ad vissza, ahol bármely tengely körüli forgatás elforgatja a másik két tengelyt?

http://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles

Ebben az esetben a szögsebességekkel és az eltelt idővel egymástól függetlenül közelítheted a három (mozgó tengely körüli) szöget, az általad leírt képletet futtatva három példányban, egymástól függetlenül. (Bár a numerikus stabilitására nem vennék mérget.) Ezt a három belső forgatást pedig átszámolod először külső forgatások sorozatára (sorrendcsere),

http://en.wikipedia.org/wiki/Euler_angles#Double_naming_problem

aztán egy forgatómátrixba

http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Ro…

Alapvetően az volt az alap elképzelés, hogy tudjuk egy omniwheel-es robot (ilyesmi) felülről ránézve milyen szögbe áll / mennyit fordult el. Ennek a számítása nagyjából a fenti képlettel megy, de ha nem teljesen sík a terep akkor ugye nem csak az egyik (Z) tengely mentén lesz elfordulás.
Illetve jött az ötlet, ha már van egy 3 tengelyes gyro a robotban, akkor PC-n meg lehetne jeleníteni egy 3D-s modellt úgy forgatva, ahogy a robot áll.

Az IC-től 3 db 16 bites előjeles egészet kapok vissza, egy-egy minden tengelyekhez, amit egy konstanssal megszorozva deg/sec-et kapok.
Az IC tengelyei, amik "mentén" mér, az IC tokjával (a NYÁKkal, a robottal) együtt forognak.

Nem (csak) konkrét megoldás érdekel, jól jön az is, ha tudom minek/miknek kell utánanézni, hogy ilyet meg tudjak oldani.

Esetleg egy filleres gyorsulasmero meg egy szinten filleres iranytu mennyire lenne maceras?
Elobbibol kideritheto a lefele irany (ha pl. lejtos teruleten mozogna), utobbitol meg kaphat egy abszolut iranyt.
Sajnos a giroszkopnak mindenkepp van valami hibaja, ami idovel osszejon, es mar csak az idoben kozeli relativ elfordulast fogod megtudni belole. Az iranytu nem teljesen pontos, de abszolut iranynak teljesen jo.

Ha nem sorozatban gyartando robotrol van szo, ahol minden cent szamit, es van eleg labad a uC-en, meggondolando.

--
ezt tényleg ennyire nem értitek? - turdus :)

Nem sorozatgyártás, csak hobbi.
Gyorsulásmérő és iránytű is tervbe van véve, de egyelőre csak gyro van. (Sajnos nem annyira filléres dolgok modulba lehet kapni MO-on, de úgy 5-10kFt, rendelésnél meg a posta drága.)
Lehet, hogy egyszerűbben meg lehetne oldani ezekkel, de kíváncsi vagyok, hogy ezzel hogy lehet megoldani.

Omniweel robothoz az iránytűt javaslom. Amit mi csináltunk abban fényesen bevált.
Van olcsó is pl:http://dx.com/p/gy-651-4-axis-fly-control-sensor-electronic-compass-atm…
20 USd és ingyen elhozzák. Bonuszként van rajta egy nyomásmérő is.
MIvel három tengelyes így ha nincs a közelben vas vagy mágnes a dőlést is számolhatod belőle.

Vagy ez: http://dx.com/p/gy-26-digital-compass-sensor-module-green-dc-3-5v-149861
Ebben csak magnetométer van. Szintén egy huszas.

Mindkettő I2C buszos, tehát csak két láb kell neki.

( DieHappy | 2012. 08. 19., v – 16:15 )

Amit meg lehet csinalni kvaterniokkal, azt meg lehet csinalni matrix-szal.
Az '3 tengely' koruli forgatast nem konnyu leirni, de lehet kozeliteni. Ez egy R(fi, eta, kappa) -matrix, ha veszed a derivaltjat kis szogekre akkor valami ilyesmit kapsz:

R~= 1 + T_1*fi + T_2*eta + T_3*kappa

A T_1, T_2 es T_3 matrixok +-1 es 0 elemekbol allnak: +1 ahol az adott szog szerinti forgatas matrixaban szinusz, -1 ahol koszinusz es 0 ahol 0 allt. Ezek az ugynevezett 3D forgatasi Lie-csoport generator matrixai. Ha a szogek nagyon kicsik, akkor a 3 szog egyuttes forgatasanak ez egy jo kozelito matrixa (az egysegmatrix 1, nem resze R elso derivaltjanak, az a Taylor sor elso tagja).

A nagy szogekre valo forgatas ezeknek a T matrixszoknak a hatvanyabol jonnek ki :

R = 1 + Sum_{i=1..inf} (T_1*fi + T_2*eta + T_3*kappa)^i/i! = exp( T_1*fi + T_2*eta + T_3*kappa )

Gyakorlatilag egy matrix-exponencialis. Kis szogekre eleg az elso 1-2-3 tagot hasznalni, mert az i! faktorialis levagja a sort.
Lehet, hogy Euler-szogekkel is meg lehet oldani.

Altlaban a 10 fok alattiakat szoktak kicsinek venni, 45 foknal mar sin(pi/4) =1/sqrt(2) nagy az elteres,
de pi/10 =~30 fok is elfogadhato. Ez is jol kozelitheto a sum T negyzettel (mas sorfejtes is van, pl (1 + T/n)^n , konnyebb is szamolni ).
De ha tenyleg 800 meres/sec van, akkor a giroszopnak ~80 fordulat/sec kellene ahhoz, hogy ezt atlepje.

( r3flow | 2012. 08. 23., cs – 07:51 )

Ilyesmit szeretnél? A videó alatt van forráskód is, bár lehet nem fog vele messzire jutni. Magában a gyroval nem mész messzire a hibák össze fognak adódni, rövid idő alatt nagyon pontatlan lesz. Egy lehetséges megoldás, hogy ezt a hibát X/Y tengelyen gyorsulásmérővel, Z tengelyen iránytűvel kompenzálod. A három szenzor adataiból egy speciálisan az adott alkalmazáshoz méretezett és a szenzorodhoz megfelelően paraméterezett (matlab vagy hasonló legyen kéznél) kalman filterrel fogod megkapni azt ami neked kell (amit a videón látsz), azaz a végén három szöged lesz, tengelyenként az elfordulás szöge (glRotate-hoz). Többet sajnos egyelőre nem tudok ehhez hozzá tenni jelenleg én is ismerkedem a témával, leghamarabb talán NagyZ tudna segíteni, az RC autó projektjében már eljutott eddig a pontig és ő ért az ehhez szükséges matekhoz is. :)

Igen, ilyesmit szeretnék. Egyelőre még nem azzal van a gond, hogy túl zajos a kimenete, hanem, hogy a PC-n nem arra fordul a 3D modell, mint amerre a gyro-t forgatom.

Az eredeti kérdés az volt, hogy mivel lehet ezt megoldani, mert ha tudom akkor pontosan utánanézek, veszek könyveket, stb... Ha ez megy és túl zajos akkor kell a Kalman szűrő, nem?