Moduł 08 · bonus

Reprezentacje orientacji

Macierz rotacji, kąty Eulera, axis-angle, wektor rotacji, kwaterniony — konwersje, gimbal lock, kiedy czego używać.

Po co tyle reprezentacji?

Pozycja w 3D to wektor — trzy liczby, koniec. Z orientacją jest inaczej: jedna i ta sama orientacja może być zapisana co najmniej na pięć różnych sposobów, każdy z innymi zaletami i wadami. Brak „uniwersalnego" zapisu, który byłby najlepszy do wszystkiego — używamy różnych w różnych sytuacjach:

Macierz rotacji — bezpośrednio mnożymy w obliczeniach geometrycznych (FK, transformacje punktów).
Kąty Eulera (RPY) — wpisuje człowiek w panel sterowania, łatwe do interpretacji wzrokowej.
Axis-angle / wektor rotacji — wzrokowo intuicyjne („obróć o 90° wokół osi pionowej"), używane np. w OpenCV.
Kwaterniony — interpolacja, składanie wielu rotacji, brak singularności — standard w grafice i niskopoziomowym sterowaniu.

Każda z tych reprezentacji opisuje element grupy $SO(3)$ — zbioru obrotów w 3D. Sama grupa to obiekt 3-wymiarowy (3 stopnie swobody), ale różne sposoby jej sparametryzowania używają różnej liczby liczb (3, 4 lub 9) z różnymi ograniczeniami.

Cel tego modułu: zrozumieć każdą z reprezentacji na poziomie intuicyjnym i obliczeniowym, znać pułapki (gimbal lock!), umieć sprawnie konwertować między nimi i wybrać właściwą do zadania.

Interaktywny eksplorator

Trzy slidery RPY definiują tę samą orientację w 3D. Po prawej zobacz ją zapisaną w pięciu reprezentacjach jednocześnie. Manipuluj suwakami i obserwuj, które liczby się zmieniają i jak (kwaterniony chodzą gładko zawsze; kąty Eulera mają nieciągłości przy pitch = ±90°; macierz zawsze ortogonalna).

Sterowanie orientacją (RPY w stopniach)

Roll (X)30.0°

Pitch (Y)45.0°

Yaw (Z)60.0°

1. Macierz rotacji R ∈ SO(3)

  0.3536  -0.5732   0.7392
  0.6124   0.7392   0.2803
 -0.7071   0.3536   0.6124

9 liczb, 6 ograniczeń (ortonormalność kolumn) → 3 stopnie swobody.

2. Kąty Eulera RPY (Z·Y·X)

roll = 30.00° (0.5236 rad)

pitch = 45.00° (0.7854 rad)

yaw = 60.00° (1.0472 rad)

3 liczby, minimalna parametryzacja. Singularność przy pitch = ±90°.

3. Axis-angle (oś, kąt)

k̂ = (0.0391, 0.7728, 0.6335)

θ = 69.36° (1.2105 rad)

4 liczby (oś jednostkowa: 3 + 1 ograniczenie + kąt: 1) → 3 DOF.

4. Wektor rotacji r = θ·k̂

r = (0.0474, 0.9354, 0.7668)

3 liczby. ‖r‖ = θ, kierunek r = oś. Używane w OpenCV i ROS.

5. Kwaternion jednostkowy

w = 0.8224

x = 0.0223

y = 0.4397

z = 0.3604

‖q‖ = 1.000000

4 liczby z 1 ograniczeniem (‖q‖ = 1). Brak singularności. q i −q to ten sam obrót.

Krok 1

Macierz rotacji R ∈ SO(3) — fundament

Najbardziej bezpośrednia reprezentacja: macierz $3 \times 3$ , której kolumny to obrazy bazowych wektorów osi po rotacji:

R = \begin{bmatrix} R\hat{\mathbf{x}} & R\hat{\mathbf{y}} & R\hat{\mathbf{z}} \end{bmatrix}

Cechy: 9 liczb, ale z 6 ograniczeniami:

Ortonormalność kolumn — każda kolumna jest wektorem jednostkowym (3 ograniczenia).
Wzajemna ortogonalność — każda para kolumn jest prostopadła (3 ograniczenia).
Razem: 9 − 6 = 3 stopnie swobody — tyle, ile potrzeba.
det(R) = +1 — wykluczamy odbicia (te miałyby det = −1).

Algebraicznie: $R^\top R = I$ oraz $\det R = +1$ . Inwersja jest banalnie tania:

R^{-1} = R^{\top}

Zalety: kompozycja rotacji = zwykłe mnożenie macierzowe ( $R_{12} = R_1 \cdot R_2$ ). Transformacja wektora — zwykłe $\mathbf{v}' = R\mathbf{v}$ . Forma używana w 99% wzorów kinematyki (FK, jakobian, równania DH).

Wady:

Redundantna (9 liczb dla 3 DOF) — po długich ciągach mnożeń narastają błędy zmiennoprzecinkowe i macierz przestaje być ściśle ortogonalna. Trzeba okresowo reortonormalizować (np. SVD lub Gram-Schmidt).
Niewygodna do interpolacji („średnia" dwóch macierzy nie jest macierzą rotacji).
Mało intuicyjna dla człowieka — patrząc na 9 liczb trudno powiedzieć, jaki to obrót.

Krok 2

Kąty Eulera (RPY) — minimum, ale z gimbal lock

Reprezentujemy rotację jako kompozycję trzech obrotów wokół osi. Najczęściej używane w robotyce: roll, pitch, yaw (przechylenie, pochylenie, odchylenie) — kolejność osi ZYX intrinsic:

R = R_z(\text{yaw}) \cdot R_y(\text{pitch}) \cdot R_x(\text{roll})

Słownie: najpierw obracamy wokół osi X (roll), potem wokół nowej osi Y (pitch), na końcu wokół najnowszej osi Z (yaw). Jeśli czytasz macierze od prawej do lewej — to jest właśnie ta kolejność.

Zalety: tylko 3 liczby (minimalna parametryzacja). Łatwo wpisać w panel, łatwo zinterpretować geometrycznie. Standardowy sposób komunikacji człowiek-robot.

Wady — patologie:

Gimbal lock — przy pitch = ±90° tracimy stopień swobody. Roll i yaw stają się nierozróżnialne (zmiana jednego daje to samo co zmiana drugiego). To nie jest błąd implementacji — to nieusuwalna konsekwencja parametryzacji.
Niejednoznaczność — ta sama orientacja może być zapisana wieloma trójkami liczb (np. (0°, 90°, 30°) ≡ (30°, 90°, 0°)).
Nieciągłość interpolacji — interpolacja liniowa kątów daje paskudne efekty wizualne (skoki przy ±180°).
Plątanina konwencji — patrz niżej.

Konwencje kątów Eulera

„Kąty Eulera" to nie jeden zapis — to cała rodzina. Różnią się kolejnością osi (XYZ, ZYX, ZYZ, ZXZ, …) i tym, czy obroty są intrinsic (wokół osi obracanego ciała) czy extrinsic (wokół osi nieruchomego świata):

Praktyczna porada: zawsze dokumentuj, którą konwencję używasz. „RPY" w robotyce zwykle oznacza ZYX intrinsic (ROS, MoveIt, OpenRAVE), ale w lotnictwie i grafice 3D bywa różnie. Konwersja źle dopasowanych konwencji to klasyczne źródło bugów w integracji z czujnikami i bibliotekami zewnętrznymi.

Krok 3

Gimbal lock — ilustracja na żywo

Każda rotacja w 3 wymiarach to złożenie obrotów wokół trzech osi. Wyobraź sobie żyroskop z trzema obręczami — każda umożliwia obrót wokół innej osi, połączone w łańcuch. Gimbal lock zachodzi, gdy dwie z tych osi się pokrywają:

Trzy obręcze gimbala

Yaw (Z)0.0°

Pitch (Y)70.0°

Roll (X)20.0°

Spróbuj: ustaw pitch ≈ 90°. Czerwona (yaw) i niebieska (roll) obręcze zaczynają obracać się wokół tej samej osi — tracisz jeden stopień swobody. Zmiana yaw daje teraz to samo co zmiana roll. To jest gimbal lock — nieusuwalna patologia kątów Eulera.

Gimbal lock w fizycznym żyroskopie utrudnia pracę pilotom (znany problem w Apollo 11), a w robotyce destabilizuje regulatory posługujące się kątami Eulera w okolicach pitch ≈ ±90°. Rozwiązanie: używać kwaternionów lub macierzy rotacji jako reprezentacji wewnętrznej (kąty Eulera tylko do wejścia/wyjścia interfejsu użytkownika).

Krok 4

Axis-angle (oś-kąt) i wektor rotacji

Twierdzenie Eulera o obrotach: każdy obrót w 3D można opisać jako jeden obrót o pewien kąt θ wokół jednej osi k̂. To jest fakt geometryczny — stosuje się również do złożenia 100 wcześniejszych obrotów.

Kierunek rotacjiwokół osi określamy regułą prawej ręki: jeśli kciuk wskazuje wzdłuż osi k̂, palce wskazują kierunek obrotu dla θ > 0. Ta sama konwencja definiuje orientację układu współrzędnych prawoskrętnego (X×Y = Z):

Reguła prawej ręki dla kartezjańskich osi i obrotów — Reguła prawej ręki — orientacja osi prawoskrętnego układu kartezjańskiego oraz kierunek obrotu wokół osi (kciuk = oś, palce = kierunek dodatniego obrotu).
Źródło: Wikimedia Commons · autor: User:Acdx, cmglee · licencja: CC BY-SA 4.0

(\hat{\mathbf{k}},\,\theta), \quad \|\hat{\mathbf{k}}\| = 1, \quad \theta \in [0, \pi]

Wzór Rodriguesa (axis-angle → macierz):

R = I + \sin\theta\,[\hat{\mathbf{k}}]_\times + (1-\cos\theta)\,[\hat{\mathbf{k}}]_\times^{\,2}

gdzie $[\hat{\mathbf{k}}]_\times$ to macierz antysymetryczna z osi k̂:

[\hat{\mathbf{k}}]_\times = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix}

Wektor rotacji — zapis kompaktowy

Połącz oś i kąt w jeden wektor 3-wymiarowy:

\mathbf{r} = \theta\,\hat{\mathbf{k}} \in \mathbb{R}^3

Długość wektora = kąt obrotu, kierunek = oś. To jest logarytm SO(3) w sensie algebr Liego — używamy go jako twist error w solverach iteracyjnych (moduł 3) oraz jako standard w OpenCV (cv::Rodrigues) i ROS.

Zalety: 3 liczby, gładkie odwzorowanie w okolicy zera (identyczność). Idealne do reprezentacji małych rotacji (np. w iteracjach IK lub korekcjach dryfu IMU).

Wady: nieciągłe przy θ → 0 (oś staje się nieokreślona — choć produkt θ·k̂ pozostaje gładki) i przy θ → π (dwie antypodalne osie dają tę samą rotację).

Krok 5

Kwaterniony jednostkowe — standard nowoczesnej robotyki

Najsubtelniejsza, ale najbardziej praktyczna reprezentacja. Kwaternion to czwórka liczb:

q = (w, x, y, z) = w + x\mathrm{i} + y\mathrm{j} + z\mathrm{k}

z mnożeniem zdefiniowanym przez Hamiltona (1843). Dla kwaternionów jednostkowych ( $\|q\| = 1$ ) istnieje piękna tożsamość — każdy obrót w 3D zapisuje się jako:

q = \left(\cos\frac{\theta}{2},\; \hat{k}_x \sin\frac{\theta}{2},\; \hat{k}_y \sin\frac{\theta}{2},\; \hat{k}_z \sin\frac{\theta}{2}\right)

( $\theta$ , $\hat{\mathbf{k}}$ jak w axis-angle). Cztery liczby z jednym ograniczeniem (norma = 1) → 3 stopnie swobody, tyle ile trzeba.

Kompozycja rotacji = mnożenie kwaternionów (po regułach Hamiltona):

q_{12} = q_1 \cdot q_2 \quad\text{(najpierw } q_2\text{, potem } q_1\text{)}

Inwersja = sprzężenie (zmień znak części urojonej):

q^{-1} = (w,\; -x,\; -y,\; -z) \quad\text{(dla } \|q\|=1\text{)}

Zalety kwaternionów

Brak singularności — gimbal lock nie istnieje. Gładkie odwzorowanie do $SO(3)$ wszędzie.
Tania kompozycja — 16 mnożeń vs 27 dla macierzy 3×3.
Numerycznie stabilne — mała utrata jednostkowości łatwo naprawiana przez normalizację (1 dzielenie).
Naturalna interpolacja — SLERP („sferyczna interpolacja") daje gładkie obroty z stałą prędkością kątową.
Mała pamięć — 4 floats vs 9 dla macierzy.

Wady kwaternionów

Mniej intuicyjne — patrząc na (0.7, 0.0, 0.0, 0.7) trudno powiedzieć od razu, jaki to obrót.
Pokrycie podwójne — kwaterniony $q$ i $-q$ reprezentują tę samą rotację. Trzeba o tym pamiętać przy porównaniach i interpolacji (SLERP wewnętrznie sprawdza to przez iloczyn skalarny).
Wymaga normalizacji po długich rachunkach (1 sqrt + 4 dzielenia, tańsze niż reortonormalizacja macierzy).

SLERP — interpolacja, którą zawsze chciałeś

Sferyczna interpolacja liniowa (Spherical Linear Interpolation, Shoemake 1985) — gładkie przejście między dwoma orientacjami $q_1$ i $q_2$ :

\text{slerp}(q_1, q_2, t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}\,q_1 + \frac{\sin(t\Omega)}{\sin\Omega}\,q_2

gdzie $\Omega$ = kąt między $q_1$ i $q_2$ w 4D. Wynik: stała prędkość kątowa od początku do końca, krótszą stroną sfery 4D. Spróbuj zinterpolować dwie orientacje suwakami w eksploratorze powyżej — kwaterniony dają zawsze ciągłą interpolację, natomiast kąty Eulera tracą ciągłość przy ±180°.

Kiedy czego używać — praktyczna ściąga

Sytuacja	Najlepsza reprezentacja	Dlaczego
FK, transformacje punktów, jakobian	Macierz	Bezpośrednie mnożenie macierzy z wektorami
Wejście/wyjście z user interface	Kąty Eulera (RPY)	Człowiek rozumie „przekręć o 30°"
Twist error w solverach iteracyjnych	Wektor rotacji (log SO(3))	3 liczby, gładkie w okolicach 0, dodawanie ma sens
Interpolacja trajektorii	Kwaternion (SLERP)	Gładkie, stała prędkość kątowa, brak singularności
Składanie wielu rotacji w łańcuchu	Kwaternion	Tańsze i numerycznie stabilniejsze niż macierze
Komunikacja z OpenCV / ROS / niskopoziomowe sterowanie	Wektor rotacji lub kwaternion	Standardy bibliotek
Prezentacja / wizualizacja / GUI	Kąty Eulera (intuicyjnie) lub axis-angle	Czytelne dla człowieka
Małe korekcje (np. dryf IMU)	Wektor rotacji	Dodawanie ma sens dla małych $\theta$ , brak nieciągłości

Dwie najczęstsze reguły praktyczne

Wewnątrz silnika obliczeniowego używajjednej reprezentacji konsekwentnie: zwykle macierzy (FK, jakobian) lub kwaternionów (interpolacja, łańcuchy transformacji). Nie konwertuj tam-i-z-powrotem niepotrzebnie.
Konwertuj na granicach systemu — gdy dane wchodzą lub wychodzą (UI, czujniki, biblioteki zewnętrzne) — i dokumentuj konwencję. Każda zmiana konwencji to potencjalny bug.

Dla zaawansowanych: związek z algebrami Liego

Wszystkie cztery „minimalne" reprezentacje (Eulera, axis-angle, wektor rotacji, kwaternion) to różne sposoby parametryzowania rozmaitości $SO(3)$ . Wektor rotacji ma szczególne znaczenie — jest wykładniczą mapą algebry Liego:

R = \exp([\mathbf{r}]_\times), \quad \mathbf{r} = \log(R)^{\vee}

Tu $\exp$ i $\log$ to macierzowe wykładnicze i logarytm. Wzór Rodriguesa to jest po prostu rozwinięcie szeregu Taylora dla exp na macierzach antysymetrycznych. Ten związek pozwala uogólniać metody numeryczne (np. „średnia" rotacji = średnia ich logów + powrót do SO(3) przez exp). Temat dla osobnego wykładu — w robotyce wystarczy wiedzieć, że istnieje i że biblioteki typu scipy.spatial.transform czy Sophus (C++) udostępniają te operacje gotowe.

Co dalej

Po tym module powinno być znacznie jaśniejsze, dlaczego w module 3 (jakobianowych) używamy logarytmu SO(3) jako twist error, a w module 1 (analitycznym) — bezpośrednio elementów macierzy R. Każda decyzja o reprezentacji ma swoje uzasadnienie. Praktyczna wskazówka na koniec: jeśli pracujesz nad kontrolerem robota, silnikiem 3D albo systemem AR/VR — zacznij od zaimplementowania klasy Rotation ze wszystkimi pięcioma reprezentacjami (jak scipy.spatial.transform.Rotation albo three.js Quaternion) i konsekwentnie używaj tylko jednej wewnętrznej, konwertując na granicy.