第10章：机器人非线性MPC

10.1 引言

在上一章中，我们探讨了线性模型预测控制（MPC）的基本原理和实现方法。然而，机器人系统本质上是高度非线性的——从旋转运动学的SO(3)流形结构，到接触力学的非光滑特性，再到执行器的饱和约束，这些非线性因素使得线性MPC在许多实际应用中显得力不从心。本章将深入探讨非线性MPC（NMPC）在机器人控制中的理论与实践，特别关注如何在保证控制性能的同时实现实时计算。

学习目标

通过本章学习，读者将能够：

理解非线性MPC的数学框架和求解算法
掌握实时迭代方案的设计与实现
处理机器人系统中的接触约束和混合动力学
设计和调试适用于实际机器人平台的NMPC控制器

10.2 非线性MPC算法

10.2.1 非线性MPC问题表述

非线性MPC是处理复杂机器人系统的核心技术，它能够直接考虑系统的非线性动力学特性，包括旋转运动的非线性、接触动力学的不连续性以及执行器的饱和约束。与线性MPC相比，NMPC能够在更大的运行范围内保持良好的控制性能，这对于执行动态运动的机器人至关重要。

考虑一个一般的非线性离散时间系统：

$$\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)$$ 其中$\mathbf{x}_k \in \mathbb{R}^{n_x}$为状态，$\mathbf{u}_k \in \mathbb{R}^{n_u}$为控制输入。非线性MPC在每个时间步求解以下有限时域优化问题： $$ \begin{align} \min_{\mathbf{u}_0, \ldots, \mathbf{u}_{N-1}} \quad & \sum_{k=0}^{N-1} l(\mathbf{x}_k, \mathbf{u}_k) + l_f(\mathbf{x}_N) \\ \text{s.t.} \quad & \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k), \quad k = 0, \ldots, N-1 \\ & \mathbf{x}_0 = \mathbf{x}(t) \\ & \mathbf{x}_k \in \mathcal{X}, \quad k = 1, \ldots, N \\ & \mathbf{u}_k \in \mathcal{U}, \quad k = 0, \ldots, N-1 \\ & \mathbf{x}_N \in \mathcal{X}_f \end{align} $$ 其中$l(\cdot, \cdot)$为阶段成本，$l_f(\cdot)$为终端成本，$\mathcal{X}$和$\mathcal{U}$分别为状态和控制约束集，$\mathcal{X}_f$为终端约束集。终端约束集的设计对保证递归可行性和稳定性至关重要。

对于机器人系统，成本函数的设计需要平衡多个目标：

跟踪性能： $$l_{\text{track}}(\mathbf{x}_k, \mathbf{x}_k^{\text{ref}}) = (\mathbf{x}_k - \mathbf{x}_k^{\text{ref}})^T\mathbf{Q}(\mathbf{x}_k - \mathbf{x}_k^{\text{ref}})$$ 其中权重矩阵$\mathbf{Q}$的选择反映了不同状态分量的相对重要性。对于四足机器人，通常位置误差的权重高于速度误差，而姿态误差（特别是横滚和俯仰）的权重最高，以保持平衡。

控制努力与能量消耗： $$l_{\text{control}}(\mathbf{u}_k) = \mathbf{u}_k^T\mathbf{R}\mathbf{u}_k$$ 权重$\mathbf{R}$不仅影响能量消耗，还影响执行器的磨损。实践中，$\mathbf{R}$常根据执行器的额定功率进行归一化。

控制平滑性： $$l_{\text{smooth}}(\mathbf{u}_k, \mathbf{u}_{k-1}) = (\mathbf{u}_k - \mathbf{u}_{k-1})^T\mathbf{S}(\mathbf{u}_k - \mathbf{u}_{k-1})$$ 这一项对减少机械振动和延长硬件寿命至关重要。过大的控制变化率会激发结构共振，导致不稳定。

约束处理的细节：

状态约束$\mathcal{X}$通常包括：

关节限位：$q_{\min} \leq q \leq q_{\max}$
速度限制：$|\dot{q}| \leq v_{\max}$
工作空间约束：避免自碰撞和环境碰撞
稳定性约束：如零力矩点（ZMP）必须在支撑多边形内

控制约束$\mathcal{U}$包括：

力矩限制：$|\tau_i| \leq \tau_{\max,i}$
功率限制：$\mathbf{u}^T\mathbf{M}_{\text{power}}\mathbf{u} \leq P_{\max}$
摩擦锥约束：$|\mathbf{f}_t| \leq \mu|\mathbf{f}_n|$（对于接触力控制）

