Moduł 15 · Rozszerzenia

Planowanie pod niepewnością

POMDP (SARSOP, DESPOT, ABT), LQG-MP (van den Berg), chance-constrained planning, stochastic MPC, scenario/tube MPC, active sensing.

TL;DR

Wszystkie moduły 1-14 zakładały deterministyczne środowisko: pozycja robota znana, model dynamiki dokładny, kontrole wykonywane bezbłędnie. W rzeczywistości:

Niepewność stanu: noise w sensorach (encodery, kamera, IMU), brak obserwacji niektórych zmiennych.
Niepewność modelu: tarcia nie są dokładnie skalibrowane, opóźnienia komunikacji nieznane.
Niepewność środowiska: ruchome obstacles (ludzie), nieznane geometrie.

Planowanie pod niepewnością traktuje to formalnie: zamiast jednego stanu mamy belief (rozkład p(x)) i optymalizujemy expected cost lub chance constraints.

POMDP (Partially Observable MDP) — formalizm ogólny. Solvery: SARSOP, DESPOT, ABT.
LQG-MP (van den Berg 2011) — propagacja covariance wzdłuż trajektorii.
Chance-constrained planning — zamiast $P(\text{kolizja}) = 0$ akceptujemy $P(\text{kolizja}) \leq \alpha$ .
Stochastic MPC: scenario, tube, robust.
Active sensing — planuj nie tylko by osiągnąć cel, ale też by zredukować niepewność (np. obróć głowę żeby zobaczyć).

POMDP — Partially Observable MDP

Standardowy formalizm: stan $s \in \mathcal{S}$ jest częściowo obserwowalny — zamiast widzieć $s$ , mamy $z \in \mathcal{Z}$ z rozkładu $p(z | s)$ .

Belief state: $b(s) = p(s | z_{0:t}, a_{0:t})$ — rozkład posterior. Policy operuje na belief'ach: $\pi: \mathcal{B} \to \mathcal{A}$ .

