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.