1 Orientacja w 3D: macierz obrotu, kąty Eulera, oś i kąt, kwaterniony

0 Prolog: czwarty przegub na Gwiazdkę

Statek Apollo wiedział, jak jest zorientowany, dzięki platformie żyroskopowej zawieszonej w trzech pierścieniach kardanowych. Platforma trzymała stały kierunek w przestrzeni, a kąty między pierścieniami odczytywane z czujników to były wprost trzy kąty orientacji statku. Inżynierowie wiedzieli, że przy jednym ustawieniu dwa pierścienie układają się w jednej płaszczyźnie i platforma traci jeden stopień swobody. Zamiast dodawać czwarty pierścień, woleli prostszy i lżejszy układ z ostrzeżeniem: komputer sygnalizował zbliżanie się do tego położenia, a dalej zamrażał platformę i trzeba ją było ustawiać na nowo według gwiazd. Michael Collins, który w czasie pobytu modułu księżycowego Apollo 11 na Księżycu krążył samotnie po orbicie i sam pilnował orientacji statku, zażartował w rozmowie z Houston, czy ktoś nie przysłałby mu czwartego przegubu na Gwiazdkę.

Ta sama osobliwość siedzi w trzech kątach, którymi opisujemy orientację samolotu na co dzień. Nie jest wadą konkretnego urządzenia, tylko geometrii: żaden zapis orientacji trzema liczbami nie działa wszędzie. Dlatego kontrolery lotu liczą na czterech liczbach.

Pytanie na start. Obracasz książkę o $90^\circ$ wokół osi północ–południe, a potem o $90^\circ$ wokół pionu. Czy da się ją przeprowadzić z położenia wyjściowego do końcowego jednym obrotem? Jeśli tak, wokół jakiej osi i o jaki kąt? Odpowiedź w części 3 i w przykładzie 6.2.

1 Układy odniesienia i macierz orientacji

Potrzebujemy dwóch układów osi.

Układ nawigacyjny NED (ang. north, east, down): oś $x$ na północ, $y$ na wschód, $z$ w dół. Przy lotach blisko Ziemi i na krótkich odcinkach traktujemy go jak układ inercjalny. Oś $z$ w dół wygląda dziwnie, ale ma zalety: wysokość to $-z$ , a dodatni kąt pochylenia oznacza nos do góry.
Układ ciała, sztywno związany z samolotem: $x$ do przodu przez nos, $y$ wzdłuż prawego skrzydła, $z$ w dół przez podłogę kabiny. Prędkość kątową samolotu zapisujemy w tych osiach jako $\boldsymbol\omega = (p, q, r)$ : przechylanie, pochylanie, odchylanie. To mierzą żyroskopy przykręcone do kadłuba.

Orientacja to położenie układu ciała względem układu nawigacyjnego. Najpełniejszy jej opis to macierz, która przelicza składowe dowolnego wektora z osi ciała na osie NED. W lekcji 2.2 Przekształcenia liniowe: obroty, zmiana bazy, transformacja Lorentza widzieliśmy, że macierz przekształcenia ma w kolumnach obrazy wektorów bazy. Tu oznacza to coś bardzo konkretnego:

Macierz orientacji (macierz kosinusów kierunkowych)

\mathbf v^{n} = R\,\mathbf v^{b},\qquad R = \begin{bmatrix} \mathbf x_b^{n} & \mathbf y_b^{n} & \mathbf z_b^{n}\end{bmatrix},\qquad R^{\mathsf T}R = I,\quad \det R = 1 .

Kolumny $R$ to osie samolotu zapisane w osiach NED. Pierwsza kolumna mówi, gdzie wskazuje nos. Przeliczenie w drugą stronę to transpozycja: $\mathbf v^{b} = R^{\mathsf T}\mathbf v^{n}$ .

Każdy element $R_{ij}$ to iloczyn skalarny dwóch wektorów jednostkowych, czyli kosinus kąta między osią $i$ układu NED a osią $j$ układu ciała. Stąd druga nazwa: macierz kosinusów kierunkowych (ang. direction cosine matrix, DCM).

Macierz ma 9 liczb, ale nie są niezależne. Warunek $R^{\mathsf T}R = I$ to 6 równań (trzy kolumny mają długość 1, trzy pary kolumn są prostopadłe). Zostają 3 stopnie swobody, tyle co w intuicji: orientację wyznaczają trzy liczby. Macierz jest zapisem nadmiarowym, ale bez żadnych osobliwości i z prostym składaniem: obrót $R_1$ , a po nim obrót $R_2$ (oba opisane w osiach stałych) to $R_2R_1$ .

Zadania · część 1

3 zadania · ok. 20 min

Pierwsza kolumna macierzy orientacji samolotu to $[0{,}814;\ 0{,}470;\ -0{,}342]$ . W którą stronę świata leci samolot i czy nos jest uniesiony?

Odpowiedź
Nos wskazuje na północny wschód (azymut $\operatorname{atan2}(0{,}470;\ 0{,}814) = 30^\circ$ ) i do góry, bo składowa „w dół” jest ujemna: $\arcsin 0{,}342 = 20^\circ$ nad poziomem.
Ile niezależnych równań daje warunek $R^{\mathsf T}R = I$ dla macierzy $3\times 3$ ? Dlaczego nie 9?

Odpowiedź
6. Macierz $R^{\mathsf T}R$ jest symetryczna, więc równania pod przekątną powtarzają te nad nią: 3 na przekątnej (długości kolumn) i 3 nad nią (prostopadłość par). Zostaje $9 - 6 = 3$ stopnie swobody.
W osiach NED przyspieszenie grawitacyjne to $[0;\ 0;\ g]$ . Jakie składowe ma w osiach ciała? Policz dla pochylenia $20^\circ$ i przechylenia $10^\circ$ (macierz z przykładu 6.1, trzeci wiersz $[-0{,}342;\ 0{,}163;\ 0{,}925]$ ).