机器人系统的非线性主要来源于：

运动学非线性：旋转矩阵的SO(3)群结构
动力学非线性：科里奥利力、离心力项
接触非线性：接触/非接触的切换、摩擦的非光滑性
执行器非线性：死区、饱和、迟滞效应

10.2.2 序列二次规划（SQP）方法

序列二次规划（SQP）是求解大规模非线性优化问题的最有效方法之一，在机器人NMPC中得到广泛应用。SQP的核心思想是通过求解一系列二次规划子问题来逼近原始非线性问题的解，每个QP子问题都是原问题在当前迭代点的局部二次近似。

理论基础

SQP方法基于牛顿法的思想，但扩展到了约束优化问题。考虑一般形式的非线性规划问题，其KKT条件为： $$ \begin{align} \nabla_x \mathcal{L}(x, \lambda, \mu) &= 0 \\ g(x) &= 0 \\ h(x) &\leq 0 \\ \mu &\geq 0 \\ \mu^T h(x) &= 0 \end{align} $$ 其中$\mathcal{L}$为拉格朗日函数。SQP通过牛顿迭代求解这个非线性方程组。

具体实现

给定当前轨迹$(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)$，首先对系统动力学进行线性化： $$ \mathbf{A}_k = \frac{\partial f}{\partial \mathbf{x}}\Big|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}, \quad \mathbf{B}_k = \frac{\partial f}{\partial \mathbf{u}}\Big|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)} $$ 线性化后的动力学约束变为： $$\delta\mathbf{x}_{k+1} = \mathbf{A}_k\delta\mathbf{x}_k + \mathbf{B}_k\delta\mathbf{u}_k + \mathbf{c}_k$$ 其中$\mathbf{c}_k = f(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k) - \mathbf{A}_k\bar{\mathbf{x}}_k - \mathbf{B}_k\bar{\mathbf{u}}_k$为线性化误差补偿项。

对成本函数进行二阶泰勒展开： $$ l(\mathbf{x}_k, \mathbf{u}_k) \approx l(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k) + \mathbf{g}_x^T\delta\mathbf{x}_k + \mathbf{g}_u^T\delta\mathbf{u}_k + \frac{1}{2}\delta\mathbf{x}_k^T\mathbf{Q}_k\delta\mathbf{x}_k + \frac{1}{2}\delta\mathbf{u}_k^T\mathbf{R}_k\delta\mathbf{u}_k + \delta\mathbf{x}_k^T\mathbf{P}_k\delta\mathbf{u}_k $$ 其中：

梯度：$\mathbf{g}_x = \nabla_x l|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}$，$\mathbf{g}_u = \nabla_u l|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}$
Hessian：$\mathbf{Q}_k = \nabla_{xx}^2 l|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}$，$\mathbf{R}_k = \nabla_{uu}^2 l|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}$
交叉项：$\mathbf{P}_k = \nabla_{xu}^2 l|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}$

这样得到的QP子问题具有稀疏块结构，可以利用专门的求解器高效求解。

算法10.1：实用SQP算法（带全局化策略）

输入：初始轨迹(x̄₀,...,x̄ₙ, ū₀,...,ūₙ₋₁)，容差ε
输出：最优轨迹(x*₀,...,x*ₙ, u*₀,...,u*ₙ₋₁)

1. 初始化：iter ← 0, μ ← μ₀（罚参数）
2. while ‖∇L‖ > ε and iter < max_iter do
3.     // 准备阶段
4.     计算雅可比矩阵 Aₖ, Bₖ, k=0,...,N-1
5.     计算Hessian矩阵 Qₖ, Rₖ, Pₖ
6.     构建QP子问题矩阵
7.     
8.     // QP求解阶段
9.     求解QP得到搜索方向(δx, δu)
10.    
11.    // 线搜索阶段
12.    α ← 1
13.    while not满足Armijo条件 do
14.        α ← βα  (β ∈ (0,1), 典型值0.5)
15.        if α < α_min then
16.            增大罚参数μ，重新求解QP
17.            break
18.        end if
19.    end while
20.    
21.    // 更新阶段
22.    (x̄, ū) ← (x̄, ū) + α(δx, δu)
23.    更新拉格朗日乘子
24.    iter ← iter + 1
25. end while

Hessian近似策略

精确计算Hessian矩阵计算量大且可能导致数值问题。实践中常用的近似策略包括：

