Moduł 08 · Optymalizacja trajektorii

Parametryzacja i generacja trajektorii

Wielomiany (cubic..septic, min-jerk), B-spline/NURBS/Bézier, TOTG, TOPP-RA, Ruckig, DMP, ProMP, KMP, reskalowanie czasowe.

TL;DR

Planery (moduły 05-07) zwracają ścieżkę — geometrię w C-space, często jako listę waypointów. Sterownik potrzebuje trajektorii: pełnej funkcji $\xi(t)$ z wyznaczoną prędkością, przyspieszeniem (i czasem jerk) w każdej chwili. Ten moduł pokazuje sposoby przejścia path → trajectory:

Wielomiany (cubic / quintic / septic / min-jerk) — proste, analityczne, dla każdej pary waypointów. Wyższy rząd = gładsza wyższa pochodna.
Splajny (Catmull-Rom, B-spline, NURBS, Bézier) — łączą wiele segmentów z gwarantowaną ciągłością wyższych pochodnych.
Time-optimal reparametrization (TOTG, TOPP-RA, Ruckig) — zachowując ścieżkę geometryczną, dobiera czasowanie respektujące limity $v_{\max}, a_{\max}, j_{\max}$ .
DMP / ProMP / KMP — generatory trajektorii uczone z demonstracji, replay z perturbacjami goal.

Dla Pandy: typowy stack to quintic spline między waypointami z RRT-Connect + Ruckig dla online reparametrization z respektowaniem limitów Pandy ( $v_{\max} \approx 2 \, \text{rad/s}, a_{\max} \approx 15 \, \text{rad/s}^2$ ).

Wielomianowe interpolacje punkt-do-punktu

Dla pojedynczego ruchu $q_0 \to q_T$ z $\dot q(0) = \dot q(T) = 0$ , możemy szukać wielomianu $q(t) = \sum_{i=0}^n a_i t^i$ . Liczba współczynników $n+1$ musi się zgadzać z liczbą warunków brzegowych (BCs):

Cubic (n=3): 4 BCs — $q(0), q(T), \dot q(0), \dot q(T)$ . Daje ciągłą prędkość, ale q̈ skacze na końcach (impulse acceleration).
Quintic (n=5): 6 BCs — dodatkowo $\ddot q(0) = \ddot q(T) = 0$ . Ciągłe accel; jerk skacze. To minimum-jerk solution(Flash & Hogan 1985 — biologicznie inspirowane modele ruchu ludzkiego).
Septic (n=7): 8 BCs — dodatkowo $\dddot q(0) = \dddot q(T) = 0$ . Ciągły jerk. Idealny dla precyzyjnych zadań (welding, painting) gdzie mechaniczne drgania pochodzą od skoków jerk.

Closed-form dla rest-to-rest $0 \to 1$ w T=1:

\begin{aligned}\text{cubic:}\;\; q(t) &= 3t^2 - 2t^3 \\ \text{quintic:}\; q(t) &= 10t^3 - 15t^4 + 6t^5 \\ \text{septic:}\;\; q(t) &= 35t^4 - 84t^5 + 70t^6 - 20t^7\end{aligned}

Wielomianowe profile — porównanie 3 rzędów

Rząd	q(t) (pozycja)	q̇(t) (prędkość)	q̈(t) (przyspieszenie)	q⃛(t) (jerk)
Cubic (3°) deg = 3 coeffs: 3, -2
Quintic (5°) deg = 5 coeffs: 10, -15, 6
Septic (7°) deg = 7 coeffs: 35, -84, 70, -20	peak: 0.50	peak: 2.19	peak: 0.00	peak: 52.50

Rząd	peak \|q̇\| · T	peak \|q̈\| · T²	peak \|q⃛\| · T³	Brzeg: q̇=0?	q̈=0?	q⃛=0?
Cubic (3°)	1.500	6.000	12.00	✓	✗ (nieciągłe)	✗ (nieciągłe)
Quintic (5°)	1.875	5.773	60.00	✓	✓	✗ (nieciągłe)
Septic (7°)	2.188	7.513	52.50	✓	✓	✓