Odpowiedź
$\mathbf g^b = R^{\mathsf T}[0;0;g]$ to $g$ razy trzecia kolumna $R^{\mathsf T}$ , czyli trzeci wiersz $R$ : $g\,[-0{,}342;\ 0{,}163;\ 0{,}925]$ . Ogólnie $g\,[-\sin\theta;\ \sin\varphi\cos\theta;\ \cos\varphi\cos\theta]$ . Ten wzór zobaczymy jeszcze raz w lekcji 3 Fuzja czujników w 3D: estymacja orientacji na kwaternionach: z niego akcelerometr „wie”, gdzie jest dół.

2 Kąty Eulera i blokada przegubu

Pilot nie myśli macierzą. Myśli trzema kątami: kurs, pochylenie, przechylenie. W lotnictwie to kąty Eulera w kolejności 3-2-1 (oś $z$ , potem $y$ , potem $x$ ). Zaczynamy od samolotu ustawionego osiami zgodnie z NED i obracamy go kolejno:

o kąt odchylenia $\psi$ wokół pionu (kurs);
o kąt pochylenia $\theta$ wokół nowej osi skrzydeł;
o kąt przechylenia $\varphi$ wokół nowej osi nosa.

Kąty Eulera 3-2-1 (odchylenie, pochylenie, przechylenie)

R = R_z(\psi)\,R_y(\theta)\,R_x(\varphi) = \begin{bmatrix} c_\theta c_\psi & s_\varphi s_\theta c_\psi - c_\varphi s_\psi & c_\varphi s_\theta c_\psi + s_\varphi s_\psi\\ c_\theta s_\psi & s_\varphi s_\theta s_\psi + c_\varphi c_\psi & c_\varphi s_\theta s_\psi - s_\varphi c_\psi\\ -s_\theta & s_\varphi c_\theta & c_\varphi c_\theta \end{bmatrix}

( $c_\theta = \cos\theta$ , $s_\theta = \sin\theta$ itd.) Kąty z macierzy: $\theta = -\arcsin R_{31}$ , $\;\varphi = \operatorname{atan2}(R_{32}, R_{33})$ , $\;\psi = \operatorname{atan2}(R_{21}, R_{11})$ . Zakresy: $\psi, \varphi \in (-180^\circ, 180^\circ]$ , $\theta \in [-90^\circ, 90^\circ]$ .

Dlaczego macierze w tej kolejności? Obroty „wokół nowych osi” można zastąpić obrotami wokół osi stałych wykonanymi w odwrotnej kolejności: najpierw $\varphi$ wokół $x$ , potem $\theta$ wokół $y$ , na końcu $\psi$ wokół $z$ , wszystkie wokół osi NED. Obrót wykonany jako pierwszy stoi w iloczynie po prawej, stąd $R_zR_yR_x$ . Tę zamianę warto sprawdzić samemu na dwóch obrotach o $90^\circ$ (zadanie 2.1).

Gdzie się psuje? Przy $\theta = \pm 90^\circ$ nos wskazuje pion, czyli oś pierwszego obrotu ( $\psi$ ) i oś ostatniego ( $\varphi$ ) się pokrywają. Zmiana $\psi$ i zmiana $\varphi$ dają wtedy ten sam obrót wokół pionu, a macierz zależy tylko od różnicy $\psi - \varphi$ (zadanie 2.2). Mamy trzy liczby, ale tylko dwa niezależne kierunki obrotu. To jest blokada przegubu (ang. gimbal lock). W platformie Apollo trzy kąty były fizycznymi kątami pierścieni, więc blokada oznaczała prawdziwą utratę stopnia swobody mechanizmu. W komputerze nic się nie blokuje, ale wzory na pochodne kątów dzielą przez $\cos\theta$ (część 5).

Zadania · część 2

4 zadania · ok. 35 min

Sprawdź na książce albo telefonie: obrót o $90^\circ$ wokół osi $z$ , a potem o $90^\circ$ wokół nowej osi $y$ daje to samo położenie co obrót o $90^\circ$ wokół stałej osi $y$ , a potem o $90^\circ$ wokół stałej osi $z$ .

Odpowiedź
Tak. W macierzach oba opisy dają $R_z(90^\circ)R_y(90^\circ)$ . Obroty wokół osi związanych z ciałem składa się „od lewej do prawej” w kolejności wykonywania, wokół osi stałych „od prawej do lewej”.
Wstaw $\theta = 90^\circ$ do macierzy z ramki i pokaż, że zależy ona tylko od $\psi - \varphi$ .

