线性MPC控制二阶弹簧阻尼系统实现轨迹跟踪的完整指南

1. 为什么要用MPC做弹簧阻尼系统的轨迹跟踪？先聊聊背景和思路

先说清楚这篇文章讲的是什么。标题里的关键词是“模型预测控制”“线性MPC”“二阶弹簧阻尼系统”“轨迹跟踪控制”——说白了，就是拿MPC（Model Predictive Control，模型预测控制）这套控制策略，去让一个典型的二阶弹簧阻尼系统跟上一条期望轨迹。系统的状态变量（位移、速度）和输出（力的输入）都受限，MPC能把这些约束直接塞进优化问题里处理，这是它相比PID最大的优势之一。

谁适合读这篇文章？如果你是做运动控制、机器人、汽车底盘控制或者自动化相关方向的学生或工程师，正在纠结“MPC到底怎么做仿真”“为什么别人的MPC能跟踪得很好而我的抖得厉害”“约束怎么加才不踩坑”，这篇文章应该能给你一个比较完整的参考。我会按“理论推导—代码实现—调参经验—踩坑记录—扩展应用”这条线来讲，整个过程都基于一个可以随手复现的二阶弹簧阻尼系统模型。

先花点时间说说为什么选这个系统。二阶弹簧阻尼系统在物理世界里到处都是：汽车的悬架、伺服电机的负载轴、工业机械臂的关节、甚至某些液位系统，动特性都可以简化成质量—弹簧—阻尼模型。它的数学结构足够简单，适合把MPC的核心机制讲透；同时它又具备振荡、超调、约束耦合这些真实系统的特征，不会让你觉得“这只是个玩具”。

我最早接触MPC是在做自主移动机器人底盘控制的项目里，当时的感觉是：传统PID只能逐环去调参数，想处理前后轮耦合和速度约束非常别扭。后来换成MPC，把问题写成优化问题，约束顺手就加了进去。当然代价也随之而来——计算量上来了、离散化搞不好会发散、预测模型的偏差会导致跟踪精度下降。这些部分恰恰是网上教程讲得最少、实际项目里坑最多的。我在这篇文章里会事无巨细地把它们都摊开讲。

2. 从动力学方程到MPC预测模型：公式推导与离散化

MPC不是凭空跑起来的，它首先要有一个预测模型。这个模型描述了：给定当前状态和未来一段时间的控制输入，系统状态会怎么演化。模型越准，控制效果越好；模型太复杂，实时性又跟不上。所以第一步，我们要把二阶弹簧阻尼系统的动力学方程写出来，然后再离散化成MPC能用的标准形式。

2.1 连续时间状态方程：质量和弹簧和阻尼怎么进状态空间

一个标准的$m$（质量）、$k$（弹簧刚度）、$c$（阻尼系数）二阶系统，受力分析很简单：

$$
m\ddot{x} + c\dot{x} + kx = F
$$

其中$x$是位移，$F$是外部控制力。把它改写成状态空间形式——引入状态向量$\mathbf{x} = [x, \dot{x}]^T$，于是：

$$
\dot{\mathbf{x}} =
\begin{bmatrix}
0 & 1 \
-\frac{k}{m} & -\frac{c}{m}
\end{bmatrix}
\mathbf{x}
+
\begin{bmatrix}
0 \
\frac{1}{m}
\end{bmatrix}
F
$$

这个模型里，弹簧项$kx$负责把系统往平衡位置拉，阻尼项$c\dot{x}$负责消耗能量、决定振荡衰减快慢，质量$m$决定了惯性大小。你把$k$调大，系统变“硬”，响应变快但容易振荡；把$c$调大，振荡变弱但响应变慢。这种直观的物理解释，对于后面理解MPC预测视野长度和权重矩阵的设置非常有帮助。

如果你把$F$设置为基于状态的反馈，比如$F = -K\mathbf{x}$，那你实际上就在做一个线性二次型调节器（LQR）或极点配置。MPC的思路则完全不同：它不直接给出一个固定增益，而是在每个采样时刻求解一个有限时域优化问题，得到最优的输入序列，然后只应用第一个输入。

