Moduł 01 · Wprowadzenie

Wprowadzenie i kontekst

Pipeline od mapy do momentu napędowego: path vs motion vs trajectory planning vs control. Klasyfikacja problemów, krótka historia, demo pick&place na Pandzie.

TL;DR

Planowanie ścieżki i trajektorii to wyznaczenie i sparametryzowanie ruchu robota między dwiema konfiguracjami przy jednoczesnym poszanowaniu geometrii (unikanie kolizji) i dynamiki (limity prędkości, przyspieszeń, jerk, momentów napędowych). W tym kursie patrzymy na problem z perspektywy redundantnego manipulatora Franka Emika Panda (7 stopni swobody), gdzie:

  • przestrzeń konfiguracyjna jest 7-wymiarowa — Q⊂R7\mathcal{Q} \subset \mathbb{R}^7, więc nie wizualizuje się jej bezpośrednio,
  • zadanie zwykle żyje w 6-wymiarowej przestrzeni operacyjnej (pose narzędzia w SE(3)SE(3)) — manipulator jest redundantny, czyli ten sam cel można osiągnąć nieskończenie wieloma konfiguracjami,
  • algorytmy muszą jednocześnie unikać kolizji (zewnętrznych i self-collision), szanować joint limits Pandy i być wystarczająco szybkie (online ≥ 10–100 Hz dla MPC).

Cztery bliskie pojęcia — różne odpowiedzialności

Społeczność robotyczna używa tych terminów wymiennie, ale w literaturze technicznej mają ostre granice. Kurs zakłada znajomość tych granic.

Krok 1

Path planning — geometria, bez czasu

Wyznaczenie ciągłej krzywej σ:[0,1]→Qfree\sigma: [0, 1] \to \mathcal{Q}_{\text{free}} łączącej qstartq_{\text{start}} z qgoalq_{\text{goal}}, omijającej kolizje. Parametr nie ma jednostek czasu — to tylko parametryzacja krzywej.

σ(0)=qstart,σ(1)=qgoal,σ(u)∈Qfree ∀u∈[0,1]\sigma(0) = q_{\text{start}}, \quad \sigma(1) = q_{\text{goal}}, \quad \sigma(u) \in \mathcal{Q}_{\text{free}}\ \forall u \in [0, 1]

Typowy wynik A*, RRT, PRM. Nie ma jeszcze prędkości ani przyspieszeń.

Krok 2

Motion planning — geometria + ograniczenia kinodynamiczne

Rozszerzenie path planning o ograniczenia różniczkowe — np. limity prędkości w przegubach albo nieholonomiczne (auto, łódź, podwozie kołowe). Wynik ma sens czasowy, ale nadal niekoniecznie pełną parametryzację q˙(t),q¨(t)\dot q(t), \ddot q(t).

σ:[0,T]→Q,∣q˙(t)∣≤q˙max⁡, …\sigma: [0, T] \to \mathcal{Q},\quad |\dot q(t)| \le \dot q_{\max},\ \dots

Charakterystyczne metody: kinodynamic RRT, SST, AO-RRT (moduł 06).

Krok 3

Trajectory planning — pełna sparametryzowana funkcja czasu

Otrzymujemy trajektorię ξ:[0,T]→Q\xi: [0, T] \to \mathcal{Q} klasy co najmniej C2C^2 (dla momentów napędowych — często wymagana klasa C3C^3, czyli ciągłość jerk). Dla każdego tt wiemy nie tylko q(t)q(t), ale również q˙(t)\dot q(t) i q¨(t)\ddot q(t).

ξ(t)∈Q,ξ˙(t), ξ¨(t), ξ...(t)∈R7\xi(t) \in \mathcal{Q},\quad \dot \xi(t),\ \ddot \xi(t),\ \dddot \xi(t) \in \mathbb{R}^7

Wejście dla sterownika. Metody: spline'y wielomianowe, B-spline, TOTG, Ruckig (moduł 08); CHOMP, TrajOpt, STOMP gdy jednocześnie optymalizujemy ścieżkę (moduł 07); MPPI, iLQR online (moduł 10).