Odpowiedź
$c_\theta = 0$ , $s_\theta = 1$ : $R = \begin{bmatrix} 0 & s_\varphi c_\psi - c_\varphi s_\psi & c_\varphi c_\psi + s_\varphi s_\psi\\ 0 & s_\varphi s_\psi + c_\varphi c_\psi & c_\varphi s_\psi - s_\varphi c_\psi\\ -1 & 0 & 0\end{bmatrix} = \begin{bmatrix} 0 & \sin(\varphi - \psi) & \cos(\varphi - \psi)\\ 0 & \cos(\varphi - \psi) & -\sin(\varphi - \psi)\\ -1 & 0 & 0\end{bmatrix}$ . Np. $(\psi, \varphi) = (40^\circ, 10^\circ)$ i $(50^\circ, 20^\circ)$ dają tę samą macierz. Kąty Eulera nie są tu jednoznaczne.
Odczytaj kąty Eulera z macierzy $R = \begin{bmatrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 1\end{bmatrix}$ .

Odpowiedź
$\theta = -\arcsin 0 = 0$ , $\varphi = \operatorname{atan2}(0, 1) = 0$ , $\psi = \operatorname{atan2}(1, 0) = 90^\circ$ . Lot poziomy na wschód.
Samolot jest w ustalonym zakręcie: przechylenie $\varphi = 30^\circ$ , pochylenie $0$ , kurs zmienia się o $\dot\psi = 0{,}1$ rad/s. Jakie prędkości kątowe $p, q, r$ mierzą żyroskopy na kadłubie? (Wzory na $p, q, r$ w części 5.)

Odpowiedź
$p = \dot\varphi - \dot\psi\sin\theta = 0$ , $\;q = \dot\theta\cos\varphi + \dot\psi\cos\theta\sin\varphi = 0{,}05$ rad/s, $\;r = -\dot\theta\sin\varphi + \dot\psi\cos\theta\cos\varphi = 0{,}087$ rad/s. Obrót wokół pionu widziany z przechylonego samolotu rozkłada się na pochylanie i odchylanie. Dlatego w zakręcie trzeba ściągnąć drążek: samolot musi się stale pochylać w swoich osiach.

3 Twierdzenie Eulera: oś i kąt

Euler udowodnił w 1775 roku coś, co wcale nie jest oczywiste: każdą zmianę orientacji bryły można wykonać jednym obrotem wokół pewnej osi. Niezależnie od tego, ile obrotów i w jakiej kolejności wykonaliśmy po drodze. Dowód jest krótki, jeśli znamy wartości własne (lekcja 2.3 Wartości i wektory własne).

$R$ zachowuje długości, więc każda wartość własna ma moduł 1: $|\lambda| = 1$ .
Wielomian charakterystyczny jest stopnia 3 o współczynnikach rzeczywistych, więc ma co najmniej jeden pierwiastek rzeczywisty, a zespolone występują w parach sprzężonych.
Iloczyn wartości własnych to $\det R = 1$ . Możliwości: $\{1, e^{i\alpha}, e^{-i\alpha}\}$ albo $\{1, -1, -1\}$ (to drugie to szczególny przypadek $\alpha = 180^\circ$ ). W obu jest $\lambda = 1$ .
Wektor własny $\mathbf n$ dla $\lambda = 1$ spełnia $R\mathbf n = \mathbf n$ : ten kierunek nie zmienia się przy obrocie. To oś obrotu. W płaszczyźnie prostopadłej do niej $R$ działa jak obrót płaski o kąt $\alpha$ .

Oś i kąt (wzór Rodriguesa)

R = I + \sin\alpha\,[\mathbf n]_\times + (1 - \cos\alpha)\,[\mathbf n]_\times^2,\qquad [\mathbf n]_\times = \begin{bmatrix} 0 & -n_3 & n_2\\ n_3 & 0 & -n_1\\ -n_2 & n_1 & 0\end{bmatrix} .

W drugą stronę: $\operatorname{tr}R = 1 + 2\cos\alpha$ , a dla $0 < \alpha < 180^\circ$ : $\;\mathbf n = \dfrac{1}{2\sin\alpha}\,[R_{32} - R_{23};\ R_{13} - R_{31};\ R_{21} - R_{12}]$ .

Macierz $[\mathbf n]_\times$ zapisuje iloczyn wektorowy: $[\mathbf n]_\times\mathbf v = \mathbf n\times\mathbf v$ . Wzór Rodriguesa rozkłada wektor na część wzdłuż osi (nie zmienia się) i część prostopadłą (obraca się o $\alpha$ w płaszczyźnie). Ślad wynika wprost z wartości własnych: $1 + e^{i\alpha} + e^{-i\alpha} = 1 + 2\cos\alpha$ .

Oś i kąt to cztery liczby z jednym więzem ( $|\mathbf n| = 1$ ) albo trzy, jeśli zapiszemy je jako jeden wektor $\alpha\mathbf n$ (wektor obrotu). Ten zapis jest najbardziej naturalny dla człowieka, ale ma kłopoty: przy $\alpha = 0$ oś jest nieokreślona, a przy $\alpha = 180^\circ$ wektory $\alpha\mathbf n$ i $-\alpha\mathbf n$ dają tę samą orientację. Składanie dwóch obrotów w tym zapisie jest uciążliwe. Wszystkie te kłopoty usuwa jedna sztuczka: zamiast kąta używać połowy kąta.

Zadania · część 3

3 zadania · ok. 30 min

Ślad macierzy orientacji wynosi 2,62. O jaki kąt trzeba obrócić samolot jednym obrotem, żeby przejść do tej orientacji z położenia zerowego?

Odpowiedź
$\cos\alpha = (2{,}62 - 1)/2 = 0{,}81$ , $\alpha = 35{,}9^\circ$ (dokładnie, dla macierzy z przykładu 6.1: $35{,}8^\circ$ ).
Znajdź oś i kąt dla $R = R_x(90^\circ)\,R_z(90^\circ)$ (najpierw $90^\circ$ wokół stałej osi $z$ , potem wokół stałej osi $x$ ).

Odpowiedź
$R = \begin{bmatrix} 0 & -1 & 0\\ 0 & 0 & -1\\ 1 & 0 & 0\end{bmatrix}$ , $\operatorname{tr}R = 0$ , więc $\cos\alpha = -\tfrac12$ , $\alpha = 120^\circ$ . $\mathbf n = \frac{1}{2\cdot 0{,}866}[0 - (-1);\ 0 - 1;\ 0 - (-1)] = [1;\ -1;\ 1]/\sqrt3$ . Odwrotna kolejność (przykład 6.2) daje inną oś, $[1;1;1]/\sqrt3$ : obroty nie są przemienne.
Dlaczego wzór na $\mathbf n$ z ramki zawodzi przy $\alpha = 180^\circ$ ? Jak wtedy znaleźć oś?

Odpowiedź
$\sin\alpha = 0$ i $R$ jest symetryczna, więc licznik i mianownik są zerowe. Przy $\alpha = 180^\circ$ wzór Rodriguesa daje $R = I + 2[\mathbf n]_\times^2 = 2\mathbf n\mathbf n^{\mathsf T} - I$ , więc $\mathbf n\mathbf n^{\mathsf T} = (R + I)/2$ : każda niezerowa kolumna $R + I$ jest równoległa do osi. To samo pokazuje wektor własny dla $\lambda = 1$ .

4 Kwaterniony

Hamilton przez lata szukał liczb, które robią w przestrzeni to, co liczby zespolone robią na płaszczyźnie (lekcja 1.7 Liczby zespolone i fazory: obrót o $\theta$ to mnożenie przez $e^{i\theta}$ ). Znalazł je w 1843 roku, w czterech wymiarach: $q = q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k$ , gdzie $\mathbf i^2 = \mathbf j^2 = \mathbf k^2 = \mathbf{ijk} = -1$ . Dla nas wygodniejszy jest zapis „skalar plus wektor”: $q = (q_0, \mathbf q)$ .

Kwaternion jednostkowy jako obrót

q = \left(\cos\tfrac{\alpha}{2},\ \mathbf n\sin\tfrac{\alpha}{2}\right),\qquad q_0^2 + |\mathbf q|^2 = 1 .

Iloczyn: $\;p\otimes q = \left(p_0q_0 - \mathbf p\cdot\mathbf q,\ \ p_0\mathbf q + q_0\mathbf p + \mathbf p\times\mathbf q\right)$ . Sprzężenie: $q^* = (q_0, -\mathbf q)$ .

Obrót wektora: $\;(0, \mathbf v^n) = q\otimes(0, \mathbf v^b)\otimes q^*$ . Składanie (osie stałe, najpierw $q_1$ , potem $q_2$ ): $\;q = q_2\otimes q_1$ , dokładnie jak $R = R_2R_1$ .

Macierz: $R = \begin{bmatrix} q_0^2 + q_1^2 - q_2^2 - q_3^2 & 2(q_1q_2 - q_0q_3) & 2(q_1q_3 + q_0q_2)\\ 2(q_1q_2 + q_0q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2q_3 - q_0q_1)\\ 2(q_1q_3 - q_0q_2) & 2(q_2q_3 + q_0q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2\end{bmatrix}$ .

Co zyskujemy?

Brak osobliwości. Każda orientacja ma kwaternion i każdy kwaternion jednostkowy jest orientacją. Przy $\alpha = 0$ kwaternion to po prostu $(1, \mathbf 0)$ , bez nieokreślonej osi.
Tanie składanie. Iloczyn kwaternionów to 16 mnożeń, iloczyn macierzy 27.
Jeden więz zamiast sześciu. Błędy numeryczne usuwa się, dzieląc przez normę.
Prosta kinematyka, liniowa w $q$ (część 5).

Ceną jest podwójne pokrycie: $q$ i $-q$ dają tę samą macierz, bo $R$ jest kwadratowa w składowych $q$ . Obrót o $360^\circ$ zmienia kwaternion z $(1, \mathbf 0)$ na $(-1, \mathbf 0)$ , a do wartości wyjściowej wraca on dopiero po $720^\circ$ . To nie jest artefakt rachunku. Obrót o $360^\circ$ i obrót o $720^\circ$ są naprawdę różne jako drogi w przestrzeni orientacji: trzymając talerz na płasko na dłoni, możesz obrócić go dwa razy wokół pionu i wrócić z ręką w położeniu wyjściowym, a po jednym obrocie ramię zostaje skręcone (tzw. sztuczka z talerzem). W praktyce sterowania wynika z tego jedna reguła: przed porównaniem dwóch kwaternionów wybierz znak tak, żeby $q_0 \ge 0$ , bo inaczej regulator „obróci samolot naokoło” o prawie $360^\circ$ (więcej w kursie astronautyki, przy sterowaniu orientacją satelity).

Zadania · część 4

3 zadania · ok. 30 min

Zapisz kwaternion obrotu o $90^\circ$ wokół osi $z$ . Podnieś go do kwadratu (pomnóż przez siebie). Jaki obrót otrzymałeś?

Odpowiedź
$q = (\cos 45^\circ, 0, 0, \sin 45^\circ) = (0{,}707;\ 0;\ 0;\ 0{,}707)$ . $q\otimes q = (0{,}5 - 0{,}5,\ 2\cdot 0{,}707\cdot 0{,}707\,\mathbf k + \mathbf 0) = (0, 0, 0, 1)$ : obrót o $180^\circ$ wokół $z$ , bo $\cos 90^\circ = 0$ , $\sin 90^\circ = 1$ .
Obróć wektor $\mathbf v = [1; 0; 0]$ kwaternionem $q = (0{,}707;\ 0;\ 0;\ 0{,}707)$ ze wzoru $q\otimes(0, \mathbf v)\otimes q^*$ .

Odpowiedź
$q\otimes(0, \mathbf v) = (-0,\ 0{,}707\,\mathbf v + \mathbf q\times\mathbf v) = (0;\ 0{,}707;\ 0{,}707;\ 0)$ (bo $\mathbf k\times\mathbf i = \mathbf j$ ). Mnożąc przez $q^* = (0{,}707;\ 0;\ 0;\ -0{,}707)$ : $(0;\ 0;\ 1;\ 0)$ . Wynik $[0; 1; 0]$ , zgodnie z obrotem o $90^\circ$ wokół $z$ .
Pokaż, że $q$ i $-q$ dają tę samą macierz $R$ . Czy to samo jest prawdą dla wzoru $q\otimes(0, \mathbf v)\otimes q^*$ ?

Odpowiedź
Każdy element $R$ to iloczyn dwóch składowych $q$ ; zmiana znaku wszystkich składowych nie zmienia iloczynów. We wzorze z mnożeniem $q$ występuje dwa razy ( $q$ i $q^*$ ), więc znaki się znoszą: tak, wynik ten sam.

5 Kinematyka: jak orientacja zmienia się w czasie

Żyroskopy mierzą $\boldsymbol\omega = (p, q, r)$ w osiach ciała. Żeby wiedzieć, jak samolot jest zorientowany, trzeba z nich policzyć zmianę orientacji i scałkować. Jak zależy od $\boldsymbol\omega$ pochodna każdego z zapisów?

Kinematyka orientacji ( $\boldsymbol\omega$ w osiach ciała)

Macierz: $\;\dot R = R\,[\boldsymbol\omega]_\times$ .

Kwaternion: $\;\dot q = \tfrac12\,q\otimes(0, \boldsymbol\omega)$ .

Kąty Eulera 3-2-1:

\dot\varphi = p + (q\sin\varphi + r\cos\varphi)\tan\theta,\qquad \dot\theta = q\cos\varphi - r\sin\varphi,\qquad \dot\psi = \frac{q\sin\varphi + r\cos\varphi}{\cos\theta} .

Odwrotnie: $p = \dot\varphi - \dot\psi\sin\theta$ , $\;q = \dot\theta\cos\varphi + \dot\psi\cos\theta\sin\varphi$ , $\;r = -\dot\theta\sin\varphi + \dot\psi\cos\theta\cos\varphi$ .

Skąd wzór na $\dot R$ ? Wektor $\mathbf v$ przyklejony do samolotu ma stałe składowe w osiach ciała, a w osiach NED zmienia się jak punkt bryły obracającej się z prędkością $\boldsymbol\omega$ : $\dot{\mathbf v}^n = \boldsymbol\omega^n\times\mathbf v^n$ (lekcja Dział 4: Ruch obrotowy i bryła sztywna). Wstawiając $\mathbf v^n = R\mathbf v^b$ i $\boldsymbol\omega^n = R\boldsymbol\omega^b$ , dostajemy $\dot R\mathbf v^b = R(\boldsymbol\omega^b\times\mathbf v^b)$ dla każdego $\mathbf v^b$ .

Porównajmy trzy wzory.

Kąty Eulera: nieliniowe, z $\cos\theta$ w mianowniku. Przy $\theta \to \pm 90^\circ$ pochodne $\dot\psi$ i $\dot\varphi$ rosną bez ograniczeń, nawet gdy samolot obraca się powoli. Np. przy $\theta = 85^\circ$ i powolnym odchylaniu $r = 0{,}1$ rad/s kurs zmienia się o $66^\circ$ /s, a przy $89^\circ$ o $328^\circ$ /s.
Macierz: liniowa w $R$ , bez osobliwości, ale 9 równań, a błędy całkowania psują ortogonalność. Prosty schemat Eulera z krokiem 0,01 s przy $\boldsymbol\omega = (0{,}3;\ 0{,}5;\ 0{,}2)$ rad/s po minucie daje $\det R = 1{,}26$ : „samolot” rozciągnął się o 12% w każdym kierunku prostopadłym do $\boldsymbol\omega$ (wzdłuż $\boldsymbol\omega$ nie zmienił się).
Kwaternion: liniowy w $q$ , cztery równania, jeden więz łatwy do przywrócenia normalizacją. Bez normalizacji ten sam schemat odchodzi od $|q| = 1$ o 3%, z normalizacją błąd znika.

Zjawisko w symulacji ma prosty opis: przy $p = 0{,}005$ rad/s nos mija pion o $0{,}6^\circ$ , kąty $\psi$ i $\varphi$ w ułamku sekundy przeskakują o $180^\circ$ , a ich pochodne sięgają dziesiątek radianów na sekundę. RK4 z krokiem 0,1 s robi na tym odcinku błąd kilkunastu stopni, który zostaje do końca pętli. Kwaternion przy tym samym kroku ma błąd poniżej $10^{-5}$ stopnia. Dlatego kontroler lotu całkuje kwaternion (albo macierz), a kąty Eulera liczy z niego tylko do wyświetlenia na ekranie.

Twierdzenie Stuelpnagla (1964) mówi, że tak musi być: żaden zapis orientacji trzema liczbami nie jest jednocześnie globalny (jednoznaczny dla wszystkich orientacji) i wszędzie nieosobliwy. Cztery liczby z jednym więzem to najmniejszy zapis bez osobliwości.

Zadania · część 5

2 zadania · ok. 20 min

Samolot startuje z $q = (1, \mathbf 0)$ i obraca się ze stałą prędkością $\boldsymbol\omega = (0, 0, r)$ . Sprawdź, że $q(t) = \left(\cos\frac{rt}{2}, 0, 0, \sin\frac{rt}{2}\right)$ spełnia równanie $\dot q = \tfrac12 q\otimes(0, \boldsymbol\omega)$ .

Odpowiedź
$\dot q = \frac r2\left(-\sin\frac{rt}{2}, 0, 0, \cos\frac{rt}{2}\right)$ . Prawa strona: $\frac12\left(-\sin\frac{rt}{2}\cdot r,\ \cos\frac{rt}{2}\,(0, 0, r) + \mathbf q\times\boldsymbol\omega\right)$ , a $\mathbf q\times\boldsymbol\omega = 0$ , bo oba wektory leżą na osi $z$ . Wyniki się zgadzają.
Samolot leci z pochyleniem $\theta = 85^\circ$ (prawie pionowo), $\varphi = 0$ , i powoli odchyla się z $r = 0{,}1$ rad/s. Policz $\dot\psi$ i $\dot\varphi$ . Co zobaczy pilot na sztucznym horyzoncie?

Odpowiedź
$\dot\psi = r/\cos 85^\circ = 1{,}15$ rad/s $= 66^\circ$ /s, $\;\dot\varphi = r\tan 85^\circ = 1{,}14$ rad/s. Samolot obraca się powoli (0,1 rad/s), ale kurs i przechylenie wirują. Wskaźnik kursu przy locie w pionie jest bezużyteczny, a przyrządy z mechanicznymi przegubami mogą się „rozkręcić”.

6 Metoda i przykłady rozwiązane

Metoda: praca z orientacją

Ustal konwencje: osie (NED czy ENU), kierunek macierzy (ciało → NED czy odwrotnie), kolejność kątów, położenie skalara w kwaternionie. Sprawdź na obrocie o $90^\circ$ .
Licz i całkuj na kwaternionie albo macierzy; normalizuj co krok.
Składaj obroty mnożeniem ( $R_2R_1$ , $q_2\otimes q_1$ ); nigdy nie dodawaj kątów.
Kąty Eulera licz tylko na wyjściu, dla człowieka. Pamiętaj o osobliwości przy $\theta = \pm 90^\circ$ .
Sprawdzenie: $R^{\mathsf T}R = I$ , $\det R = 1$ , $|q| = 1$ , $\operatorname{tr}R = 1 + 2\cos\alpha = 4q_0^2 - 1$ .

Przykład 6.1 Od kątów Eulera do osi, kąta i kwaternionu

Samolot: kurs $\psi = 30^\circ$ , pochylenie $\theta = 20^\circ$ , przechylenie $\varphi = 10^\circ$ . Policz macierz orientacji, oś i kąt obrotu oraz kwaternion.

Z ramki w części 2: $R = \begin{bmatrix} 0{,}814 & -0{,}441 & 0{,}379\\ 0{,}470 & 0{,}883 & 0{,}018\\ -0{,}342 & 0{,}163 & 0{,}925\end{bmatrix}$ .
Ślad: $2{,}622$ , więc $\cos\alpha = 0{,}811$ , $\alpha = 35{,}8^\circ$ .
Oś: $\mathbf n = \frac{1}{2\sin\alpha}[0{,}163 - 0{,}018;\ 0{,}379 + 0{,}342;\ 0{,}470 + 0{,}441] = [0{,}124;\ 0{,}616;\ 0{,}778]$ .
Kwaternion: $q_0 = \cos 17{,}9^\circ = 0{,}9515$ , $\mathbf q = \sin 17{,}9^\circ\,\mathbf n = [0{,}038;\ 0{,}189;\ 0{,}239]$ .

Wynik: $q = (0{,}9515;\ 0{,}038;\ 0{,}189;\ 0{,}239)$ ; jeden obrót o $35{,}8^\circ$ wokół osi skierowanej głównie w dół i na wschód.

Sprawdzenie: $4q_0^2 - 1 = 2{,}622 = \operatorname{tr}R$ ✓. Ten sam kwaternion daje iloczyn $q_z(30^\circ)\otimes q_y(20^\circ)\otimes q_x(10^\circ)$ ✓. Uwaga: kąt $35{,}8^\circ$ to nie suma $30 + 20 + 10$ ani długość wektora $(30, 20, 10)$ , równa $37{,}4$ .

Przykład 6.2 Dwa obroty o 90°, jeden obrót o 120°

Pytanie z prologu: obrót o $90^\circ$ wokół stałej osi $x$ , a potem o $90^\circ$ wokół stałej osi $z$ . Jaki jeden obrót daje ten sam efekt?

$q_x = (0{,}707;\ 0{,}707;\ 0;\ 0)$ , $\;q_z = (0{,}707;\ 0;\ 0;\ 0{,}707)$ .
$q = q_z\otimes q_x$ : skalar $0{,}5 - 0 = 0{,}5$ ; wektor $0{,}707\cdot(0{,}707;0;0) + 0{,}707\cdot(0;0;0{,}707) + (0;0;0{,}707)\times(0{,}707;0;0) = (0{,}5;\ 0{,}5;\ 0{,}5)$ .
$q_0 = 0{,}5 = \cos(\alpha/2)$ , więc $\alpha = 120^\circ$ ; oś $\propto(0{,}5;\ 0{,}5;\ 0{,}5)$ , czyli $\mathbf n = [1;1;1]/\sqrt3$ .

Wynik: jeden obrót o $120^\circ$ wokół przekątnej sześcianu. Trzy osie układu zamieniają się cyklicznie miejscami: $x\to y\to z\to x$ .

Sprawdzenie: $R_z(90^\circ)R_x(90^\circ) = \begin{bmatrix} 0 & 0 & 1\\ 1 & 0 & 0\\ 0 & 1 & 0\end{bmatrix}$ (lekcja 2.2 Przekształcenia liniowe: obroty, zmiana bazy, transformacja Lorentza), ślad $0 = 1 + 2\cos 120^\circ$ ✓.

Przykład 6.3 Ile kosztuje osobliwość

W symulacji 3 samolot robi pętlę z $q = 0{,}5$ rad/s i przechyla się z $p = 0{,}005$ rad/s. Jak blisko pionu przechodzi nos i co dzieje się z kątami Eulera?

Prędkość kątowa $\boldsymbol\omega = (0{,}005;\ 0{,}5;\ 0)$ jest stała, więc samolot obraca się wokół jednej osi, odchylonej od osi skrzydeł o $\arctan(0{,}005/0{,}5) = 0{,}57^\circ$ . Nos zatacza stożek i po $\pi/(2\cdot 0{,}5) = 3{,}1$ s mija pion właśnie o $0{,}57^\circ$ .
W tym miejscu $\cos\theta$ spada do $0{,}01$ , a $\psi$ i $\varphi$ muszą w kilka setnych sekundy przeskoczyć o prawie $180^\circ$ (nos przechodzi „przez biegun”). Pochodne sięgają dziesiątek rad/s; w symulacji największe $|\dot\psi|$ to 40–70 rad/s, zależnie od tego, gdzie wypadają kroki.
RK4 z krokiem 0,1 s nie rozdziela tego przeskoku: błąd orientacji po pętli $\approx 15^\circ$ (wynik jest bardzo czuły na dokładną wartość $p$ ); z krokiem 0,02 s $\approx 0{,}004^\circ$ . Kwaternion: poniżej $10^{-5}$ stopnia przy obu krokach.

Wynik: kąty Eulera wymagają tym mniejszego kroku, im dokładniej samolot przechodzi przez pion. Kwaternion nie ma takiego miejsca.

Sprawdzenie: przy $p = 0$ (pętla idealnie w płaszczyźnie) $\varphi$ i $\psi$ są stale zerowe i kąty Eulera całkują się bezbłędnie; kłopot robi dopiero mała składowa „w bok” ✓.

7 Błędne intuicje, których trzeba się oduczyć

„Kąty Eulera to wektor obrotu.”: Nie da się ich dodawać ani składać po współrzędnych, a ich pochodne nie są prędkością kątową: $(\dot\varphi, \dot\theta, \dot\psi) \ne (p, q, r)$ . W zakręcie z przechyleniem żyroskop pochylania pokazuje $q \ne 0$ , choć $\theta$ się nie zmienia.
„Blokada przegubu to problem starych platform mechanicznych.”: To własność każdego zapisu trzema liczbami. W oprogramowaniu objawia się dzieleniem przez $\cos\theta$ i skokami kątów, nie zatrzymaniem mechanizmu.
„Kwaternion to cztery kąty.”: To oś pomnożona przez sinus połowy kąta plus kosinus połowy kąta. Ma jeden więz, więc niesie trzy stopnie swobody, tyle co orientacja.
„Obrót o 360° to to samo co brak obrotu.”: Dla orientacji tak, dla kwaternionu nie: $q$ zmienia znak. Regulator, który porównuje kwaterniony bez sprawdzenia znaku, może wykonać zbędny pełny obrót.
„Kolejność obrotów nie ma znaczenia, jeśli kąty są małe.”: Dla małych kątów różnica jest małego rzędu drugiego (iloczyn kątów). Dlatego małe obroty można dodawać, i na tym opiera się linearyzacja w lekcjach o samolocie. Dla kątów rzędu dziesiątek stopni różnica jest duża.

8 Zadania do przemyślenia

Te zadania wymagają więcej myślenia niż liczenia.

Zadania do przemyślenia

5 zadań · ok. 1,5 godziny

Kwaternion z macierzy. Z ramki: $q_0 = \tfrac12\sqrt{1 + \operatorname{tr}R}$ , $\;\mathbf q = \frac{1}{4q_0}[R_{32} - R_{23};\ R_{13} - R_{31};\ R_{21} - R_{12}]$ . Kiedy ten wzór jest numerycznie zły i jak sobie wtedy poradzić?
Rozwiązanie
1. Gdy $q_0 \to 0$ (obrót o prawie $180^\circ$ ): dzielimy małe różnice przez małą liczbę i błędy zaokrągleń rosną.
2. Rozwiązanie (metoda Shepperda): z przekątnej policz kwadraty wszystkich czterech składowych, np. $4q_1^2 = 1 + R_{11} - R_{22} - R_{33}$ , wybierz największą i z niej licz pozostałe. Największa składowa ma kwadrat co najmniej $1/4$ , więc dzielenie jest bezpieczne.
Małe obroty. Pokaż, że dla małych kątów $R \approx I + [\boldsymbol\delta]_\times$ , gdzie $\boldsymbol\delta = (\delta\varphi, \delta\theta, \delta\psi)$ , a $q \approx (1, \boldsymbol\delta/2)$ . Dlaczego to ważne dla filtru Kalmana orientacji?
Rozwiązanie
1. Wzór Rodriguesa z $\sin\alpha\approx\alpha$ , $1 - \cos\alpha\approx 0$ : $R\approx I + \alpha[\mathbf n]_\times = I + [\boldsymbol\delta]_\times$ , gdzie $\boldsymbol\delta = \alpha\mathbf n$ . Kąty Eulera dla małych obrotów są składowymi $\boldsymbol\delta$ (kolejność nie ma znaczenia w pierwszym rzędzie). Kwaternion: $\cos(\alpha/2)\approx 1$ , $\sin(\alpha/2)\approx\alpha/2$ .
2. Filtr Kalmana potrzebuje stanu w przestrzeni wektorowej (dodawanie, kowariancja). Dlatego w lekcji 3 Fuzja czujników w 3D: estymacja orientacji na kwaternionach estymujemy nie kwaternion, tylko mały wektor błędu orientacji $\boldsymbol\delta$ wokół bieżącej estymaty, i składamy go z kwaternionem mnożeniem.
Inna kolejność kątów. Dla satelitów i bąków używa się często kątów 3-1-3 (precesja, nutacja, obrót własny). Gdzie mają osobliwość? Dlaczego dla samolotu lepsze są 3-2-1?
Rozwiązanie
1. Kąty 3-1-3 mają osobliwość przy nutacji $0$ i $180^\circ$ : wtedy pierwszy i trzeci obrót są wokół tej samej osi $z$ . Dla bąka (lekcja Dział 4: Ruch obrotowy i bryła sztywna) to rzadki przypadek pionowej osi, a nutacja ma czytelny sens fizyczny.
2. Samolot prawie zawsze lata blisko poziomu ( $\theta \approx 0$ ), a kąty 3-2-1 są tam najdalej od osobliwości i mają sens dla pilota. Kąty 3-1-3 miałyby osobliwość właśnie w locie poziomym.
Najkrótsza droga. Regulator orientacji dostaje błąd $q_e = q_\text{zad}^*\otimes q$ . Prawdziwy błąd to obrót o $350^\circ$ wokół pewnej osi. Co się stanie, jeśli regulator będzie dążył do $q_e = (1, \mathbf 0)$ , a co, jeśli zaakceptuje też $(-1, \mathbf 0)$ ?
Rozwiązanie
1. Obrót o $350^\circ$ to ta sama orientacja co obrót o $-10^\circ$ . Kwaternion $q_e$ ma wtedy $q_0 = \cos 175^\circ < 0$ .
2. Regulator dążący tylko do $(1, \mathbf 0)$ „odkręci” $350^\circ$ naokoło. Zmiana znaku $q_e$ , gdy $q_0 < 0$ , daje obrót o $10^\circ$ w drugą stronę. Markley i Crassidis nazywają niechciany pełny obrót unwinding.
Sprawdzian telefonem. Telefon z aplikacją pokazującą kąty orientacji (wiele aplikacji z czujnikami ma taki tryb) połóż płasko, a potem podnieś do pionu ekranem do siebie. Co dzieje się z kątem „kursu” i „przechylenia” tuż przy pionie i dlaczego?
Rozwiązanie
1. Przy pionie (pochylenie $\pm 90^\circ$ ) kąty kursu i przechylenia zaczynają skakać albo zmieniać się gwałtownie przy drobnych ruchach: to osobliwość kątów Eulera.
2. Telefon wewnętrznie liczy na kwaternionie lub macierzy (system Android udostępnia „wektor obrotu” właśnie w tej postaci), a kąty to tylko przeliczenie na wyjściu. Dlatego sam obraz w grach i aplikacjach AR się nie psuje.

9 Sprawdź się

Odpowiedz w myślach, zanim rozwiniesz odpowiedź.

1 Co jest w kolumnach macierzy orientacji

R

(ciało → NED)?

Osie samolotu zapisane w osiach NED; pierwsza kolumna to kierunek nosa.

2 Ile stopni swobody ma orientacja i ile liczb ma każdy zapis?

3 stopnie swobody. Macierz: 9 liczb, 6 więzów. Kąty Eulera: 3 (osobliwość). Kwaternion: 4, jeden więz.

3 Gdzie kąty Eulera 3-2-1 mają osobliwość i dlaczego?

Przy $\theta = \pm 90^\circ$ : osie obrotów $\psi$ i $\varphi$ się pokrywają, macierz zależy tylko od $\psi\mp\varphi$ , a w kinematyce dzielimy przez $\cos\theta$ .

4 Jak z macierzy obrotu odczytać kąt obrotu?

$\operatorname{tr}R = 1 + 2\cos\alpha$ ; oś to wektor własny dla $\lambda = 1$ .

5 Jak wygląda kwaternion obrotu o

\alpha

wokół

\mathbf n

$(\cos\frac\alpha2,\ \mathbf n\sin\frac\alpha2)$ . Kwaterniony $q$ i $-q$ to ta sama orientacja.

6 Jaka jest kinematyka kwaternionu?

$\dot q = \frac12 q\otimes(0, \boldsymbol\omega)$ , z $\boldsymbol\omega$ w osiach ciała; liniowa w $q$ , bez osobliwości. Po każdym kroku normalizacja.

10 Karta wzorów i co dalej

Macierz i kąty Eulera

$\mathbf v^n = R\mathbf v^b$ , $R^{\mathsf T}R = I$ , $\det R = 1$

$R = R_z(\psi)R_y(\theta)R_x(\varphi)$

$\theta = -\arcsin R_{31}$ , $\varphi = \operatorname{atan2}(R_{32}, R_{33})$ , $\psi = \operatorname{atan2}(R_{21}, R_{11})$

$\dot\psi = \frac{q s_\varphi + r c_\varphi}{c_\theta}$ , $\dot\theta = qc_\varphi - rs_\varphi$ , $\dot\varphi = p + (qs_\varphi + rc_\varphi)\tan\theta$

$\dot R = R[\boldsymbol\omega]_\times$

Oś, kąt, kwaternion

$R = I + \sin\alpha[\mathbf n]_\times + (1 - \cos\alpha)[\mathbf n]_\times^2$

$\operatorname{tr}R = 1 + 2\cos\alpha = 4q_0^2 - 1$

$q = (\cos\frac\alpha2, \mathbf n\sin\frac\alpha2)$ , $q \equiv -q$

$p\otimes q = (p_0q_0 - \mathbf p\cdot\mathbf q,\ p_0\mathbf q + q_0\mathbf p + \mathbf p\times\mathbf q)$

$(0, \mathbf v^n) = q\otimes(0, \mathbf v^b)\otimes q^*$ ; składanie $q_2\otimes q_1$

$\dot q = \frac12 q\otimes(0, \boldsymbol\omega)$

Do przeczytania

Obowiązkowo: H. Schaub, J. L. Junkins, Analytical Mechanics of Aerospace Systems, rozdz. 3.1–3.4 (macierz kosinusów kierunkowych, kąty Eulera, wektor obrotu, parametry Eulera, czyli kwaterniony).
Dla chętnych: F. L. Markley, J. L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control, rozdz. 2.7–2.9 (kwaterniony i wszystkie zapisy orientacji, także parametry Rodriguesa) oraz 3.1–3.2 (ich kinematyka, z blokadą przegubu w ogólnej postaci). Uwaga na ich konwencję kwaternionu.

Wiemy już, jak opisać orientację i jak ją całkować, gdy znamy prędkość kątową. Ale skąd się bierze prędkość kątowa? Z momentów sił i z tego, jak masa jest rozłożona w bryle. W następnej lekcji zobaczymy, że w trzech wymiarach moment pędu i prędkość kątowa nie muszą być równoległe, a obrót wokół jednej z trzech osi bryły jest niestabilny. Klucz francuski rzucony w stanie nieważkości to pokazuje.