Planowanie deterministyczne i grafowe

TL;DR

Planery grafowe traktują przestrzeń konfiguracyjną jako dyskretny graf: węzły to konkretne konfiguracje (lub komórki gridu), krawędzie to dopuszczalne ruchy z kosztami. Problem znalezienia ścieżki staje się klasycznym shortest path search.

• Dijkstra (1959) — eksploruje wg $g(n)$ (koszt od startu). Bez heurystyki. Optymalny, ale wolny dla dużych grafów.
• A* (Hart, Nilsson, Raphael 1968) — Dijkstra + heurystyka $h(n)$ (estymata kosztu do goal). Eksploruje wg $f(n) = g(n) + h(n)$. Optymalny gdy $h$ jest admissible ($h(n) \leq h^*(n)$).
• Weighted A* (Pohl 1970) — $f = g + w h$,$w \geq 1$. Szybsza eksploracja, gwarancja $\text{cost} \leq w \cdot \text{optimal}$.
• Greedy Best-First — tylko $h$, bez $g$. Bardzo szybki, ALE nie-optymalny i może utknąć.
• Theta* (Nash 2007) — any-angle: ścieżka NIE musi iść po krawędziach gridu. Lepiej wygląda (bez „ząbków"), krótsza.
• D* Lite, ARA*, AD* — re-planning dla scen dynamicznych lub anytime planning.

Granica: curse of dimensionality. Grid 30×30 ma 900 komórek; grid 30⁷ (7-DOF Panda) ma 2.2 × 10¹⁰ — eksploracja niewykonalna. Dlatego grid-based dobrze działa dla mobile robots (2-3D), a manipulator wymaga sampling-based (moduł 06).

A* — anatomia algorytmu

Pseudokod:

open ← priority queue: [start]
g[start] = 0
while open not empty:
  n ← pop min by f(n) = g(n) + h(n)
  if n == goal: return reconstruct_path(parent)
  mark n closed
  for each neighbor m of n:
    new_g = g[n] + cost(n, m)
    if new_g < g[m]:
      g[m] = new_g
      parent[m] = n
      open.push(m, priority = new_g + h(m))

Kluczowe pojęcia:

• $g(n)$ = actual cost od startu do $n$ (znane z eksploracji).
• $h(n)$ = heurystyka — estymata kosztu od $n$ do goal (nieznana dokładnie).
• $f(n) = g(n) + h(n)$ = estimated total cost ścieżki przez $n$.

Warunki na heurystykę

• Admissibility: $h(n) \leq h^*(n)$ (nigdy nie przeszacowuje rzeczywistej odległości). Gwarantuje optymalność A*.
• Consistency / monotonia: $h(n) \leq c(n, m) + h(m)$ dla każdej krawędzi $(n, m)$. Implikuje admissibility. Dodatkowo gwarantuje, że nigdy nie ponownie ekspandujemy zamkniętej komórki (więcej efektywności).
• Jeśli $h \equiv 0$: A* = Dijkstra.
• Im $h$ „dokładniejsze" (bliższe $h^*$), tym mniej komórek A* musi rozwinąć. W skrajności gdy $h = h^*$, A* idzie prosto ścieżkę optymalną bez jakiejkolwiek eksploracji.

Typowe heurystyki dla grid

• Manhattan: $|dc| + |dr|$ — admissible tylko dla 4-connected.
• Octile: $\max(|dc|, |dr|) + (\sqrt{2} - 1) \min(|dc|, |dr|)$ — admissible dla 8-connected (używamy w naszej animacji).
• Euclidean: $\sqrt{dc^2 + dr^2}$ — admissible, ale luźniejsza (mniejsze h ⇒ więcej eksploracji).

Weighted A*, Greedy, anytime planning

Weighted A* (Pohl 1970): zamień $f = g + h$ na $f = g + w \cdot h$, $w \geq 1$. Większa waga przesuwa balans w stronę heurystyki:

• $w = 1$ ⇒ klasyczny A* (optymalny).
• $w > 1$ ⇒ ścieżka znaleziona szybciej (mniej eksploracji) kosztem optymalności. Gwarancja: $\text{cost} \leq w \cdot \text{cost}^*$.
• $w \to \infty$ ⇒ Greedy Best-First (tylko $h$, ignoruje $g$).

Anytime planners (ARA*, AD*) wykorzystują tę własność iteracyjnie: zaczynają z dużym $w$ (szybkie ale suboptymalne), potem kolejne uruchomienia zmniejszają $w$ i poprawiają ścieżkę. W każdym momencie mamy „najlepsze co znaleźliśmy do tej pory" — przydatne dla ograniczonego budżetu czasowego (autopilot UAV).

ARA* (Likhachev, Gordon, Thrun 2003) — Anytime Repairing A*. Reużywa open set i wartości $g$ między iteracjami, zamiast restartować.

AD* (Likhachev et al. 2005) — Anytime Dynamic A*. Łączy anytime z replanning (D* Lite).

Re-planning dla scen dynamicznych: D* Lite

Klasyczny A* zakłada statyczną scenę. W praktyce (mobilny robot z czujnikiem laser) świat odkrywany jest stopniowo — za każdym razem trzeba przeplanować.

D* Lite(Koenig & Likhachev 2002) — inkrementalny A*: zachowuje stan eksploracji, gdy odkryta zostanie nowa przeszkoda tylko aktualizuje to, co konieczne. Czas re-planning ~ $O(\Delta \cdot k)$ gdzie $\Delta$ = liczba dotkniętych komórek, $k$ = stała.

Standard dla mobile robots z SLAMem (Curiosity, Perseverance — D* Lite jako warstwa planowania nad mapą Marsa).

LPA*(Lifelong Planning A*, Koenig & Likhachev 2004) — uogólnienie D* Lite; aktualizuje plan przy ZMIANIE kosztu dowolnej krawędzi (nie tylko dodaniu przeszkód).

Theta* — any-angle planning

Klasyczny A* na 8-connected gridzie zwraca ścieżki tylko wzdłuż 8 dyskretnych kierunków. Dla otwartych przestrzeni daje charakterystyczne „ząbki" — wizualnie nienaturalne i suboptymalne.

Theta* (Nash, Daniel, Koenig, Felner 2007) modyfikuje update kroku A*: gdy odkrywamy sąsiada $m$, sprawdzamy czy parent obecnego węzła ma line-of-sight do $m$. Jeśli tak, ustawiamy tego parent'a dziadka jako rodzica $m$ (zamiast bezpośredniego ojca). Daje ścieżki, które idą w „dowolnym kącie", nie tylko po krawędziach.

Theta* zachowuje optymalność tylko w przybliżeniu (gwarantuje $\leq 1.05 \cdot \text{optimum}$), ale ścieżki są znacznie krótsze i naturalniejsze niż A*. Włącz w animacji powyżej — porównaj.

Lazy Theta*, Field D* — dalsze warianty z optymalizacjami line-of-sight checks.

Visibility graphs i Voronoi diagrams

Dla scen z polygonalnymi przeszkodami (mapy 2D wektorowe, nie grid raster) istnieją dwie klasyczne reprezentacje:

Visibility graph

Węzły = wierzchołki wszystkich przeszkód + start + goal. Krawędzie = wszystkie pary węzłów widoczne (segment nie przecina żadnej przeszkody). Po zbudowaniu — Dijkstra/A* w O(E).

Twierdzenie: optymalna ścieżka euklidesowa w 2D z polygonalnymi przeszkodami składa się z segmentów łączących wierzchołki przeszkód (i start/goal). Visibility graph ZAWIERA tę ścieżkę. Dlatego daje dokładnie optymalne wyniki — nie tylko aproksymację jak grid.

Koszt: konstrukcja O(n² log n) (sprawdzanie widoczności dla par wierzchołków). Dla 1000 wierzchołków = 1 sekunda. Skaluje się kiepsko dla skomplikowanych map.

Voronoi diagram

Decompozycja przestrzeni na regiony „najbliższe" do każdej przeszkody. Krawędzie diagramu są w środku „korytarzy" między przeszkodami — to są najbezpieczniejsze drogi (najdalej od czegokolwiek).

Algorytm: zbuduj Voronoi (np. Fortune sweep, O(n log n)). Połącz start/goal z najbliższą krawędzią. Szukaj ścieżki w grafie krawędzi Voronoi.

Plusy: maksymalna clearance (bezpieczne dla mobile robots).
Minusy: ścieżki są dłuższe niż optymalne; trudno dla scen 3D.

State lattice planners — dla pojazdów nieholonomicznych

Standardowy grid zakłada że robot może w każdej komórce iść w 8 dowolnych kierunków. Dla samochodu (czy łodzi, drona ze stałym kątem skrętu) to nieprawda — istniejeminimalny promień skrętu, dynamika ogranicza zbiór osiągalnych konfiguracji w jednym kroku.

State lattice: węzły gridu to stany dynamiczne (pozycja, orientacja), krawędzie tomotion primitives — predefiniowane wykonalne trajektorie:

• Jechać prosto 5 m
• Skręcić w lewo o 45° z promieniem 10 m
• Skręcić w prawo o 45° z promieniem 10 m
• itp. (typowo 5-15 motion primitives)

Po zdefiniowaniu primitives — A* / Dijkstra na rozszerzonym grafie. Pickatka: tylko wybrane przejścia z każdego stanu, ale każde spełnia ograniczenia dynamiczne.

Klasyczne implementacje: SBPL (Search-Based Planning Library, Maxim Likhachev). Stosowane w autonomicznych samochodach (Carnegie Mellon, DARPA Urban Challenge), traktorach Deere.

Curse of dimensionality — gdzie graf-based pęka

Liczba komórek w jednolitym gridzie wymiaru $d$ z $N$ komórkami na oś:

Stąd planery grafowe są standardem dla 2-4 DOF (mobile robots, auta, drony), a dla manipulatorów nieprzydatne — zostaje sampling-based (moduł 06) lub trajektoryjna optymalizacja (moduł 07).

Hybrydy: SBPL + manipulation — dyskretyzuj tylko wymiar najistotniejszy (np. pose chwytaka), pozostałe joints rozwiązuj analitycznie (IK). Działa dla 4-6 DOF.

Referencje

• Dijkstra, „A Note on Two Problems in Connexion with Graphs" (Numerische Mathematik, 1959).
• Hart, Nilsson, Raphael, „A Formal Basis for the Heuristic Determination of Minimum Cost Paths" (IEEE SSC 1968) — oryginalny A*.
• Pohl, „First results on the effect of error in heuristic search" (Machine Intelligence 1970) — Weighted A*.
• Koenig & Likhachev, „D* Lite" (AAAI 2002).
• Likhachev, Gordon, Thrun, „ARA*: Anytime A* with Provable Bounds on Sub-Optimality" (NIPS 2003).
• Nash, Daniel, Koenig, Felner, „Theta*: Any-Angle Path Planning on Grids" (AAAI 2007).
• Pivtoraiko, Knepper, Kelly, „Differentially constrained mobile robot motion planning in state lattices" (J. Field Robotics 2009).
• SBPL (Search-Based Planning Library, Likhachev et al.): sbpl/sbpl.
• LaValle, Planning Algorithms (CUP 2006), rozdział 2: Discrete Planning.