Moduł 12 · MPC i bezpieczeństwo

Ograniczenia i bezpieczeństwo formalne

Joint/vel/acc/jerk limits, singularity avoidance, CBF/HOCBF/CBF-QP, reachability, ISO/TS 15066, funkcje Lapunowa (SOS, neural).

TL;DR

Planery (moduły 05-10) powinny zwracać bezpieczne trajektorie, ale w praktyce: limity joints przekraczane, kolizje przy szybkim ruchu, niedeterministyczność (sampling), blokady w pobliżu singularności. Dla pracujących z ludźmi (cobotów, mobile robots, autonomicznych aut) to niedopuszczalne. Potrzebne formalne gwarancje:

Joint/vel/acc/jerk limits — twarde constraints w planerze i sterowniku.
Singularity avoidance — damping + manipulability gradient w null-space (moduł 11).
Control Barrier Functions (CBF) — matematycznie udowodnione gwarancje set invariance: jeśli system startuje w bezpiecznym zbiorze i spełnia CBF condition, NIGDY z niego nie wyjdzie.
Reachability analysis — przewidywanie wszystkich możliwych stanów w przyszłości (forward reachable sets).
ISO/TS 15066 — standardy bezpieczeństwa dla cobotów (speed-and-separation, power-and-force limiting).
Funkcje Lapunowa — formalna stabilność systemu. SOS programming, neural Lyapunov dla learned policies.

Limity przegubów — formalizacja

Dla Pandy (moduł 02, lib/panda/limits.ts) limity są publicznie dokumentowane:

\begin{aligned}q_{\min} &\leq q \leq q_{\max} \\ |\dot q| &\leq \dot q_{\max} \\ |\ddot q| &\leq \ddot q_{\max} \\ |\dddot q| &\leq \dddot q_{\max}\end{aligned}

Konkretnie: $\dot q_{\max} = (2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61)$ rad/s, $\ddot q_{\max} = (15, 7.5, 10, 12.5, 15, 20, 20)$ rad/s².

Sposoby wymuszenia

Soft: penalty w funkcji kosztu (CHOMP, MPC z soft constraints). Łatwe ale BRAK gwarancji.
Hard: constraints równościowe / nierównościowe w QP/NLP. Solver dba o spełnienie. Wymagane dla certyfikacji.
Sterowanie (real-time): klamp w pętli. Najprostsze, ale tworzy „kant" w sterowaniu — system może destabilizować się przy limitach.
Reskalowanie czasowe (TOPP-RA, moduł 08) — zachowaj ścieżkę geometryczną, spowolnij w miejscach gdzie limit naruszony.

Klasyczny problem: gdy planer nie patrzy na limity, ale sterownik klampuje — robot „grzeje się" w limicie i nie idzie śladem trajektorii. Skutek: chattering, kolizje (bo faktyczna pozycja odbiega od zadanej), uszkodzenie napędów.

Unikanie singularności

Singularność (moduł 02): konfiguracja gdzie jakobian $J(q)$ traci rank. Dwa skutki:

Loss of capability: pewien kierunek prędkości TCP staje się nieosiągalny.
Sterowanie wybucha: $J^+$ rośnie do nieskończoności. PI/PD na pose error generuje $\dot q \to \infty$ .

Standardowe strategie:

Damped Least Squares (moduł 11): $J^+_{\text{DLS}} = J^\top (J J^\top + \lambda^2 I)^{-1}$ . Adaptive: $\lambda \propto 1/\sigma_{\min}(J)$ (Maciejewski, Klein 1985, Chiaverini 1997).
Manipulability gradient w null-space (moduł 11): wymuś $\dot q_{\text{secondary}} = k \cdot \nabla w(q)$ aby odepchnąć od singularności.
Workspace boundary: oznacz konfiguracje z $w(q) < w_{\text{thresh}}$ jako „niebezpieczne", traktuj jak obstacle w planerze.
Reachability-based prevention: zanim wykonasz ruch, sprawdź czy trajektoria całość minie singularność z marginesem.

Control Barrier Functions (CBF)

CBF (Ames et al. 2017) to formalne narzędzie do set invariance: jeśli system jest w bezpiecznym zbiorze $\mathcal{S} = \{x : h(x) \geq 0\}$ , czy zostanie tam na zawsze?

CBF condition: dla każdego stanu na brzegu $\mathcal{S}$ istnieje sterowanie utrzymujące w środku:

\sup_{u \in \mathcal{U}} \big[ \dot h(x, u) + \alpha(h(x)) \big] \geq 0

gdzie $\alpha$ = klasa $\mathcal{K}_\infty$ (typowo liniowa, $\alpha(h) = \gamma h$ , $\gamma > 0$ ).

Twierdzenie (Ames 2017): jeśli CBF condition jest spełniony i system startuje w $\mathcal{S}$ , to set is forward invariant — system zawsze pozostaje w $\mathcal{S}$ .

CBF-QP — safety filter

Daną mamy $u_{\text{nom}}(x)$ (nominal controller, np. PD do goal). CBF-QP nakłada minimalną modyfikację by spełniony był CBF condition:

u_{\text{safe}} = \arg\min_u \|u - u_{\text{nom}}\|^2 \quad \text{s.t.} \quad \dot h(x, u) + \alpha h(x) \geq 0, \; u \in \mathcal{U}

Dla single integrator $\dot x = u$ i okrągłej przeszkody $h = \|x - c\|^2 - r^2$ , constraint jest liniowy w u — QP redukuje się do projekcji na półprzestrzeń (closed-form), ~10 μs per krok.

CBF-QP — przesuń czerwoną przeszkodę myszą

h(x) = 0.1664 ✓ safe

α (CBF)3.0

k_p1.5

r (obs)0.10

trajektoria z CBF (rzeczywista)trajektoria nominal (bez CBF, hipotetyczna)startgoal

