Kompendium matematyczne

Podstawowe operacje

Dla wektorów $\mathbf{a}, \mathbf{b} \in \mathbb{R}^3$:

• Iloczyn skalarny (dot product) — daje liczbę: $\mathbf{a}\cdot\mathbf{b} = a_x b_x + a_y b_y + a_z b_z = |\mathbf{a}||\mathbf{b}|\cos\theta$. Mierzy „jak bardzo dwa wektory wskazują w tę samą stronę". Zero ⟺ prostopadłe.
• Iloczyn wektorowy (cross product) — daje wektor prostopadły do obu, o długości równej polu równoległoboku rozpiętego przez nie: $|\mathbf{a}\times\mathbf{b}| = |\mathbf{a}||\mathbf{b}|\sin\theta$.
• Norma (długość): $|\mathbf{a}| = \sqrt{a_x^2 + a_y^2 + a_z^2} = \sqrt{\mathbf{a}\cdot\mathbf{a}}$.

Wzór jawny dla iloczynu wektorowego:

Gdzie tego używasz dalej: iloczyn wektorowy w M3 (Jakobiany) — kolumna jakobianu dla przegubu obrotowego ma postać $\hat z_i \times (\mathbf{p}_n - \mathbf{p}_i)$; w M9 (Newton-Euler) — człon żyroskopowy $\boldsymbol\omega\times(I\boldsymbol\omega)$ i prawie każdy człon propagacji prędkości.

Mnożenie

Dla $A \in \mathbb{R}^{m\times n}$ i $B \in \mathbb{R}^{n\times p}$ wynik $C = AB$ ma wymiar $m \times p$, a element:

Czyli element (i, j) wyniku to iloczyn skalarny i-tego wiersza A z j-tą kolumną B. Kolejność ma znaczenie: $AB \neq BA$ w ogólności. To podstawowa przyczyna trudności DH (kolejność transformacji ważna).

Transpozycja

$(A^\top)_{ij} = A_{ji}$ — odbicie względem przekątnej. Ważna tożsamość: $(AB)^\top = B^\top A^\top$ (kolejność się odwraca!).

Wyznacznik — geometrycznie

Dla macierzy kwadratowej $A \in \mathbb{R}^{n\times n}$ wyznacznik $\det A$ to czynnik skalujący objętość: jeśli jednostkowy n-sześcian zostanie odwzorowany przez A, to powstaje równoległoboczy obiekt o objętości $|\det A|$. Znak mówi czy orientacja została zachowana (+) czy odwrócona (−).

Konsekwencja praktyczna: $\det A = 0$ ⟺ A jest osobliwa (singular) — odwzorowuje przestrzeń do mniejszego wymiaru, nie jest odwracalna. To dokładnie definicja singularności kinematycznej w robotyce (jakobian J ma $\det J = 0$).

Odwrotność i ortogonalność

$A^{-1}$ spełnia $A A^{-1} = A^{-1} A = I$. Istnieje tylko gdy $\det A \neq 0$.

Macierz ortogonalna: $Q^\top Q = I$, czyli $Q^{-1} = Q^\top$. Ogromnie tanie odwracanie — zamiast wywoływać numeryczną inwersję (eliminacja Gaussa, koszt $O(n^3)$), wystarczy przepisać współrzędne. Wszystkie macierze rotacji są ortogonalne — stąd w kodzie zawsze widzisz R^T a nie inv(R).

Gdzie tego używasz dalej: mnożenie macierzy w każdej FK (T_0^n = T_0^1 · T_1^2 · …); transpozycja zamiast inwersji w wszystkich modułach kinematyki (R^T = R⁻¹); wyznacznik J w M7 (singularności).

Grupa $SO(3)$ (Special Orthogonal Group w 3D) to zbiór macierzy 3×3 spełniających dwa warunki:

To dokładnie 3 stopnie swobody (np. 3 kąty Eulera albo oś + kąt). Pomimo że R ma 9 elementów, tylko 3 są niezależne — pozostałe 6 jest wymuszonych przez ograniczenia ortogonalności.

Trzy elementarne rotacje

Rotacja wokół osi x o kąt α:

Wokół y o β:

Wokół z o γ:

Składanie: rotacja „najpierw wokół z o γ, potem wokół y o β, potem wokół x o α" to mnożenie macierzy w kolejności od prawej do lewej:

Ta kolejność (xyz „intrinsic" vs „extrinsic", zyx itd.) to klasyczna pułapka — dlatego w aplikacji konsekwentnie używamy kwaternionów jako reprezentacji wewnętrznej, a kątów Eulera tylko do UI.

Gdzie tego używasz dalej: M8 (Reprezentacje orientacji) — pełne porównanie macierzy rotacji, kątów Eulera, axis-angle i kwaternionów, z interaktywnym demo gimbal-locka.

Dla wektora $\mathbf{a} = (a_x, a_y, a_z)^\top$ definiujemy macierz skew-symmetric (antysymetryczną) $[\mathbf{a}]_\times$:

Kluczowa własność: iloczyn wektorowy zapisuje się jako mnożenie macierzy:

Dlaczego to istotne? Bo pozwala traktować iloczyn wektorowy jako operację liniową, co odblokowuje cały aparat algebry liniowej (diagonalizacja, wartości własne, gradient). W jakobianach manipulatora kolumny zawierają $[\hat{z}_i]_\times \mathbf{r}$ — dzięki temu sam jakobian J jest macierzą, którą można odwracać/pseudoinwertować.

Gdzie tego używasz dalej: M3 — kolumny jakobianu jako $J_i = [\,[\hat z_i]_\times \mathbf{r};\;\hat z_i]$; M9 — propagacja momentu pędu i implementacja iloczynów wektorowych w kodzie (Newton-Euler korzysta wprost z [w]× v).

Grupa $SE(3)$ (Special Euclidean Group w 3D) opisuje transformacje sztywne — rotacje + translacje. Naturalna reprezentacja: para $(R, \mathbf{t})$ gdzie $R \in SO(3)$, $\mathbf{t} \in \mathbb{R}^3$. Razem 6 stopni swobody (3 rotacji + 3 translacje).

Po co macierz 4×4 zamiast pary (R, t)?

Pojedyncza transformacja punktu $\mathbf{p}$:

To nie jest mnożenie macierzowe, bo dochodzi addytywne $+\mathbf{t}$. Składanie dwóch transformacji wymaga wtedy osobnej formuły:

Sztuczka: dodajemy „wirtualną" czwartą współrzędną równą 1 i pakujemy R i t w jedną macierz 4×4. Wtedy zarówno transformacja punktu, jak i składanie transformacji sprowadzają się do zwykłego mnożenia macierzy:

Punkt zapisujemy jako wektor 4-elementowy $\tilde{\mathbf{p}} = (p_x, p_y, p_z, 1)^\top$ (wektor jednorodny):

Składanie wielu transformacji to teraz po prostu iloczyn macierzy: $T_0^n = T_0^1 \cdot T_1^2 \cdots T_{n-1}^n$ — i o to chodziło. Dlatego cała kinematyka manipulatora opiera się na tej reprezentacji (DH zwraca macierze 4×4).

Gdzie tego używasz dalej: M0, M1 i każdy moduł kinematyki — wszystko czego dotyka FK manipulatora jest mnożeniem macierzy 4×4.

Bryła sztywna to ciało, w którym odległości między dowolnymi dwoma punktami są niezmienne w czasie. Może się obracać i poruszać (translacja+rotacja jako całość), ale nie deformować.

Kluczowy wzór: jeśli środek bryły O porusza się z prędkością $\mathbf{v}_O$, a bryła obraca się z prędkością kątową $\boldsymbol\omega$, to prędkość dowolnego punktu P położonego względem O w odległości $\mathbf{r}$ wynosi:

Pierwsza składowa ($\mathbf{v}_O$) — wspólna dla wszystkich punktów bryły (translacja jako sztywne ciało). Druga ($\boldsymbol\omega\times\mathbf{r}$) — dodatkowy ruch z powodu obrotu wokół O, prostopadły do r i do osi obrotu.

Przyspieszenie — analogicznie ale z dwoma członami

Różniczkując wzór na prędkość po czasie dostajemy:

Trzy składowe: dziedziczone $\mathbf{a}_O$, tangencjalne (od $\boldsymbol\varepsilon$ — przyspieszenia kątowego), dośrodkowe (od $\boldsymbol\omega^2$ — zawsze skierowane do osi obrotu).

Gdzie tego używasz dalej: M9 (Dynamika Newton-Euler) — dokładnie te dwa wzory są stosowane do propagacji ω, ε, v, a od jednego ogniwa do następnego w forward sweep. Wzór na prędkość punktu wraca w każdym kroku 2–3 algorytmu. Bez niego dynamika manipulatora nie ma sensu.