# 第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: 运动学、动力学）。现在我们面对的是一个从理论到实践的桥梁问题： > 给定一个已知动力学模型的机器人系统，如何计算使其从初始状态到达目标状态的最优控制信号？ 这个问题有三个层次： 1. **无约束最优控制**：系统是线性的，代价是二次的——引出 **LQR**（Linear Quadratic Regulator），这是所有最优控制方法的理论基石。 2. **非线性轨迹优化**：系统是非线性的，但通过沿标称轨迹的局部线性化迭代求解——引出 **DDP/iLQR**（Differential Dynamic Programming / iterative LQR），这是现代机器人轨迹优化的工业标准。 3. **在线重规划**：执行过程中不断以当前状态为起点重新求解有限时域问题——引出 **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 都是**开环离线规划**方法——它们一次性生成长时域的控制序列，然后执行。这在机器人实际运行中面临两个致命问题： 1. **模型误差累积**：长时间的开环执行使动力学建模误差不断累积。 2. **计算延迟**：长时域规划的计算时间可能超过控制周期。 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 闭环系统是渐近稳定的： 1. **终端代价** $$\phi(x)$$ 是无限时域最优代价的**局部上界**：$$\phi(x) \geq V_{\infty}(x) \; \forall x \in \mathcal{X}_f$$ 2. **终端约束集** $$\mathcal{X}_f$$ 在局部控制器下是**正不变的** (positive invariant) 3. **终端控制器**使 $$\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 架构**：维护多个模块，每个模块包含三个组件： 1. **逆向模型**：产生该模块的前馈运动指令 2. **前向模型**：预测该模块的运动指令的感觉后果 3. **责任预测器** (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)$$（运动学量） - **计算过程**：方差依赖于运动指令的幅度（信号依赖噪声），因此最优轨迹取决于**动力学**特性 $$x_T = \int_0^T dt \int_0^t ds \, H(t-s) \cdot [u(s) + \xi(s)]$$ 其中 $$\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 核心论点 论文的核心论点是三重的： 1. **内部模型是真实存在于神经系统中的计算实体**，不是控制理论家的虚构。 2. **小脑是内部模型的主要存储位置**，其独特的神经回路结构（苔状纤维 → 平行纤维 → Purkinje 细胞 → 深部核团）天然适合监督学习。 3. **内部模型在运动控制和轨迹规划中都是必需的**，反馈控制由于延迟限制无法独立支撑快速协调运动。 #### 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 的框架对机器人控制有三层直接启示： 1. **前向模型用于预测**（等价于现代 MPC 中的 rollout simulator） 2. **逆向模型用于前馈**（等价于计算力矩控制） 3. **学习由反馈误差驱动**（等价于自适应控制中的参数更新律） --- ### 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) 这些接触事件在**多个感觉模态**（触觉、视觉、听觉、本体感觉）中同时产生离散的感觉信号。通过比较预测的感觉事件与实际的感觉事件，运动系统可以： 1. 监控任务进展（"我是否已经成功抓起物体？"） 2. 检测预测误差并触发纠正动作（"物体比预期的重，需要增加握力"） 3. 更新物体的内部表征（"这个瓶子的重量是 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 ```python 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) ```python 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) ```python 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 控制 ```python 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 的典型使用流程**： 1. 将机器人动力学在平衡点附近线性化，获得 $$A$$, $$B$$ 矩阵 2. 设计代价矩阵 $$Q$$（状态误差惩罚）和 $$R$$（控制努力惩罚） 3. 调用 `lqr_backward` 获得反馈增益 $$K_t$$ 4. 在前向循环中施加 $$u_t = -K_t (x_t - x_t^{\text{ref}})$$ **iLQR 的典型使用流程**： 1. 提供 `dynamics` 和 `cost` 的 Python 函数及其导数 2. 给出初始猜测的控制序列 `u_init`（通常为零或上一次优化的解） 3. 调用 `ilqr_solve` 获得优化后的轨迹 4. 对于实时控制，结合 MPC 框架在每一步重新调用 iLQR **关键工程参数调优**： | 参数 | 初始值 | 增大效果 | 缩小效果 | |------|--------|---------|---------| | `Q` 对角元 | 10 (位置), 1 (速度) | 更激进的纠偏，可能振荡 | 更慢的收敛，更平滑 | | `R` 对角元 | 0.1 | 更保守的控制，更节能 | 更激进的控制，更多能量 | | `mu_init` | 1.0 | 初始更保守（类梯度下降） | 初始更激进（类 Newton 法） | | `horizon` (MPC) | 20-50 | 更好的全局规划，更多计算 | 更快的计算，更短视 | --- ## 五、核心 Takeaway 与延伸阅读 ### 5.1 五个核心洞察 1. **LQR = 最优控制的解析金标准** 任何线性二次型问题都有唯一闭式解，通过 Riccati 方程向后递推获得。LQR 的线性反馈结构 $$u = -Kx$$ 不是启发式的设计选择，而是**最优性**的必然结果——所有其他控制方法（MPC, RL）在局部都应逼近这一结构。 2. **iLQR = 轨迹优化的事实标准** iLQR 通过沿标称轨迹的局部 LQR 子问题迭代逼近非线性最优控制问题的解。其关键优势是 (a) 解析计算 Q 函数的导数而非使用有限差分，(b) 每次迭代都产生一条动力学可行的轨迹（通过前向 rollout），(c) 自适应的 Levenberg-Marquardt 正则化使收敛高度鲁棒。 3. **内部模型是运动控制的必要条件，不是可选项** 从 Kawato 的力场适应实验到 Flanagan 的抓握力-负载力耦合，大量独立证据表明神经系统使用前向模型预测感觉后果、使用逆向模型生成前馈指令——这与工程控制中的观测器-控制器对偶完全一致。延迟使纯反馈控制不可能支撑快速运动。 4. **MPC 弥合了离线规划与在线控制之间的鸿沟** MPC 的滚动时域优化思想——只求解有限步的未来，但每一步都重新求解——在实际机器人系统中比离线全时域规划更可靠。这是因为 (a) 模型误差不会无限累积，(b) 外部扰动可以被即时响应，(c) 计算延迟可以通过调整预测时域来管理。 5. **接触事件是操作任务的"自然时钟"** 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` ---
← 第07章 运动控制的发展视角
↑ 目录
第09章 计算神经科学与预测控制 →