Gauss-Newton近似：忽略拉格朗日函数的二阶项，仅保留目标函数的二阶信息： $$\mathbf{H}_{\text{GN}} = \mathbf{J}^T\mathbf{J}$$ 其中$\mathbf{J}$为残差的雅可比矩阵。这种近似保证了Hessian的正定性。

BFGS更新：通过递推公式更新Hessian近似： $$\mathbf{H}_{k+1} = \mathbf{H}_k + \frac{\mathbf{y}_k\mathbf{y}_k^T}{\mathbf{y}_k^T\mathbf{s}_k} - \frac{\mathbf{H}_k\mathbf{s}_k\mathbf{s}_k^T\mathbf{H}_k}{\mathbf{s}_k^T\mathbf{H}_k\mathbf{s}_k}$$ 其中$\mathbf{s}_k = \mathbf{x}_{k+1} - \mathbf{x}_k$，$\mathbf{y}_k = \nabla f_{k+1} - \nabla f_k$。

有限内存L-BFGS：仅存储最近m次更新的向量对$\{(\mathbf{s}_i, \mathbf{y}_i)\}$，适合大规模问题。

10.2.3 内点法与障碍函数

内点法是处理不等式约束的强大工具，在机器人NMPC中特别适合处理复杂的状态和控制约束。与活动集方法不同，内点法始终保持在可行域的内部，通过障碍函数逐步逼近约束边界。

对数障碍函数方法

对于不等式约束$g(\mathbf{x}, \mathbf{u}) \leq 0$，内点法将原问题转化为一系列无约束问题： $$ \min_{\mathbf{x}, \mathbf{u}} f(\mathbf{x}, \mathbf{u}) - \mu \sum_{i=1}^{m} \log(-g_i(\mathbf{x}, \mathbf{u})) $$ 其中$\mu > 0$为障碍参数。对数障碍函数具有以下性质：

当$g_i \to 0^-$时，$-\log(-g_i) \to +\infty$，阻止解接触约束边界
障碍函数在可行域内部是光滑的，便于使用梯度方法
随着$\mu \to 0$，障碍问题的解收敛到原问题的解

中心路径与路径跟随

内点法的理论基础是中心路径的概念。对于每个$\mu > 0$，存在唯一的最优解$(\mathbf{x}^*(\mu), \mathbf{u}^*(\mu))$，这些解构成的轨迹称为中心路径。路径跟随策略通过逐步减小$\mu$沿着中心路径逼近最优解：

算法10.2：原始-对偶内点法

输入：初始点(x₀, u₀, λ₀, s₀)，初始障碍参数μ₀
输出：最优解(x*, u*)

1. k ← 0, μ ← μ₀
2. while μ > μ_tol do
3.     // 求解障碍子问题的KKT系统
4.     repeat
5.         计算原始-对偶残差：
6.         r_dual = ∇f + J_g^T λ
7.         r_primal = g + s
8.         r_comp = Λs - μe  // 互补性条件
9.         
10.        // 求解牛顿方向
11.        [Δx]   [H    J_g^T  0 ] [-r_dual ]
12.        [Δλ] = [J_g   0     I ]·[-r_primal]
13.        [Δs]   [0    S     Λ ] [-r_comp  ]
14.        
15.        // 步长选择（保持严格可行性）
16.        α_primal = min(1, 0.995·min{-s_i/Δs_i : Δs_i < 0})
17.        α_dual = min(1, 0.995·min{-λ_i/Δλ_i : Δλ_i < 0})
18.        
19.        // 更新变量
20.        (x, u) ← (x, u) + α_primal·(Δx, Δu)
21.        λ ← λ + α_dual·Δλ
22.        s ← s + α_primal·Δs
23.    until 收敛
24.    
25.    // 更新障碍参数
26.    σ = (s^T λ)/(m·μ)  // 中心性度量
27.    μ ← max(μ_min, min(σ·μ, 0.2·μ))
28.    k ← k + 1
29. end while

其中$\mathbf{H}$为拉格朗日函数的Hessian，$\mathbf{J}_g$为约束的雅可比，$\mathbf{S} = \text{diag}(s)$，$\boldsymbol{\Lambda} = \text{diag}(\lambda)$。

实现细节与数值稳定性

预条件子设计： KKT系统通常病态，需要精心设计的预条件子。对于机器人系统，利用动力学方程的块结构： $$ \mathbf{P} = \begin{bmatrix} \mathbf{H}_{11}^{-1} & 0 & 0 \\ 0 & \mathbf{H}_{22}^{-1} & 0 \\ 0 & 0 & (\boldsymbol{\Lambda}^{-1}\mathbf{S})^{-1} \end{bmatrix} $$ Mehrotra预测-校正策略：

预测步：求解仿射方向（$\mu = 0$）
计算最大步长和中心性参数
校正步：加入中心性和高阶校正项
组合方向：预测方向 + 校正方向

这种策略可以显著减少迭代次数。

内点法的优势在机器人控制中尤为明显：

光滑收敛：避免了活动集切换导致的抖动
温启动友好：前一时刻的解通常是很好的初始点
并行潜力：KKT系统的块结构便于并行分解
数值稳定：避免了退化情况下的数值困难

10.2.4 多重打靶法

多重打靶法是一种并行化的轨迹优化方法，特别适合长时域的机器人运动规划问题。其核心思想是将长轨迹分解为多个短段，每段可以独立优化，然后通过协调机制确保段间连续性。

问题分解

考虑时域$[0, T]$，将其划分为$M$个子区间：$[t_0, t_1], [t_1, t_2], \ldots, [t_{M-1}, t_M]$。每个子区间内的优化问题： $$ \min_{x_i, u_i} \int_{t_{i-1}}^{t_i} l(x_i(\tau), u_i(\tau))d\tau $$ 受动力学约束： $$\dot{x}_i = f(x_i, u_i), \quad t \in [t_{i-1}, t_i]$$ 段间连续性约束： $$x_i(t_i) = x_{i+1}(t_i), \quad i = 1, \ldots, M-1$$

协调机制

增广拉格朗日方法： $$ \mathcal{L}_\rho = \sum_{i=1}^{M} J_i(x_i, u_i) + \sum_{i=1}^{M-1} \left[ \lambda_i^T(x_i(t_i) - x_{i+1}(t_i)) + \frac{\rho}{2}|x_i(t_i) - x_{i+1}(t_i)|^2 \right] $$ ADMM迭代框架：

多重打靶ADMM算法：

1. 并行阶段：每个处理器i求解
   min J_i(x_i, u_i) + λ_i^T x_i(t_i) - λ_{i-1}^T x_i(t_{i-1})

       + (ρ/2)‖x_i(t_i) - z_i‖² + (ρ/2)‖x_i(t_{i-1}) - z_{i-1}‖²

2. 协调阶段：更新接口变量
   z_i = (x_i(t_i) + x_{i+1}(t_i))/2

3. 对偶更新：
   λ_i ← λ_i + ρ(x_i(t_i) - z_i)

并行效率分析

多重打靶法的加速比取决于：

段数M：理论上加速比接近M，但通信开销会降低效率
子问题复杂度：每段的状态维数和时间步数
收敛速度：ADMM的收敛率为$O(1/k)$，可通过加速技术改进

实践中的分段策略：

腿式机器人步态周期分解：
|--站立相--|--摆动相--|--站立相--|--摆动相--|
     GPU1       GPU2       GPU3       GPU4
       ↓         ↓          ↓          ↓
    局部MPC   局部MPC    局部MPC    局部MPC
       ↓         ↓          ↓          ↓
    [通信与协调：确保相位转换的连续性]

这种方法特别适合于：

周期性运动：利用步态的重复结构
长时域规划：如10秒以上的复杂动作序列
GPU加速：每段可以在独立GPU上求解

10.3 实时迭代方案

10.3.1 实时迭代（RTI）框架

实时迭代是一种专门为在线优化设计的方法，其核心思想是：不等待完全收敛，而是在每个控制周期执行固定数量的优化迭代。

RTI框架包含两个阶段：

准备阶段（在获得新测量之前）：

基于预测状态进行线性化
构建和因子化KKT矩阵
预计算所有可以离线完成的操作

反馈阶段（获得新测量后）：

更新初始条件约束
求解线性系统得到控制增量
应用第一个控制输入

这种分离使得反馈延迟最小化，通常可以达到亚毫秒级。

10.3.2 热启动策略

有效的热启动对NMPC的实时性能至关重要。常用策略包括：

移位初始化：

新预测时域：
[u₁, u₂, ..., uₙ₋₁, uₙ₋₁]  # 复制最后一个控制
[x₁, x₂, ..., xₙ, f(xₙ, uₙ₋₁)]  # 前向积分

轨迹库方法：维护一个典型运动模式的轨迹库，根据当前状态选择最接近的轨迹作为初始猜测。

学习基热启动：使用神经网络学习从状态到最优轨迹的映射： $$(\mathbf{x}_0^*, \ldots, \mathbf{x}_N^*, \mathbf{u}_0^*, \ldots, \mathbf{u}_{N-1}^*) = \pi_\theta(\mathbf{x}(t))$$

