Moduł 09 · Metody reaktywne

Metody reaktywne i policy-based

Pola potencjałowe, navigation functions Rimona-Koditscheka, OSC Khatibego, dynamical systems (SEDS, LPV-DS), RMP/RMPflow, Geometric Fabrics.

TL;DR

Planery z modułów 05-07 są deliberate — najpierw planują CAŁĄ ścieżkę, potem wykonują. Świetnie dla statycznych scen, ale wolne (RRT-Connect ~50ms dla Pandy) i wymagają pełnego state of the world. Czasem chcemy kontroler reaktywny: $\dot q = \pi(q, \text{world})$ — bezpośrednio z bieżącej obserwacji wyznacz prędkość. To podstawa visual servoing, human-robot interaction, sterowania w 1 kHz.

Pola potencjałowe (Khatib 1986) — pionierska metoda, prostą, szybka, ale problem lokalnych minimów.
Navigation functions (Rimon-Koditschek 1992) — specjalnie skonstruowane potencjały bez lokalnych minimów, ale tylko dla sphere-worlds.
Operational Space Control (Khatib 1987) — sterowanie w task-space, dynamika robota włączona.
Dynamical Systems (LASA, SEDS, LPV-DS) — uczone z demonstracji, gwarancja stabilności.
RMP / RMPflow (Cheng 2018) — algebraiczna kompozycja policy z różnych task spaces; bez minimów lokalnych.
Geometric Fabrics (NVIDIA 2020) — uogólnienie RMP z gwarancjami stabilności i energy conservation.

Pola potencjałowe — pionierska metoda Khatibego

Idea (Khatib 1986): wymyśl sztuczne pole energii potencjalnej $U(q)$ takie, że minimum globalne jest w goal, a wysokie wartości — przy przeszkodach. Wtedy ruch wzdłuż gradient descent naturalnie prowadzi do goal omijając przeszkody:

\dot q = -\nabla U(q) = -\nabla U_{\text{att}}(q) - \sum_i \nabla U_{\text{rep},i}(q)

Standardowe komponenty:

U_{\text{att}}(q) = \tfrac{1}{2} k_{\text{att}} \|q - q_{\text{goal}}\|^2 \quad \text{(kwadratowy)}

U_{\text{rep},i}(q) = \begin{cases} \tfrac{1}{2} k_{\text{rep}} \left( \dfrac{1}{d_i(q)} - \dfrac{1}{\rho_0} \right)^2 & d_i < \rho_0 \\ 0 & \text{w p.p.} \end{cases}

gdzie $d_i(q)$ = odległość do brzegu i-tej przeszkody, $\rho_0$ = promień wpływu (poza nim odpychanie wynosi 0). Formuła FIRAS (Force Inducing an Artificial Repulsion from Surface).

Implementacja prosta: ~20 linii kodu. Real-time (1 kHz) bez żadnego problemu. Dlatego do dziś używana jako baseline dla obstacle avoidance w mobilnych robotach i jako warstwa bezpieczeństwa nad innymi planerami.

Pole potencjałowe — gradient descent (krok 0 / 31)

⚠ utknięto w lokalnym minimum

preset

strzałki −∇U

k_att1.00

k_rep0.040

ρ₀0.18

