Planowanie próbkowe

Algorytm:

T ← {q_start}
for i = 1..N:
  q_rand ← sample(Q)                  # z prob. β: goal, inaczej uniform
  q_near ← nearest(T, q_rand)
  q_new  ← steer(q_near, q_rand, ε)   # krok o ε ku q_rand
  if collision_free(q_near, q_new):
    T ← T ∪ {q_new} with parent q_near
    if dist(q_new, q_goal) < r_goal:
      return path(q_start → q_new)

Voronoi bias: drzewo „chce" eksplorować — większy region T = większe prawdopodobieństwo, że $q_{\text{rand}}$ wyląduje w jego komórce Voronoi.$q_{\text{near}}$ dla takiego sampla zwykle leży na brzegu drzewa, więc nowy węzeł rozszerza region. To klucz do dlaczego RRT zbiega do całej $\mathcal{Q}_{\text{free}}$.

Goal bias $\beta \in [0, 0.5]$ — z prawdopodobieństwem $\beta$ samplem jest $q_{\text{goal}}$. Mała wartość (~5%) wystarcza by drzewo „znalazło" goal w rozsądnym czasie. Za duża $\beta$ (>30%) sprawia że drzewo „rozkłada się" w lokalnym minimum przed dotarciem do celu.

W demo niżej: testuj suwakami $\varepsilon$ (step size — kompromis: krótki = drobny ruch ale wolny progres; długi = częste kolizje) i $\beta$ (goal bias — patrz scenariusz z mapą narrow-passage aby zobaczyć efekty extremalnych wartości).

Kluczowa właściwość: probabilistic completeness

...pod warunkiem że ścieżka istnieje. To słabsze niż zwykła kompletność (gwarancja w skończonym czasie) — nie ma algorytmu sample-based który byłby kompletny w klasycznym sensie, bo wąski korytarz może wymagać dowolnie wielu sampli.

RRT NIE jest optymalny. Ścieżka zwracana przez RRT jest jakąś ścieżką w drzewie — typowo 30-50% dłuższa od optymalnej. Optymalność wymaga RRT* (poniżej).

Dwa drzewa: $T_a$ od start, $T_b$ od goal. W każdej iteracji:

1. $T_a$ rośnie ku losowemu $q_{\text{rand}}$ (EXTEND).
2. Jeśli sukces — $T_b$ wykonuje CONNECT ku nowemu węzłowi $q_{\text{new}}$ — to extend wielokrotny, dopóki nie dojdzie albo nie zderzy się z przeszkodą.
3. Jeśli skleiły się — ścieżka znaleziona.
4. Swap $T_a \leftrightarrow T_b$ — drzewa na zmianę inicjują iterację.

Greedy CONNECT eliminuje większość „bezowocnych" kroków RRT. Empirycznie 5-15× szybciej niż jednostronny RRT na typowych zadaniach manipulacji. To domyślny planer w MoveIt2 i OMPL dla manipulatorów.

Dwa dodatki do RRT:

1. Choose parent: zamiast naivnie dodawać $q_{\text{new}}$ jako dziecko $q_{\text{near}}$, przeszukaj wszystkich sąsiadów w $r(n)$ i wybierz tego, który da najmniejszy koszt ścieżki $q_{\text{start}} \to q_{\text{new}}$.
2. Rewire: dla każdego węzła $q'$ w $r(n)$, sprawdź czy przejście przez $q_{\text{new}}$ daje $q'$ krótszą ścieżkę. Jeśli tak — przepinamy rodzica $q'$ na $q_{\text{new}}$.

Promień rewire dla asymptotycznej optymalności:

gdzie $d$ = wymiar C-space. Dla 2D maleje $\propto \sqrt{\log n / n}$, więc dla $n = 10^4$ mamy $r \approx 0.03$. W naszej implementacji uproszczono do stałej (suwak rewire r).

Konsekwencja: z każdym rewire ścieżka się poprawia. W demo poniżej obserwuj pomarańczowe krawędzie (świeżo rewired) — ścieżka „prostuje się" w czasie. Dla bardzo długiego biegu ($n = 10^5$) ścieżka zbiega do optymalnej.

Praktyczne ulepszenia RRT*

• Informed RRT* (Gammell et al. 2014): po znalezieniu pierwszej ścieżki o koszcie $c_{\text{best}}$, sample tylko z elipsoidy o ogniskach (start, goal) i długości $c_{\text{best}}$. Wszelkie punkty poza tym obszarem na pewno nie polepszą ścieżki.
• RRT-X(Otte & Frazzoli 2014): online version — odpowiada na pojawienie się/zniknięcie przeszkód bez restartu.
• Quick-RRT* — ancestor rewire: rewire przez kilka pokoleń w górę, nie tylko bezpośrednich sąsiadów. Szybsza zbieżność.

Cały odrębny paradygmat: zamiast budować drzewo per zapytanie, raz konstruuj roadmap grafu w $\mathcal{Q}_{\text{free}}$ (offline), potem odpowiadaj na wiele zapytań start/goal przez Dijkstrę (online).

Faza learning:

1. Wylosuj $N$ punktów w $\mathcal{Q}$, odrzuć te w kolizji.
2. Dla każdego: znajdź $k$ najbliższych (lub w promieniu $r$), połącz krawędzią jeśli lokalny planer (linia prosta) jest wolny od kolizji.

Faza query: dodaj start/goal do roadmapu, połącz z najbliższymi, uruchom Dijkstrę.

Kiedy ma sens:roboty stacjonarne z fixed sceną (pick&place na linii produkcyjnej), wielokrotne zapytania. Nie ma sensu dla scen dynamicznych (każda zmiana sceny = przeliczenie roadmapu).

Warianty:

• PRM*: promień $r(n) \propto (\log n / n)^{1/d}$ — asymptotyczna optymalność.
• Lazy PRM: nie sprawdzaj kolizji krawędzi przy budowie roadmapu, tylko przy query. Tańsze gdy lista zapytań jest mała.
• Visibility PRM: agresywne filtrowanie sampli — dodawaj tylko guardy (widzialne tylko z osobliwych regionów) lub connectory (łączące guardów). Ekstremalnie rzadki roadmap, dobry dla wąskich korytarzy.

Praktyczne reguły

• Uniform — domyślny, sprawdza się dla większości scen.
• Halton — preferowany dla małej liczby sampli ($n < 1000$) lub gdy wymagane jest deterministyczne pokrycie. Quasi-random ma niższą dyskrepancję niż pseudo-random.
• Gaussian — gdy mamy prior na lokalizację rozwiązania (np. local refinement po global plan).
• Bridge test — gdy scenariusz ma wąskie korytarze. Hsu et al. 2003.
• Obstacle-based — gdy ścieżka prawdopodobnie będzie przebiegała blisko przeszkód (manipulacja, wąskie miejsca).