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.