Moduł 10 · MPC i bezpieczeństwo

Sterowanie predykcyjne i online planning

Linear MPC, NMPC, MPPI, Tube/Log/Smooth MPPI, STORM, DDP, iLQR, iLQG, Crocoddyl, CEM, real-time iteration, warm-starting.

TL;DR

Model Predictive Control (MPC) łączy planowanie i sterowanie w jednym mechanizmie: w każdym kroku sterowania rozwiązuj optimization problem dla N kroków przyszłości, zastosuj tylko pierwszy control, powtórz. Ta strategia receding horizon daje:

Adaptacyjność — automatyczne uwzględnienie zaburzeń (każda iteracja widzi aktualny stan).
Constraint handling — ograniczenia naturalnie wchodzą w QP/NLP.
Prediction — system „widzi" przyszłość N kroków, reaguje zanim coś się stanie.

Rodziny MPC:

Linear MPC — model liniowy $x_{k+1} = A x_k + B u_k$ , koszt kwadratowy. QP per krok, fast (μs).
NMPC — model nieliniowy, NLP per krok. Wolniejsze (ms), ale obsługuje dynamiki manipulatora.
iLQR / DDP / iLQG — wykorzystują strukturę dynamiczną (Bellman + backward pass) — szybsze niż NMPC dla długich horyzontów.
Sampling-based MPC (MPPI, CEM) — gradient-free, K rolloutów + weighted update. Działa dla nieróżniczkowalnych kosztów.
Tube MPC, Robust MPC — gwarantują stabilność przy bounded disturbances.

Linear MPC — fundament

Załóżmy liniowy model w postaci dyskretnej:

x_{k+1} = A x_k + B u_k, \quad x_k \in \mathbb{R}^n, \; u_k \in \mathbb{R}^m

Problem MPC dla horyzontu $N$ i bieżącego stanu $x_0$ :

\min_{u_0, \ldots, u_{N-1}} \; \sum_{k=0}^{N-1} \big[ x_k^\top Q x_k + u_k^\top R u_k \big] + x_N^\top Q_f x_N

s.t. $x_{k+1} = A x_k + B u_k, \quad u_k \in \mathcal{U}, \; x_k \in \mathcal{X}$ .

Po stackingu predykcji to standard QP:

\min_u \tfrac{1}{2} u^\top H u + g^\top u \quad \text{s.t.} \; A_{\text{ineq}} u \leq b

Rozwiązuje się przez aktywne zbiory (qpOASES), interior point (OSQP) lub embedded code generation (FORCES Pro, μ-controller). Typowy czas: 1-100 μs dla N=20, n=10, m=4 → 1-100 kHz update rate na CPU.

Stabilność

Klasyczne wyniki (Mayne et al. 2000): Linear MPC z terminal cost $x_N^\top P x_N$ gdzie $P$ = solution of discrete algebraic Riccati equation + terminal set constraint daje gwarantowaną stabilność asymptotyczną.

W praktyce: zaniedbywane (terminal cost = LQR P) gdy horyzont długi.

Linear MPC — point mass tracking (t = 0.00 s, N = 20)

horizon

N

reference

referenceactual (executed)predicted (N kroków)horyzont planowania

Co zauważyć: żółty obszar to horyzont planowania — przy każdym kroku MPC rozwiązuje QP dla tych N kroków przyszłości, ale stosuje TYLKO pierwszy control. Następny krok — re-plan z nowego stanu (receding horizon). Pomarańczowa przerywana to predykcja w chwili t; niebieska gruba to to, co faktycznie się wydarzyło. Krótkie N (≤ 5) — niestabilne, niebieska linia goni z opóźnieniem. Długie N (30+) — gładsze tracking, ale rośnie koszt obliczeniowy. Spróbuj zmienić reference na step żeby zobaczyć jak MPC „widzi" przyszłą zmianę z wyprzedzeniem.

Receding horizon — czemu zawsze re-plan

W każdym kroku MPC rozwiązuje QP dla N kroków przyszłości, ale aplikuje tylko pierwszy. To pozwala na:

Disturbance rejection — następny krok widzi aktualny stan, nie przewidywany.
Model mismatch correction — błędy modelu automatycznie kompensowane przez feedback.
Adaptive reference — gdy goal się zmieni, MPC natychmiast adaptuje.

NMPC — Nonlinear MPC

Dla manipulatora Pandy model jest nieliniowy:

M(q) \ddot q + C(q, \dot q) \dot q + g(q) = \tau

