Předmět BROB - Robotika. 4 Kvaternionová matematika

Transkript

1 Předmět BROB - Robotika Jméno Ročník Řeháček Tomáš , Blaha Vít , Michna Jakub Studijní skupina AMT Kontroloval Hodnocení Dne Číslo úlohy Název úlohy 4 Kvaternionová matematika Úkol Seznamte se s oblastí kvaternionové matematiky. Vytvořte základní rutiny pro operace s kvaterniony. Vytvořte knihovnu testovacích funkcí pro otestování základních problematických operací s kvaterniony. Zaměřte se na problém zvaný Gimbal Lock. Kvaternion (Quaternion) V matematice se pojmem kvaternion označuje nekomutativní rozšíření komplexních čísel. Poprvé byly kvaterniony popsány Williamem Rowanem Hamiltonem v roce Nejdříve byly považovány za nevhodný a uměle vykonstruovaný objekt, jelikož porušovaly komutativní zákon ab = ba, postupně ale našly uplatnění jak v teoretické fyzice, tak v aplikované matematice (přestože mnohdy se jejich použití lze za jistou cenu vyhnout s pomocí vektorů). Ve skutečnosti je však mezi kvaterniony a 4-rozměrnými vektory principiální rozdíl: Operace dělení je mezi dvěma kvaterniony definována, zatímco mezi dvěma vektory tato operace vůbec neexistuje. Každý kvaternion je lineární kombinací prvků 1, i, j, k. A představuje orientaci ve 3D prostoru. Použitá souřadnicová soustava

2 Zápis kvaternionu: q = (w,(x, y, z))=(w,v) kde w je skalár a v je vektor v trojrozměrném prostoru. Pro složky kvaternionu platí následující vztahy: i 2 = j 2 = k 2 = ijk = 1 i*j=k j*i=-k j*k=i k*j=-i k*i=j i*k=-j Použité vzorce Převod eulerových úhlů na kvaternion: yaw=> ψ, pitch=> θ, roll=> φ Q = w x y z = cos(φ/2)cos(θ /2)cos(ψ/2) - sin(θ/2)sin(φ/2)sin(ψ/2) sin(φ/2)sin(θ /2)cos(ψ/2) + cos(θ/2)cos(φ/2)sin(ψ/2) sin(φ/2)cos(θ /2)cos(ψ/2) + cos(θ/2)sin(φ/2)sin(ψ/2) cos(φ/2)sin(θ /2)cos(ψ/2) - sin(θ/2)cos(φ/2)sin(ψ/2) Převod kvaternionu na eulerovy úhly: w,x,y,z složky kvaternionu yaw = atan2(2*y*w-2*x*z, 1-2*y2-2*z2) pitch = asin(2*x*y + 2*z*w) roll = atan2(2*x*w-2*y*z, 1-2*x2-2*z2) pokud dojde ke gimbal locku, splývá rotace yaw a roll, proto je yaw = (±) 2 * atan2(x, w) roll = 0 Převod vektoru na kvaternion: -chybějící údaj o rotaci vektoru zvolen w=0, pokud je vektor nulový w=1 vektor =>kvaternion (x,y,z) =>(0,(x,y,z)) (0,0,0) =>(1,(0,0,0)) Převod kvaternionu na vektor: -dochází zde ke ztrátě informace o rotaci kvaternionu kolem vlastní osy kvaternion=>vektor (w,(x,y,z)) =>(x,y,z)

3 Součet kvaternionů: Q 1 +Q 2 =(w 1 +w 2, x 1 +x 2, y 1 +y 2, z 1 +z 2 ) Součin kvaternionů: - představuje kombinovanou rotaci dvou quaternionů v prostoru (součet eulerových úhlů) Q1*Q2= w w 1 *w 2 - x 1 *x 2 - y 1 *y 2 - z 1 *z 2 x w = 1 *x 2 + x 1 *w 2 + y 1 *z 2 - z 1 *y 2 y w 1 *y 2 - x 1 *z 2 + y 1 *w 2 + z 1 *x 2 z w 1 *z 2 + x 1 *y 2 - y 1 *x 2 - z 1 *w 2 Součin kvaternionu a konjungovaného kvaternionu: - výsledkem je quaternion představující rotaci, jež musíme provést, abychom se dostali z orientace quaternionu Q1 do orientace quaternionu Q2 (rozdíl eulerových úhlů) Q1*konj(Q2)= w w 1 *w 2 - x 1 *(-x 2 ) - y 1 *(-y 2 ) - z 1 *(-z 2 ) x w 1 *(-x 2 ) + x 1 *w 2 + y 1 *(-z 2 ) - z 1 *(-y 2 ) = y w 1 *(-y 2 ) - x 1 *(-z 2 ) + y 1 *w 2 + z 1 *(-x 2 ) z w 1 *(-z 2 ) + x 1 *(-y 2 ) - y 1 *(-x 2 ) - z 1 *w 2 Kde konjugace je: konj(q) = w w x -x = y -y z -z Normalizace kvaternionu: Q= w/norma x/norma y/norma z/norma Normalizace vektoru: vektor= x/norma y/norma z/norma

4 Návod k používání programu -Vstupní data je možno zadávat ve tvaru vektoru, eulerových úhlů nebo kvaternionu, eulerovy úhly je pak možno zadávat ve stupních nebo v radiánech. -Kliknutím na tlačítko Převod se převedou zadané úhly nebo vektory na odpovídající kvaterniony. Poté je možno kliknutím na odpovídající tlačítko (Q1+Q2, Q1*Q2 nebo Q1*konj(Q2)) provést příslušnou operaci. Výsledek se zobrazuje ve formě normovaného kvaternionu, normovaného vektoru, a eulerových úhlů. -Zatrhnutím položky Počítat přímo z úhlů (z vektorů) odpadá nutnost klikat na tlačítko Převod před provedením požadované operace. -Tlačítka Q1 převést a Q2 převést slouží k převodu zadaného kvaternionu na eulerovy ůhly a vektor. - Zadávání eulerových úhlů je možné provést posunutím příslušného posuvníku, nebo vepsáním hodnoty do příslušné kolonky. Do nevyplněných kolonek bude dosazena 0. Posuvníky jsou v rozmezí <-180 ; 180> pro Yaw a Roll a <-90 ; 90> Pitch

5 Screenshoty programu

6 Závěr Program slouží k výpočtům s kvaterniony. Základní funkce programu jsou: Převod eulerových úhlů na kvaternion Převod vektoru na kvaternion (nekonečně mnoho řešení pro každý vektor) Převod kvaternionu na eulerovy úhly Převod kvaternionu na vektor (dochází ke ztrátě informací) Základní matematické operace s kvaterniony Výpočty s eulerovými úhly přes kvaterniony Program hlásí, pokud při výpočtu dojde k tzv. gimbal locku. Hlavní zdroje informací: