Moduł 04 · numeryczne

Optymalizacja

Nelder–Mead, gradient descent, SQP z ograniczeniami, algorytmy ewolucyjne.

Czym właściwie jest optymalizacja?

Wyobraź sobie, że jesteś w górach. Stoisz w nieznanym miejscu i chcesz zejść jak najniżej — do dna doliny. Nie masz mapy, ale masz dwie informacje: na jakiej wysokości teraz jesteś (możesz zmierzyć) i w którą stronę teren opada (możesz zobaczyć stopami). Co robisz? Idziesz w stronę spadku, krok po kroku, każdy krok kontrolując, czy jest niżej. Jeśli przypadkiem wszedłeś pod górę — cofasz się. To jest istota optymalizacji numerycznej.

Formalnie: mamy funkcję $f(x)$ , która każdej wartości $x$ przypisuje liczbę („wysokość", „koszt", „błąd"). Szukamy $x^*$ , dla którego ta liczba jest najmniejsza. Tę wartość $x^*$ nazywamy argumentem minimum:

x^* = \arg\min_x f(x)

W jednym wymiarze wygląda to tak (zaczynamy z $x_0$ i schodzimy w dół):

W więcej niż jednym wymiarze (np. 2D) wizualizujemy zwykle linie konturowe funkcji — punkty o tej samej wartości. Algorytm zaczyna w punkcie startowym i przeskakuje zboczem ku centrum (minimum), prostopadle do konturów:

Gradient descent w 2D z liniami konturowymi — Klasyczna ilustracja gradient descent w 2D. Linie konturowe pokazują wartości funkcji kosztu, ścieżka iteracji prowadzi prostopadle do konturów ku minimum globalnemu.
Źródło: Wikimedia Commons · autor: Zerodamage / Oleg Alexandrov · licencja: Public Domain

Dla funkcji jednej zmiennej możemy często znaleźć minimum analitycznie — przyrównujemy pochodną do zera ( $f'(x) = 0$ ) i rozwiązujemy równanie. Ale w praktyce funkcja jest skomplikowana albo wielowymiarowa, więc używamy iteracji numerycznej. W każdym kroku robimy mały ruch w stronę spadku:

x_{k+1} = x_k - \alpha\,f'(x_k)

$\alpha$ to długość kroku (rozmiar pojedynczej iteracji). Za małe — wolna zbieżność. Za duże — ryzyko przeskoczenia minimum i oscylacji. Strategie wyboru $\alpha$ to bogata część teorii optymalizacji (np. Armijo line search, którą zobaczysz niżej).

A co to ma wspólnego z IK?

W IK chcemy znaleźć kąty przegubów $q$ , dla których końcówka robota trafia w zadaną pozę $T^*$ . Możemy to sformułować jako optymalizację: zdefiniujmy funkcję kosztu $J(q)$ mierzącą, jak bardzo robot się myli:

J(q) = \|\mathbf{p}(q) - \mathbf{p}^*\|^2 + \|R(q) - R^*\|^2_F

(pierwsza składowa to kwadrat odległości pomiędzy aktualną a zadaną pozycją końcówki, druga — analogicznie dla orientacji). Wartość $J(q) = 0$ oznacza idealne trafienie w pozę. Każda inna konfiguracja daje $J(q) > 0$ . Zatem:

q^* = \arg\min_q J(q) \quad\Longleftrightarrow\quad \text{IK}

Rozwiązanie IK to znalezienie minimum funkcji kosztu. Tylko że teraz $x$ stało się 6-wymiarowym wektorem $q = (q_1, \dots, q_6)$ , a krajobraz „gór" stał się 6-wymiarowy. Reszta logiki jest taka sama jak w 1D — schodzimy w dół iteracyjnie.

Dlaczego osobny moduł, skoro moduł 3 też tym jest?

Solvery Jakobianowe z modułu 3 też minimalizują funkcję kosztu — tylko bardzo specyficznym sposobem (linearyzacja przez Jakobian). Tu rozszerzamy spojrzenie:

Funkcja kosztu może być bogatsza. Nie tylko błąd pozy — możemy dodać kary za przekroczenie limitów przegubów, kary za bliskość singularności, kary za kolizje, kary za odległość od „neutralnej" pozycji robota. Każdy taki cel to dodatkowy człon w $J(q)$ .
Algorytmy są bardziej elastyczne. Nelder-Mead nie wymaga gradientu — działa nawet na funkcjach nieróżniczkowalnych (np. binarne „w kolizji?"). SQP narzuca twarde ograniczenia (nie jako kary, lecz jako ścisłe warunki). Algorytmy ewolucyjne eksplorują globalnie, nie lokalnie.
Mniej wrażliwe na singularności. Solvery optymalizacyjne nie odwracają Jakobianu jawnie, więc problemy z uwarunkowaniem nie eskalują tak gwałtownie.

Cena: zazwyczaj wolniejsze niż DLS dla pojedynczego zapytania IK (dziesiątki–setki ms vs ~ms). Ale dla problemów z bogatymi ograniczeniami lub redundancją robota są bezkonkurencyjne.

IK jako problem optymalizacji — formalnie

Po tym wstępie możemy zapisać IK ścisle:

q^* = \arg\min_{q \in Q} \; J(q) \quad\text{s.t.}\quad q_i^\text{min} \le q_i \le q_i^\text{max},\; g_k(q) \le 0

$J(q)$ — funkcja kosztu (podobna do tej z modułu 3, ale może być bogatsza). s.t. (subject to) wprowadza ograniczenia: twarde dla limitów przegubów oraz ograniczenia ogólne $g_k(q) \le 0$ (np. kolizje).

W tym module standardowa funkcja kosztu wygląda tak:

J(q) = w_p\,\|\mathbf{p}(q) - \mathbf{p}^*\|^2 + w_o\,\|\boldsymbol{\omega}(q, R^*)\|^2 + w_\ell \sum_i \max(0,\; q_i - q_i^\text{max})^2 + \max(0,\; q_i^\text{min} - q_i)^2

Trzy człony, każdy mierzący inną „karę":

$w_p\,\|\mathbf{p}(q) - \mathbf{p}^*\|^2$ — kara za błąd pozycji końcówki (wagi $w_p$ )
$w_o\,\|\boldsymbol{\omega}\|^2$ — kara za błąd orientacji (waga $w_o$ )
$w_\ell \sum_i \max(\dots)^2$ — kara „miękka" za przekroczenie limitów (zero gdy w zakresie)

Zalety tego sformułowania:

Miękko narzucamy ograniczenia przegubowe (jako kara kwadratowa) — w odróżnieniu od twardego odcięcia, które potrafi „zablokować" solver na granicy.
Dodawać drugorzędne cele: odległość od singularności (regularyzacja $-\gamma\,\log\det(JJ^\top)$ ), bliskość neutralnej pozycji, unikanie kolizji (pole odstraszające).
Łączyć priorytety: $w_p \gg w_o$ oznacza „za wszelką cenę osiągnij pozycję, orientacja — jeśli się uda". To zasadniczo inna filozofia niż analityczna IK, która traktuje pozycję i orientację równoprawnie.

Krok 1

Nelder–Mead — szukamy minimum bez gradientu

Pomysł intuicyjny: wyobraź sobie, że nie znasz gradientu — nie wiesz, w którą stronę idzie spadek. Możesz tylko zmierzyć wysokość w kilku punktach. Co robisz?

Postaw na terenie trójkąt (a w przestrzeni $n$ -wymiarowej — figurę z $n+1$ wierzchołków, zwaną simpleksem). Zmierz wysokość każdego rogu. Najgorszy róg (najwyżej położony) — odbij przez środek pozostałych. Jeśli nowy róg jest niższy niż wszystkie poprzednie — świetnie, idź dalej w tym kierunku. Jeśli nie pomogło — ścisnij trójkąt do wewnątrz. Iteruj.

To jest klasyczna metoda Nelder–Mead (1965). W każdej iteracji simpleks przesuwa się i deformuje, „pełzając" w kierunku minimum. Cztery podstawowe operacje:

Posortuj wierzchołki po wartości kosztu.
Reflection: odbij najgorszy przez centroid pozostałych: $\mathbf{x}_r = \bar{\mathbf{x}} + \alpha (\bar{\mathbf{x}} - \mathbf{x}_\text{worst})$ .
Expansion: jeśli odbity był wyjątkowo dobry, pójdź dalej w tym kierunku.
Contraction: jeśli odbity nie pomógł, kontraktuj w stronę centroida.
Shrink: jeśli nic nie działa — zwiń cały simpleks ku najlepszemu wierzchołkowi.

Zalety: nie wymaga gradientów, więc działa nawet jeśli funkcja kosztu jest nie-gładka (np. ma sztywne progi kolizyjne, „ścianki" z penalty). Implementacja jest trywialna (kilkadziesiąt linii kodu).

Wady: brak teoretycznych gwarancji zbieżności dla funkcji niewypukłych. Dla wymiarów $n > 10$ staje się powolne (simpleks ma $n+1$ punktów do utrzymania). Dla naszych 6 wymiarów IK jest jeszcze rozsądny.

// pseudo-Nelder-Mead
simplex = [x0, x0+e1·step, x0+e2·step, ..., x0+en·step];
while not converged:
  sort(simplex by f(x))                    // posortuj
  centroid = mean(simplex except worst)    // środek bez najgorszego
  reflected = centroid + α·(centroid - worst)
  if f(reflected) < f(best):  try expansion
  elif f(reflected) < f(second_worst): replace worst
  elif f(reflected) < f(worst): contract
  else: shrink toward best

Wizualizacja na żywo — Nelder-Mead w 2D

Funkcja kosztu: $f(x, y) = 0.5(x-2)^2 + 2(y-1)^2$ — paraboloida asymetryczna, minimum w $(2, 1)$ . Simpleks startuje w lewym dolnym rogu z trzech wierzchołków. Naciśnij ▶ odtwórz i obserwuj jak trójkąt „pełza" w stronę zielonego minimum, deformując się przy każdej iteracji.

iteracja 0/23Start

Najlepszy wierzchołek: f(-2.500, -1.000) = 18.1250

Trójkąt fioletowy to aktualny simpleks (3 wierzchołki w 2D, w n-D byłoby n+1). Każda iteracja zastępuje najgorszy wierzchołek (czerwony) nowym punktem, generowanym przez jedną z czterech operacji: reflection, expansion, contraction, shrink. Zielona kropka po prawej to minimum globalne x* = (2, 1).

Najlepszy wierzchołek (zielony) nigdy się nie pogarsza — jest najmocniejszą gwarancją Nelder-Meada. Czerwony (najgorszy) jest zastępowany w każdym kroku. Kolejna ramka pokazuje, którą operację zastosowano: reflection dominuje na początku (długie skoki), contraction przy końcu (precyzyjne dopasowanie).

Krok 2

Gradient descent — schodzenie po zboczu

Pomysł intuicyjny: jeśli MOŻESZ obliczyć gradient (kierunek najszybszego wzrostu), to po prostu idź w przeciwną stronę. Każdy krok zmniejsza wysokość — przynajmniej jeśli krok nie jest za duży.

Klasyczna metoda Gradient Descent:

q_{k+1} = q_k - \alpha\,\nabla J(q_k)

$\nabla J(q)$ to wektor pochodnych cząstkowych — mówi, jak bardzo każdy z 6 przegubów zmienia funkcję kosztu (w otoczeniu aktualnej konfiguracji). Dla naszej funkcji kosztu:

\nabla J(q) = -2\,J^\top\,\mathbf{W}\,\mathbf{e}(q)

gdzie $J$ to jakobian (z modułu 3), $\mathbf{e}(q)$ to wektor błędu pozy, $\mathbf{W}$ — diagonalna macierz wag (pozycja vs orientacja).

Problem długości kroku: stała wartość $\alpha$ nie zawsze działa — w stromych dolinach wystarczy mały krok, na płaskich równinach trzeba dużych skoków. Warunek Armijo rozwiązuje to adaptacyjnie:

J(q + \alpha\,\mathbf{d}) \le J(q) + c_1\,\alpha\,\nabla J(q)^\top \mathbf{d}

Słownie: po zrobieniu kroku nowa wartość kosztu ma być wyraźnie mniejsza niż gdyby gradient był liniowy. Jeśli nie jest — zmniejszamy $\alpha$ (typowo połowicznie: $\alpha \leftarrow \beta \alpha$ , $\beta = 0{,}5$ ) i próbujemy ponownie. Jeśli jest — akceptujemy krok.

Obserwacja teoretyczna: dla naszego kosztu IK z $w_o = 0$ (tylko pozycja, brak orientacji) i bez limitów, GD z Armijo pokrywa się z Jacobian Transpose z modułu 3 — to jest dokładnie ten sam algorytm, opisany z innej perspektywy. To jest piękny moment łączący moduły 3 i 4: różne tradycje matematyczne, ten sam algorytm.

Ograniczenie: w dolinach o nierównym uwarunkowaniu (długie i wąskie, jak w pobliżu singularności) GD zygzakuje i jest powolny. Lepsze metody korzystają z drugiego rzędu (Hessianu) — patrz SQP niżej.

Wizualizacja na żywo — gradient descent w 2D

Ta sama funkcja kosztu, co poprzednio. Suwakiem regulujesz długość kroku $\alpha$ . Czerwona strzałka pokazuje krok $-\alpha\nabla f$ , niebieska linia — historię iteracji:

iteracja 0/17

krok α =0.30

Spróbuj różnych wartości α: 0.05 = bardzo wolne ale stabilne; 0.30 = sensowne; 0.50+ = oscylacje (kierunek y ma steeper gradient).

Aktualnie: x = (-2.000, -2.000),f(x) = 26.0000

∇f = (-4.000, -12.000), ‖∇f‖ = 12.649

Czerwona strzałka pokazuje krok x_k+1 = x_k − α·∇f(x_k). Niebieski ślad — historia iteracji. Zauważ: dolinę (wąską oś) GD zwykle pokonuje zygzakowato, a długą oś — wolno. To klasyczna patologia GD na funkcjach o nierównej skali.

Eksperymentuj z $\alpha$ :

$\alpha = 0{,}05$ — bardzo wolne, ale stabilne. Każdy krok mały, do minimum potrzeba dziesiątek iteracji.
$\alpha = 0{,}30$ — sensowny kompromis. Kilkanaście iteracji wystarczy.
$\alpha = 0{,}50+$ — krok za duży! Zauważ jak ścieżka zygzakuje między ścianami doliny. Oś y ma steeper gradient (współczynnik 4), więc duży krok przeskakuje przez minimum w pionie. Klasyczny przykład wrażliwości GD na uwarunkowanie funkcji kosztu.

Ten zygzak to powód, dla którego w prawdziwych zastosowaniach używamy metod adaptacyjnych (Adam — patrz moduł 5) albo metod drugiego rzędu (Newton, BFGS — patrz SQP niżej).

Krok 3

SQP — Sequential Quadratic Programming

Pomysł intuicyjny: GD robi „liniowe" przybliżenie funkcji kosztu (gradient = pochodna pierwszego rzędu). Newton robi „kwadratowe" (gradient + Hessian = pochodne drugiego rzędu).SQP idzie krok dalej: w każdej iteracji budujelokalny problem kwadratowy z liniowymi ograniczeniami i rozwiązuje go ściśle.

Dla każdej iteracji:

Aproksymuj Hessian Lagrangianu (BFGS lub dokładny).
Rozwiąż lokalny problem kwadratowy (QP) ze zlinearyzowanymi ograniczeniami.
Wykonaj krok z line search spełniającym warunki KKT (Karusha–Kuhna–Tuckera).

Zalety:

Twarde ograniczenia — limity $q_i^{\text{min}} \le q_i \le q_i^{\text{max}}$ są zachowywane ściśle, nie jako kary.
Wykorzystuje krzywiznę — szybciej zbiega niż GD na trudnych funkcjach.
Konwergencja kwadratowa blisko optimum — liczba dokładnych cyfr podwaja się z iteracją.

Wady: kosztowne (rozwiązanie QP per iteracja to $O(n^3)$ ), wymaga biblioteki (np. SciPy minimize(method='SLSQP') czy trust-constr).

W aplikacji uruchamiamy SQP w przeglądarce przez Pyodide (SciPy skompilowany do WebAssembly — patrz moduł 1, sekcja „Ten sam solver, dwa języki"). W tym module używamy lekkich solverów TS (Nelder-Mead, GD) bez SQP — SQP wymagałby integracji z Pyodide, która jest poza zakresem tego modułu, ale dostępna w przyszłych rozszerzeniach.

Wizualizacja na żywo — projected gradient z aktywnym ograniczeniem

Ta sama funkcja kosztu, ale teraz dodajemy twarde ograniczenie: $g(x, y) = y + 0{,}5x - 1{,}5 \le 0$ — obszar nad czerwoną prostą jest zakazany. Minimum bez ograniczenia ( $2, 1$ ) leży powyżej linii — czyli w obszarze niedopuszczalnym. Algorytm musi znaleźć minimum na granicy:

iteracja 0/21

Aktualnie: x = (-2.000, -2.000),f(x) = 26.0000

Czerwona strzałka = pełny kierunek −∇f (jaki zrobiłby zwykły GD). Po napotkaniu czerwonej linii (granica ograniczenia) fioletowa strzałka = kierunek po rzutowaniu gradientu na styczną do ograniczenia. Tak właśnie SQP/projected gradient utrzymuje punkt w obszarze dopuszczalnym. Minimum z ograniczeniem: (1.5, 0.75), leży na granicy (różne od minimum bez ograniczenia (2, 1)).

Co dokładnie pokazuje wizualizacja (uproszczona wersja SQP — projected gradient):

Czerwona prosta — granica ograniczenia $g(x,y) = 0$ . Czerwony pasek nad nią — obszar niedopuszczalny.
Zwykły GD (czerwona strzałka) chciałby iść prosto na $(2, 1)$ , ale wszedłby w obszar zakazany.
Po napotkaniu granicy krok zostaje zrzutowany na styczną do ograniczenia (fioletowa strzałka) — usuwamy składową gradientu prostopadłą do granicy. Algorytm „ślizga się" wzdłuż linii.
Pełny SQP robi dokładnie to samo, ale używa Hessianu zamiast samego gradientu (krok kwadratowy). Trajektoria byłaby krótsza, ale charakter „ślizgania się po granicy" identyczny.

Minimum z ograniczeniem leży w $(1{,}5,\ 0{,}75)$ — nie w $(2, 1)$ ! To pokazuje, że ograniczenia zmieniają nie tylko trajektorię, ale i samo rozwiązanie. W IK robota twarde limity przegubowe potrafią kompletnie zmienić wybraną gałąź rozwiązania.

Porównanie na żywo

Trzy solvery startują z aktualnej konfiguracji głównego kontrolera i zmierzają do tej samej pozy $T^*$ . Obserwuj wykresy zbieżności (pionowa = funkcja kosztu w skali log) i porównaj liczbę iteracji oraz czas. Możesz manipulować wagami funkcji kosztu — zobacz, jak zmienia to dynamikę.

Konfiguracja przegubów

θ₁2.8°

θ₂-77.7°

θ₃6.8°

θ₄8.5°

θ₅-19.3°

θ₆172.0°

Poza efektora (T₀⁶)

Pozycja [m]

x = 0.5000

y = 0.1500

z = 0.3000

Orientacja RPY [°]

R = 0.00

P = 90.00

Y = 0.00

Poza docelowa T*

Pozycja [m]

xyz

Orientacja RPY [°]

RPY

Wagi w funkcji kosztu

w_pos (pozycja)w_ori (orientacja)w_lim (limity przegubów)

metoda	iteracje	residuum	czas [ms]	status

Trajektorie końcówki w trakcie iteracji

Każdy z trzech solverów startuje z tego samego seeda i zmierza do tej samej pozy docelowej (czerwona kropka). Pełna linia pokazuje ścieżkę TCP po kolejnych iteracjach — widać wprost, jak szybko i jaką drogąsolver dochodzi do celu. Szary „duch” to robot w końcowej konfiguracji znalezionej przez metodę.

Kiedy używać optymalizacji zamiast Jakobiana?

Twarde ograniczenia przegubowe / kolizyjne — SQP, trust-region lub metoda kar skutecznie je obsługują; czysty DLS ignoruje.
Drugorzędne cele — odległość od singularności, bliskość neutralnej konfiguracji, minimalizacja zużycia energii; wchodzą do kosztu jako dodatkowe człony.
Manipulatory redundantne ( $n > 6$ ) — niezerową przestrzeń zerową jakobianu (null-space) można zagospodarować dodatkowym kryterium.
Nieróżniczkowalne cele — np. binarne „czy w kolizji z tym obiektem". Nelder-Mead, CMA-ES, PSO, Symulowane Wyżarzanie.
Globalna wyszukiwarka rozwiązań — algorytmy ewolucyjne z restartami potrafią znajdować alternatywne gałęzie niedostępne dla metod lokalnych.

Wybrane strategie zaawansowane (sygnalnie)

CMA-ES — Covariance Matrix Adaptation Evolution Strategy (Hansen & Ostermeier). Stochastyczna, skuteczna dla $n \le 100$ .
Trust-region — zamiast linii poszukiwania, adaptacyjny region zaufania dla kroku Newtona. SciPy trust-krylov, trust-ncg.
Interior-point — dla twardych ograniczeń nierównościowych; stosowane w optymalizacji trajektorii.
ADMM / rozwiązania rozdzielone — gdy koszt ma strukturę separowalną (pozycja + orientacja + limity).

Co dalej

W module 5 porzucamy klasyczny cyfrowy przepis i patrzymy, co stanie się, gdy IK chcemy wyuczyć na danych — od naiwnego MLP (który słabo radzi sobie z wielomodalnością 8 rozwiązań) po nowoczesne Invertible Neural Networks, samplujące pełny posterior. W module 6 wracamy do benchmarku, który zmierzy wszystkie solvery z modułów 1–5 na tym samym zbiorze testowych poz.