Co zauważyć: wszystkie 3 wielomiany realizują ten sam ruch (q: 0 → 1 w T = 1 s) z zerową prędkością na końcach. Cubic ma nieciągłe q̈ na brzegach (skok, „kick"). Quintic dodaje warunek q̈(0)=q̈(T)=0 — gładkie accel, ale jerk skacze. Septic gwarantuje ciągłe q⃛ aż do 3. pochodnej — idealne dla precyzyjnych ruchów (kosztem wyższych szczytów: peak |q̇| rośnie z 1.5 (cubic) do ~1.88 (quintic) do ~2.18 (septic)). Wniosek: wyższy rząd ⇒ gładsze ale wymaga większego zapasu w limitach robota.

Skalowanie do dowolnego ruchu

Z wzoru dla unitarnego ruchu (0→1 w T=1) skalujemy do $q_0 \to q_T$ w czasie T:

q(t) = q_0 + (q_T - q_0) \cdot f(t/T)

gdzie $f(\tau)$ = closed-form dla unitarnego. Pochodne skalują się: $\dot q_{\max} = \Delta q / T \cdot c_v$ , gdzie $c_v$ = peak unitarny (z tabeli: 1.5/1.875/2.187). Konsekwencja: dla cubic potrzebujemy 50% większego zapasu na prędkości niż quintic dla tego samego ruchu.

Splajny — łączenie wielu segmentów

Gdy ścieżka ma więcej niż 2 waypointy, łączymy wielomiany w splajn. Klasyfikacja:

Interpolating — krzywa przechodzi przez wszystkie waypointy. Catmull-Rom, cubic Hermite.
Approximating — krzywa przybliża waypointy (przechodzi tylko przez pierwszy i ostatni). B-spline, Bézier o stopniu > 3.
Rational (NURBS) — uogólnia B-spline o wagi i non-uniform knot vector. Może dokładnie reprezentować okręgi, elipsy, stożkowe — standard w CAD/CAM.

Catmull-Rom jest prosty w implementacji i dobry dla animacji (klucz: przechodzi przez punkty, daje C¹). Demo poniżej — przeciągnij punkty.

Catmull-Rom spline — przeciągnij punkty kontrolne

Catmull-Rom to cubic interpolating spline — krzywa przechodzi przez wszystkie punkty kontrolne z gładkością C¹ (ciągła pierwsza pochodna). W każdym segmencie tangens jest obliczany z poprzedniego i następnego punktu. Inne warianty: B-spline (krzywa NIE przechodzi przez punkty, ale daje ciągłość C²; control points „kontrolują" globalny kształt) i Bézier (krzywa przechodzi przez pierwszy i ostatni punkt, ale pośrednie punkty „wciągają"). NURBS to non-uniform rational B-spline — uogólnienie z wagami i nieuniform knot vector, standard w CAD.

B-spline w skrócie

B-spline stopnia $k$ z $n+1$ control points i knot vector $u_0, \ldots, u_{n+k+1}$ :

C(u) = \sum_{i=0}^{n} P_i \cdot B_{i,k}(u)

gdzie $B_{i,k}$ to bazowe funkcje B-spline (Cox-de Boor recurrence). Krzywa NIE przechodzi przez control points (poza końcami w clamped). Zalety:

Local control — przesunięcie jednego $P_i$ wpływa tylko na $k+1$ sąsiednich segmentów.
Wyższa ciągłość automatycznie: stopień 3 = C².
Convex hull property — krzywa zawsze leży w otoczce convex control points (bezpieczeństwo dla planowania).

Implementacja w robotyce: MoveIt2 używa quintic B-spline jako domyślne wygładzanie po RRT-Connect. Pinocchio i acados implementują B-spline w C++/Python.

B-spline / NURBS — interaktywny edytor control points

Eksperymenty: (1) Przeciągnij jeden CP — zauważ local control: krzywa zmienia się tylko w sąsiednich segmentach (zasięg

k+1

). (2) Zwiększ degree — krzywa staje się gładsza ale wolniej reaguje na CP. (3) Włącz NURBS i podnieś jedną wagę do 5 — krzywa „ciągnie się" do tego CP (przy

w \to \infty

przechodzi przez niego). (4) Pokaż bazowe funkcje — partition of unity

\sum_i B_{i,k}(u) = 1

wszędzie w domenie; każda

B_{i,k}

ma wsparcie na

k+2

kolejnych przedziałach knot vector.