2.2 零阶保持离散化：采样时间怎么选才合理

嵌入式控制器和仿真算法处理的都是离散信号，所以得把连续模型离散化。最常用的方法是零阶保持器（ZOH）离散化：假设每个采样周期内控制输入$F$保持不变，然后对矩阵指数积分。

离散化结果如下：

$$
\mathbf{x}(k+1) = A_d\mathbf{x}(k) + B_d F(k)
$$

其中：

$$
A_d = e^{A_c T_s}, \quad B_d = \int_0^{T_s} e^{A_c \tau} d\tau B_c
$$

$T_s$是采样周期。很多刚入门的朋友直接用欧拉法近似，这在小采样周期下问题不大，但一旦$T_s$稍大，欧拉法的精度就会下降，甚至导致预测模型和真实系统严重偏离，MPC的“预测”就变成了“瞎猜”。

采样时间的选择我一般遵循三条原则：

• $T_s$不能大于系统主导时间常数的$1/10$到$1/20$，否则离散模型丢失动态特性；
• $T_s$要能覆盖控制器计算时间——控制周期必须大于单步MPC求解时间，否则实时性崩了；
• $T_s$要和执行机构响应速度匹配。弹簧阻尼系统的执行器如果是电机，那$T_s$取5ms~20ms通常是比较舒服的一个区间。

我在仿真演示中取了$m=1,\text{kg}$，$k=2,\text{N/m}$，$c=1,\text{N·s/m}$，$T_s=0.1,\text{s}$。这个系统自然频率大约在$\omega_n=\sqrt{k/m}\approx1.41,\text{rad/s}$，时间常数大约0.7秒，所以0.1秒的采样周期能捕捉到系统动态的主要变化，同时计算负担又小，适合做教学演示和调参验证。

2.3 增量形式与控制量约束：避免静态偏差的常见做法

直接以$F(k)$作为优化变量，当模型有误差或存在未知扰动时，MPC会留下静态偏差。为了消除这个问题，工程上非常常见的方法是把控制量改成增量形式$\Delta u(k) = u(k) - u(k-1)$，把$u(k-1)$作为额外的状态。

扩展后的状态向量为：

$$
\tilde{\mathbf{x}}(k) = [x(k), \dot{x}(k), F(k-1)]^T
$$

预测方程变为：

$$
\tilde{\mathbf{x}}(k+1) =
\begin{bmatrix}
A_d & B_d \
0 & 1
\end{bmatrix}
\tilde{\mathbf{x}}(k)
+
\begin{bmatrix}
B_d \
1
\end{bmatrix}
\Delta u(k)
$$

这样做的直接好处是：可以在代价函数里对对$\Delta u$施加惩罚，限制控制量的变化率；同时只要优化问题可行，模型有偏差时系统也能通过积分作用消除稳态误差。我强烈建议你在做任何MPC仿真时，一开始就采用增量形式，不然后面加扰动或者模型失配时，追踪效果会很尴尬。

约束条件也顺便定义好：

$$
F_{\min} \le F(k+i) \le F_{\max}, \quad \Delta F_{\min} \le \Delta F(k+i) \le \Delta F_{\max}
$$

以及状态的约束：

$$
x_{\min} \le x(k+i) \le x_{\max}
$$

比如物理上执行器最大输出力为$5,\text{N}$，位移不能超过$[-0.5, 0.5],\text{m}$。让MPC在高性能条件下主动“触碰”这些边界，这正是模型预测控制比传统线性控制器有压倒性优势的地方。

3. 轨迹跟踪问题的MPC数学建模：代价函数、约束、预测时域

模型有了，接下来就是把“轨迹跟踪”这个目标翻译成一个标准的二次规划（QP）问题。整个过程可以分解成：定义代价函数、定义约束、形成QP矩阵。这一节我尽量把每个矩阵的维度都写清楚，因为代码实现时的很多out-of-index错误，根源都在这里。

3.1 代价函数设计：输出误差、控制增量、终端代价

轨迹跟踪的