Dyskretyzowane (np. RK4): $x_{k+1} = f(x_k, u_k)$ z $f$ nieliniowym. Problem MPC staje się NLP (Nonlinear Programming), rozwiązywany przez SQP (jak w TrajOpt, moduł 07) lub interior-point IPOPT.

Real-Time Iteration (RTI) — Diehl et al. 2002

Pełen NLP solver wymaga wielu zewnętrznych iteracji, każda z wewnętrzny QP. Za wolne dla 1 kHz. RTI: uruchamiaj tylko jedną iterację SQP per krok sterowania — QP jest liniaryzacją wokół poprzedniego rozwiązania. Sub-millisecond update rate.

Warm-starting: jako initial guess dla iteracji $k$ , weź rozwiązanie z iteracji $k-1$ shifted o 1 krok. Po shiftcie potrzeba tylko uzupełnić ostatni krok (typowo zerami).

Frameworki NMPC

ACADO (Houska, Ferreau, Diehl 2011) — code generation, embedded. Dziś przemianowane na acados.
do-mpc (Lucia, Tatulea-Codrean 2017) — Python wrapper wokół CasADi + IPOPT.
MPC-Tools — MATLAB.
OCS2 (ETH) — SLQ (sequential linear quadratic) dla legged robots.

iLQR / DDP — exploit struktury dynamicznej

NMPC traktuje problem jako gęsty NLP — nie wykorzystuje sequential charakteru (każdy stan zależy tylko od poprzedniego). DDP (Differential Dynamic Programming, Mayne 1966) i iLQR(Iterative LQR, Li & Todorov 2004) wykorzystują strukturę przez backward Riccati sweep:

Forward pass: dla bieżącej trajektorii $(\bar x_k, \bar u_k)$ , oblicz $V(x_k)$ w każdym kroku.
Backward pass: Bellman backward — $V_k(x) = \min_u (\ell(x,u) + V_{k+1}(f(x,u)))$ . Aprokowane przez Taylor 2-go rzędu wokół $(\bar x, \bar u)$ , daje feedback gains $K_k$ i feedforward $k_k$ .
Update: $u_k = \bar u_k + k_k + K_k (x_k - \bar x_k)$ . Wykonaj forward pass, powtórz.

Złożoność: $O(N (n^3 + m^3))$ per iteracja vs $O((Nm)^3)$ dla dense NLP. Dla $N = 100, n = 14, m = 7$ (Panda) — DDP ~5 ms, dense IPOPT ~500 ms. Stąd DDP/iLQR dominują w real-time MPC manipulatorów.

iLQG (Iterative LQG) — DDP + stochastic noise. Crocoddyl (Mastalli et al. 2020) — najpopularniejsza biblioteka, używana w ANYmal, Cassie, dual-arm manipulation.

Demo: iLQR na 1D double-integratorze

Klasyczny pedagogiczny przykład: punkt materialny w 1D, stan $x_t = (p_t, v_t)$ , sterowanie $u_t = a_t$ . Dynamika liniowa:

x_{t+1} = A x_t + B u_t,\quad A=\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix},\quad B=\begin{bmatrix}\tfrac{1}{2}\Delta t^2\\ \Delta t\end{bmatrix}

Koszt biegnący z nieliniowym członem barierowym:

c(x,u) = (p-p_{\text{goal}})^2 + w_v v^2 + B_0\exp\!\big(-(p-p_b)^2/\sigma^2\big) + R u^2

Człon bump czyni koszt nieliniowym w $p$ — gdyby go nie było, iLQR zbiegłby w jednej iteracji (czyste LQR). Z bump-em każda iteracja liniaryzuje koszt wokół bieżącej trajektorii i daje inny zestaw gainsów $K_t$ , $k_t$ .

iLQR na 1D double-integratorze — backward Riccati sweep

p(t) — pozycja

v(t) — prędkość

u(t) — sterowanie

J vs iteracja (zbieżność)

Wynik backward Riccati sweep (iter 0)

Iteracja 0 = forward rollout z u=0 (brak backward sweep). Przesuń suwak iteracji ≥ 1 by zobaczyć K_t, V_xx_t.

kursor czasu: —

Co obserwować: (1) ustaw bump = 0 — koszt liniowy, iLQR zbiega w jednej iteracji (LQR analityczny). (2) zwiększ bump — koszt nieliniowy w

p

, iLQR potrzebuje kilku iteracji. (3) wykres

K_t

pokazuje feedback gain wzdłuż horyzontu — typowo wysoki na początku (długi czas na korekcję) i niższy pod koniec. (4)

V_{xx,t}