Co testować: włącz preset trap (U-shape) — przy domyślnych parametrach robot wpada do wnętrza pułapki i utknie w lokalnym minimum (czerwone kółko + „stuck!"). Zwiększ k_rep żeby zobaczyć jak repulsja wzmacnia się przy ścianach. Zmniejsz ρ₀ żeby ograniczyć zasięg odpychania — czasem pomaga uciec z pułapki, ale ryzyko kolizji rośnie. Heatmap: niebieski = niskie U (dolina), czerwony = wysokie U (góry/przeszkody). Trajektoria szuka „dolin" wzdłuż gradient descent.

Klasyczny problem: lokalne minima

Włącz preset trap w demo powyżej. Przeszkoda U-shape generuje obszar lokalnego minimum wewnątrz wnęki — równoważy się tam siła ku goal i siły odpychające z trzech ścian. Robot utknie i nigdy nie dotrze do celu.

Charakterystyka problemu:

Pojawia się dla nie-wypukłych przeszkód lub w wąskich korytarzach.
Nie da się tego naprawić tylko zmianą wag (k_rep, ρ₀) — to struktura geometryczna. Inne parametry tylko zmienią gdzie minimum jest, nie wyeliminują problemu.
Powyższe stanowi matematycznie udowodnione ograniczenie klasycznych pól potencjałowych: w ogólnej topologii nie istnieje pole bez lokalnych minimów.

Strategie radzenia sobie

Random perturbation — gdy stuck, dodaj losowość; szansa wyjścia. Prosta, ale niedeterministyczna.
Wavefront propagation — buduj distance-from-goal jako fala z BFS na gridzie; gradient tej funkcji NIE ma lokalnych minimów. Wymaga dyskretyzacji.
Navigation functions (poniżej) — specjalnie skonstruowane potencjały.
RMP / Fabrics — kompozycja policy, niezależne obstacle dla każdej przeszkody.

Navigation functions (Rimon-Koditschek 1992)

Rimon & Koditschek matematycznie udowodnili istnienie potencjału bez lokalnych minimów dla sphere-worlds — sferycznych przeszkód wewnątrz sferycznego obszaru. Konstrukcja:

\varphi(q) = \frac{\gamma_k(q)}{\big( \gamma_k(q)^\kappa + \beta(q) \big)^{1/\kappa}}

gdzie $\gamma_k(q) = \|q - q_{\text{goal}}\|^{2k}$ — siła attractive z power-law, $\beta(q) = \prod_i \beta_i(q)$ — iloczyn funkcji repulsive (dla każdej i-tej przeszkody $\beta_i = 0$ na brzegu, > 0 poza, < 0 wewnątrz). $\kappa > 0$ — parameter tuning.

Twierdzenie Rimona-Koditscheka: istnieje $\kappa_0$ takie, że dla $\kappa > \kappa_0$ , $\varphi$ ma jedyne globalne minimum (w goal) i brak innych krytycznych punktów (poza siodłowymi punktami o miarze 0 na brzegach przeszkód).

Ograniczenie: tylko dla diffeomorphic do sphere-worlds. Komplikacja: trzeba zbudować mapowanie diffeomorficzne ze swojej sceny do sphere-worlds (analitycznie!). W praktyce stosowane dla prostych geometrii (mapowanie + transport do prawdziwej sceny).

Operational Space Control (Khatib 1987)

OSC to kontroler dynamiczny w task-space, nie po prostu kinematyczny gradient. Idea: chcemy zadać przyspieszenie TCP $\ddot x_d \in \mathbb{R}^6$ (np. z PD na pose error), wyznaczyć moment napędowy $\tau$ realizujący to z uwzględnieniem dynamiki robota:

\tau = J^\top \big[ \Lambda(q) \ddot x_d + \mu(q, \dot q) + p(q) \big] + (I - J^\top J^{+\top}) \tau_0

gdzie:

$\Lambda(q) = (J M^{-1} J^\top)^{-1}$ — efektywna masa w task-space (operational space inertia matrix).
$\mu, p$ — projekcje Coriolisa i grawitacji na task-space.
$(I - J^\top J^{+\top}) \tau_0$ — null-space command (wykorzystuje redundancję, moduł 11) bez zaburzania task-space.

Daje to controlled inertia: gdy push'niemy TCP w kierunku x, robot zareaguje jak masa $\Lambda_{xx}$ w tym kierunku — niezależnie od konfiguracji q. Stąd OSC jest kanonicznym kontrolerem dla physical HRI (cobots).

Dynamical Systems uczone z demonstracji

Zamiast wymyślać pole, można je uczyć z demonstracji ekspertów. Trajektorię modelujemy jako stable dynamical system:

\dot x = f(x), \quad f \text{ stabilne w } x_{\text{goal}}

SEDS (Khansari-Zadeh & Billard 2011)

Stable Estimator of Dynamical Systems. $f$ uczone jako mixture of Gaussians; constraint optymalizacji wymusza global asymptotic stability (twierdzenie Lapunowa). Daje rodzinę trajektorii zbieżnych do goal niezależnie od stanu początkowego.

LPV-DS (Figueroa & Billard 2018)

Linear Parameter-Varying Dynamical Systems. Mieszanka liniowych DS warunkowana położeniem — bardziej elastyczne niż SEDS, zachowuje gwarancje stabilności.

Contraction-based DS

Generalizacja: zamiast Lyapunov (stabilność w jednym punkcie), contraction (stabilność orbity, dwie trajektorie zbliżają się eksponencjalnie). Pozwala na zadania cykliczne (kontur, malowanie).

LASA library (EPFL Aude Billard Lab) — dataset demonstracji ludzi rysujących krzywe + implementacje SEDS, LPV-DS. lasa.epfl.ch

RMP / RMPflow — algebraiczna kompozycja

Riemannian Motion Policies (Ratliff et al. 2018): uogólnienie pól potencjałowych z dodaniem metryki opisującej „ważność" w danym kierunku. Policy w danym task space:

\text{RMP} = (f(x, \dot x), \, M(x, \dot x))

gdzie $f$ to desired accel, $M$ to matrix metryki (mass-weighted importance). Wiele RMP z różnych task spaces można złożyć przez weighted-LSQ:

\ddot q^* = \big( \sum_i J_i^\top M_i J_i \big)^{-1} \sum_i J_i^\top M_i f_i

To uogólnia null-space projection z modułu 11 — task1 (wysoka $M_1$ ) dominuje gdy aktywne, task2 (niska $M_2$ ) działa tylko gdy task1 ma „wolną przestrzeń".

RMPflow: pełen framework do projektowania kompozycji policy:

Pull: każda przeszkoda i goal w odpowiednim task space (np. distance-to-obstacle, distance-to-goal).
Combine: weighted-LSQ na każdym poziomie hierarchii.
Push: projekcja do joint space.

Brak lokalnych minimów dla typowych kompozycji (gdy każde policy jest dobrze zaprojektowane). To kluczowa zaleta nad klasycznymi polami — dlaczego? Kluczowy mechanizm: anizotropowa metryka $M_i$ dla każdej policy. Repulsive od przeszkody jest „silna" TYLKO w kierunku do tej przeszkody; attractive do goal jest „silna" prostopadle. W weighted-LSQ kompozycji wygrywa policy z największym $M_i$ w danym kierunku — co eliminuje równoważenie sił między dwiema ścianami U-shape (klasyczna pułapka klasycznych pól).

RMP composition — pull policies → combined acceleration

501 kroków

preset

k_p (attr)8.0

α (rep)25

ρ₀0.25

attractive policyrepulsive (jedna per przeszkoda)trajektoria (composition)

Co testować: włącz preset trap — to ta sama U-shape pułapka co w polu potencjałowym. RMP wychodzi z pułapki, bo każda ściana ma OSOBNE policy z anizotropową metryką: gdy zbliżasz się do ściany bocznej, jej metryka dominuje (push w bok), a attractive (do goal) relaksuje się. Composition weighted-LSQ daje wynik nie- równoważny sumie gradientów. W klasycznym polu potencjałowym wszystkie repulsive dodawają się do jednego

\nabla U

, dając lokalne minimum. Strzałki: zielona = attractive force, czerwone = każda przeszkoda osobno, fioletowa = trajektoria wynikająca z composition.

Porównanie z polem potencjałowym (sekcja powyżej)

Klasyczne pole potencjałowe: jeden scalar potencjał U, gradient $-\nabla U$ = suma gradientów składowych. W U-shape suma jest 0 wewnątrz pułapki → robot stoi.

RMP: każda policy ma matrix metryki. Inverse Hessian kompozycji (weighted-LSQ) $(\sum_i M_i)^{-1}$ jest anizotropowy — przy ścianach z dwóch stron, kierunki kompresji rozkładają się, ale kierunek prostopadły (wyjście z pułapki) zostaje swobodny. Dlatego RMP wychodzi tam, gdzie pole potencjałowe utyka.

Geometric Fabrics (NVIDIA 2020)

Geometric Fabrics (Van Wyk, Xie, Li 2020): uogólnienie RMP z dodatkowymi gwarancjami:

Geometric consistency: zachowanie kierunku ruchu nawet przy zmianie szybkości — wynik jest niezależny od reparametryzacji czasu.
Energy conservation: total kinetic energy malała tylko gdy explicit dissipation. Daje natural „push" dynamics — robot „chce" się ruszyć po prowadzącej geodezji.
Provable stability via Lyapunov analysis na poziomie całego stacku (nie tylko pojedynczego policy).

Fabrics wdrożone w cuRobo (NVIDIA's GPU motion planner) jako warstwa reaktywna nad RRT-Connect. Demo „chwytak goni cel" w wysokim tempie (50 Hz update) bez kolizji.

Optimization Fabrics (Spahn, Wisse, Alonso-Mora 2021) — wariant z natywną integracją constraint-based obstacle avoidance.

Reactive obstacle avoidance — circular fields i harmonic potentials

Circular fields (Singh, Stephanou, Wen 1996)

Zamiast czystej repulsji $\propto -\nabla d$ , dodaj komponent styczny do brzegu przeszkody — siłę „okrążenia". Robot omija przeszkody przez „prowadzenie wokoło" zamiast prostego odpychania.

Daje gładsze trajektorie i częściowo łagodzi problem lokalnych minimów (gdy „rolniemy" wokół przeszkody, kierunek styczny prowadzi na drugą stronę).

Harmonic potentials (Connolly, Burns, Weiss 1990)

Wymagaj $\nabla^2 U = 0$ (Laplace equation). Wynik: potencjał z twierdzenia maksimum— ekstrema mogą być tylko na brzegu (przeszkody i goal), w wnętrzu domeny brak lokalnych minimów. Wymaga rozwiązania PDE (np. relaxation, $\mathcal{O}(n^2)$ dla gridu) — wolniejsze niż klasyczne potencjały, ale gwarancyjnie bez lokalnych minimów.

Ściąga

Klasyczne pola potencjałowe

\dot q = -\nabla U_{\text{att}}(q) - \sum_i \nabla U_{\text{rep},i}(q)

Attractive: $\tfrac{1}{2} k_{\text{att}} \|q - q_{\text{goal}}\|^2$
FIRAS repulsive: aktywuje się gdy $d < \rho_0$
Problem: lokalne minima przy nie-wypukłych przeszkodach

Rozwiązania problemu minimów

Navigation functions (Rimon-Koditschek) — udowodnione bez minimów (sphere-worlds)
Wavefront / harmonic — globalne info propagowane
RMP / Fabrics — kompozycja policy
Random perturbation — prosta heurystyka

Operational Space Control

Khatibego dynamiczne sterowanie w task space. Daje stałe efektywne inertia $\Lambda(q)$ niezależnie od konfiguracji.

Hierarchia metod policy-based

Pola potencjałowe (1986) — prosty baseline
Nav functions (1992) — bez minimów, ale wymaga geom. konstrukcji
Dynamical Systems / SEDS (2011) — uczone, stable
RMP / RMPflow (2018) — algebraiczna kompozycja
Geometric Fabrics (2020) — energy-conserving

Referencje

Khatib, „Real-Time Obstacle Avoidance for Manipulators and Mobile Robots" (IJRR 1986) — oryginalny paper o polach potencjałowych.
Khatib, „A unified approach for motion and force control of robot manipulators: The operational space formulation" (IEEE J. Robotics & Automation 1987) — OSC.
Rimon & Koditschek, „Exact Robot Navigation Using Artificial Potential Functions" (IEEE TRO 1992) — navigation functions.
Connolly, Burns & Weiss, „Path planning using Laplace's equation" (ICRA 1990) — harmonic potentials.
Singh, Stephanou & Wen, „Real-time robot motion control with circulatory fields" (ICRA 1996).
Khansari-Zadeh & Billard, „Learning Stable Nonlinear Dynamical Systems With Gaussian Mixture Models" (IEEE TRO 2011) — SEDS.
Figueroa & Billard, „A Physically-Consistent Bayesian Non-Parametric Mixture Model for Dynamical System Learning" (CoRL 2018) — LPV-DS.
Ratliff, Issac, Kappler, Birchfield, Fox, „Riemannian Motion Policies" (arXiv 2018) — RMP.
Cheng, Mukadam, Issac, Birchfield, Fox, Ratliff, Boots, „RMPflow: A Computational Graph for Automatic Motion Policy Generation" (IJRR 2021) — RMPflow.
Van Wyk, Xie, Li, Tremblay, Birchfield, Fox, Ratliff, „Geometric Fabrics for the Acceleration-based Design of Robotic Motion" (arXiv 2020).