Eksperymenty: kliknij i przeciągnij obstacle w drogę robota — CBF deformuje trajektorię (fioletowa) wokół. Linia przerywana czerwona pokazuje co zrobiłoby nominal control bez CBF (idzie prosto, wjedzie w obstacle). Suwak α: mała α → wczesna reakcja (gładsza trajektoria, ale „za daleko"); duża α → reakcja w ostatniej chwili (bliżej obstacle, ostrzejszy turn). Klasyczny trade-off. Suwak r zmienia rozmiar — większy obstacle wymaga większego ominięcia.

Wybór funkcji α

Liniowa $\alpha(h) = \gamma h$ z parametrem $\gamma > 0$ :

Mała γ (~0.5): wczesna reakcja, robot odsuwa się daleko od obstacle. Bezpieczne ale konserwatywne.
Duża γ (~10): reakcja w ostatniej chwili. Trajektoria bliżej obstacle, szybka ale agresywna.
Typowo $\gamma \in [1, 5]$ .

HOCBF (Higher-Order CBF)

Dla systemów wyższego rzędu (jak double integrator $\ddot x = u$ ), klasyczna CBF condition daje constrainttylko dla pozycji, ignoruje prędkość. Skutek: system może wbiec w obstacle ( $\dot x \cdot (x - c) < 0$ ⇒ jedziemy w niego).

HOCBF (Xiao, Belta 2019) — uogólnienie:

\psi_0(x) = h(x), \quad \psi_1(x) = \dot \psi_0 + \alpha_1(\psi_0), \quad \ldots

Constraint: $\dot \psi_r + \alpha_{r+1}(\psi_r) \geq 0$ , gdzie $r$ = relative degree (najmniejsze $r$ takie, że $u$ pojawia się w $\psi_r$ ).

Dla double integrator + sphere obstacle, relative degree = 2: constraint pojawia się w $\ddot h$ . Explicit:

\ddot h + (\alpha_1 + \alpha_2) \dot h + \alpha_1 \alpha_2 h \geq 0

Po podstawieniu $h = \|p - c\|^2 - r^2$ i propagacji:

2 (p - c)^\top u \geq -2 \|v\|^2 - (\alpha_1 + \alpha_2) \cdot 2 (p - c)^\top v - \alpha_1 \alpha_2 (\|p - c\|^2 - r^2)

To liniowy constraint na $u$ , więc QP redukuje się do projekcji na półprzestrzeń (jak w classic CBF). Demo poniżej porównuje 3 tryby: brak filtra, classic CBF (uznaje $\dot x = u$ , ignoruje inercję), HOCBF.

HOCBF dla double integrator (relative degree 2)

✓ safe · |v| = 0.000

filter

\alpha_1

3.0

\alpha_2

(HOCBF)3.0

k_p (nom)3.0

Co testować: zwiększ k_p do 5+ żeby PD nominal generował wysokie przyspieszenia. Porównaj 3 tryby filtra: brak — punkt wjeżdża w obstacle (kolizja). classic CBF (1st) — działa dla wolnego ruchu, ale gdy k_p duże i punkt ma dużą prędkość, wjedzie w obstacle mimo CBF — bo classic CBF widzi tylko gradient h, ignoruje v. HOCBF — bezpieczny niezależnie od prędkości, bo constraint na

\ddot h

uwzględnia

\dot h

. Niebieska strzałka pokazuje wektor prędkości.

Kluczowy insight

Classic CBF dla single integrator $\dot x = u$ ma relative degree 1 — sterowanie pojawia się od razu w $\dot h$ . Constraint $\dot h + \alpha h \geq 0$ jest poprawny.

Dla double integrator $\ddot x = u$ mamy relative degree 2: $\dot h = 2(p-c)^\top v$ nie zależy od $u$ , więc classic CBF nie ogranicza $\ddot h$ kontrolowanego sterowaniem. W efekcie gdy punkt ma dużą prędkość ku obstacle, classic CBF spóźnia się — wjeżdża w obstacle mimo "spełnienia" constraint na $\dot h + \alpha h$ w chwili 0. HOCBF traktuje $\dot h$ jako dodatkową zmienną stanu i zapewnia, że ALEŻ obie wartości (h i ḣ) pozostają w bezpiecznym podzbiorze.

Exponential CBF

Wariant z eksponencjalnym tłumieniem; gwarancja $h(t) \geq h(0) e^{-\gamma t}$ (nie tylko ≥ 0). Robustniejsze dla model uncertainty.

Reachability Analysis

Forward reachable set: zbiór wszystkich możliwych stanów $x(t)$ jakie system $\dot x = f(x, u)$ może osiągnąć w czasie $t$ , przy $u \in \mathcal{U}$ i niepewnościach. Jeśli ten zbiór nie przecina się z obstacle set, system jest bezpieczny do $t$ .

Reprezentacje reachable set:

Zonotopes (sum of intervals) — szybkie, over-approximation.
Ellipsoids — closed-form propagation dla linear systems.
Polytopes — dokładne dla LTI systems, ale skomplikowane obliczeniowo.
Hamilton-Jacobi-Bellman (HJB) reachability — exact dla nieliniowych. Grid w state space, level set propagation. Praktycznie tylko dla 2-4 DOF.

Praktyka dla Pandy: po każdej iteracji planera, sprawdź czy następne $K = 2-3$ kroków w predykowanej trajektorii nie wjedzie w obstacle. Jeśli tak, stop i re-plan. Simple. Toolboxy: CORA (Continuous Reachability Analyzer, TU Munich), HyperFlow, BoundEd.

ISO/TS 15066 — bezpieczeństwo cobotów

Specyfikacja ISO/TS 15066 (2016) definiuje 4 tryby bezpiecznej interakcji robot-człowiek (collaborative operations):

Tryb 1: Safety-rated monitored stop

Robot pracuje pełną prędkością. Gdy człowiek wchodzi w safety zone (wykrywany kurtyną LIDAR/kamerami), robot się zatrzymuje (kategoria stop 2 — utrzymuje moment napędowy). Wznawia pracę gdy człowiek wychodzi. Klasyczny tryb dla pick&place obok człowieka.

Tryb 2: Hand-guiding

Człowiek prowadzi robota ręką (teach by demonstration). Robot stosuje minimalny moment, podążając za ręką. Aktualnie używany do programowania trajektorii.

Tryb 3: Speed and Separation Monitoring (SSM)

Robot i człowiek pracują w tym samym obszarze. Dynamicznie: im bliżej człowiek, tym wolniej robot. Formuła:

v_{\text{max}}(d) = \max(0, k_v \cdot (d - d_{\min}))

gdzie $d$ = odległość robot-człowiek, $d_{\min}$ = margines bezpieczeństwa, $k_v$ = wsp. proporcjonalności. Implementowane jako CBF constraint w sterowniku.

Tryb 4: Power and Force Limiting (PFL)

Robot fizycznie kontaktuje człowieka (zamierzony kontakt) alesiła kontaktu jest ograniczona poniżej progu bóludla danej części ciała. Tabela 1 ISO/TS 15066: np. dla czoła < 130 N, dla dłoni < 280 N. Wymaga kalibracji impulsu per część ciała.

Panda jest certyfikowanym cobotem spełniającym ISO/TS 15066 (mode 3 + 4). Wbudowane czujniki momentu w każdym joincie pozwalają na bezpośrednie monitorowanie kontaktu.

Funkcje Lapunowa — formalna stabilność

CBF gwarantuje set invariance (NIE wyjdziemy z bezpiecznego zbioru). Funkcja Lapunowa gwarantuje zbieżność do celu:

V(x) > 0 \text{ dla } x \neq x^*, \quad V(x^*) = 0, \quad \dot V(x) < 0 \text{ dla } x \neq x^*

Klasyczna stabilność Lyapunova: jeśli istnieje taka $V$ , system zbiega do $x^*$ . Ostrzejsza wersja: exponential stability $\dot V \leq -\gamma V$ .

CLF — Control Lyapunov Function

Dla sterowanych systemów: istnieje funkcja $V$ i sterowanie $u^*(x)$ takie, że $\dot V \leq -\gamma V$ dla wszystkich $x \neq x^*$ . CLF-QP (analogicznie do CBF-QP):

u^* = \arg\min_u \|u - u_{\text{nom}}\|^2 \quad \text{s.t.} \quad \dot V + \gamma V \leq \delta

gdzie $\delta$ = relaxation (jeśli twardo nie zawsze wykonalne).

SOS programming (Sum of Squares)

Jak znaleźć Lyapunov function dla nieliniowego systemu?SOS (Parrilo 2000) — parametryzuj $V(x)$ jako wielomian, znajdź współczynniki przez semidefinite programming taki, że $V \succeq 0$ i $-\dot V \succeq 0$ jako sum of squares. Solvery: SOSTOOLS (Caltech/Stanford), SumOfSquares.jl.

Neural Lyapunov

Reprezentuj $V$ jako sieć neuronową, ucz przez adversarial counter-example training. Skaluje się do wysokowymiarowych systemów (DeepNL, Boffi et al. 2020). Wymaga verification na końcu (Marabou, dReal) by zagwarantować ścisłe spełnienie warunków.

Ściąga

CBF condition

\dot h(x, u) + \alpha(h(x)) \geq 0

Safe set $\mathcal{S} = \{x : h(x) \geq 0\}$ , $\alpha$ = klasa $\mathcal{K}_\infty$ , typowo $\alpha(h) = \gamma h$ .

CBF-QP safety filter

u_{\text{safe}} = \arg\min \|u - u_{\text{nom}}\|^2 \;\text{s.t.}\; \dot h + \alpha h \geq 0

HOCBF dla relative degree r

$\psi_0 = h, \; \psi_{k+1} = \dot\psi_k + \alpha_{k+1}(\psi_k)$ . Constraint: $\dot\psi_r + \alpha_{r+1}(\psi_r) \geq 0$ .

Tryby ISO/TS 15066

Safety-rated monitored stop
Hand-guiding
Speed-and-Separation Monitoring (SSM)
Power-and-Force Limiting (PFL)

Lyapunov stability

V > 0 dla x ≠ x*, V(x*) = 0
V̇ < 0 dla x ≠ x*
SOS programming dla wielomianowych V
Neural Lyapunov dla wysokowymiarowych

Implementacja Pandy

Joint limits: hard constraints w QP/NLP
Singularity: DLS + manipulability gradient w null-space
Self-collision: ACM matrix (moduł 04)
External collision: CBF z LiDAR/camera updates
HRI: ISO/TS 15066 SSM/PFL z momentem napędowym

Referencje

Ames, Coogan, Egerstedt, Notomista, Sreenath, Tabuada, „Control Barrier Functions: Theory and Applications" (ECC 2019) — przegląd CBF.
Ames, Xu, Grizzle, Tabuada, „Control Barrier Function Based Quadratic Programs for Safety Critical Systems" (IEEE TAC 2017) — CBF-QP.
Xiao, Belta, „Control Barrier Functions for Systems with High Relative Degree" (CDC 2019) — HOCBF.
Nguyen, Sreenath, „Exponential Control Barrier Functions for Enforcing High Relative-Degree Safety-Critical Constraints" (ACC 2016).
Maciejewski & Klein, „Numerical Filtering for the Operation of Robotic Manipulators through Kinematically Singular Configurations" (J. Robotics Systems 1988) — DLS.
ISO/TS 15066:2016, „Robots and robotic devices — Collaborative robots" — specyfikacja techniczna.
Parrilo, „Structured Semidefinite Programs and Semialgebraic Geometry Methods in Robustness and Optimization" (PhD thesis, Caltech 2000) — SOS.
Althoff, „An Introduction to CORA 2015" (ARCH 2015) — Continuous Reachability Analyzer.
Boffi, Tu, Matni, Slotine, Sindhwani, „Learning Stability Certificates from Data" (CoRL 2020) — Neural Lyapunov.
Franka Robotics docs — implementacja ISO/TS 15066 w Pandzie. frankaemika.github.io/docs.