Optymalizacja trajektorii

Trajektoria parametryzowana jako wektor waypointów $\xi = [q_0, q_1, \ldots, q_N]^\top \in \mathbb{R}^{(N+1)d}$. Zadanie:

gdzie $L$ to lokalny lagranżjan (smoothness, energy, clearance), $h$ to ograniczenia (kolizja, joint limits, dynamika).

Inicjalizacja krytyczna — typowo linia prosta w C-space lub ścieżka z RRT (warm start dla optymalizacji = hybrydowe planery: sample + optimize).

Typowe człony funkcji kosztu:

• Smoothness: $\|\ddot q\|^2$ lub $\|\dddot q\|^2$ (jerk-minimal trajektorie).
• Obstacle: $c_{\text{obs}}(q) = \max(0, \epsilon - \Phi(q))^2 \cdot |\dot q|$ — kara aktywna gdy $\Phi(q) < \epsilon$ (przeszkoda bliżej niż margines), waga proporcjonalna do prędkości (kara rośnie gdy szybko przelatuje).
• Energy: $\sum_t \|\tau_t\|^2$ dla trajektorii dynamicznej (wymaga inwersji dynamiki).
• Time: $T$ (długość trajektorii).
• Manipulability: $-w(q)$ by trzymać się dalej od singularności (moduł 02).

CHOMP łączy dwa pomysły:

1. Funkcjonalny gradient — traktuje trajektorię jako element przestrzeni funkcyjnej, liczy gradient kosztu względem niej. Dla obstacle term — dosłownie $-\nabla \Phi(q_t)$ (kierunek „uciekania" od przeszkód, moduł 03).
2. Covariant preconditioning — gradient mnożony przez $M^{-1}$ gdzie $M$ to Hessian smoothness term. Daje to lepszą zbieżność i naturalnie propaguje zmiany jednego waypointa na sąsiednie (efekt „guma ciągnie").

Update rule (uproszczony, bez covariant preconditioning):

Gdzie gradient na i-tym waypoincie:

Pierwszy człon (smoothness): negatywny dyskretny Laplacian — siła ciągnąca $q_t$ ku średniej z sąsiadów (linia prosta jest jego minimum). Drugi (obstacle): siła odpychająca proporcjonalna do głębokości w pasie bezpieczeństwa.

W demo poniżej: obserwuj trajektorię deformującą się od linii prostej do zakrzywionej krzywej omijającej przeszkody. Strzałki $-\nabla J$ na każdym waypoincie pokazują kierunek update. Ghost'y co 8 iteracji — historia deformacji. Suwaki $w_s, w_o, \varepsilon$ w czasie rzeczywistym przeliczają cały bieg.

Eksperymenty do wypróbowania:

• Zwiększ $w_o$ z 15 do 35 — trajektoria odsuwa się dalej od przeszkód, ale wydłuża.
• Zmniejsz $w_s$ do 0.3 — trajektoria może mieć widoczne „zygzaki", smooth term mniej dominuje.
• Zwiększ $\varepsilon$ z 0.06 do 0.12 — większy margines bezpieczeństwa, krzywa znacznie szerszy łuk wokół przeszkód.

Wariant Incremental CHOMP

Online'owy wariant: re-run optymalizacji po każdej iteracji sterownika, z warm-start poprzednią trajektorią + horyzontowym odcięciem. Łączy planowanie + sterowanie. Konkurencja MPC (moduł 10) — różnica: CHOMP iteruje gradient descent dla pełnej trajektorii, MPC rozwiązuje QP nad krótkim horyzontem.

CHOMP wymaga różniczkowalnej funkcji kosztu — gradient musi istnieć. W praktyce niektóre koszty są nieróżniczkowalne:

• Binarny collision check (FCL zwraca true/false, brak gradientu)
• Niefizyczne koszty (np. „odległość od demonstracji eksperta" liczona przez DTW)
• Koszty oparte na nauczonych modelach (sieć neuronowa może mieć gradient szumowy)

STOMP omija to przez gradient-free sampling. Algorytm:

for iter = 0..maxIter:
  for k = 1..K:                              # K rolloutów
    ε_k ~ N(0, Σ)                            # zaszumiona perturbacja
    ξ_k = ξ + ε_k                            # zaszumiona trajektoria
    J_k = cost(ξ_k)                          # ewaluacja (callbox)
  w_k = exp(-J_k / λ) / Σ_j exp(-J_j / λ)    # softmax wag
  ξ ← ξ + Σ_k w_k · ε_k                      # update jako średnia ważona

Kluczowe parametry:

• $K$ (typowo 10-30) — liczba rolloutów per iteracja. Więcej = lepsza estymacja gradientu, ale wolniej.
• $\sigma$ — odchylenie standardowe szumu. Większe = szersza eksploracja.
• $\lambda$ — temperatura softmax. Mała$\lambda$ ⇒ greedy (dominuje 1-2 najlepsze rollouty); duża ⇒ uśrednianie po wielu (eksploracja).

Macierz kowariancji $\Sigma$ wybierana tak, by szum był skorelowany wzdłuż trajektorii — pojedyncze waypointy nie przesuwają się niezależnie, tylko całe „odcinki" (gładkie perturbacje). Typowo $\Sigma = (A^\top A)^{-1}$ gdzie $A$ macierz drugiej różnicy (=Hessian smoothness).

STOMP vs CHOMP — kiedy które

• CHOMP szybsze gdy gradient jest dostępny i tani; wymaga gładkości.
• STOMP wolniejsze (każda iteracja = K ewaluacji), ale działa dla dowolnej (czarno-skrzynkowej) funkcji kosztu. Trywialnie parallelizuje się (każdy rollout osobno).
• Hybryda ITOMP — CHOMP wewnątrz, STOMP do rozwiązywania perturbacji przy lokalnych minimach.

TrajOpt (Schulman, Ho, Lee, Awwal, Bradlow, Abbeel; 2013) używa Sequential Convex Optimization (SCO) — kolejne lokalne convex aproksymacje pełnego nieliniowego problemu.

Każda iteracja:

1. Linearyzuj nielinearne constraints wokół bieżącej trajektorii $\xi^{(k)}$.
2. Wyminimum convex problem (QP) wewnątrz trust region $\|\xi - \xi^{(k)}\| \leq \delta$.
3. Sprawdź czy lokalna predykcja zgadzała się z prawdziwym kosztem — jeśli tak, zwiększ $\delta$; jeśli nie, zmniejsz i powtórz.

Kluczowy trick — penalty z adaptacyjnym μ: constraints kolizji przekształcamy w karę $\mu \sum_t \mathrm{viol}(q_t)$ z dużym $\mu$. Gdy iteracja produkuje feasible trajektorię, $\mu$ nie rośnie; gdy nie — podwajamy. Konwergencja w kilku-kilkunastu zewnętrznych iteracjach.

GuSTO (Bonalli 2019) — ulepszenie SCO z lepszymi gwarancjami zbieżności (trust-region SQP z line-search).

Trzy kluczowe różnice TrajOpt vs CHOMP

• Constraint handling: CHOMP używa soft penalty w funkcji kosztu (gradient z miękkim odpychaniem). TrajOpt traktuje obstacle jako hard constraintz adaptive μ — gwarancja feasibility w limicie.
• Trust region: CHOMP zaufa pełnemu gradient step'owi (z fixed learning rate). TrajOpt weryfikuje czy linearyzacja była dobra — jeśli nie, zmniejsza krok. Sprawia że TrajOpt rzadziej dywerguje.
• Konwergencja: CHOMP O(N) iteracji do konwergencji. TrajOpt O(log(1/ε)) outer + O(N) inner — wykładniczo szybsza w pobliżu rozwiązania, kosztem droższej pojedynczej iteracji (QP solve).