Tube-MPC原理与Matlab实现：鲁棒控制中的管式结构

如果你用过MPC控制一个稍微像样的对象，一定遇到过这种场面：仿真里一切完美，约束守得住，超调也不大，但把同一套预测模型搬到有模型误差和外部扰动的对象上，状态就像脱缰野马一样直接顶到约束边界上，甚至越界。问题不完全出在MPC算法本身，而在于预测模型太“自信”——它假设自己算出来的名义轨迹就是真实轨迹。Tube-MPC（管式模型预测控制）就是针对这个痛点的一种鲁棒控制方案，它把名义预测轨迹和真实轨迹之间的偏离装进一根由鲁棒控制不变集（RCI）决定的“管道”里，真实状态只能在这根管道内波动，因此即使在Lipschitz非线性系统里也能同时保证约束满足和闭环稳定。

这篇文章我会从原理讲到Matlab代码落地，适合对MPC有基础、想进一步理解鲁棒MPC、或者正在做非线性系统控制仿真复现的工程师和研究生。我会避开那种“公式堆完就跑”的写法，尽量把每一步的动机、计算逻辑、参数选择和踩坑经验都交代清楚。

1. 当MPC的“预测之眼”被模型误差蒙蔽：管式结构为什么是更聪明的鲁棒方案

1.1 标称MPC的边界穿越：一个再常见不过的失败场景

说句实话，初次接触MPC的人很容易被它的名字骗了——MPC确实在每一个采样时刻滚动求解一个有限时域最优控制问题，但它求解的是“名义模型”下的最优，不是“真实对象”下的最优。一旦模型和对象之间存在偏差，比如质量参数估不准、摩擦系数变了、外部风扰进来，预测的状态轨迹就只是“看起来很美”。

我举个具体的例子。假设你在做一阶倒立摆的平衡控制，标称模型里摆杆质量m是0.5 kg，实际对象是0.55 kg。MPC在预测窗口里认为，施加某个控制序列后，角度约束可以保持在±0.2 rad以内。但真实对象因为质量偏大，在同样的控制量下响应偏慢，结果第6步预测时感到还稳得住，实际第8步就突破了约束边界的±0.2 rad。这时候MPC还没来得及反应，约束已经被违反。这类情况在含硬约束的实际系统里往往是不可接受的——比如机械臂末端不能撞到障碍物，气压管路压力不能超限。

所以，单纯在预测模型里做“最优”，但完全不对模型失配和外部扰动负责，是标称MPC天生的短板。而解决这个短板的方法很多，最务实的路线之一，就是Tube-MPC。

1.2 min-max MPC为什么大家用得少

早期鲁棒MPC的主流思路是min-max MPC：在每一个采样时刻，把未来所有可能的扰动序列都纳入优化，目标是让最坏情况下的代价最小。这个方法理论很干净——一旦解出来，闭环对所有的扰动场景都有性能保证。但它有一个致命问题：计算量随预测时域和扰动集合维度呈指数增长。

假设扰动集W用多面体表示，每个顶点代表一种扰动实现，预测时域N=10，那么扰动序列的组合数就是顶点数的10次方，这还没考虑状态约束和控制约束。对于Lipschitz非线性系统，min-max问题里还嵌套了非线性动力学，求解难度直接飞升。很多做控制的同行都会说：min-max MPC写论文很漂亮，做工程很痛苦。

那有没有办法把“扰动鲁棒性”和“在线优化复杂度”解耦？Tube-MPC给出了一个很聪明的回答：在线只需解一个常规的名义MPC问题，所有扰动的“吸收”交给离线设计好的误差管和反馈律。

1.3 Tube-MPC的核心思路：名义优化加误差管

Tube-MPC的基本思想可以概括成三句话：

• 名义系统走名义轨迹：MPC在线求解一个不考虑扰动的名义模型，得出一条名义状态轨迹x̄(k)和名义控制序列ū(k)。
• 真实系统跟着名义系统走：真实控制量不直接用ū(k)，而是加一项反馈修正u(k) = ū(k) + K·(x(k) − x̄(k))，其中K是离线设计好的反馈增益。
• 误差被“管道”罩住：定义误差e(k) = x(k) − x̄(k)，鲁棒控制不变集S保证只要e(k)在S内，不管扰动怎么来，e(k+1)还在S内。