— drugi rząd cost-to-go, rośnie cofając się od końca horyzontu (akumulacja przyszłego kosztu). (5) krzywa

J

vs iteracja powinna być monotonicznie malejąca dzięki line-search α.

Co pokazuje wizualizacja

p(t), v(t), u(t) — trajektoria po wybranej iteracji. Iteracja 0 = forward rollout z $u=0$ (pelna bezruchowa trajektoria).
K_t = [K_p, K_v] — feedback gain wzdłuż horyzontu, wynik backward sweep: $K_t = -Q_{uu,t}^{-1} Q_{ux,t}$ .
k_t — feedforward, też z backward sweep: $k_t = -Q_{uu,t}^{-1} Q_{u,t}$ .
V_xx — cost-to-go drugiego rzędu, akumuluje się cofając od końca horyzontu (terminal: $V_{xx,N} = 2 \alpha_f I$ ).
J vs iter— line-search z α < 1 gwarantuje monotoniczność.

Ćwiczenie: ustaw bump = 0 → konwergencja w 1 iteracji. Stopniowo rośnij bump → liczba iteracji rośnie, bo kwadratowa aproksymacja kosztu staje się coraz mniej dokładna. Skrajnie wysokie $B_0$ może spowodować odrzucenie kroku przez line-search.

MPPI — Sampling-Based MPC

Wszystkie powyższe wymagają różniczkowalnego modelu i kosztu. Co gdy mamy:

Symulator MuJoCo z kontaktem (nieróżniczkowalny przy impacts)?
Sieć neuronową jako model dynamiki?
Koszt z indicator function (zerojedynkowa kolizja)?

Rozwiązanie: MPPI (Model Predictive Path Integral, Williams et al. 2017) — gradient-free MPC oparty na sampling.

Per krok sterowania:

Sample K perturbacji $\varepsilon_k \sim \mathcal{N}(0, \Sigma)$ wzdłuż całego horyzontu.
Wykonaj rollout dla każdego $u + \varepsilon_k$ , oblicz $J_k$ .
Softmax weighting: $w_k = \exp(-J_k / \lambda) / Z$ .
Update nominalu: $u \leftarrow u + \sum_k w_k \varepsilon_k$ .
Wykonaj $u_0$ , shift trajectory, powtórz.

MPPI — step 0 / 40

best cost: 14.00 · ✓ reached

K rollouts50

σ szumu0.60

λ temp0.30

Co zobaczyć: szare/zielone cienkie linie = K rolloutów ze szumem (zielony = niska koszt, ważone wyższą wagą). Fioletowa gruba linia = nominal control (mean ważony perturbacji). Niebieska gruba = faktyczna trajektoria wykonana. MPPI vs STOMP (moduł 07): obie używają weighted-rollouts, ale MPPI jest online closed-loop — w każdym kroku roluje K trajektorii, aplikuje tylko pierwszy control, shift'uje nominal o 1, dodaje zero na końcu (warm-start). Daje 50-100 Hz update rate na CPU.

Warianty MPPI

Tube-MPPI — łączy MPPI z tube MPC dla robustness przy stochastic dynamics.
Log-MPPI (Tao, Boots 2022) — log-likelihood weighting zamiast soft-max; lepsza zbieżność dla wąskich rozkładów.
Smooth MPPI — dodaje smoothing po update żeby uniknąć high-frequency chattering.
STORM (Bhardwaj et al. 2022) — STocastic Optimization for Reactive Motion. MPPI dla 7-DOF manipulatorów, ~50 Hz update na GPU.

MPPI vs CEM (Cross-Entropy Method): oba używają K rolloutów, ale CEM bierze top-N elite (np. 10%) i fits Gaussian do nich (zamiast soft-max). MPPI bardziej smooth update, CEM agresywniej fokusuje na najlepszych. Wybór: empiryczny.

CEM (Cross-Entropy Method) jako planer online

Per iteracja:

Sample K control sequences from current Gaussian $\mathcal{N}(\mu, \Sigma)$ .
Rollout + oblicz koszt każdej.
Wybierz top $\rho \cdot K$ najlepszych (typowo $\rho = 0.1$ ).
Fit Gaussian do top — nowe $\mu, \Sigma$ .
Iteruj kilka razy, weź $\mu$ jako control.

CEM jest bardziej greedy niż MPPI (skupia się tylko na top), ale zbiega szybciej do lokalnego optimum. Standard w model-based RL — PETS (Chua 2018), PlaNet (Hafner 2019).

Real-time iteration i warm-starting

Wszystkie MPC mają wspólny problem: computational delay (czas między pomiarem a aplikacją). Dla 1 kHz update musi się wykonać < 1 ms.

