Metody Jakobianowe

Dlaczego nie zamkniętą formułą?

Metoda analityczna (moduły 1–2) daje dokładne rozwiązanie i jest właściwym wyborem, gdy znamy zamkniętą formę dla danej geometrii — np. Puma 560 (forma A Piepera) czy UR5 (forma B). Schody zaczynają się gdy: (a) manipulator ma 7+ DOF — wtedy IK ma nieskończenie wiele rozwiązań i potrzeba dodatkowych kryteriów (Franka Panda, KUKA LBR iiwa); (b) używamy nietypowej geometrii bez gotowego wyprowadzenia — roboty chirurgiczne, prototypy, custom mechanizmy; (c) uczenie się dynamicznie zmienia model (np. estymacja online długości ogniw przy soft-robotach).

Dla typowych przemysłowych 6-DOF rozwiązanie zamknięte zawsze istnieje (Raghavan–Roth dowodzą, że dowolny 6-DOF ma co najwyżej 16 rzeczywistych rozwiązań i wszystkie da się wyznaczyć analitycznie), ale w praktyce jego wyprowadzenie bywa pracochłonne. Solvery numeryczne dają rozwiązanie „od ręki" dla dowolnej geometrii podanej tabelą DH — bez ręcznej algebry, bez zgadywania struktury wzorów. To czyni je domyślną opcją w prototypowaniu.

Potrzebujemy więc solverów numerycznych — działających dla dowolnego manipulatora, nawet jeśli równania są nieliniowe w sposób, który uniemożliwia ich odwrócenie ręcznie. Wszystkie metody tego modułu oparte są na jednym pomyśle: linearyzacji przez Jakobian.

Intuicja: metoda Newtona w jednym wymiarze

Zanim przejdziemy do przestrzeni konfiguracji 6-DOF, popatrzmy na analogiczny problem w 1D. Chcemy rozwiązać równanie $f(x) = 0$ dla pewnej gładkiej, nieliniowej funkcji $f$. Bez zamkniętej formuły — używamy metody Newtona–Raphsona:

1. Zgadujemy startową wartość $x_0$.
2. Liczymy styczną do wykresu w punkcie $(x_0, f(x_0))$.
3. Przedłużamy styczną do przecięcia z osią X — nowe przybliżenie $x_1 = x_0 - f(x_0)/f'(x_0)$.
4. Powtarzamy: $x_2$ ze stycznej w $x_1$, i tak dalej.

Każda kolejna styczna „lepiej się dopasowuje" do miejsca zerowego $x^*$. Po kilku iteracjach osiągamy dowolną precyzję:

Kluczowe spostrzeżenie: w jednym kroku nie próbujemy od razu trafić w rozwiązanie. Zamiast tego zastępujemy trudną, nieliniową funkcję $f$ jej łatwą, liniową aproksymacją (styczną) i rozwiązujemy problem liniowy — który umiemy. Dopiero powtarzanie tego schematu doprowadza nas do celu.

Czym właściwie jest Jakobian?

W 1D rolę „wrażliwości" wyjścia na wejście pełni pochodna $f'(x)$. Mówi ona: „jeżeli zmienię $x$ o jednostkę, to $y = f(x)$ zmieni się w przybliżeniu o $f'(x)$ jednostek". W robotyce mamy wejście $q = (q_1, \dots, q_n)$ (kąty przegubów) i wyjście $\xi = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)$ (prędkość liniowa i kątowa końcówki) — sześć liczb wyjścia, n liczb wejścia. „Pochodna" takiego odwzorowania to macierz — właśnie Jakobian $J(q) \in \mathbb{R}^{6 \times n}$:

Wiersze macierzy odpowiadają sześciu składowym prędkości końcówki (3 translacyjne + 3 obrotowe). Kolumny macierzy odpowiadają poszczególnym przegubom — każda kolumna $J_i$ mówi, jak jednostkowa prędkość i-tego przegubu wpływa na końcówkę:

Innymi słowy: wyobraź sobie, że obracasz tylko jeden przegub, resztę trzymasz nieruchomo. Końcówka zakreśla łuk — lokalnie wektor jej prędkości to właśnie kolumna Jakobianu odpowiadająca temu przegubowi. Jeśli wszystkie przeguby obracają się jednocześnie, ich wkłady się dodają liniowo (bo różniczkowanie jest liniowe):

Trzy kluczowe rzeczy do zapamiętania o Jakobianie:

1. Jakobian zależy od $q$. To nie jest „stała macierz robota" — to pochodna cząstkowa w konkretnym punkcie. Gdy zmieniasz konfigurację, Jakobian się zmienia. Stąd w pętli iteracyjnej musimy go przeliczać za każdym razem.
2. Jakobian łączy prędkości, nie pozycje. Relacja $\xi = J\dot q$ jest między prędkością końcówki a prędkościami przegubów. Dla małych przyrostów (lokalnie) mamy $\Delta x \approx J \, \Delta q$ — stąd iteracyjne rozwiązanie IK.
3. Dla przegubów obrotowych istnieje jawny wzór. Nie trzeba różniczkować symbolicznie — kolumnę $J_i$ można zapisać geometrycznie (zobacz StepPanel 1 niżej): wkład do prędkości liniowej końcówki to $\hat{\mathbf{z}}_i \times (\mathbf{p}_\text{ee} - \mathbf{p}_i)$ (iloczyn wektorowy osi obrotu z wektorem „promień od osi do końcówki"); wkład do prędkości kątowej — to sama oś $\hat{\mathbf{z}}_i$. Dla przegubu przesuwnego jest jeszcze prościej: tylko wkład liniowy $\hat{\mathbf{z}}_i$, zero wkładu kątowego.

Newton w 6 wymiarach — powrót do IK

Teraz łączymy obie intuicje. W IK rozwiązujemy $\mathbf{F}(q) = \mathbf{0}$, gdzie $\mathbf{F}(q) = T^{*} \ominus f(q)$ jest wektorowym błędem pozy (6-wymiarowym — translacja + rotacja). W metodzie Newtona rolę „dzielenia przez pochodną" (z 1D) pełni pseudoinwersja Jakobianu $J^\dagger$:

(znak się odwraca, bo definiujemy błąd jako $\mathbf{e} = T^* \ominus f(q)$, nie $f(q) - T^*$, ale struktura kroku jest identyczna). Resztę wariantów — Jacobian Transpose, DLS, Adaptive DLS — zobaczymy niżej jako modyfikacje tego samego kroku, różnie radzące sobie z trudnościami, gdy Jakobian jest źle uwarunkowany.

Anatomia pojedynczej iteracji

Pojedyncza iteracja solvera Jakobianowego to pięć etapów wykonywanych w stałej kolejności. Po piątym sprawdzamy warunek stopu i albo kończymy, albo wracamy do pierwszego z nową wartością $q$:

Zauważ: $J$ liczymy od nowa w każdej iteracji — bo zależy od aktualnego $q_k$. To nie jest „statyczna macierz robota"; to pochodna cząstkowa wyliczana w bieżącym punkcie przestrzeni konfiguracji. Gdyby FK była liniowa, wystarczyłaby jedna iteracja (tak jak Newton na funkcji liniowej trafia w zero od razu). FK jest silnie nieliniowe — stąd konieczność pętli.

Pseudokod

q ← q_seed              // zgadnięta/znana konfiguracja startowa
for k = 0, 1, 2, … do
    T_k ← FK(q)                            // 1. FK
    e_k ← twist_error(T_k, T_target)       // 2. błąd w SE(3)
    if  ‖e_k‖  <  ε  then  return  q       //    warunek stopu
    J_k ← jacobian(q)                      // 3. jakobian w aktualnym punkcie
    Δq  ← solve_step(J_k, e_k, method)     // 4. krok (Transpose / Pinv / DLS / SDLS)
    q   ← q + α · Δq                       // 5. aktualizacja (α = step size)
end for
return  q  // osiągnięto limit iteracji bez zbieżności

Klucz do zrozumienia — dwa poziomy „nieliniowości"

• Zewnętrzna pętla po iteracjach — to ta, która pozwala nam pokonać nieliniowość FK. Po każdym kroku linearyzujemy od nowa w nowym punkcie $q_k$.
• Wewnętrzny krok liniowy — rozwiązujemy układ $J\,\Delta q = e$. To jest łatwe (algebra liniowa), ale wrażliwe: przy złym uwarunkowaniu $J$ krok staje się ogromny, a wtedy punkt $q_{k+1}$ wylatuje poza obszar, w którym linearyzacja obowiązywała. Stąd modyfikacje — tłumienie, ograniczanie kroku — które poznamy dalej.

Co oznacza „zbieżność" i kiedy jej brak

Zbieżność to spadek normy błędu $\|\mathbf{e}_k\|$ do zera. W dobrym przypadku spadek jest kwadratowy (przy pełnym Newtonie i pseudoinwersji w okolicy rozwiązania): błąd w iteracji $k+1$ jest proporcjonalny do kwadratu błędu w iteracji $k$. Oznacza to, że liczba cyfr dokładnych się podwaja z każdym krokiem.

Co może pójść nie tak:

• Rozbieżność — gdy krok $\Delta q$ jest za duży (np. w pobliżu singularności $J$ źle uwarunkowana), $q_{k+1}$ skacze do obszaru, gdzie linearyzacja już nie ma sensu. Kolejne iteracje pogłębiają problem. Ratunek: tłumienie (DLS) albo line-search.
• Oscylacje — solver oscyluje w okolicy rozwiązania zamiast w nie wchodzić. Typowe dla Jacobian Transpose ze zbyt dużym $\alpha$ albo dla metod bez adaptacyjnego kroku.
• Stagnacja — krok staje się mikroskopijny (np. $J^\top \mathbf{e} \approx 0$ gdy Transpose trafi w lokalny pseudominimalny punkt). Solver „utyka" bez osiągnięcia tolerancji.
• Zbieg do innej gałęzi — dla Pumy mamy 8 rozwiązań; iteracyjny solver przyciąga się do tego, które jest najbliższe seedowi. Zmiana seeda może dać inną gałąź. To nie jest błąd — to cecha metody.

W panelu „Porównanie na żywo" niżej zobaczysz te zjawiska w praktyce: Transpose często stagnuje (potrzebuje tysięcy iteracji), Pseudoinwersja bywa niestabilna przy singularnościach, DLS jest niezawodny ale z residualnym błędem, Adaptive DLS łączy zalety obydwu.

Syntetyczne porównanie

Co dalej

W module 4 sformułujemy IK jako problem optymalizacji z ograniczeniami (ograniczenia przegubowe, unikanie kolizji, multi-cel) i zastosujemy solvery globalne (Nelder–Mead, SQP, ewolucyjne). W module 7 wrócimy do tematu manipulacyjności i pokażemy, jak analiza SVD Jakobianu daje operatywną miarę odległości od singularności — i jak można ją włączyć jako dodatkowy cel optymalizacyjny w przestrzeni zerowej jakobianu.

Powiązania z dynamiką: Jakobian w tym module mapuje q̇ → ẋ (pierwsza pochodna). Druga pochodna i siły potrzebne do wytworzenia ruchu to temat modułu 9 (Newton-Euler): ten sam łańcuch propagacji od bazy do efektora, ale z prędkościami, przyspieszeniami i siłami w każdym ogniwie.