这样一来，在线优化只需要面对名义模型，真实的约束验证只需要在名义约束上“缩”出一个安全裕度：名义状态x̄必须落在状态约束X与S的Pontryagin差集X ⊖ S内，名义控制ū必须落在U ⊖ K·S内。只要这些收缩后的约束满足，那么实际状态x一定满足原始约束X，实际控制u一定落在U。

这个方法在线就是一个普通QP或者NLP，离线多花点时间算集合、算反馈增益，工程可接受度比min-max高得多。

2. Tube-MPC的三根承重柱：名义系统、误差动态和鲁棒控制不变集

2.1 名义系统：“平行世界”里的理想模型

名义系统在许多论文里写作x̄(k+1) = f(x̄(k)) + B·ū(k)，它的作用是提供给MPC求解器一个“参考剧本”。需要强调一个容易混淆的点：名义系统不一定要和真实系统完全一样。在Tube-MPC的常见处理中，我们把真实系统的不确定性、模型失配、外部扰动统一折叠进误差系统里，因此名义系统完全可以取一个更简单的线性模型。

在Lipschitz非线性系统场景里，我建议的做法是：取系统的某一参考线性化模型作为名义模型，把所有非线性残差和外部扰动都当作“等效扰动”。这样做的好处非常直接——名义MPC变成一个标准QP，在线求解速度极快，鲁棒性由管和反馈律兜底。代价是保守性：等效扰动变大后，管S会变胖，名义可行域会收缩。这是一个典型的“模型精度与在线计算复杂度”的权衡。

2.2 误差动态与反馈增益K：把偏差往回拽

实际系统的状态更新可以写成：

x(k+1) = f(x(k)) + B·u(k) + d(k)

其中d(k)∈W是有界扰动，可以是外部扰动、建模误差、参数摄动的综合效果。定义误差e(k) = x(k) − x̄(k)，并把控制律写成u(k) = ū(k) + K·e(k)，代入后得到误差动态：

e(k+1) = [f(x̄(k)+e(k)) − f(x̄(k))] + B·K·e(k) + d(k)

如果f是线性系统，这段误差动态就会退化成一个线性递推：

e(k+1) = (A + B·K)·e(k) + d(k) = A_K·e(k) + d(k)

关键是让A_K是Schur稳定的，即所有特征值都在单位圆内。这样误差会自然收缩，管S的尺寸就小。K的设计一般直接用离散LQR或者极点配置。LQR的好处是可以显式调节状态误差和控制误差的权重，从而在设计K的时候平衡“收敛速度”和“控制能量”。

从控制直觉上讲：K越强，误差收缩越快，管S理论上可以越小；但K太强会导致K·S这个集合很大，进而把名义控制的可行域U ⊖ K·S压得很小。所以K不是越大越好，它和S、U、X四者在同一套约束里互相牵扯。

2.3 RCI集合：离线算好的一根“管道”

鲁棒控制不变集的定义一句话就能说清：对于集合S和误差动态，如果“e(k)∈S”能推出“对任意d(k)∈W，都有e(k+1)∈S”，那么S就是鲁棒控制不变集。

这个性质非常关键，它把单个状态的有界性升级成“集合层面”的不变性。很多人会问：我直接用一个大球把误差包住不行吗？可以，但那不是不变集。普通的包络集合只能说明“此刻误差没超界”，不能保证下一步不超界。RCI则保证了递归可行性——只要初始误差落在S内，后面每一刻的误差都跑不出S。这正是MPC里“递归可行性”概念在鲁棒框架下的对应物。

对线性误差系统e(k+1) = A_K·e(k) + d(k)，计算最大鲁棒正不变集的标准做法是从原点集合出发迭代：

S₀ = {0}
Sᵢ₊₁ = A_K·Sᵢ ⊕ W