10.3.3 部分收敛与次优性分析

RTI产生的次优解需要理论保证。关键结果是：

定理10.1（收缩性）：在适当的假设下，RTI方案产生的闭环系统是渐近稳定的，且跟踪误差满足： $$|\mathbf{e}_{k+1}| \leq \rho |\mathbf{e}_k| + \epsilon$$ 其中$\rho < 1$为收缩率，$\epsilon$为次优性导致的稳态误差界。

实践中，我们通过以下指标监控RTI性能：

KKT残差范数
约束违反度
预测与实际轨迹的偏差

10.3.4 延迟补偿技术

计算延迟是实时控制的主要挑战。假设计算延迟为$\tau$，简单的延迟补偿策略是：

预测补偿：在时刻$t$，我们预测$t+\tau$时刻的状态： $$\hat{\mathbf{x}}(t+\tau) = f_{\text{pred}}(\mathbf{x}(t), \mathbf{u}(t-\delta t), \tau)$$ 然后基于预测状态求解MPC问题。

多率方案：

高频率（1kHz）：简单反馈控制器
    ↑
    补偿信号
    ↑
低频率（100Hz）：NMPC优化器

10.4 接触隐式MPC

10.4.1 接触动力学的隐式表述

传统的接触处理方法需要预先指定接触模式序列，这限制了控制器的适应性。接触隐式方法将接触力作为优化变量，自动确定接触模式。

考虑带接触的机器人动力学： $$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{S}^T\mathbf{u} + \mathbf{J}_c^T(\mathbf{q})\mathbf{f}_c$$ 其中$\mathbf{J}_c$为接触雅可比，$\mathbf{f}_c$为接触力。接触约束通过互补性条件表达： $$ \begin{align} 0 \leq \phi(\mathbf{q}) \perp \mathbf{f}_n \geq 0 \quad &\text{（法向）} \\ |\mathbf{f}_t| \leq \mu \mathbf{f}_n \quad &\text{（摩擦锥）} \end{align} $$ 其中$\phi(\mathbf{q})$为接触距离函数。

10.4.2 互补性约束处理

互补性约束$0 \leq a \perp b \geq 0$（即$a \geq 0, b \geq 0, ab = 0$）是非光滑的，直接优化困难。常用处理方法：

松弛方法： $$ab \leq \epsilon$$ Fischer-Burmeister函数： $$\Phi(a, b) = \sqrt{a^2 + b^2} - a - b = 0$$ 光滑化NCP函数： $$\Phi_\mu(a, b) = a + b - \sqrt{a^2 + b^2 + 2\mu^2} = 0$$ 随着$\mu \to 0$，光滑化函数收敛到原互补性条件。

10.4.3 光滑化方法与松弛技术

在实际实现中，我们通常采用延拓法（continuation method）逐步减小光滑化参数：

算法10.2：光滑化互补性MPC

输入：初始光滑化参数μ₀，收缩因子ρ ∈ (0,1)
输出：最优轨迹

1. μ ← μ₀
2. while μ > μ_min do
3.     求解光滑化NMPC问题
4.     使用解作为下一次迭代的热启动
5.     μ ← ρ × μ
6. end while

摩擦锥约束的处理更加复杂。常用的方法是多面体近似：

摩擦锥近似（俯视图）：
     |
   / | \
  /  |  \     真实圆锥
 /___|___\
     |
  正方形     六边形      八边形
  (4面)      (6面)       (8面)

每增加一个面，优化变量增加但近似精度提高。实践中，4-6个面通常足够。

10.4.4 混合系统MPC

接触模式切换使系统成为混合系统。我们可以用有限状态机描述：

状态机示例（单腿）：
    ┌─────────┐
    │  飞行   │←─────┐
    └────┬────┘      │
         │离地       │
         ↓           │
    ┌─────────┐      │
    │  接触   │──────┘
    └─────────┘   抬腿

混合MPC的关键挑战是模式序列的组合爆炸。实用方法包括：

固定模式序列：预定义可行的接触序列，如四足机器人的标准步态。

分支MPC：考虑有限的模式分支：

时刻k:  当前模式
         /    \
时刻k+1: 模式A  模式B
        / \    / \
时刻k+2: ...  ...