Strategie:

RTI (Diehl 2002) — jedna iteracja SQP per krok, używaj liniaryzacji z poprzedniego.
Warm-start — początkowy guess = poprzednie rozwiązanie shifted o 1 krok.
Multiple shooting + sparse Jacobian — używaj struktury problemu (moduł 07).
Approximate solve — rozwiąż QP do tolerancji $10^{-3}$ zamiast $10^{-8}$ .
Code generation (FORCES Pro, acados) — specjalizowany kod C dla konkretnego problemu.
GPU parallelism — MPPI/CEM trivially parallel over K rolloutów.

Implementacje praktyczne

Framework	Typ	Typowe zastosowanie
mujoco_mpc	iLQG + Predictive Sampling	Google's MJPC — interactive MPC w MuJoCo Studio. Bezpośrednie demo dla manipulatorów (Panda, Allegro).
acados	NMPC z RTI + HPIPM solver	Embedded MPC dla autonomicznych pojazdów, dronów. Code generation w C.
Crocoddyl	iLQR / DDP / SQP	Manipulatory + legged robots. ANYmal, Cassie, dual-arm manipulation.
OCS2	SLQ (Sequential LQ)	Legged locomotion (ETH). ANYmal real-time MPC z constraintami.
TinyMPC	Linear MPC, ADMM	Microcontroller-class MPC dla nano-drones (Cortex-M).
cuRobo (NVIDIA)	MPPI + sampling-based	GPU MPC dla 7-DOF manipulatorów. K = 1000+ rolloutów per iteracja, 50+ Hz.
do-mpc	NMPC (CasADi + IPOPT)	Research / prototypy. Python frontend, łatwy do napisania.

Ściąga

Receding horizon pattern

Zmierz stan $x_0$
Rozwiąż optimization for next N steps: $u^* = \arg\min J(x_0, u_{0..N-1})$
Zastosuj tylko $u_0^*$
Powtórz

Linear MPC = QP

\min_u \tfrac{1}{2} u^\top H u + g^\top u \quad \text{s.t.} \; A u \leq b

MPPI update

u \leftarrow u + \sum_{k=1}^K w_k \varepsilon_k, \quad w_k = \frac{e^{-J_k/\lambda}}{\sum_j e^{-J_j/\lambda}}

iLQR / DDP

Bellman backward pass z aprox. 2-go rzędu wokół trajektorii. Daje feedback gains $K_k$ . $O(N \cdot n^3)$ per iteracja.

Wybór algorytmu

Linear, fast, no contact → Linear MPC + OSQP
Nonlinear, smooth, manipulator → iLQR / Crocoddyl
Non-diff cost, contact, sim-in-the-loop → MPPI / mujoco_mpc
GPU available, K=1000 rolloutów → cuRobo / STORM
Embedded MCU → TinyMPC / FORCES Pro

Referencje

Mayne, Rawlings, Rao & Scokaert, „Constrained model predictive control: Stability and optimality" (Automatica 2000) — fundament linear MPC + dowody stabilności.
Mayne, „A Second-Order Gradient Method for Determining Optimal Trajectories of Non-linear Discrete-Time Systems" (Int. J. Control 1966) — DDP origin.
Li & Todorov, „Iterative linear-quadratic regulator design for nonlinear biological movement systems" (ICINCO 2004) — iLQR.
Diehl, Bock, Schlöder, Findeisen, Nagy, Allgöwer, „Real-time optimization and nonlinear model predictive control of processes governed by differential-algebraic equations" (J. Process Control 2002) — RTI scheme.
Williams, Wagener, Goldfain, Drews, Rehg, Boots, Theodorou, „Information Theoretic MPC for Model-Based Reinforcement Learning" (ICRA 2017) — MPPI.
Bhardwaj, Sundaralingam, Mousavian, Chentanez, Fox, Birchfield, Boots, Ratliff, „STORM: An Integrated Framework for Fast Joint-Space Model-Predictive Control for Reactive Manipulation" (CoRL 2022).
Mastalli et al., „Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control" (ICRA 2020).
Verschueren et al., „acados — a modular open-source framework for fast embedded optimal control" (Math. Prog. Comp. 2021).
Howell, Gileadi, Tunyasuvunakool, Zakka, Erez, Tassa, „Predictive Sampling: Real-time Behaviour Synthesis with MuJoCo" (arXiv 2022) — mujoco_mpc.
Nguyen, Schwager, et al., „TinyMPC: Model-Predictive Control on Resource-Constrained Microcontrollers" (ICRA 2024).