其中⊕表示Minkowski和，也就是两个集合逐点相加。当A_K Schur稳定时，这个序列单调收敛到满足S = A_K·S ⊕ W的最大鲁棒正不变集。实际代码里就是循环计算多面体A_K·S与W的Minkowski和，直到集合不再明显变化。

S的几何形状是个以零点为中心的多面体。它的“半径”取决于三件事：扰动集W的大小、系统矩阵A_K的谱半径、以及K的设计。扰动越大、A_K的谱半径越接近1，S就越大。管越大，名义可行域X ⊖ S就越小，MPC能用的状态空间就越少。这就是Tube-MPC的“守恒单价”，理解这一点后面调参才不会瞎忙。

3. Lipschitz非线性如何走进这套框架：从非线性残差到等价扰动

3.1 直接线性化的直觉与陷阱

前面用线性误差动态当例子，是因为好算。但标题里明确提了Lipschitz非线性系统，这里就必须把非线性摆到台面上来。很多第一次实现的人会想：我直接把非线性函数f在原点线性化，得到A₀ = ∂f/∂x(0)，然后不管名义轨迹走到哪，都用这个A₀算A_K，行不行？

答案是要分情况。如果非线性比较温和，比如只在某个小区间内波动，那A₀线性化加上一个足够保守的等效扰动，确实能凑合。可一旦名义轨迹偏离原点比较远，f(x̄+e) − f(x̄) − A₀·e这一项就会变得很大。你还是把这一项当成扰动加了进去，但扰动界的估计会变得极其保守，管S会胀到让X ⊖ S空掉，整个MPC直接没有可行域。

所以更严谨的处理是：把非线性残差明确纳入扰动界，并且用Lipschitz常数来给出可计算的界。

3.2 Lipschitz常数：非线性上限的“GPS坐标”

一个函数f在集合Ω上满足Lipschitz条件，意思是存在常数L，使得对任意x, y∈Ω：

‖f(x) − f(y)‖ ≤ L·‖x − y‖

这个L就是Lipschitz常数。对控制对象来说，它刻画了“状态偏差导致的非线性作用强度的上界”。对很多常见非线性项，L是可以解析估算的：

• f(x) = sin(x)：一阶导数cos(x)在实数范围内绝对值不超过1，所以L=1。
• f(x) = x²在区间[−a, a]上：导数2x的绝对值最大为2a，所以L=2a。
• 多维向量的范数形式，L可以取雅可比矩阵在集合Ω上的范数上界，即L = sup_{x∈Ω} ‖∂f/∂x(x)‖。

在Matlab里，如果函数是符号定义的，可以直接用雅可比矩阵然后数值搜索最大奇异值；如果是黑箱模型，可以用数值差分加优化扫描来估算。估算L不是越精确越好，但要保证“不要太保守”——L给大了，管S就偏胖；L给小了，管S可能不够包住非线性残差，真出了事还是越界。这里我的经验是：先解析估算一个L，然后在仿真中采集误差轨迹的最大范数，反推残差是否真的被包住了。如果余量太小，就放大L重算S。

3.3 把残差打包成等效扰动，再迭代求解一致的RCI集合

为了把非线性纳入RCI框架，我们做一个“打包”操作。定义等效扰动：

w_eff(k) = [f(x̄(k)+e(k)) − f(x̄(k)) − A₀·e(k)] + d(k)

其中A₀是系统在某个工作点处的雅可比。利用Lipschitz条件和三角不等式：

‖f(x̄+e) − f(x̄) − A₀·e‖ ≤ (L + ‖A₀‖)·‖e‖

如果e被限制在管S内，且S的“半径”有一个保守估计r，使得S ⊆ B(0, r)，那么这一项的范数上界就是(L + ‖A₀‖)·r。把d的界也加进来，就得到等效扰动集W_eff是B(0, δ + (L + ‖A₀‖)·r)与W的并/叠加。

这里存在一个循环依赖：W_eff的界依赖S的半径r，而S的半径又依赖W_eff。解决的办法是迭代验证：