Time-optimal reparametrization

Powyższe metody nie szanują limitów robota: cubic z $\Delta q = 5 \, \text{rad}$ w T=0.5s może wymagać $\dot q \approx 15 \, \text{rad/s}$ — daleko poza $\dot q_{\max} = 2.17 \, \text{rad/s}$ Pandy. Trzeba dłuższego czasu.

Time-optimal reparametrization: zachowaj kształt ścieżki, dobierz szybkość przejścia po niej tak, by każda chwila respektowała limity.

Trapezoidal vs S-curve

Najprostszy schemat — trapezoidal velocity profile: 3 fazy (acc, cruise, decel). Daje nieciągłe q̈ (skokowy bang-bang accel). Lepsze: S-curve = 7 faz z ograniczonym jerk. Profil accel ma kształt trapezoidu, velocity literalnie wygląda jak „S". Standard w sterowaniach CNC i robotach przemysłowych.

Trapezoidal vs S-curve — porównanie

Profil	q(t)	q̇(t)	q̈(t)	q⃛(t)
Trapezoidal T = 2.267 s
S-curve T = 2.517 s

D (dystans)1.00

v_{\max}

0.60

a_{\max}

1.00

j_{\max}

4.0

Co zauważyć: Trapezoidal: q̈ (accel) ma kształt prostokątny — szybko przełącza się między

\pm a_{\max}

, q⃛ to seria impulsów (nieskończony jerk). S-curve: accel rośnie liniowo z ograniczonym jerk — fizycznie wykonalne dla robota z elastycznym sterowaniem. Trapezoidal jest krótszy w czasie, ale S-curve generuje mniej drgań mechanicznych. Ruckig (online jerk-limited) to standardowa implementacja używana w przemysłowych kontrolerach. Próbka: zwiększ

j_{\max}

— S-curve zbliża się do trapezoidal.

Algorytmy w praktyce

TOTG(Kunz & Stilman 2012) — Time-Optimal Trajectory Generation. Pracuje z 2-segmentowym profilem (constant accel / decel). Otwarty kod w MoveIt2.
TOPP-RA (Pham 2018) — Reachability Analysis podejście. Operuje na fazowej przestrzeni $(s, \dot s)$ gdzie $s$ = parametr ścieżki. Najbardziej elastyczne — obsługuje constraints momentów, limit kąta między path i osi grawitacji.
Ruckig (Berscheid 2021) — online jerk-limited S-curve. Online: pełen przelot w ~1 µs, można wywoływać 1 kHz w pętli sterowania. Obsługuje arbitrary initial state (nie-zerowa vel/acc start). Standard 2024 dla przemysłowych manipulatorów. pantor/ruckig.

Movement Primitives — DMP, ProMP, KMP

Powyższe metody zakładają, że my definiujemy kształt trajektorii (wielomian, splajn). Czasem łatwiej zademonstrować ruch (np. nauczycielem prowadząc ramieniem robota) i odtworzyć go z perturbacjami.

Dynamic Movement Primitives (DMP, Ijspeert 2002)

DMP modeluje trajektorię jako wymuszony tłumiony oscylator:

\tau \ddot y = \alpha_z (\beta_z (g - y) - \dot y) + f(s)

gdzie $y(t)$ to pozycja, $g$ to goal, $\alpha_z, \beta_z$ stałe (typowo 25, 6.25), $s(t)$ = faza (od 1 do 0), $f(s)$ = forcing term — kombinacja funkcji bazowych Gaussa, uczona z demonstracji przez regresję:

f(s) = \frac{\sum_i \psi_i(s) w_i}{\sum_i \psi_i(s)} \cdot s \cdot (g - y_0)

Kluczowa zaleta: zmiana goal $g \to g'$ daje nową trajektorię o tym samym kształcie — system asymptotycznie zbiega do nowego goal, zachowując demonstrowany styl ruchu. Idealne dla:

Generalizacji: jeden demo „chwyć kubek" daje rodzinę trajektorii dla różnych pozycji kubka.
Adaptacji online: zmiana goal w trakcie wykonania (avoidance perturbed pose).
Imitation learning: niski koszt obliczeniowy (każda DOF niezależna), 50-100 funkcji bazowych wystarcza.