隐式整数规划：引入二进制变量$\gamma \in \{0,1\}$表示接触状态： $$ \begin{align} \mathbf{f}_n &\leq \gamma \cdot f_{\max} \\ \phi(\mathbf{q}) &\leq (1-\gamma) \cdot \phi_{\max} \end{align} $$ 这导致混合整数非线性规划（MINLP），可用分支定界法求解。

10.5 案例研究：Spot四足机器人的运动控制

10.5.1 Spot的动力学模型

Boston Dynamics的Spot是一个具有12个自由度的四足机器人（每条腿3个关节）。其浮动基座动力学可表示为： $$ \begin{bmatrix} \mathbf{M}_b & \mathbf{M}_{bj} \\ \mathbf{M}_{bj}^T & \mathbf{M}_j \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{x}}_b \\ \ddot{\mathbf{q}} \end{bmatrix} + \begin{bmatrix} \mathbf{h}_b \\ \mathbf{h}_j \end{bmatrix} = \begin{bmatrix} \mathbf{0} \\ \boldsymbol{\tau} \end{bmatrix} + \sum_{i=1}^{4} \mathbf{J}_i^T \mathbf{f}_i $$ 其中下标$b$表示基座（6自由度），$\mathbf{q} \in \mathbb{R}^{12}$为关节角度。

简化的单刚体模型用于MPC： $$ m\ddot{\mathbf{p}} = m\mathbf{g} + \sum_{i=1}^{4} \mathbf{f}_i $$ $$ \mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times \mathbf{I}\boldsymbol{\omega} = \sum_{i=1}^{4} (\mathbf{r}_i - \mathbf{p}) \times \mathbf{f}_i $$

10.5.2 MPC控制器设计

Spot的MPC控制器采用分层架构：

控制架构：
┌──────────────────────┐
│   高层规划器(1-10Hz) │ ← 用户命令
│   (步态、足端轨迹)   │
└──────────┬───────────┘
           ↓
┌──────────────────────┐
│    MPC控制器(100Hz)  │ ← 状态估计
│  (质心动力学、接触力) │
└──────────┬───────────┘
           ↓
┌──────────────────────┐
│   WBC控制器(1000Hz)  │ ← 关节反馈
│   (关节力矩指令)     │
└──────────────────────┘

MPC层的优化问题：

$$ \begin{align} \min \quad & \sum_{k=0}^{N-1} \left[ |\mathbf{x}_k - \mathbf{x}_k^{\text{ref}}|_Q^2 + \sum_{i=1}^{4} |\mathbf{f}_{i,k}|_R^2 \right] \\ \text{s.t.} \quad & \text{单刚体动力学} \\ & \mathbf{f}_{i,k} = \mathbf{0} \quad \text{if } \gamma_{i,k} = 0 \text{ (摆动相)} \\ & \mathbf{f}_{i,n} \geq f_{\min} \quad \text{if } \gamma_{i,k} = 1 \text{ (支撑相)} \\ & |\mathbf{f}_{i,t}| \leq \mu \mathbf{f}_{i,n} \quad \text{(摩擦锥)} \end{align} $$

10.5.3 实时性能优化

为达到100Hz的控制频率，采用以下优化技术：

问题重构为QP

通过固定接触序列和线性化动力学，将NMPC转化为QP：

$$ \min_{\mathbf{z}} \quad \frac{1}{2}\mathbf{z}^T\mathbf{H}\mathbf{z} + \mathbf{g}^T\mathbf{z} $$ $$ \text{s.t.} \quad \mathbf{A}\mathbf{z} = \mathbf{b}, \quad \mathbf{C}\mathbf{z} \leq \mathbf{d} $$

其中$\mathbf{z} = [\mathbf{x}_0^T, \mathbf{f}_0^T, \ldots, \mathbf{x}_N^T]^T$。

稀疏性利用

KKT系统具有带状结构：

KKT矩阵结构：
[H₀  A₀ᵀ              ]
[A₀  H₁  A₁ᵀ          ]
[    A₁  H₂  A₂ᵀ      ]
[        ⋱   ⋱   ⋱    ]
[            Aₙ₋₁ Hₙ  ]

使用带状矩阵求解器可将复杂度从$O(N^3n^3)$降至$O(Nn^3)$。

代码优化

SIMD向量化：利用AVX指令集加速矩阵运算
内存对齐：确保数据结构对齐到缓存行
零分配：预分配所有内存，避免运行时分配

10.5.4 实验结果分析

典型的性能指标：

计算性能：

平均求解时间：3-5ms
最坏情况：8ms
QP迭代次数：10-20

控制性能：

位置跟踪误差：< 5cm（平地）
姿态跟踪误差：< 5°
地形适应高度：±20cm