1. 给定一个初始半径猜测r₀；
2. 用r₀算出带非线性的等效扰动界；
3. 用这个扰动界去迭代计算RCI集合S_new；
4. 看看S_new是否真的满足S_new ⊆ B(0, r₀)。如果满足，说明假设自洽，输出S_new；如果不满足，把r₀调大，回到第2步。

这个迭代在Matlab里实现很直接，通常两三次就能稳定。它本质上是在说：我先假定误差不会跑出某个半径，然后计算在这个假定下误差到底会不会跑出去，如果会，我扩大假定继续算，直到收敛。

当然，这套处理的代价是保守性。如果L太大，而K的收缩能力不够强，那么迭代可能不收敛，或者收敛后的S大到没有实用价值。这时候就需要反思：要么K再加强一点，要么把非线性工作区再细分，用局部Tube或者时变Tube。

3.4 如果L太大怎么办：保守也要有底

实际系统里L太大很常见，比如强非线性执行器、死区、饱和，或者状态工作范围很宽。遇到这种问题，我不建议硬把整个状态空间塞进一套单管方案。更实用的做法是分段处理：

• 把状态空间划分成若干个区域，在每个区域里分别线性化、分别估计Lipschitz常数、分别设计K和S，形成“分段管式MPC”。
• 或者提高K的设计强度，用更高增益反馈把误差收缩得更快，让S变小。
• 也可以适当牺牲最优性，把名义约束进一步收缩，换取更大的管空间。

这些方法会增加离线设计复杂度，但这样换来在线依然是一个可控的优化问题，对很多实时系统是值得的。

4. MATLAB实现：从系统建模到MPC主循环的完整落地

下面我以一个二维离散时间系统为例，完整走一遍Tube-MPC的代码实现路径。这个系统是常见的“弱非线性双积分器”，带一个sin非线性项，是比较典型的Lipschitz非线性测试对象。

系统模型：

• x₁(k+1) = x₁(k) + Ts·x₂(k)
• x₂(k+1) = x₂(k) + Ts·(sin(x₁(k)) + u(k)) + d₂(k)

其中Ts是采样周期，d₂(k)是外部扰动，取|d₂|≤δ。这个系统的线性化矩阵：

• A₀ = [1, Ts; 0, 1]
• B = [0; Ts]

非线性残差项是[0; Ts·sin(x₁)]，它的Lipschitz常数在x₁全局范围内是L = Ts。

4.1 系统离散化与Lipschitz常数估计

首先定义系统基础参数：

matlab复制% 系统参数
Ts = 0.1;           % 采样周期
A0 = [1, Ts; 0, 1];
B  = [0; Ts];
n  = 2;             % 状态维度

% Lipschitz常数估计（sin项的导数为cos，上界为1）
L_sin = 1.0;
L = Ts * L_sin;     % 非线性残差的Lipschitz常数

% 扰动界
delta = 0.02;       % 外部扰动上限

这里计算L的物理含义是：sin(x₁)项在状态偏差e₁上的作用，经过采样周期放大后，最大斜率为Ts。如果你的系统更复杂，可以用Matlab符号工具箱求雅可比，再在工作范围内求范数上界：

matlab复制syms x1 x2 u
f = [x1 + Ts*x2; x2 + Ts*(sin(x1) + u)];
J = jacobian(f, [x1; x2]);
% 在工作区间内数值扫描 ||J||_2 的上界

4.2 用MPT3离线构造RCI集合和管约束

我采用MPT3工具箱来做多面体运算，如果没有安装，可以用mpt3官网的安装包，或者自己写多面体的支撑函数和Minkowski和，但对于二维系统老老实实用MPT3最省事。

matlab复制% 设计反馈增益K（离散LQR）
Q = diag([10, 1]);
R = 0.1;
K = dlqr(A0, B, Q, R);
A_K = A0 + B*K;

% 扰动集W：无穷范数有界，二维
W = Polyhedron('lb', [-delta; -delta], 'ub', [delta; delta]);