Krok 4

Control — śledzenie trajektorii momentami napędowymi

Sterownik realizuje ξ(t)\xi(t) w robocie fizycznym przez generację momentów napędowych τ∈R7\tau \in \mathbb{R}^7. Najprostsza forma: PD z feedforward z modelu dynamiki Pandy τ=M(q)ξ¨+C(q,q˙)ξ˙+g(q)+Kpe+Kde˙\tau = M(q)\ddot \xi + C(q,\dot q)\dot \xi + g(q) + K_p e + K_d \dot e.

Sterowanie nie jest planowaniem, ale jest jego klientem: zła trajektoria ξ\xi czyni dobry sterownik bezsilnym. Granica: ten kurs kończy się na wyjściu ξ(t)\xi(t); sterowanie pojawia się tylko jako kontekst w modułach 10, 11, 12.

Pełny pipeline w działaniu — pick & place

Poniżej kanoniczne zadanie: Panda pobiera obiekt z prawej strony stanowiska i odkłada na lewą. Kliknij bloki pipeline'u, żeby zobaczyć które algorytmy z kursu realizują daną fazę. Naciśnij ▶ — przed ruchem fazy planowania „myślą" sekwencyjnie, w trakcie ruchu aktywne jest sterowanie.

t = -0.60s / 6.00s
start
approach pick
grasp
lift
transport
approach place
release
return
PercepcjaReprezentacja scenyPlanowanieGeneracja trajektoriiSterowanie

Planowanie

input → output
Wejście
(q_start, q_goal, scena)
Wyjście
ścieżka geometryczna ξ̃ = (q₀,…,q_N)
Reprezentatywne metody
  • A*, D*, Theta* (grafowe)
  • RRT*, BIT*, PRM (próbkowe)
  • CHOMP, TrajOpt (optymalizacja)
  • MPPI, DDP (online)
  • Diffusion Policy, MπNets (uczone)
Moduły kursu
05 · Grafowe06 · Próbkowe07 · Optymalizacja10 · MPC14 · Uczone
Uwaga: pokazana animacja używa liniowej interpolacji w C-space z cosinusowym ease-in/out — to celowe uproszczenie wprowadzenia. Pełna parametryzacja czasowa z respektowaniem limitów vel/acc/jerk Pandy oraz konstrukcja splajnów klasy C2C^2 i C3C^3 — moduł 08.

Klasyfikacja problemów planowania

Cztery niezależne osie. Kombinacja zwykle wskazuje na konkretną rodzinę metod. Wybierz wartości — lista pasujących algorytmów zaktualizuje się natychmiast.

Filtr klasyfikacji

Kiedy planuje(i)
Zasięg planowania(i)
Świat(i)
Kinematyka(i)
Najedź kursorem na nazwę osi lub jedną z opcji, by zobaczyć objaśnienie. Wszystkie cztery osie są niezależne — można je łączyć (np. „online + lokalne + dynamiczne" wskazuje rodzinę MPC/MPPI).
Pasujące algorytmy (15/15)
  • M05A*— ekspansja grafu z heurystyką f = g + h
  • M05D* Lite— inkrementalny A* dla zmian w mapie
  • M06RRT— drzewo rozrastające się przez sampling
  • M06RRT*— asymptotycznie optymalna wersja RRT (rewire)
  • M06BIT*— batch informed trees — heuristic + sampling razem
  • M06Kinodynamic RRT— RRT z modelem dynamiki — szanuje constraints na q̇
  • M07CHOMP— gradient descent na trajektorii z kosztem SDF
  • M07TrajOpt (SCO)— SQP z linearyzacją constraintów kolizji
  • M09Pola potencjałowe— ruch wzdłuż -∇U; problem minimów lokalnych
  • M09Geometric Fabrics— energy-conserving uogólnienie RMPflow
  • M10MPC liniowy— QP nad horyzontem N, predykcja z modelu
  • M10MPPI— K rolloutów ważonych eksponencjalnie
  • M10iLQR / DDP— Riccati backward sweep z linearyzacją wokół trajektorii
  • M14Diffusion Policy— denoising losowej trajektorii uczonym modelem
  • M14MπNets— sieć neuronowa generuje sample dla planera

Klasyfikacja jest orientacyjna. Wiele algorytmów mieści się w więcej niż jednej kategorii (np. MPPI w trybie single-shot jest „offline", ale projektowany do pętli online). Dla studenta sednem jest: gdy w papierze przeczytasz „local online dynamic", spodziewaj się sterownika typu MPC lub fabric'a, nie globalnego planera grafowego.

Krótka historia — cztery papery, które zdefiniowały dziedzinę

Rozszerzona timeline (od Shakey 1966 do diffusion policy 2023) — moduł 20.

Tomás Lozano-Pérez
MIT, 1983

Sformułował planowanie ścieżki jako geometryczne zadanie w przestrzeni konfiguracyjnej (C-space) — pierwszy systematyczny opis.

Spatial Planning: A Configuration Space Approach (IEEE Trans. Comput., 1983)
Oussama Khatib
Stanford, 1986

Wprowadził pola potencjałowe i operational space control — reaktywne planowanie ruchu manipulatora w czasie rzeczywistym.

Real-Time Obstacle Avoidance for Manipulators and Mobile Robots (Int. J. Robotics Research, 1986)
Lydia Kavraki
Stanford, 1996

Probabilistic Roadmap (PRM) — pierwszy próbkowy planer skalowalny na wysokie wymiary; otworzyła epokę sampling-based motion planning.

Probabilistic Roadmaps for Path Planning in High-Dim. Config. Spaces (IEEE Trans. Robotics, 1996)
Steven LaValle & James Kuffner
Iowa State / CMU, 1998

Rapidly-exploring Random Tree (RRT) — drzewo zbiegające do całej przestrzeni, fundament wszystkich późniejszych wariantów (RRT*, Informed RRT*, BIT*).

Rapidly-Exploring Random Trees: A New Tool for Path Planning (TR 98-11, 1998)

Mapa kursu — które moduły dotyczą której fazy pipeline'u

Ściąga

Trzy definicje (zapamiętaj rozróżnienie)

  • Path: σ:[0,1]→Qfree\sigma : [0,1] \to \mathcal{Q}_{\text{free}} — krzywa w C-space, bez czasu.
  • Trajectory: ξ:[0,T]→Q\xi : [0,T] \to \mathcal{Q} — funkcja czasu klasy C2C^2 lub C3C^3, znamy q(t),q˙(t),q¨(t)q(t), \dot q(t), \ddot q(t).
  • Control: τ(t)∈R7\tau(t) \in \mathbb{R}^7 — moment napędowy realizujący trajektorię.

Cztery osie klasyfikacji

  • Kiedy planuje: offline / online
  • Zasięg: globalne / lokalne
  • Świat: statyczny / dynamiczny
  • Kinematyka: holonomiczna / nieholonomiczna

Pipeline (kierunek przepływu)

percepcja → reprezentacja sceny → planowanie → generacja trajektorii → sterowanie

Referencje do dalszej lektury

  • LaValle, Planning Algorithms(Cambridge University Press, 2006) — kanoniczny podręcznik, dostępny za darmo online: lavalle.pl/planning.
  • Lynch & Park, Modern Robotics: Mechanics, Planning, and Control (Cambridge, 2017) — silne wprowadzenie matematyczne (Lie groups), wykłady i ćwiczenia na Northwestern.
  • Siciliano et al., Robotics: Modelling, Planning and Control (Springer, 2009) — pełna dynamika i sterowanie.
  • OMPL — Open Motion Planning Library, referencyjne implementacje wszystkich klasycznych próbkowych algorytmów: ompl.kavrakilab.org.
  • MoveIt2— ROS2 stack do planowania manipulatorów (w tym Pandy): moveit.ai.
  • Franka Robotics docs — parametry DH, limity, FCI: frankaemika.github.io/docs.