第8章 机器人动力学、控制与运动规划
原课程: Robots That Learn, UC Berkeley CS294-291, Spring 2026 本章基于 Lecture 5B: Robot Dynamics, Control and Motion Planning 精读论文: Kawato (1999) Internal models for motor control and trajectory planning; Flanagan, Bowman & Johansson (2006) Control strategies in object manipulation tasks
一、本章概要
1.1 核心问题
前两章我们建立了运动控制的生物基础（L5A: 发育认知、CPG、运动基元）和机器人机构学（L2B: 运动学、动力学）。现在我们面对的是一个从理论到实践的桥梁问题：
给定一个已知动力学模型的机器人系统，如何计算使其从初始状态到达目标状态的最优控制信号？
这个问题有三个层次：
- 无约束最优控制：系统是线性的，代价是二次的——引出 LQR（Linear Quadratic Regulator），这是所有最优控制方法的理论基石。
- 非线性轨迹优化：系统是非线性的，但通过沿标称轨迹的局部线性化迭代求解——引出 DDP/iLQR（Differential Dynamic Programming / iterative LQR），这是现代机器人轨迹优化的工业标准。
- 在线重规划：执行过程中不断以当前状态为起点重新求解有限时域问题——引出 MPC（Model Predictive Control），这是从四足机器人到自动驾驶的量产级控制架构。
这三个层次不是互相替代的关系，而是构成了一个完整的控制栈：MPC 在外层以低频（~50 Hz）生成参考轨迹，LQR/DDP 在内层以高频（~1 kHz）计算反馈增益，而这一切的基础是对机器人动力学的内部表征——即 Kawato (1999) 所称的内部模型 (internal models)。
1.2 生物学与工程学的交叉
本章的一个独特之处在于它的双重来源：
- Kawato (1999) 从神经科学角度论证了小脑是内部模型（前向模型和逆向模型）的主要存储位置，并提出反馈误差学习（feedback-error learning）作为内部模型的获取机制。
- Flanagan et al. (2006) 从行为学角度展示了人类在物体操作任务中如何利用预测性控制——内部模型不仅用于运动生成，还用于感知预测和任务监控。
这两篇论文共同揭示了一个深刻的原则：预测是运动控制的核心。无论生物神经系统还是工程控制系统，有效的控制都依赖于对未来状态的准确预测。
1.3 与前后章节的关系
Ch07 运动控制的发展视角 ──→ Ch08 机器人动力学与控制 ──→ Ch09 计算神经科学
(运动基元/CPG) (LQR/DDP/MPC/内部模型) (前向/逆向模型深化)
↓
Ch11 强化学习
(MDP → 最优控制的对偶)
本章的数学工具是最优控制的标准化语言，为后续 RL 章节（Ch11）建立价值函数和贝尔曼方程提供了直接的先修知识。特别地，iLQR 与基于模型的 RL（如 PILCO, MBPO）共享相同的轨迹优化内核。
二、核心概念与数学基础
2.1 最优控制问题的标准形式
离散时间确定性最优控制问题定义为：
\[\boxed{\min_{u_0, \ldots, u_{T-1}} \; J(x_0, u_{0:T-1}) = \sum_{t=0}^{T-1} \ell_t(x_t, u_t) + \ell_T(x_T)}\]满足动力学约束：
\[x_{t+1} = f_t(x_t, u_t), \quad t = 0, \ldots, T-1\]其中：
- \(x_t \in \mathbb{R}^n\): 状态向量（机器人中通常包含关节角度 \(\theta\)、角速度 \(\dot{\theta}\)）
- \(u_t \in \mathbb{R}^m\): 控制向量（关节力矩 \(\tau\)）
- \(\ell_t(x_t, u_t)\): 运行代价 (running cost)
- \(\ell_T(x_T)\): 终端代价 (terminal cost)
- \(f_t\): 离散时间动力学映射
与机器人动力学的联系：连续时间动力学方程 \(\tau = M(\theta)\ddot{\theta} + C(\theta, \dot{\theta})\dot{\theta} + g(\theta)\) 可以通过显式欧拉积分或 Runge-Kutta 方法离散化： \(x_{t+1} = x_t + \Delta t \cdot F(x_t, u_t)\) 其中 \(x = [\theta; \dot{\theta}]\)，\(u = \tau\)。
2.2 动态规划与贝尔曼原理
贝尔曼最优性原理 (Bellman’s Principle of Optimality, 1957) 是所有最优控制方法的理论基础：
一条最优策略具有这样的性质：无论初始状态和初始决策如何，剩余的决策必须构成由第一个决策产生的状态出发的最优策略。
用价值函数 \(V_t(x_t)\) 表示从状态 \(x_t\) 出发、时间步 \(t\) 到终端的最优代价 (cost-to-go)：
\[\boxed{V_t(x_t) = \min_{u_t} \left[ \ell_t(x_t, u_t) + V_{t+1}\big(f_t(x_t, u_t)\big) \right]}\]边界条件：\(V_T(x_T) = \ell_T(x_T)\)。
这个递归关系将 T 步决策问题分解为 T 个单步决策问题。我们从终端 \(t=T\) 开始向后递推 (backward pass)，逐步计算每个时刻的最优价值和最优策略。
2.3 线性二次型调节器 (LQR)：第一性原理推导
LQR 是最优控制中唯一具有解析闭式解的非平凡问题。假设系统是线性的，代价是二次的：
\[f_t(x_t, u_t) = A_t x_t + B_t u_t\] \[\ell_t(x_t, u_t) = \frac{1}{2}(x_t^T Q_t x_t + u_t^T R_t u_t), \quad \ell_T(x_T) = \frac{1}{2}x_T^T Q_T x_T\]其中 \(Q_t \succeq 0\) (半正定)，\(R_t \succ 0\) (正定)。
2.3.1 价值函数的二次型结构
我们从归纳假设出发：假设价值函数具有二次型形式：
\[V_t(x_t) = \frac{1}{2} x_t^T P_t x_t + p_t^T x_t + c_t\]检验终端条件 \(t=T\)：\(V_T(x_T) = \frac{1}{2}x_T^T Q_T x_T\)，因此 \(P_T = Q_T\), \(p_T = 0\), \(c_T = 0\)，归纳基础成立。
假设 \(V_{t+1}\) 具有上述二次型形式，代入贝尔曼方程：
\[V_t(x_t) = \min_{u_t} \left[ \frac{1}{2}(x_t^T Q_t x_t + u_t^T R_t u_t) + \frac{1}{2} x_{t+1}^T P_{t+1} x_{t+1} + p_{t+1}^T x_{t+1} + c_{t+1} \right]\]利用动力学约束 \(x_{t+1} = A_t x_t + B_t u_t\)，展开：
\[\begin{aligned} V_t(x_t) = \min_{u_t} \Big[ &\frac{1}{2} x_t^T Q_t x_t + \frac{1}{2} u_t^T R_t u_t \\ &+ \frac{1}{2} (A_t x_t + B_t u_t)^T P_{t+1} (A_t x_t + B_t u_t) \\ &+ p_{t+1}^T (A_t x_t + B_t u_t) + c_{t+1} \Big] \end{aligned}\]2.3.2 最优控制的闭式解
将括号内的表达式看作 \(u_t\) 的二次函数，求梯度并设为零：
\[\frac{\partial}{\partial u_t}[\cdots] = R_t u_t + B_t^T P_{t+1}(A_t x_t + B_t u_t) + B_t^T p_{t+1} = 0\]整理：
\[(R_t + B_t^T P_{t+1} B_t) u_t + B_t^T P_{t+1} A_t x_t + B_t^T p_{t+1} = 0\]定义： \(S_t := R_t + B_t^T P_{t+1} B_t\)
解出最优控制：
\[\boxed{u_t^* = -S_t^{-1} B_t^T P_{t+1} A_t \, x_t - S_t^{-1} B_t^T p_{t+1}}\]对于零线性项的二次代价 (\(p_{t+1} = 0\)，即无目标状态偏置)，最优控制具有线性状态反馈形式：
\[\boxed{u_t^* = -K_t x_t}, \quad K_t := S_t^{-1} B_t^T P_{t+1} A_t\]这是本章最重要的单个公式：最优控制器是一个线性反馈律，其增益 \(K_t\) 完全由系统动力学 \((A_t, B_t)\) 和代价权重 \((Q_t, R_t)\) 通过 Riccati 方程确定。
2.3.3 Riccati 递推方程
将 \(u_t^*\) 代回 \(V_t(x_t)\) 表达式，归并同类项。比较二次项系数，得到 离散时间矩阵 Riccati 方程 (Discrete-time Algebraic Riccati Equation, DARE)：
\[\boxed{P_t = Q_t + A_t^T P_{t+1} A_t - A_t^T P_{t+1} B_t (R_t + B_t^T P_{t+1} B_t)^{-1} B_t^T P_{t+1} A_t}\]一阶项和常数项的递推为： \(p_t = A_t^T p_{t+1} - A_t^T P_{t+1} B_t S_t^{-1} B_t^T p_{t+1}\) \(c_t = c_{t+1} - \frac{1}{2} p_{t+1}^T B_t S_t^{-1} B_t^T p_{t+1}\)
算法 (LQR Backward Pass):
P_T = Q_T
for t = T-1, ..., 0:
S_t = R_t + B_t^T P_{t+1} B_t
K_t = S_t^{-1} B_t^T P_{t+1} A_t
P_t = Q_t + A_t^T P_{t+1} (A_t - B_t K_t)
算法 (LQR Forward Pass):
for t = 0, ..., T-1:
u_t = -K_t x_t
x_{t+1} = A_t x_t + B_t u_t
2.3.4 稳态 LQR 与代数 Riccati 方程
对于时不变系统 (\(A_t = A\), \(B_t = B\), \(Q_t = Q\), \(R_t = R\)) 和无限时域 (\(T \to \infty\))，\(P_t\) 收敛到稳态解 \(P_{\infty}\)，满足代数 Riccati 方程 (ARE)：
\[\boxed{P = Q + A^T P A - A^T P B (R + B^T P B)^{-1} B^T P A}\]此时反馈增益为常数：\(u_t = -K_{\infty} x_t\), \(K_{\infty} = (R + B^T P B)^{-1} B^T P A\)。
稳定性保证：若 \((A, B)\) 可镇定且 \((A, Q^{1/2})\) 可检测，则 ARE 存在唯一正定解 \(P \succ 0\)，且闭环系统 \(A - BK_{\infty}\) 是渐近稳定的。
2.4 微分动态规划 (DDP) 与迭代 LQR (iLQR)
LQR 要求线性的动力学和二次的代价，这在实际机器人系统中几乎从来不被满足。DDP 和 iLQR 通过沿标称轨迹的局部近似来扩展 LQR 到非线性系统。
2.4.1 Q-函数的二阶展开
定义状态-控制对的 Q-函数（Bellman 方程右侧）：
\[Q_t(x_t, u_t) = \ell_t(x_t, u_t) + V_{t+1}\big(f_t(x_t, u_t)\big)\]对当前标称轨迹 \((\bar{x}_{0:T}, \bar{u}_{0:T-1})\) 附近的扰动 \(\delta x_t = x_t - \bar{x}_t\), \(\delta u_t = u_t - \bar{u}_t\)，进行二阶 Taylor 展开：
\[\boxed{Q_t(\bar{x}_t + \delta x_t, \bar{u}_t + \delta u_t) \approx Q_0 + \begin{bmatrix} Q_x \\ Q_u \end{bmatrix}^T \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} + \frac{1}{2} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}^T \begin{bmatrix} Q_{xx} & Q_{xu} \\ Q_{ux} & Q_{uu} \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}}\]其中各导数的计算（利用链式法则和 \(V_{t+1}\) 的二次型假设）：
\[\begin{aligned} Q_x &= \ell_x + f_x^T V_x \\ Q_u &= \ell_u + f_u^T V_x \\ Q_{xx} &= \ell_{xx} + f_x^T V_{xx} f_x + V_x \cdot f_{xx} \\ Q_{uu} &= \ell_{uu} + f_u^T V_{xx} f_u + V_x \cdot f_{uu} \\ Q_{ux} &= \ell_{ux} + f_u^T V_{xx} f_x + V_x \cdot f_{ux} \end{aligned}\]其中 \(f_x = \partial f / \partial x\), \(f_u = \partial f / \partial u\) 是 Jacobian 矩阵，\(f_{xx}\), \(f_{uu}\), \(f_{ux}\) 是 Hessian（三阶张量的适当缩并）。\(\ell_x\), \(\ell_u\), \(\ell_{xx}\), \(\ell_{uu}\), \(\ell_{ux}\) 是运行代价的导数。
2.4.2 最优扰动与价值函数更新
在 \(Q_t\) 的近似的二次展开下，最小化关于 \(\delta u_t\) 的二次形式：
\[\delta u_t^* = \arg\min_{\delta u} \left[Q_u^T \delta u + \frac{1}{2}\delta u^T Q_{uu} \delta u + \delta u^T Q_{ux} \delta x \right]\]解得（假设 \(Q_{uu} \succ 0\)）：
\[\boxed{\delta u_t^* = \underbrace{-Q_{uu}^{-1} Q_u}_{\text{前馈项 } k_t} \;-\; \underbrace{Q_{uu}^{-1} Q_{ux}}_{\text{反馈增益 } K_t} \, \delta x_t}\]将 \(\delta u_t^*\) 代回 \(Q_t\) 展开式，得到价值函数的更新：
\[\begin{aligned} \Delta V &= -\frac{1}{2} Q_u^T Q_{uu}^{-1} Q_u \\ V_x &\leftarrow Q_x - Q_{xu} Q_{uu}^{-1} Q_u \\ V_{xx} &\leftarrow Q_{xx} - Q_{xu} Q_{uu}^{-1} Q_{ux} \end{aligned}\]这里的关键结构是：\(\Delta V = -\frac{1}{2}Q_u^T Q_{uu}^{-1} Q_u \leq 0\)（因为 \(Q_{uu} \succ 0\)），这意味着每次迭代的期望代价下降严格非正——这是算法收敛性的核心保证。
2.4.3 iLQR 作为 DDP 的简化
DDP 使用动力学 \(f\) 的二阶导数 (\(f_{xx}\), \(f_{uu}\), \(f_{ux}\)) 来构造 \(Q_{xx}\), \(Q_{uu}\), \(Q_{ux}\)。但在高维系统中，计算 \(f\) 的 Hessian 计算量巨大。
iLQR (iterative LQR, Tassa et al. 2012, 2014) 的关键简化：忽略动力学的二阶项。即，在 \(Q\) 的展开中仅使用 \(f\) 的 Jacobian，舍弃 \(V_x \cdot f_{xx}\) 等项：
\[\begin{aligned} Q_{xx} &\approx \ell_{xx} + f_x^T V_{xx} f_x \\ Q_{uu} &\approx \ell_{uu} + f_u^T V_{xx} f_u \\ Q_{ux} &\approx \ell_{ux} + f_u^T V_{xx} f_x \end{aligned}\]这与标准的 LQR 在局部具有完全相同的代数结构。因此，iLQR 可以理解为：沿非线性轨迹在每个时刻求解一个局部 LQR 子问题。
为保持 \(Q_{uu}\) 的正定性（保障下降方向），在实际实现中使用正则化： \(Q_{uu}^{\text{reg}} \leftarrow Q_{uu} + \mu I, \quad \mu > 0\)
\(\mu\) 的自适应调整策略：如果当前迭代的代价下降，减小 \(\mu\)（更接近 Newton 步）；如果代价上升，增大 \(\mu\)（更接近梯度下降）。这种 Levenberg-Marquardt 风格的启发式是 iLQR 在实际机器人系统中高度鲁棒的关键原因。
2.4.4 iLQR 完整算法
输入: 初始标称轨迹 \(\bar{x}_{0:T}, \bar{u}_{0:T-1}\)，动力学模型 \(f\)，代价函数 \(\ell_t, \ell_T\)，收敛阈值 \(\epsilon\)
输出: 优化后的轨迹和控制序列
repeat until convergence:
// Forward Pass (阶段 1): 沿标称轨迹计算动力学和代价
for t = 0, ..., T-1:
compute f_t(bar{x}_t, bar{u}_t)
compute ell_t(bar{x}_t, bar{u}_t)
compute f_x, f_u, ell_x, ell_u, ell_xx, ell_uu, ell_ux
compute ell_T, ell_T_x, ell_T_xx at bar{x}_T
// Backward Pass (阶段 2): 递推计算 V_x, V_xx, k_t, K_t
V_x = ell_{T,x}, V_xx = ell_{T,xx}
for t = T-1, ..., 0:
Q_x = ell_x + f_x^T V_x
Q_u = ell_u + f_u^T V_x
Q_xx = ell_xx + f_x^T V_xx f_x
Q_uu = ell_uu + f_u^T V_xx f_u
Q_ux = ell_ux + f_u^T V_xx f_x
Q_uu_reg = Q_uu + mu * I // Levenberg-Marquardt 正则化
k_t = -Q_uu_reg^{-1} Q_u // 前馈项 (open-loop improvement)
K_t = -Q_uu_reg^{-1} Q_ux // 反馈增益 (closed-loop gain)
V_x = Q_x - K_t^T Q_uu k_t
V_xx = Q_xx - K_t^T Q_uu_reg K_t
// Forward Pass (阶段 3): 线搜索执行新控制律
x_hat_0 = x_0
for alpha in [1.0, 0.5, 0.25, ...]: // 回溯线搜索
for t = 0, ..., T-1:
u_hat_t = bar{u}_t + alpha * k_t + K_t * (x_hat_t - bar{x}_t)
x_hat_{t+1} = f_t(x_hat_t, u_hat_t)
if cost(x_hat, u_hat) < cost(bar{x}, bar{u}):
bar{x}, bar{u} = x_hat, u_hat
decrease mu // 接受: 下一步更接近 Newton
break
else:
increase mu // 拒绝: 下一步更接近梯度下降
if max_t(|k_t|) < epsilon: converged
2.4.5 线搜索 (Line Search) 机制
iLQR 的线搜索与标准优化器略有不同。不是在单一搜索方向上调整步长，而是调整前馈项 \(k_t\) 的比例：
\[u_t^{\text{new}} = \bar{u}_t + \alpha k_t + K_t(x_t^{\text{new}} - \bar{x}_t)\]其中 \(\alpha \in (0, 1]\) 通过回溯确定。注意 \(k_t\) 和 \(K_t\) 本身就具有最优下降特性——当 \(\alpha = 1\) 时是 Newton 步长，当 \(\mu \to \infty\) 时退化为梯度下降。
2.5 模型预测控制 (MPC)
2.5.1 滚动时域原理
LQR 和 iLQR 都是开环离线规划方法——它们一次性生成长时域的控制序列，然后执行。这在机器人实际运行中面临两个致命问题：
- 模型误差累积：长时间的开环执行使动力学建模误差不断累积。
- 计算延迟：长时域规划的计算时间可能超过控制周期。
MPC (Model Predictive Control) 通过滚动时域优化 (receding horizon optimization) 解决这些问题：
\[\boxed{\text{在每个控制周期 } k: \quad \min_{u_{k:k+H-1}} \sum_{i=0}^{H-1} \ell(x_{k+i}, u_{k+i}) + \phi(x_{k+H})}\]其中 \(H \ll T\) 是预测时域 (prediction horizon) 长度。仅执行第一步控制 \(u_k^*\)，然后重新测量状态 \(x_{k+1}\)，以 \(k+1\) 为起点重新优化。
MPC 算法框架:
for each control step k:
1. 测量当前状态 x_k
2. 求解有限时域最优控制问题（预测时域 H）
3. 施加 u_k^*（仅第一个控制）
4. k ← k + 1，回到步骤 1
2.5.2 终端代价与稳定性
MPC 的有限时域优化只是无限时域问题的截断近似。如果终端代价 \(\phi(x_{k+H})\) 选择不当，MPC 可能不稳定。
稳定性定理 (Mayne et al., 2000)：若满足以下三个条件，MPC 闭环系统是渐近稳定的：
- 终端代价 \(\phi(x)\) 是无限时域最优代价的局部上界：\(\phi(x) \geq V_{\infty}(x) \; \forall x \in \mathcal{X}_f\)
- 终端约束集 \(\mathcal{X}_f\) 在局部控制器下是正不变的 (positive invariant)
- 终端控制器使 \(\phi\) 在 \(\mathcal{X}_f\) 内单调递减
工程实践：对机器人系统，\(\phi(x)\) 通常选为 LQR 的稳态价值函数 \(x^T P_{\infty} x\)（对线性化后的系统），这保证了局部最优性和稳定性。
2.5.3 子最优性分析
令 \(J^*(x)\) 为无限时域最优代价，\(J_{\text{MPC}}(x)\) 为 MPC 的实际闭环代价。两者的差距由预测时域 \(H\) 限制：
\[J_{\text{MPC}}(x) - J^*(x) \leq \frac{\beta^H}{1 - \beta} \cdot C\]其中 \(\beta \in (0, 1)\) 与闭环系统的收缩速率相关，\(C\) 是与代价结构有关的常数。这给出了一个实用的工程指导：\(H\) 的选取应与系统的”记忆”时间相匹配——对于机器人系统，通常 \(H = 10 \sim 50\) 步（对应 0.2 ~ 1.0 秒）即可获得接近最优的性能。
2.6 内部模型：前向模型、逆向模型与反馈误差学习
本节基于 Kawato (1999) 的理论框架。
2.6.1 建模与控制中的三类结构
考虑一个被控对象 (controlled object) 及其动力学映射 \(\mathcal{D}: u \mapsto x\)（控制到轨迹）。
| 模型类型 | 数学定义 | 功能 | 生物学对应 |
|---|---|---|---|
| 前向模型 (Forward Model) | \(\hat{x} = \hat{\mathcal{D}}(u)\) | 预测: 给定当前状态和运动指令，预测下一状态 | 小脑-大脑皮层回路 |
| 逆向模型 (Inverse Model) | \(\hat{u} = \hat{\mathcal{D}}^{-1}(x_d)\) | 反演: 给定期望轨迹，计算所需运动指令 | 小脑 Purkinje 细胞 |
| 反馈控制器 (Feedback Controller) | \(u_{\text{fb}} = K(x_d - x)\) | 纠正: 根据误差在线调节控制信号 | 脊髓反射弧 |
关键洞察 (Kawato, 1999, Fig 1)：生物反馈回路存在不可忽略的延迟（视觉反馈 150-250 ms，脊髓反射 30-50 ms）。这意味着单纯依赖反馈控制无法执行快速运动（如网球发球，总时长约 150 ms）。因此，神经系统必须通过运动学习获取逆向动力学模型，实现前馈控制。
2.6.2 反馈误差学习 (Feedback-Error Learning)
Kawato (1987, 1999) 的反馈误差学习理论提供了一个不需要”教师信号”（即理想运动指令）的逆向模型学习方法。其核心思想是：反馈控制器的输出被用作训练逆向模型的误差信号。
设反馈控制器产生的指令为 \(u_{\text{fb}}(t)\)。在前馈-反馈混合控制架构中，总控制信号为：
\[u(t) = u_{\text{ff}}(t) + u_{\text{fb}}(t)\]其中 \(u_{\text{ff}}(t)\) 是逆向模型的输出。学习规则：使用 \(u_{\text{fb}}(t)\) 作为监督信号更新逆向模型的参数 \(\theta\)：
\[\Delta \theta = -\eta \cdot \frac{\partial u_{\text{ff}}}{\partial \theta} \cdot u_{\text{fb}}\]直观解释：在学习初期，\(u_{\text{ff}} \approx 0\)，控制主要依赖 \(u_{\text{fb}}\)。随着学习进行，逆向模型逐渐精确，\(u_{\text{ff}}\) 能够独立完成任务，\(u_{\text{fb}} \to 0\)。此时，系统实现了从反馈依赖到前馈主导的平滑过渡。
收敛性分析（简化形式）：假设被控对象是线性的 \(x = G \cdot u\)，逆向模型 \(u_{\text{ff}} = \hat{G}^{-1} \cdot x_d\)，反馈控制器 \(u_{\text{fb}} = K (x_d - x)\)。在反馈误差学习下：
\[\frac{d}{dt}\|\hat{G}^{-1} - G^{-1}\| \to 0 \quad \text{as} \quad t \to \infty\]只要反馈增益 \(K\) 使闭环系统保持稳定。这一收敛性质是反馈误差学习的核心理论保证。
2.6.3 多对前向-逆向模型 (MPFIM)
Wolpert & Kawato (1998) 提出了多重配对前向-逆向模型 (Multiple Paired Forward-Inverse Models, MPFIM) 框架来解决多环境/多工具的泛化问题。
问题：单一的全局逆向模型无法同时适应网球拍和叉子的动力学，因为不同工具的动力学特性差异巨大，会导致灾难性干扰 (catastrophic interference)。
MPFIM 架构：维护多个模块，每个模块包含三个组件：
- 逆向模型：产生该模块的前馈运动指令
- 前向模型：预测该模块的运动指令的感觉后果
- 责任预测器 (responsibility predictor)：基于上下文信息（视觉、指令、时序）预测该模块在当前情形下被使用的先验概率
模块的选择和混合通过贝叶斯框架实现：
\[r_i = \frac{\pi_i \cdot P(\text{error}_i)}{\sum_j \pi_j \cdot P(\text{error}_j)}\]其中 \(r_i\) 是第 \(i\) 个模块的责任信号 (responsibility signal)，\(\pi_i\) 是责任预测器的先验输出，\(P(\text{error}_i)\) 是前向模型预测误差的似然。
重要性：MPFIM 将工具使用的泛化问题从”获取一个普适的逆向模型”转化为”获取一组专用模型并学习何时使用哪个”——这在计算上更可行，也更加符合神经科学的实验数据（Shadmehr & Mussa-Ivaldi 1994, Gandolfo et al. 1996, Flanagan et al. 1999）。
2.7 最小方差模型与轨迹规划
Kawato (1999, Section: Trajectory formation) 讨论了轨迹规划的两大传统：
| 模型类型 | 代表 | 目标函数 | 变量域 |
|---|---|---|---|
| 运动学模型 | Flash & Hogan (1985) 最小 Jerk 模型 | \(\int (\dddot{x})^2 dt\) | 手部位置 |
| 动力学模型 | Uno et al. (1989) 最小力矩变化模型 | \(\int (\dot{\tau})^2 dt\) | 关节力矩 |
Harris & Wolpert (1998) 的最小方差模型 (Minimum Variance Model) 提供了一个巧妙的统一：
- 目标函数：最小化终点位置方差 \(\text{Var}(x_T)\)（运动学量）
- 计算过程：方差依赖于运动指令的幅度（信号依赖噪声），因此最优轨迹取决于动力学特性
其中 \(\xi(s)\) 是信号依赖噪声：\(\text{Var}[\xi(s)] \propto \vert u(s)\vert ^2\)。最小化 \(\text{Var}(x_T)\) 等价于最小化 \(\int_0^T \vert u(t)\vert ^2 \, w(t) dt\)，其中权重 \(w(t)\) 随时间递增——运动接近终点时的控制噪声对终点精度的影响更大。这自然地解释了为什么人类运动的接近终点阶段会减速（为减小控制信号幅度）。
三、论文精读
3.1 Kawato (1999): Internal Models for Motor Control and Trajectory Planning
Kawato 的这篇综述是内部模型理论的集大成之作，发表于 Current Opinion in Neurobiology。
3.1.1 核心论点
论文的核心论点是三重的：
- 内部模型是真实存在于神经系统中的计算实体，不是控制理论家的虚构。
- 小脑是内部模型的主要存储位置，其独特的神经回路结构（苔状纤维 → 平行纤维 → Purkinje 细胞 → 深部核团）天然适合监督学习。
- 内部模型在运动控制和轨迹规划中都是必需的，反馈控制由于延迟限制无法独立支撑快速协调运动。
3.1.2 关键实验证据
证据 1：力场适应的后效应 (Shadmehr & Mussa-Ivaldi 1994)
受试者在机械臂产生的速度依赖力场中进行点到点手臂运动。最初轨迹发生畸变，但经过反复练习后恢复为近似直线。突然移去力场后，轨迹向相反方向畸变——这一后效应 (after-effect) 直接证明了受试者获取了力场的逆向动力学模型，而非仅仅增强了手臂刚度。
证据 2：抓握力-负载力耦合 (Johansson & Westling 1984; Flanagan & Wing 1997)
当人手持物体进行垂直运动时，抓握力 (grip force) 精确地与负载力 (load force) 同步调制，甚至在负载力出现之前——这需要皮层运动指令的传出拷贝 (efference copy) 通过前向模型预测未来负载力。fMRI 研究（Tamada et al. 1999）确认了该过程中小脑外侧的特定激活。
证据 3：小脑 Purkinje 细胞的编码特性 (Kobayashi et al. 1998)
在猴的眼球跟随反应 (Ocular Following Responses, OFR) 中，腹侧旁小叶 (VPFL) 的 Purkinje 细胞的简单棘波发放模式可以被一个线性逆向动力学模型精确重建：
\[\text{firing rate}(t) = \alpha \cdot \ddot{\theta}(t) + \beta \cdot \dot{\theta}(t) + \gamma \cdot \theta(t)\]更妙的是，简单棘波和爬行纤维输入的复杂棘波的发放波形呈现镜像对称——这与反馈误差学习理论中爬行纤维传递运动指令误差信号、简单棘波被误差引导学习的假设完全一致。
3.1.3 理论与工程的桥梁
Kawato 的框架对机器人控制有三层直接启示：
- 前向模型用于预测（等价于现代 MPC 中的 rollout simulator）
- 逆向模型用于前馈（等价于计算力矩控制）
- 学习由反馈误差驱动（等价于自适应控制中的参数更新律）
3.2 Flanagan, Bowman & Johansson (2006): Control Strategies in Object Manipulation Tasks
这篇综述聚焦于人类物体操作任务中的控制策略，发表于 Current Opinion in Neurobiology。
3.2.1 核心理念：接触事件作为感觉运动控制点
Flanagan 等人的核心创新是提出了感觉运动控制点 (Sensorimotor Control Points) 的概念。在操作任务中（如拿起一个物体、移动、放下），各阶段之间由明确的接触事件 (contact events) 界定：
- 手指接触物体表面
- 物体离开支撑面 (lift-off)
- 物体接触目标位置 (contact with target)
- 释放物体 (release)
这些接触事件在多个感觉模态（触觉、视觉、听觉、本体感觉）中同时产生离散的感觉信号。通过比较预测的感觉事件与实际的感觉事件，运动系统可以：
- 监控任务进展（”我是否已经成功抓起物体？”）
- 检测预测误差并触发纠正动作（”物体比预期的重，需要增加握力”）
- 更新物体的内部表征（”这个瓶子的重量是 500g”）
3.2.2 预测性控制与感觉衰减
论文中的一个精妙实验 (Bays, Wolpert & Flanagan, 2005) 展示了感知衰减 (perceptual attenuation) 与预测性控制的关联：
在敲击任务中，受试者右手指主动敲击放置于左手指上方的力传感器，同时一个力矩电机以可变延迟产生一个等价的测试敲击。受试者感知到的测试敲击强度在主动敲击前后约 100 ms 的窗口内被显著衰减。衰减的时间轮廓与接球实验中的预测性抓握力调控轮廓惊人地相似。
这表明：自我产生的、可预测的感觉事件被主动抑制，以增强外部产生事件的显著性——这是一个贯穿感知和行动的通用预测性处理原则。
3.2.3 物体动力学的表征与泛化
Flanagan 等人讨论了物体动力学表征的几个关键性质：
独立表征：操纵物体时，手臂运动指令和抓握力指令是独立控制的。Danion (2004) 和 Descoins et al. (2006) 的实验表明，附着在手臂上的弹性载荷不会影响抓握力，但附着在物体上的同样载荷会增加抓握力。这证明运动系统使用面向物体的内部模型 (object-specific internal models)。
表征坐标系：Mah & Mussa-Ivaldi (2003) 使用虚拟倒立摆平衡任务发现，学会的动力学不能在不同手臂位形间迁移，这表明新颖的动力学最初以内在坐标（肌肉/关节空间）编码。但随着充分练习是否能形成以物体为中心的抽象表征，仍然是个开放问题。
感知与动作的分离：Flanagan & Beltzner (2000) 的大小-重量错觉实验揭示了感知预测和感觉运动预测使用独立且分离的重量表征——即使手已经适应了物体的真实重量（抓握力的感觉运动预测是准确的），大小-重量错觉仍然顽固存在。
3.2.4 注视行为与学习
Sailer, Flanagan & Johansson (2005) 通过研究新颖视觉运动映射学习过程中的注视行为 (gaze behavior)，揭示了学习的三阶段结构：
| 阶段 | 注视行为 | 特征 |
|---|---|---|
| 探索阶段 (exploratory) | 注视”追逐”光标，反应式扫视 | 无法控制，试图发现映射规则 |
| 技能获取阶段 (acquisition) | 注视逐渐前摄，领先于光标 | 开始形成控制，前摄注视出现 |
| 技能精炼阶段 (refinement) | 注视锁定在目标位置 | 熟练执行，注视策略接近自然运动 |
这表明：前摄注视行为（注视领先于动作）是技能获取的一个可量化的行为标记，它反映了内部模型对行动后果的预测能力。
四、算法与代码实现
以下代码基于 NumPy/SciPy 实现，覆盖本章的三个核心算法：LQR、iLQR 和 MPC。所有实现均包含完整的线搜索和正则化机制。
4.1 离散时间 LQR
import numpy as np
from scipy.linalg import solve_discrete_are, solve
from typing import Tuple, List, Callable
def lqr_backward(
A: np.ndarray, # (T-1, n, n) 或 (n, n) 若时不变
B: np.ndarray, # (T-1, n, m) 或 (n, m)
Q: np.ndarray, # (T, n, n) 或 (n, n) -- 包含 Q_T
R: np.ndarray, # (T-1, m, m) 或 (m, m)
) -> Tuple[List[np.ndarray], List[np.ndarray]]:
"""离散时间 LQR 的向后递推（Riccati 方程）
对应 2.3.3 节的算法：
P_t = Q_t + A^T P_{t+1} A - A^T P_{t+1} B (R + B^T P_{t+1} B)^{-1} B^T P_{t+1} A
K_t = (R + B^T P_{t+1} B)^{-1} B^T P_{t+1} A
Args:
A: 系统矩阵，shape (n, n) 或 (T-1, n, n)
B: 输入矩阵，shape (n, m) 或 (T-1, n, m)
Q: 状态代价，shape (n, n) 或 (T, n, n) -- 索引 T 是 Q_T
R: 控制代价，shape (m, m) 或 (T-1, m, m)
Returns:
K_list: 反馈增益列表 [K_0, ..., K_{T-1}]，每个 shape (m, n)
P_list: Riccati 解列表 [P_0, ..., P_T]，每个 shape (n, n)
"""
# 自动处理广播维度
if A.ndim == 2:
T = Q.shape[0] if Q.ndim == 3 else 1
A = np.broadcast_to(A, (T, A.shape[0], A.shape[1]))
if B.ndim == 2:
B = np.broadcast_to(B, (T, B.shape[0], B.shape[1]))
if Q.ndim == 2:
Q = np.broadcast_to(Q, (T + 1, Q.shape[0], Q.shape[1]))
if R.ndim == 2:
R = np.broadcast_to(R, (T, R.shape[0], R.shape[1]))
T = A.shape[0]
n, m = B.shape[1], B.shape[2]
K_list = []
P = Q[T].copy() # 终端条件：P_T = Q_T
P_list = [P.copy()]
for t in range(T - 1, -1, -1):
# 2.3.3 节 Eq. (Riccati):
S = R[t] + B[t].T @ P @ B[t] # S_t = R_t + B^T P_{t+1} B
S_inv = np.linalg.inv(S)
K = S_inv @ B[t].T @ P @ A[t] # K_t = S_t^{-1} B^T P_{t+1} A_t
P = Q[t] + A[t].T @ P @ A[t] - A[t].T @ P @ B[t] @ K
# ^-- Q_t + A^T P_{t+1} (A_t - B_t K_t)
K_list.insert(0, K)
P_list.insert(0, P.copy())
return K_list, P_list
def lqr_forward(
x0: np.ndarray, # (n,) 初始状态
A: np.ndarray, # (T-1, n, n) 系统矩阵
B: np.ndarray, # (T-1, n, m) 输入矩阵
K_list: List[np.ndarray], # 反馈增益列表
) -> Tuple[List[np.ndarray], List[np.ndarray]]:
"""LQR 前向模拟：施加 u_t = -K_t x_t
Args:
x0: 初始状态
A: 系统矩阵序列
B: 输入矩阵序列
K_list: 反馈增益序列 [K_0, ..., K_{T-1}]
Returns:
x_traj: [x_0, ..., x_T]，状态轨迹
u_traj: [u_0, ..., u_{T-1}]，控制轨迹
"""
T = len(K_list)
x = x0.copy()
x_traj = [x.copy()]
u_traj = []
for t in range(T):
u = -K_list[t] @ x # u_t = -K_t x_t
u_traj.append(u)
x = A[t] @ x + B[t] @ u # x_{t+1} = A_t x_t + B_t u_t
x_traj.append(x.copy())
return x_traj, u_traj
def solve_infinite_horizon_lqr(
A: np.ndarray, # (n, n)
B: np.ndarray, # (n, m)
Q: np.ndarray, # (n, n) -- 半正定
R: np.ndarray, # (m, m) -- 正定
) -> Tuple[np.ndarray, np.ndarray]:
"""求解无限时域 LQR：代数 Riccati 方程
对应 2.3.4 节: P = Q + A^T P A - A^T P B (R + B^T P B)^{-1} B^T P A
使用 scipy 的 solve_discrete_are 进行数值稳定求解。
Returns:
P: Riccati 稳态解 (n, n)
K: 最优反馈增益 (m, n)
"""
P = solve_discrete_are(A, B, Q, R)
K = solve(R + B.T @ P @ B, B.T @ P @ A)
return P, K
4.2 迭代 LQR (iLQR)
def ilqr_solve(
x0: np.ndarray, # (n,) 初始状态
u_init: np.ndarray, # (T, m) 初始控制序列
dynamics: Callable, # dynamics(x, u) -> x_next
cost: Callable, # cost(x, u) -> float
cost_final: Callable, # cost_final(x) -> float
dynamics_jac: Callable, # dynamics_jac(x, u) -> (f_x, f_u)
cost_grad: Callable, # cost_grad(x, u) -> (ell_x, ell_u)
cost_hess: Callable, # cost_hess(x, u) -> (ell_xx, ell_uu, ell_ux)
max_iter: int = 100,
tol: float = 1e-6,
mu_init: float = 1.0,
mu_min: float = 1e-6,
mu_max: float = 1e10,
delta0: float = 2.0,
) -> Tuple[List[np.ndarray], List[np.ndarray], List[float]]:
"""迭代 LQR (iLQR) 求解器
对应 2.4.4 节的完整算法，包含自适应正则化和线搜索。
核心原理：沿标称轨迹将非线性动力学 f 一阶展开，
将代价二阶展开，在每个时间步求解局部 LQR 子问题。
Args:
x0: 初始状态
u_init: 初始控制序列 guess，shape (T, m)
dynamics: f(x, u) -> x_next
cost: ell(x, u) -> scalar
cost_final: ell_T(x) -> scalar
dynamics_jac: (x, u) -> (f_x (n,n), f_u (n,m))
cost_grad: (x, u) -> (ell_x (n,), ell_u (m,))
cost_hess: (x, u) -> (ell_xx (n,n), ell_uu (m,m), ell_ux (m,n))
max_iter: 最大迭代次数
tol: k_t 前馈增益的收敛阈值
mu_init: Levenberg-Marquardt 正则化初始值
mu_min, mu_max: 正则化参数边界
delta0: 期望代价下降因子
Returns:
x_traj: 状态轨迹 [x_0, ..., x_T]
u_traj: 控制轨迹 [u_0, ..., u_{T-1}]
cost_history: 每次迭代的总代价
"""
T = len(u_init)
n, m = x0.shape[0], u_init.shape[1]
# 初始化标称轨迹：用初始控制序列 rollout
x_bar = [x0.copy()]
for t in range(T):
x_bar.append(dynamics(x_bar[-1], u_init[t]))
u_bar = [u.copy() for u in u_init]
# 计算当前轨迹的总代价
def total_cost(xs, us):
J = sum(cost(xs[t], us[t]) for t in range(T)) + cost_final(xs[T])
return J
J_curr = total_cost(x_bar, u_bar)
cost_history = [J_curr]
mu = mu_init
for iteration in range(max_iter):
# === Forward Pass (阶段 1): 计算 Jacobians 和代价导数 ===
f_x_list, f_u_list = [], []
ell_x_list, ell_u_list = [], []
ell_xx_list, ell_uu_list, ell_ux_list = [], [], []
for t in range(T):
f_x, f_u = dynamics_jac(x_bar[t], u_bar[t])
ell_x, ell_u = cost_grad(x_bar[t], u_bar[t])
ell_xx, ell_uu, ell_ux = cost_hess(x_bar[t], u_bar[t])
f_x_list.append(f_x)
f_u_list.append(f_u)
ell_x_list.append(ell_x)
ell_u_list.append(ell_u)
ell_xx_list.append(ell_xx)
ell_uu_list.append(ell_uu)
ell_ux_list.append(ell_ux)
# 终端代价的 Hessian/gradient
ell_T_x = cost_grad(x_bar[T], None)[0] # 仅 x 梯度, u=None
ell_T_xx = cost_hess(x_bar[T], None)[0] # 仅 x Hessian
# === Backward Pass (阶段 2): 递推计算 k_t, K_t ===
V_x = ell_T_x.copy()
V_xx = ell_T_xx.copy()
k_list = [None] * T
K_list = [None] * T
diverge = False
for t in range(T - 1, -1, -1):
# 2.4.3 节 iLQR 的 Q 函数展开 (无动力学二阶项)
Q_x = ell_x_list[t] + f_x_list[t].T @ V_x
Q_u = ell_u_list[t] + f_u_list[t].T @ V_x
Q_xx = ell_xx_list[t] + f_x_list[t].T @ V_xx @ f_x_list[t]
Q_uu = ell_uu_list[t] + f_u_list[t].T @ V_xx @ f_u_list[t]
Q_ux = ell_ux_list[t] + f_u_list[t].T @ V_xx @ f_x_list[t]
# Levenberg-Marquardt 正则化 (2.4.3 节)
Q_uu_reg = Q_uu + mu * np.eye(m)
# 求解 k_t, K_t (2.4.2 节)
try:
Q_uu_inv = np.linalg.inv(Q_uu_reg)
except np.linalg.LinAlgError:
mu = min(mu * 10, mu_max)
diverge = True
break
k_list[t] = -Q_uu_inv @ Q_u # 前馈项
K_list[t] = -Q_uu_inv @ Q_ux # 反馈增益
# 价值函数更新 (2.4.2 节)
V_x = Q_x + K_list[t].T @ Q_uu @ k_list[t] # 正则化不参与 V 更新
V_xx = Q_xx + K_list[t].T @ Q_uu @ K_list[t]
if diverge or any(k is None for k in k_list):
continue
# === Forward Pass (阶段 3): 线搜索 (2.4.5 节) ===
alpha = 1.0 # 初始尝试 Newton 步长
accepted = False
for _ in range(10): # 最多 10 次回溯
x_new = [x0.copy()]
u_new = []
for t in range(T):
dx = x_new[-1] - x_bar[t]
u = u_bar[t] + alpha * k_list[t] + K_list[t] @ dx
u_new.append(u)
x_new.append(dynamics(x_new[-1], u))
J_new = total_cost(x_new, u_new)
if J_new < J_curr:
accepted = True
break
alpha *= 0.5
# === 接受/拒绝更新，调整正则化参数 ===
if accepted:
x_bar, u_bar = x_new, u_new
dJ = J_curr - J_new
J_curr = J_new
cost_history.append(J_curr)
# 如果代价显著下降，减小正则化 (更接近 Newton)
if dJ > 0:
mu = max(mu / delta0, mu_min)
# 收敛检查：前馈项的大小
max_k = max(np.max(np.abs(k)) for k in k_list)
if max_k < tol:
print(f"iLQR converged in {iteration + 1} iterations, "
f"final cost = {J_curr:.6f}")
break
else:
# 拒绝: 增加正则化 (更接近梯度下降)
mu = min(mu * delta0, mu_max)
if mu >= mu_max:
print(f"iLQR: mu reached maximum, stopping at iteration {iteration}")
break
return x_bar, u_bar, cost_history
4.3 模型预测控制 (MPC)
def mpc_controller(
x0: np.ndarray, # (n,) 初始状态
horizon: int, # 预测时域 H
num_steps: int, # 运行总步数
dynamics: Callable, # dynamics(x, u) -> x_next
cost: Callable, # cost(x, u) -> float
cost_final: Callable, # cost_final(x) -> float
dynamics_jac: Callable, # dynamics_jac(x, u) -> (f_x, f_u)
cost_grad: Callable, # cost_grad(x, u) -> (ell_x, ell_u)
cost_hess: Callable, # cost_hess(x, u) -> (ell_xx, ell_uu, ell_ux)
u_nominal: np.ndarray = None, # (m,) 标称控制（默认零）
ilqr_kwargs: dict = None,
) -> Tuple[List[np.ndarray], List[np.ndarray]]:
"""模型预测控制 (MPC) 求解器
对应 2.5.1 节的滚动时域优化框架。
在每个控制步 k:
1. 以当前状态 x_k 为起点
2. 求解 H 步 iLQR 子问题
3. 执行第一步控制 u_k^*
4. 测量新状态，滑动窗口
Args:
x0: 初始状态
horizon: 预测时域长度 H
num_steps: 实际执行的步数 T_actual
dynamics, cost, cost_final, dynamics_jac, cost_grad, cost_hess:
与 iLQR 相同的函数接口
u_nominal: 默认控制（默认零）
Returns:
x_traj: 实际状态轨迹
u_traj: 实际执行的控制序列
"""
if ilqr_kwargs is None:
ilqr_kwargs = dict(max_iter=20, tol=1e-4)
n = x0.shape[0]
m = dynamics_jac(x0, np.zeros(1))[1].shape[1] if u_nominal is None else u_nominal.shape[0]
default_u = np.zeros(m) if u_nominal is None else u_nominal
x = x0.copy()
x_traj = [x.copy()]
u_traj = []
for step in range(num_steps):
# Step 1 & 2: 求解 H 步 iLQR (2.5.1 节步骤 2)
# 用前一步优化解作为当前 warm start
u_warm = np.tile(default_u, (horizon, 1)) if step == 0 else u_plan[1:]
# 如果 warm start 不够长，补齐
if len(u_warm) < horizon:
u_warm = np.vstack([u_warm,
np.tile(default_u, (horizon - len(u_warm), 1))])
try:
x_plan, u_plan, _ = ilqr_solve(
x, u_warm[:horizon],
dynamics, cost, cost_final,
dynamics_jac, cost_grad, cost_hess,
**ilqr_kwargs
)
u = u_plan[0] # 仅执行第一步 (2.5.1 节步骤 3)
except Exception as e:
print(f"MPC step {step}: iLQR failed ({e}), using fallback control")
u = default_u.copy()
# Step 3: 施加控制
u_traj.append(u.copy())
x = dynamics(x, u) # 实际动力学（可能包含噪声/模型误差）
x_traj.append(x.copy())
# Step 4: 滑动窗口 -- 下一轮循环从新状态 x 开始
return x_traj, u_traj
4.4 完整示例：倒立摆 (Cart-Pole) 的 iLQR 控制
def cartpole_dynamics(x: np.ndarray, u: np.ndarray) -> np.ndarray:
"""倒立摆离散动力学 (半隐式欧拉积分)
状态 x = [position, velocity, angle, angular_velocity]
控制 u = [force] (施加在小车上的水平力)
连续动力学: M(q) q̈ + C(q, q̇) q̇ + G(q) = B u
"""
dt = 0.02 # 20ms 控制周期 (~50Hz)
mc, mp = 1.0, 0.1 # 小车质量, 摆杆质量
l = 0.5 # 摆杆半长
g = 9.81
pos, vel, ang, ang_vel = x
force = u[0]
s = np.sin(ang)
c_ang = np.cos(ang)
# 质量矩阵
m11 = mc + mp
m12 = mp * l * c_ang
m22 = mp * l * l + (1.0 / 3.0) * mp * l * l
# Coriolis
h2 = -mp * l * s * ang_vel**2
# 重力
g2 = -mp * g * l * s
# 求解加速度
det = m11 * m22 - m12 * m12
dd_pos = ( m22 * force - m12 * (-h2 - g2)) / det
dd_ang = (-m12 * force + m11 * (-h2 - g2)) / det
# 半隐式欧拉: 先更新速度，再更新位置
vel_next = vel + dt * dd_pos
ang_vel_next = ang_vel + dt * dd_ang
pos_next = pos + dt * vel_next
ang_next = ang + dt * ang_vel_next
return np.array([pos_next, vel_next, ang_next, ang_vel_next])
# ===== 使用示例 =====
if __name__ == "__main__":
# 1. 简单 LQR 测试: 双积分器系统
A = np.array([[1.0, 0.1], [0.0, 1.0]]) # dt=0.1 的双积分器
B = np.array([[0.005], [0.1]])
Q = np.diag([10.0, 1.0])
R = np.array([[0.1]])
P_inf, K_inf = solve_infinite_horizon_lqr(A, B, Q, R)
print(f"LQR steady-state gain K_inf = {K_inf}")
print(f"Closed-loop poles: {np.linalg.eigvals(A - B @ K_inf)}")
# 2. 有限时域 LQR 测试
T_horizon = 50
A_seq = np.tile(A, (T_horizon, 1, 1))
B_seq = np.tile(B, (T_horizon, 1, 1))
Q_seq = np.tile(Q, (T_horizon + 1, 1, 1))
R_seq = np.tile(R, (T_horizon, 1, 1))
K_list, P_list = lqr_backward(A_seq, B_seq, Q_seq, R_seq)
x0 = np.array([1.0, 0.0]) # 初始位置偏差 1.0
x_traj, u_traj = lqr_forward(x0, A_seq, B_seq, K_list)
print(f"Terminal state: {x_traj[-1]}")
print(f"Terminal position error: {x_traj[-1][0]:.6f}")
4.5 代码使用说明
LQR 的典型使用流程：
- 将机器人动力学在平衡点附近线性化，获得 \(A\), \(B\) 矩阵
- 设计代价矩阵 \(Q\)（状态误差惩罚）和 \(R\)（控制努力惩罚）
- 调用
lqr_backward获得反馈增益 \(K_t\) - 在前向循环中施加 \(u_t = -K_t (x_t - x_t^{\text{ref}})\)
iLQR 的典型使用流程：
- 提供
dynamics和cost的 Python 函数及其导数 - 给出初始猜测的控制序列
u_init（通常为零或上一次优化的解） - 调用
ilqr_solve获得优化后的轨迹 - 对于实时控制，结合 MPC 框架在每一步重新调用 iLQR
关键工程参数调优：
| 参数 | 初始值 | 增大效果 | 缩小效果 |
|---|---|---|---|
Q 对角元 |
10 (位置), 1 (速度) | 更激进的纠偏，可能振荡 | 更慢的收敛，更平滑 |
R 对角元 |
0.1 | 更保守的控制，更节能 | 更激进的控制，更多能量 |
mu_init |
1.0 | 初始更保守（类梯度下降） | 初始更激进（类 Newton 法） |
horizon (MPC) |
20-50 | 更好的全局规划，更多计算 | 更快的计算，更短视 |
五、核心 Takeaway 与延伸阅读
5.1 五个核心洞察
-
LQR = 最优控制的解析金标准 任何线性二次型问题都有唯一闭式解，通过 Riccati 方程向后递推获得。LQR 的线性反馈结构 \(u = -Kx\) 不是启发式的设计选择，而是最优性的必然结果——所有其他控制方法（MPC, RL）在局部都应逼近这一结构。
-
iLQR = 轨迹优化的事实标准 iLQR 通过沿标称轨迹的局部 LQR 子问题迭代逼近非线性最优控制问题的解。其关键优势是 (a) 解析计算 Q 函数的导数而非使用有限差分，(b) 每次迭代都产生一条动力学可行的轨迹（通过前向 rollout），(c) 自适应的 Levenberg-Marquardt 正则化使收敛高度鲁棒。
-
内部模型是运动控制的必要条件，不是可选项 从 Kawato 的力场适应实验到 Flanagan 的抓握力-负载力耦合，大量独立证据表明神经系统使用前向模型预测感觉后果、使用逆向模型生成前馈指令——这与工程控制中的观测器-控制器对偶完全一致。延迟使纯反馈控制不可能支撑快速运动。
-
MPC 弥合了离线规划与在线控制之间的鸿沟 MPC 的滚动时域优化思想——只求解有限步的未来，但每一步都重新求解——在实际机器人系统中比离线全时域规划更可靠。这是因为 (a) 模型误差不会无限累积，(b) 外部扰动可以被即时响应，(c) 计算延迟可以通过调整预测时域来管理。
-
接触事件是操作任务的”自然时钟” Flanagan 等人揭示了一个被工程控制理论长期忽视的事实：物体操作任务不是连续同质的运动，而是由离散的接触事件分割的阶段序列。这些接触事件在多个感觉模态中产生同步信号，是感觉运动系统对齐预测与现实的天然锚点。为操作任务设计控制器时，应该围绕接触事件而非时间序列组织控制逻辑。
5.2 开放问题与研究方向
- 非线性 MPC 的实时计算：对于高维系统（人形机器人，n ~ 50-100），即使 iLQR 的单次迭代也可能超出控制周期（1-5 ms）。研究方向包括：并行化 DDP (如 GPU-accelerated DDP)、基于学习的 warm start。
- 内部模型的表征格式：Mah & Mussa-Ivaldi (2003) 的实验表明新颖动力学最初以内在坐标编码，但大量练习后是否会形成抽象的、以物体为中心的表征仍然未知。
- 感觉运动控制点理论的形式化：Flanagan 等人的控制点假说目前主要是描述性的。如何将其形式化为可优化的数学模型（如混合系统或切换动力学）是一个有前景的方向。
- 信号依赖噪声与最优反馈控制：Todorov & Jordan (2002) 的最优反馈控制理论将信号依赖噪声纳入 LQR 框架，自然地解释了运动冗余、非对称刚度等生物运动特征——在机器人控制中尚未广泛应用。
5.3 推荐延伸阅读
| 文献 | 主题 | 相关性 |
|---|---|---|
| Tassa, Erez & Todorov (2012). Synthesis and stabilization of complex behaviors through online trajectory optimization. IROS. | iLQR + MPC 在类人机器人上的应用，本章代码的直接参考 | 核心 |
| Todorov & Jordan (2002). Optimal feedback control as a theory of motor coordination. Nature Neuroscience. | 最优反馈控制统一理论，将信号依赖噪声纳入 LQG 框架 | 核心 |
| Mayne, Rawlings, Rao & Scokaert (2000). Constrained model predictive control: Stability and optimality. Automatica. | MPC 稳定性的权威理论分析 | 参考 |
| Shadmehr & Mussa-Ivaldi (1994). Adaptive representation of dynamics during learning of a motor task. J. Neuroscience. | 力场适应的经典实验，Kawato 综述的核心证据 | 参考 |
| Tedrake (2023). Underactuated Robotics. MIT Course Notes (Ch. 10-13). | 轨迹优化与 DDP 的教科书级讲解 | 参考 |
| Wolpert, Diedrichsen & Flanagan (2011). Principles of sensorimotor learning. Nature Reviews Neuroscience. | 感觉运动学习的综合综述，MPFIM 框架的延续 | 延伸 |
六、练习与思考
6.1 基础练习
练习 1: Riccati 方程的手工推导 给定一维系统 \(x_{t+1} = a x_t + b u_t\)，代价 \(J = \sum_{t=0}^{T-1} (q x_t^2 + r u_t^2) + q_T x_T^2\)。 (a) 设价值函数 \(V_t(x) = p_t x^2\)，写出 \(p_t\) 关于 \(p_{t+1}\) 的递推关系。 (b) 证明 \(\lim_{T\to\infty} p_0\) 存在，并给出稳态 \(K_{\infty}\) 的表达式。 (c) 当 \(a=1, b=1, q=1, r=1\) 时，计算 \(K_{\infty}\)。
练习 2: LQR 权重灵敏度分析 对双积分器系统 \(A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}\), \(B = \begin{bmatrix} 0 \\ \Delta t \end{bmatrix}\)，\(\Delta t = 0.1\)，分析代价权重 \((q_{\text{pos}}, q_{\text{vel}}, r)\) 对闭环极点位置的影响。画出 \(K_{\infty}\) 的两个分量随 \(q_{\text{pos}}/r\) 比值变化的曲线，解释物理意义。
6.2 编程练习
练习 3: 实现 DDP（全二阶版本）
在提供的 iLQR 代码基础上，增加动力学二阶导数项 (\(f_{xx}\), \(f_{uu}\), \(f_{ux}\))，实现完整的 DDP。对倒立摆系统比较 iLQR 和 DDP 的收敛速度（迭代次数和总时间）。
提示：动力学 Hessian \(f_{xx}\) 是 \((n, n, n)\) 三阶张量，其与价值函数梯度的缩并 \(\sum_k V_{x,k} f_{xx,k}\) 等价于 np.tensordot(V_x, f_xx, axes=1)。
练习 4: MPC 预测时域的敏感性分析 对倒立摆平衡任务（初始角度 \(\theta_0 = 0.2\) rad），分别使用 \(H = 5, 10, 20, 50, 100\) 的预测时域运行 MPC，记录： (a) 单步平均计算时间 (b) 平衡性能 (RMS angle error) (c) 总控制能量 \(\sum u_t^2\) 绘制性能-计算时间的权衡曲线。是否存在一个”甜点”（sweet spot）时域？
练习 5: 反馈误差学习的数值模拟 实现一个简化的反馈误差学习模拟：
- 被控对象：一维质点 \(m\ddot{x} = u\)（真实动力学），离散化 \(\Delta t = 0.01\)
- 逆向模型：\(\hat{u} = \hat{m} \cdot \ddot{x}_d\)，初始估计 \(\hat{m} = 0\)（完全不了解系统）
- 反馈控制器：\(u_{\text{fb}} = K_p (x_d - x) + K_d (\dot{x}_d - \dot{x})\)
- 目标轨迹：\(x_d(t) = \sin(2\pi t)\) (a) 验证 \(\hat{m}\) 是否收敛到真实值 \(m\) (b) 分析反馈增益 \((K_p, K_d)\) 对学习速度的影响 (c) 在 \(m=1\) 训练后，将真实质量突然变为 \(m=2\)，观察模型的重新适应过程
6.3 论文思考题
练习 6: 内部模型的必要性（延迟分析） Kawato (1999, Fig 1) 论证反馈延迟使纯反馈控制不可行。假设反馈延迟为 \(\tau = 50\) ms，被控对象是质量为 \(m=1\) kg 的质点在粘性阻尼 \(b=0.5\) Ns/m 下运动： \(\ddot{x} + b\dot{x} = u(t - \tau)\) (a) 对于频率为 \(f\) 的正弦参考轨迹，在什么频率以上，延迟导致的相位滞后超过 \(90^\circ\)？ (b) 如果必须在该频率以上运行，前向模型需要多精确才能将相位误差补偿到 \(10^\circ\) 以内？
练习 7: 感觉运动控制点的形式化 Flanagan et al. (2006) 提出接触事件是操作任务的控制点。考虑”拾起水瓶、打开瓶盖、喝水、盖上瓶盖、放回桌子”的完整任务序列： (a) 识别该任务中所有的接触事件（至少 5 个） (b) 标注每个接触事件在哪些感觉模态中被编码（触觉/视觉/听觉/本体感觉） (c) 参考 MPFIM 框架，设计一个基于接触事件检测的阶段切换有限状态机（状态 + 转移条件）
练习 8: 信号依赖噪声与轨迹曲率 Harris & Wolpert (1998) 的最小方差模型预测轨迹曲率 \(\kappa\) 与角速度 \(\omega\) 满足 \(2/3\) 幂律关系：\(\kappa \propto \omega^{-2/3}\)。假设控制信号噪声方差 \(\sigma_u^2 = \alpha \vert u\vert ^2\)： (a) 推导终点方差 \(\text{Var}(x_T)\) 关于控制序列 \(\{u_t\}\) 的表达式 (b) 在给定总控制能量约束下，证明最优控制序列随接近终点而减小幅度 (c) 讨论这个结果与”速度-精度权衡”（Fitts’ Law）的关系
章节贡献: 本章的 LQR/iLQR/MPC 代码实现可以作为独立库使用。完整的倒立摆和双积分器测试用例可用
pip install numpy scipy后直接运行。代码文件路径:chapters/ch08_机器人动力学与控制.md