% 迭代计算最大鲁棒正不变集S
S = Polyhedron('lb', [0; 0], 'ub', [0; 0]);   % 原点集合
for i = 1:200
    S_next = (A_K * S) + W;                     % MPT3中 * 是线性映射，+ 是Minkowski和
    if S_next.contains(S) && S.contains(S_next)
        S = S_next;
        break;
    end
    S = S_next;
end

% 检查S的规模
S.minHRep();
fprintf('RCI集合S的不等式数量: %d\n', size(S.A, 1));

这里有几个容易踩的坑。第一个是S_next.contains(S) && S.contains(S_next)的判断，MPT3里直接比较==有时会因为浮点误差失败，用互包含判断更稳定。第二个是如果A_K不是Schur稳定的，S_next会越来越大，循环到200次都不会收敛。所以建议在迭代前先检查eig(A_K)的最大模是否小于1。

得到S之后，需要计算名义约束集：

matlab复制% 原始状态约束和控制约束
X = Polyhedron('lb', [-1.5; -1.0], 'ub', [1.5; 1.0]);
U = Polyhedron('lb', -1.0, 'ub', 1.0);

% 管收缩后的名义约束
X_tube = X - S;                  % Pontryagin差集
U_tube = U - (K * S);

注意X - S在MPT3里是Pontryagin差集，不是集合减法。如果运算结果是空集，说明管太大、状态可行域已经被完全挤掉了，这时候必须回头调小扰动界或者增强K。

4.3 名义MPC的优化问题建模

名义系统取线性模型x̄(k+1) = A₀·x̄(k) + B·ū(k)，这样名义MPC是一个标准QP。我用YALMIP来搭优化问题，求解器可以选quadprog或OSQP。

matlab复制% 预测时域
N = 10;

% 决策变量
u_bar = sdpvar(repmat(1,1,N), repmat(1,1,N));
x_bar = sdpvar(repmat(n,1,N+1), repmat(1,1,N+1));

% 目标权重
Qm = diag([10, 1]);
Rm = 0.1;

constraints = [];
objective = 0;

% 初始名义状态固定为当前x_bar_init
constraints = [constraints, x_bar{1} == x_bar_init];

for k = 1:N
    % 名义动力学约束
    constraints = [constraints, x_bar{k+1} == A0*x_bar{k} + B*u_bar{k}];
    % 管收缩后的状态约束
    constraints = [constraints, X_tube.A * x_bar{k} <= X_tube.b];
    % 管收缩后的控制约束
    constraints = [constraints, U_tube.A * u_bar{k} <= U_tube.b];
    % 目标函数
    objective = objective + x_bar{k}'*Qm*x_bar{k} + u_bar{k}'*Rm*u_bar{k};
end
% 终端状态约束，可以取终端不变集或X_tube本身
constraints = [constraints, X_tube.A * x_bar{N+1} <= X_tube.b];
objective = objective + x_bar{N+1}'*Qm*x_bar{N+1};

% 求解
options = sdpsettings('solver', 'quadprog', 'verbose', 0);
optimize(constraints, objective, options);

有人可能会问：管收缩后的约束为什么要用X_tube.A * x_bar{k} <= X_tube.b，而不是直接判断x_bar{k}∈X_tube？这其实是多面体约束的标准展开形式，X_tube内部就是以半空间不等式A·x ≤ b存储的，所以取出来直接用就行。

如果名义模型也想保留非线性，那这个优化问题就不再是QP，需要把A0*x_bar{k} + B*u_bar{k}改成非线性函数f(x_bar{k}, u_bar{k})，并用fmincon或IPOPT求解。我在实际项目中试过，非线性NMPC加Tube结构不是不行，只是在线求解时间会明显增加，而且非线性NLP的热启动策略比QP难调很多。所以能线性化名义模型就先用线性化。

4.4 闭环仿真主循环：实际状态、名义状态与反馈控制的关系

下面是整个Tube-MPC最核心的闭环循环。要特别关注一个问题：名义状态x̄到底怎么更新。很多人第一次写会写成x̄ = x，直接用实测状态做名义MPC初值，这样其实就把误差清零了，管式结构的误差动态约束就被破坏了。