鲁棒性测试：

扰动响应（20kg负载突加）：
位置偏差(cm)
  10│     ╱╲
   8│    ╱  ╲
   6│   ╱    ╲
   4│  ╱      ╲___
   2│ ╱           ───
   0└─┴─┴─┴─┴─┴─┴─┴─┴─
     0 0.5 1 1.5 2 时间(s)

10.6 本章小结

本章深入探讨了非线性MPC在机器人控制中的理论与实践。关键要点包括：

核心概念：

非线性MPC框架：通过迭代求解非凸优化问题处理系统非线性
SQP算法：将非线性问题序列化为QP子问题
实时迭代：通过部分收敛实现实时控制
接触隐式方法：统一处理接触模式切换

关键公式：

非线性系统动力学：$\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)$
SQP线性化：$\mathbf{A}_k = \frac{\partial f}{\partial \mathbf{x}}|_{(\bar{\mathbf{x}}_k, \bar{\mathbf{u}}_k)}$
互补性条件：$0 \leq \phi(\mathbf{q}) \perp \mathbf{f}_n \geq 0$
Fischer-Burmeister函数：$\Phi(a,b) = \sqrt{a^2 + b^2} - a - b$

实践要点：

热启动对实时性能至关重要
问题结构（稀疏性、分块）的利用可大幅提升效率
延迟补偿是实际部署的必要技术
分层控制架构有效平衡了计算复杂度和控制性能

10.7 练习题

基础题

题目10.1：推导SQP算法的KKT条件考虑约束优化问题： $$\min f(\mathbf{x}) \quad \text{s.t.} \quad g(\mathbf{x}) = 0, \quad h(\mathbf{x}) \leq 0$$ 推导SQP迭代的KKT系统。

提示：使用拉格朗日函数并考虑二阶近似。

参考答案

KKT系统为： $$ \begin{bmatrix} \nabla^2 L & \nabla g^T & \nabla h^T \\ \nabla g & 0 & 0 \\ \text{diag}(\lambda) \nabla h & 0 & \text{diag}(h) \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta \nu \\ \Delta \lambda \end{bmatrix} = - \begin{bmatrix} \nabla f + \nabla g^T \nu + \nabla h^T \lambda \\ g \\ \text{diag}(\lambda) h \end{bmatrix} $$ 其中$L = f + \nu^T g + \lambda^T h$为拉格朗日函数。

题目10.2：分析RTI的收敛性证明在线性系统下，RTI方案与标准MPC的性能差距有界。

提示：考虑Riccati递归的收敛性。

参考答案

对于LQR问题，Riccati迭代收敛率为： $$|P_{k+1} - P^*| \leq \rho |P_k - P^*|$$ 其中$\rho = |\mathbf{A} - \mathbf{B}\mathbf{K}^*|^2 < 1$。因此RTI产生的次优成本满足： $$J_{\text{RTI}} - J^* \leq \text{tr}((P_0 - P^*)\Sigma)$$ 其中$\Sigma$为初始状态协方差。

题目10.3：设计四足机器人的简化MPC 给定质量$m=30$kg，惯量$\mathbf{I}=\text{diag}(0.5, 1.0, 0.8)$kg·m²，设计一个10步预测时域的MPC控制器。

提示：使用单刚体模型，固定接触序列。

参考答案

状态向量：$\mathbf{x} = [\mathbf{p}^T, \boldsymbol{\theta}^T, \dot{\mathbf{p}}^T, \boldsymbol{\omega}^T]^T \in \mathbb{R}^{12}$ 控制输入：$\mathbf{u} = [\mathbf{f}_1^T, \mathbf{f}_2^T, \mathbf{f}_3^T, \mathbf{f}_4^T]^T \in \mathbb{R}^{12}$ 离散化周期：$\Delta t = 0.01$s 权重矩阵：$\mathbf{Q} = \text{diag}(100, 100, 100, 50, 50, 50, 1, 1, 1, 0.1, 0.1, 0.1)$ $\mathbf{R} = 10^{-4} \mathbf{I}_{12}$

挑战题

题目10.4：接触隐式MPC的凸松弛设计一种凸松弛方法，使接触隐式问题可用凸优化求解。分析松弛的保守性。

提示：考虑McCormick松弛或SDP松弛。

参考答案