Optymalna policy maksymalizuje $V(b) = \max_a \big[ R(b, a) + \gamma \mathbb{E}[V(b')] \big]$ . Bellman w przestrzeni belief.

Solvery

SARSOP (Kurniawati 2008) — Point-Based Value Iteration. Optymalna dla średnich problemów.
DESPOT (Somani 2013) — sample-based, scaling do dużych state spaces.
ABT (Kurniawati 2016) — Adaptive Belief Tree. Online, używa pomiarów do refining belief.

Praktyka: POMDP dla manipulatora jest zwykle niewykonalna obliczeniowo (state space too large). Stosowana dla mobile robots, prostych task-level decisions.

LQG-MP (van den Berg 2011)

Linear-Quadratic Gaussian Motion Planning — propagacja niepewności gaussowskiej wzdłuż trajektorii. Założenia:

Dynamika linearyzowana: $x_{k+1} = A_k x_k + B_k u_k + w_k$ , $w_k \sim \mathcal{N}(0, Q)$ .
Obserwacja: $z_k = C_k x_k + v_k$ , $v_k \sim \mathcal{N}(0, R)$ .
Estymator: Kalman filter. Sterowanie: LQR feedback.

Wynik: w każdym kroku rozkład state'u to gaussian z covariance $\Sigma_k$ propagowanym przez Riccati. Wizualizacja: elipsoidy niepewności wzdłuż osi czasu.

Algorytm wybiera trajektorię (start, goal) minimalizującą oczekiwane prawdopodobieństwo kolizji jako sumę po krokach $P(\text{collision} | \Sigma_k)$ .

LQG-MP — propagacja covariance wzdłuż trajektorii

σ_max (3σ) = 0.095

σ_proc (

\sqrt{Q}

)0.050

σ_meas (

\sqrt{R}

)0.020

obserwowalny (Kalman update)kontur obstacle + 3σ (chance constraint)

nominal trajectoryelipsy 3σ niepewnościobstaclezacieśnione 3σ (chance constraint)

Co testować: wyłącz obserwowalny — elipsy rosną monotonnie (bez korekty Kalmana). Włącz — elipsy zostają ograniczone bo measurement update kompensuje process noise. σ_proc kontroluje szybkość wzrostu, σ_meas — jak skuteczna jest korekta. Chance constraint: pomarańczowy kontur = obstacle inflated o 3σ_max — gwarantuje

P(\text{kolizja}) < 0.27\%

jeśli trajektoria omija ten zacieśniony obszar (Boole approximation). Patrz: van den Berg, „LQG-MP" (IJRR 2011).

Klucz: dwa konkurencyjne procesy

Process noise $w_k \sim \mathcal{N}(0, Q)$ — zaburzenia dynamiki (wiatr, tarcie). Powoduje wzrost $\Sigma$ przez propagation $\Sigma_{k+1}^- = A \Sigma_k A^\top + Q$ .
Measurement update (Kalman filter) — gdy obserwujemy $z = C x + v$ , posterior $\Sigma$ jest mniejsza. Im lepsze sensory (mniejsze $R$ ), tym silniej korygujemy.

Jeśli system jest nieobserwowalny (sensor nie widzi wszystkich wymiarów stanu), pewne komponenty $\Sigma$ rosną monotonnie — niepewność nie da się ograniczyć. Demo: wyłącz „obserwowalny" — elipsy rosną liniowo.

Chance-constrained planning

Hard collision constraint $\sigma(t) \in \mathcal{Q}_{\text{free}}$ dla niepewnego systemu jest zwykle infeasible — zawsze istnieje (nieskończenie mała) szansa kolizji. Zamiast tego:

P(\sigma(t) \in \mathcal{Q}_{\text{obs}}) \leq \alpha

gdzie $\alpha$ ~ 1-5%. To chance constraint. Praktycznie zacieśniamy margines bezpieczeństwa proporcjonalnie do $\sigma_k$ : jeśli niepewność wzdłuż osi $x$ to $3\sigma$ , planer omija obstacle o dodatkowe $3\sigma$ .

Boole approximation: $P(\bigcup_k A_k) \leq \sum_k P(A_k)$ — niezależna kontrola na każdej krawędzi przeszkody. Konserwatywne, ale tractable.

Stochastic MPC

Scenario MPC

Sample K scenariuszy zaburzeń $\{w_k^{(i)}\}_{i=1}^N$ . Optymalizuj średni koszt po wszystkich. Daje empirical guarantee: $\hat P(\text{naruszenie}) \to P_{\text{true}}$ z $N \to \infty$ .

Tube MPC

Zamiast planować nominal trajectory, planuj tube — region którego rzeczywista trajektoria nie opuści (under bounded disturbance). Nominal trajectory + ancillary feedback controller utrzymuje system w tubie.

Klasyczne wyniki (Mayne 2005): jeśli system jest asymptotycznie stabilizowalny z linear feedback dla $w \in \mathcal{W}$ bounded, istnieje tube $\mathcal{T}$ taki, że nominal + feedback zawsze w $\mathcal{T}$ . Planowanie wokół rozszerzonych obstacles.

Robust MPC (min-max)

$\min_u \max_w J(x, u, w)$ — worst-case. Konserwatywne ale gwarantuje feasibility. Algorytmy: H∞ MPC, robust positively invariant sets.

Active sensing — planuj by widzieć

Klasyczny planer: cel = pozycja goal. Active sensing dodaje: cel = minimalizuj niepewność.

Information gain: $I(b, a) = H(b) - \mathbb{E}_z[H(b')]$ (zmniejszenie entropii belief). Planuj sekwencję $a^* = \arg\max [\alpha \cdot R(b, a) + \beta \cdot I(b, a)]$ .

Zastosowania:

Mobile robot SLAM: dokąd jechać żeby minimalizować covariance mapy?
Manipulation pre-grasp: w jaką stronę obrócić kamerę żeby zobaczyć cześć obiektu?
Object search: gdzie szukać klucza w pomieszczeniu?

Bayesian optimization + Gaussian Processes — standard framework dla active sampling.

Ściąga

Formalizmy

MDP — pełna obserwowalność stanu
POMDP — częściowa obserwowalność, belief state
LQG — Gaussian assumption + linear dynamics
Chance-constrained — P(violation) ≤ α

Solvery POMDP

SARSOP — point-based VI, średnie problemy
DESPOT — sample-based, large state
ABT — online, adaptive

LQG-MP

Linearyzuj + Kalman + LQR. Propaguj Σ przez Riccati. Wybierz trajectory min E[P(collision)].

Stochastic MPC

Scenario — sample K realizacji
Tube — invariant set + feedback
Robust min-max — worst case

Active sensing

Maksymalizuj R + β · I (cost + information gain). Standard: Bayesian optimization, GP-based.

Referencje

Kurniawati, Hsu, Lee, „SARSOP: Efficient Point-Based POMDP Planning by Approximating Optimally Reachable Belief Spaces" (RSS 2008).
Somani, Ye, Hsu, Lee, „DESPOT: Online POMDP Planning with Regularization" (NIPS 2013).
van den Berg, Wilkie, Guy, Niethammer, Manocha, „LQG-MP: Optimized Path Planning for Robots with Motion Uncertainty and Imperfect State Information" (IJRR 2011).
Blackmore, Ono, Williams, „Chance-Constrained Optimal Path Planning With Obstacles" (IEEE TRO 2011).
Mayne, Seron, Raković, „Robust model predictive control of constrained linear systems with bounded disturbances" (Automatica 2005) — Tube MPC.
Calafiore & Campi, „The Scenario Approach to Robust Control Design" (IEEE TAC 2006).
Schwager, Slotine, Rus, „Decentralized, Adaptive Coverage Control for Networked Robots" (IJRR 2009) — active sensing.
Cassandra, Kaelbling, Kurien, „Acting under uncertainty: Discrete Bayesian models for mobile-robot navigation" (IROS 1996) — klasyczne POMDP dla mobile robots.