正确做法是：x̄(k)从MPC解出来的预测轨迹第一项x̄(1|k)继承，它不是实测状态x(k)。实测状态与名义状态之间的差由反馈增益K去吸收。

matlab复制% 初始化
x_meas = [0.5; -0.2];      % 实际初始状态
x_bar = [0.5; -0.2];       % 名义初始状态，要求落在X_tube内
Nsim = 100;

% 存储记录
x_log = zeros(n, Nsim);
u_log = zeros(1, Nsim);
e_log = zeros(n, Nsim);

for k = 1:Nsim
    % 1. 求解名义MPC，得到当前名义状态x_bar下的最优控制序列u_bar_seq
    x_bar_init = x_bar;
    optimize(constraints, objective, options);   % 实际会封装成函数
    u_bar_seq = value(u_bar);
    x_bar_seq = value(x_bar);
    
    % 2. 真实控制量 = 名义控制量 + 反馈校正
    e = x_meas - x_bar;
    u_real = u_bar_seq{1} + K * e;
    u_log(k) = u_real;
    
    % 3. 对真实对象施加控制，加入扰动
    x_next(1) = x_meas(1) + Ts * x_meas(2);
    x_next(2) = x_meas(2) + Ts * (sin(x_meas(1)) + u_real) + delta * randn();
    x_meas = x_next;
    
    % 4. 名义状态沿预测轨迹走一步，注意不要直接用x_meas
    x_bar = x_bar_seq{2};   % MPC解里的第二项，就是下一时刻的名义状态
    
    % 记录
    x_log(:, k) = x_meas;
    e_log(:, k) = e;
end

如果x_meas在某些场景下偏离x_bar太多，误差e可能落在S外。这通常发生在初始化不合法、扰动界估计过低、或者K设计不足时。所以闭环主循环里最好加一个断言：

matlab复制assert(S.contains(e), '误差超出RCI集合，需要检查扰动界或K设计');

5. 真正动手时会踩的坑：扰动界、初始可行域与数值稳定性

5.1 扰动界W和Lipschitz残差界：一对互相拉扯的“天平”

Tube-MPC的全部保守性几乎都体现在S的尺寸上，而S的尺寸由W和L共同决定。这里有一个常见的工程误区：为了“绝对安全”，把扰动界设成实际值的两倍、三倍。你以为这样更鲁棒，其实可能直接把X_tube挤成空集，名义MPC从一开始就不可行。

我的建议是：先用实际扰动数据的90%分位数或者3σ值估计一个基础扰动界，然后通过仿真观察误差轨迹。如果误差距离S边界还有很大裕量，可以适当放大扰动界；如果误差经常贴着边界甚至越界，再考虑增大W或者增大L。一句话：扰动界要“够用就好”，它不是一个能随便放大的安全垫。

L的估计同样如此。L给大了，非线性残差被高估，等效扰动变大，S膨胀；L给小了，非线性残差突破扰动假设，误差跑出S。解析估算是起点，闭环仿真校核才是最终决定。

5.2 初始名义状态到底怎么选

闭环仿真的第一步非常关键：x̄(0)必须落在X_tube里，而且e(0) = x(0) − x̄(0)必须在S内。这两个条件同时满足，才能让整个递推过程“开上帝视角”一样自洽地往下走。

如果实测x(0)本身就在X_tube里，那最简单：直接取x̄(0) = x(0)，e(0) = 0。但如果x(0)在X里但不在X_tube里，比如它离约束边界太近，硬取x̄(0)=x(0)就会导致名义MPC第一个优化问题就不可行。

此时需要把x̄(0)投影到X_tube中，同时保证x(0)−x̄(0)∈S。如果S是原点对称的，等价于x̄(0)∈(x(0) ⊖ S)。这个投影本身是一个二次规划，直接用YALMIP或quadprog解：