McCormick松弛将双线性项$\gamma \mathbf{f}$替换为新变量$\mathbf{w}$： $$ \begin{align} \mathbf{w} &\geq \mathbf{f} - (1-\gamma)f_{\max} \\ \mathbf{w} &\leq \mathbf{f} \\ \mathbf{w} &\leq \gamma f_{\max} \\ \mathbf{w} &\geq 0 \end{align} $$ 当$\gamma \in \{0,1\}$时，松弛是紧的。连续松弛$\gamma \in [0,1]$引入的误差界为$O(f_{\max}(1-\gamma)\gamma)$。

题目10.5：多机器人协调MPC 设计一个分布式MPC方案，协调3个机器人搬运一个共同物体。如何处理耦合约束？

提示：使用ADMM或双重分解。

参考答案

使用ADMM框架：

局部问题（机器人$i$）： $$\min_{x_i, u_i} J_i(x_i, u_i) + \rho/2 |A_i x_i - z + y_i|^2$$
协调问题： $$z = \arg\min_z \sum_i |A_i x_i - z + y_i|^2$$
对偶更新： $$y_i \leftarrow y_i + A_i x_i - z$$ 其中$z$表示共享的物体状态，$A_i$为机器人$i$对物体的作用映射。

题目10.6：学习增强的NMPC 设计一个结合深度学习的NMPC框架，用神经网络学习模型误差并在线补偿。讨论稳定性保证。

提示：考虑鲁棒MPC或管道MPC方法。

参考答案

框架设计：

离线学习残差模型：$\Delta f = f_{\text{real}} - f_{\text{nominal}}$
使用GP或深度集成量化不确定性：$\Delta f \sim \mathcal{N}(\mu_\theta(x,u), \Sigma_\theta(x,u))$
鲁棒MPC表述： $$\min \max_{|\Delta f| \leq \epsilon} J(x,u,\Delta f)$$
管道约束保证安全： $$\mathcal{X}_{\text{safe}} = \{x : \exists u, \forall \Delta f, x' = f(x,u) + \Delta f \in \mathcal{X}_{\text{safe}}\}$$

题目10.7：开放问题——实时性与最优性的权衡讨论在机器人NMPC中，如何量化和优化实时性与最优性之间的权衡。考虑不同的应用场景（如无人机vs人形机器人），提出自适应策略。

提示：这是一个开放性问题，考虑计算资源分配、任意时算法、性能界等。

参考答案

权衡策略：

任意时MPC：维护多个不同精度的解，根据可用时间返回最佳解
自适应时域：高动态时缩短预测时域，稳态时延长
精度调度：根据状态危急程度分配计算资源
性能监控：在线估计次优性gap：$\hat{\epsilon} = |\nabla J|/|\mathbf{H}^{-1}|$

应用特定考虑：

无人机：优先保证高频响应，可容忍较大稳态误差
人形机器人：平衡任务需要高精度，可接受较大延迟
协作机器人：安全约束必须严格满足，性能可适当牺牲

10.8 常见陷阱与错误

数值问题

问题1：Hessian矩阵病态

症状：优化器收敛慢或发散
原因：时间尺度差异大，权重选择不当
解决：
归一化状态和控制变量
使用Tikhonov正则化：$\mathbf{H} \leftarrow \mathbf{H} + \epsilon\mathbf{I}$
自适应缩放：$\tilde{\mathbf{x}} = \mathbf{D}_x\mathbf{x}$，选择$\mathbf{D}_x$使变量同量级

问题2：约束不可行

症状：QP求解器返回infeasible
原因：线性化误差累积，初始猜测太差
解决：
使用松弛变量：$g(x) \leq s$，$\min |s|$
约束软化：将硬约束改为罚函数
可行性恢复阶段

实时性问题

问题3：偶发的求解时间尖峰

症状：平均3ms，偶尔30ms
原因：活动集变化，缓存未命中
解决：
固定最大迭代次数
预热缓存
实时调度优先级设置

问题4：延迟导致的振荡

未补偿延迟的影响：
 期望 ─────────────
 实际 ～～～～～～～～ (振荡)

解决：
Smith预测器
增加阻尼：$\mathbf{R} \leftarrow \mathbf{R} + \alpha\mathbf{R}_d$
降低增益

建模错误

问题5：接触模型不准确

症状：实际接触力与预测差异大
原因：摩擦系数估计错误，接触刚度假设不当
解决：
在线参数估计
鲁棒MPC设计
力传感器反馈

调试技巧：

可视化预测轨迹：检查是否物理合理
监控KKT残差：判断收敛质量
记录时间profile：识别性能瓶颈
仿真验证：先在仿真中调试，再部署实机
降级测试：从简单场景逐步增加复杂度