Probabilistic Movement Primitives (ProMP, Paraschos 2013)

DMP daje deterministyczną trajektorię. ProMP modelujerozkład: $\xi(t) \sim \mathcal{N}(\mu(t), \Sigma(t))$ . Trening z wielu demonstracji daje średnią + niepewność. Pozwala na:

Conditional sampling: „daj mi trajektorię, ale chwytak musi w t=2s być w pozycji X" → ProMP zwraca conditional posterior.
Wielokrotne demonstracje uśrednia + retains variance.
Coupling multiple DOFs (ProMP może mieć covariance między joints).

Kernelized Movement Primitives (KMP, Huang 2019)

KMP używa Gaussian Process zamiast Gaussian basis functions. Dajecontinuous-time reprezentację (sample w dowolnej chwili) + naturalna inkorporacja constraints jako conditioning. Bardziej elastyczna niż DMP, ale wolniejsza inference (O(n²) per sample).

Reskalowanie czasowe — formalnie

Dana jest geometryczna ścieżka $\sigma: [0, 1] \to \mathcal{Q}$ . Wprowadzamy parametryzację $s(t): [0, T] \to [0, 1]$ i wykonujemy $\xi(t) = \sigma(s(t))$ . Pochodne:

\dot \xi = \sigma'(s) \dot s, \quad \ddot \xi = \sigma''(s) \dot s^2 + \sigma'(s) \ddot s

Limity $\dot \xi_{\max}, \ddot \xi_{\max}$ przekładają się na limity $(\dot s, \ddot s)$ dla każdego punktu wzdłuż ścieżki. Time-optimal: maksymalizuj $\dot s$ (czyli minimalizuj $T = \int \frac{1}{\dot s} ds$ ).

Wynik: na każdym punkcie ścieżki istnieje maksymalny dopuszczalny $\dot s(s)$ . Te ograniczenia + dynamiczna kompatybilność (ciągłość prędkości, limity acc) dają numerycznie rozwiązany problem optymalizacji 1D.

Algorytmy: TOTG, TOPP-RA, NumericalIntegration Method (Bobrow 1988 — pierwszy paper o tym podejściu).

Ściąga

Closed-form polynomials (rest-to-rest, 0 → 1, T = 1)

Cubic: q = 3t² − 2t³
Quintic (min-jerk): q = 10t³ − 15t⁴ + 6t⁵
Septic: q = 35t⁴ − 84t⁵ + 70t⁶ − 20t⁷

Splajny

Catmull-Rom: interpolating, C¹, 4 punkty per segment
Cubic B-spline: approximating, C², local control
NURBS: B-spline + wagi (CAD/CAM)
Bézier: control points wciągają krzywą

Time-optimal

Trapezoidal — 3 fazy, brak limitu jerk
S-curve — 7 faz, jerk-limited (przemysł)
Algorytmy: TOTG, TOPP-RA, Ruckig (online)

Movement Primitives

DMP: forced damped oscillator, replay z perturbed goal
ProMP: rozkład trajektorii, conditional sampling
KMP: GP-based, continuous time

Referencje

Flash & Hogan, „The coordination of arm movements: an experimentally confirmed mathematical model" (J. Neuroscience 1985) — quintic = min-jerk.
Bobrow et al., „Time-Optimal Control of Robotic Manipulators Along Specified Paths" (IJRR 1985).
Kunz & Stilman, „Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity" (RSS 2012) — TOTG.
Pham, „A General, Fast, and Robust Implementation of the Time-Optimal Path Parameterization Algorithm" (IEEE TRO 2014) — TOPP-RA.
Berscheid & Kröger, „Online Trajectory Generation with Time-Optimal Jerk-Limited Algorithm" (IEEE Robotics & Automation Letters 2021) — Ruckig.
Ijspeert, Nakanishi, Hoffmann, Pastor, Schaal, „Dynamical Movement Primitives: Learning Attractor Models for Motor Behaviors" (Neural Computation 2013) — DMP.
Paraschos, Daniel, Peters, Neumann, „Probabilistic Movement Primitives" (NeurIPS 2013).
Huang, Rozo, Silvério, Caldwell, „Kernelized Movement Primitives" (IJRR 2019).
Piegl & Tiller, The NURBS Book (Springer, 1997) — bible dla NURBS.