matlab复制x_bar_0 = sdpvar(n, 1);
constraints_init = [X_tube.A * x_bar_0 <= X_tube.b, ...
                    S.A * (x0 - x_bar_0) <= S.b];
optimize(constraints_init, sum((x_bar_0 - x0).^2), options);
x_bar_init = value(x_bar_0);

实测工程里，这个投影几乎从不失败，只要X_tube非空。但它说明一个道理：Tube-MPC的可行域本质上是从原始约束里“挖掉”一个管后的区域，初始状态必须在压缩后的区域里，而不是原始区域里。

5.3 热启动、集合运算和求解器：三个容易翻车的细节

先说热启动。名义MPC如果每步都冷启动，即从零初值求解，预测时域一长，很容易遇到不可行或者内点法迭代步数过多。尤其在状态接近X_tube边界时，冷启动的QP经常会给出偏离期望的次优解。更稳妥的做法是把上一步求解得到的x_bar_seq和u_bar_seq整体前移，作为当前QP的初值。YALMIP里设置assign(u_bar, u_bar_old_seq)，再配合sdpsettings('usex0', 1)就可以实现热启动。

第二个坑是集合运算。MPT3在低维（2D、3D）下很流畅，但维度一旦上到5维以上，minkowskiSum的顶点枚举会非常慢。如果你的系统状态维度超过4维，建议放弃枚举多面体，改用支持函数或者椭圆近似RCI集合。椭圆RCI集合同样可以用LMI离线求解，虽然比多面体保守一些，但计算量随维度线性增长，工程落地更现实。

第三个坑是QP求解器的可行性容差。默认情况下的quadprog容差可能比较宽松，在管收缩量很小时，求解器可能会返回一个轻微违反X_tube约束的解。实际上我不止一次遇到“仿真里误差基本在S内，但偶尔少少越界”的情况，最后发现不是算法错，而是QP求解器在约束边界附近提前终止了。把optimoptions里的ConstraintTolerance调到1e-8，通常能解决。

5.4 验证闭环是否真的满足“管性质”

调试阶段，我习惯每次仿真结束后画三张图，确认管式结构是否真正成立了：

• 第一张，画误差轨迹e1、e2以及S在误差平面上的投影。如果误差轨迹完全落在S内，说明RCI集合设计正确。
• 第二张，画实际状态轨迹、名义状态轨迹和原始约束边界。实际状态必须在X内，名义状态必须在X_tube内。若实际状态突破了约束而名义状态还在X_tube内，说明误差已经跑出S，问题大概率出在K或扰动界。
• 第三张，画实际控制量u和名义控制量ū，确认u始终落在U内，并且u和ū的偏差Δu = K·e没有超出U ⊖ KS的预留空间。

这三张图配合起来，基本能定位绝大多数实现问题。我自己的经验是，80%的“Tube-MPC越界”其实不是算法问题，而是初始化或数值容差问题，单纯的鲁棒MPC逻辑只要按着不变集设计，一次通过的几率很高。

6. 一点个人体会

整套Tube-MPC实现下来，我最大的感受是：鲁棒性不是在线拼命调，而是离线把能算好的先算好。RCI集合看起来只是一个几何工具，但它把“最坏情况下的递归可行性”这个抽象概念变成了一个可以触摸的对象。你不需要在每个采样时刻对一堆扰动场景做优化，你只要在离线阶段把管子的直径量好，在线阶段把名义轨迹的“舒适区”留出来，剩下的交给反馈律去吸收偏差。

做完这套以后，我后续在项目里会继续尝试两个扩展方向：一个是把扰动集从常值改为时变的，对应实际场景中的负载突变和时变风扰；另一个是研究分布式Tube-MPC，把多个Agent的误差管耦合关系解耦掉。这两个方向都还是围绕着同一根“管”在做文章，但难度和实用价值都会再上一个台阶。如果你也要在自己的项目里做鲁棒非线性MPC，我的建议是先把今天这套二维系统跑通，把S、X_tube、U_tube这几个量的相互作用吃透，再去碰更复杂的系统和高维工况——这根“管”的脾气摸顺了，后面很多问题都顺了。