写在前面

pid作为一种通用的泛用的控制方案，在某些工况下还是难以满足需求，比如动作模仿、轨迹规划、调速控制等，只要控制目标涉及到运动的动态过程，pid的效果总是差强人意；
因此，需要引入MPC(模型预测控制)，直接针对运动过程中的加速度/速度/位置等状态量直接控制，在更大程度上控制运动的动态。

具体推导

以最常用的离散线性 MPC / LTV-MPC为例，从模型到 QP（二次规划）

问题设置

离散时间线性系统（也可视作每步线性化后的 LTV）：

$$x_{k+1}=A{x}_k+B{u}_k$$

• $(x_k\in {\mathbb{R}}^n)$：状态
• $(u_k\in {\mathbb{R}}^m)$：控制输入
• 预测时域 (N)：向前看 N 步
• 参考轨迹：$(x^{ref}\ast k+i,u^{ref}\ast k+i)$

定义误差变量（更方便写代价）：

$${\tilde{x}}_k=x_k-x_k^{ref},{\tilde{u}}_k=u_k-u_k^{ref}$$

若参考是常值或零，直接把 ref 省略即可。

---

1) 预测模型展开：把未来状态写成“当前状态 + 未来输入”的线性函数

定义未来控制序列（决策变量）：

$$U\triangleq \left[\begin{array}{c}{u}_k{u}_{k+1}⋮u_{k+N-1}\end{array}\right]\in {\mathbb{R}}^{mN}$$

未来状态堆叠向量：

$$X\triangleq \left[\begin{array}{c}{x}_{k+1}{x}_{k+2}⋮x_{k+N}\end{array}\right]\in {\mathbb{R}}^{nN}$$

逐步展开：

• $(x_{k+1}=A{x}_k+B{u}_k)$
• $(x_{k+2}=A{x}_{k+1}+B{u}_{k+1}=A^2{x}_k+AB{u}_k+B{u}_{k+1})$
• $(x_{k+3}=A^3{x}_k+A^{2}B{u}_k+AB{u}_{k+1}+B{u}_{k+2})$
• …

于是得到紧凑形式：

$$X=\mathcal{A}{x}_k+\mathcal{B}U$$

其中

$$\mathcal{A}=\left[\begin{array}{c}A{A}^2⋮A^N\end{array}\right]\in {\mathbb{R}}^{nN\times n}$$

$$\mathcal{B}=\left[\begin{array}{ccccccccccccccccccccc}B& 0& 0& \cdots & 0AB& B& 0& \cdots & 0A^{2}B& AB& B& \cdots & 0⋮& ⋮& ⋮& \ddots & ⋮A^{N-1}B& A^{N-2}B& A^{N-3}B& \cdots & B\end{array}\right]\in {\mathbb{R}}^{nN\times mN}$$

（这是 MPC 推导里最核心的一步：预测矩阵。）

如果是 LTV（每步 ($A_i,B_i$) 不同），形式仍是 ($X=\mathcal{A}{x}_k+\mathcal{B}U$)，只不过 ($\mathcal{A},\mathcal{B}$) 由链式乘积构造（代码里按递推填块矩阵即可）。

---

2) 代价函数：二次型 + 终端项（标准 LQR 风格）

最常见的 MPC 目标是“跟踪 + 控制努力”：

$$J=\sum _{i=1}^N(x_{k+i}-x^{ref}\ast k+i{)}^{T}Q(x\ast k+i-x_{k+i}^{ref})$$

$$$\sum _{i=0}^{N-1}(u_{k+i}-u^{ref}\ast k+i{)}^{T}R(u\ast k+i-u_{k+i}^{ref})$$

再加终端权重（可选但常用）：

$$(x_{k+N}-x^{ref}\ast k+N{)}^T{Q}_f(x\ast k+N-x_{k+N}^{ref})$$
为了写成堆叠形式，定义块对角矩阵：

$$\bar{Q}=\mathrm{blkdiag}(Q,Q,\dots ,Q,Q_f)\in {\mathbb{R}}^{nN\times nN}$$

$$\bar{R}=\mathrm{blkdiag}(R,R,\dots ,R)\in {\mathbb{R}}^{mN\times mN}$$

以及参考堆叠：

$$X^{ref}=\left[\begin{array}{c}{x}^{ref}\ast k+1⋮x^{ref}\ast k+N\end{array}\right],U^{ref}=\left[\begin{array}{c}{u}^{ref}\ast k⋮u^{ref}\ast k+N-1\end{array}\right]$$

则代价可写成：

$$J=(X-X^{ref}{)}^T\bar{Q}(X-X^{ref})+(U-U^{ref}{)}^T\bar{R}(U-U^{ref})$$

把 ($X=\mathcal{A}{x}_k+\mathcal{B}U$) 代入，得到只关于 ($U$) 的二次函数：

$$J=(\mathcal{A}{x}_k+\mathcal{B}U-X^{ref}{)}^T\bar{Q}(\mathcal{A}{x}_k+\mathcal{B}U-X^{ref})+(U-U^{ref}{)}^T\bar{R}(U-U^{ref})$$

展开并整理成标准 QP 形式：

$$J=\frac{1}{2}{U}^{T}HU+f^{T}U+\text{const}$$

其中（常用写法，注意 QP 的 (1/2) 约定）

$$H=2({\mathcal{B}}^T\bar{Q}\mathcal{B}+\bar{R})$$

$$f=2{\mathcal{B}}^T\bar{Q}(\mathcal{A}{x}_k-X^{ref})-2\bar{R}{U}^{ref}$$

常数项与优化无关可丢掉。

到这里，已经把 MPC 的“最优控制”变成了一个凸二次规划：H 半正定（通常正定），很好解。

---

3) 约束：把输入/状态/变化率都写成线性不等式

3.1 输入约束

每步：

$$u_{min}\le u_{k+i}\le u_{max}$$

堆叠成：

$$U_{min}\le U\le U_{max}$$

这是一组简单的 box constraints。

3.2 状态约束

每步：

$$x_{min}\le x_{k+i}\le x_{max}$$

堆叠为：

$$X_{min}\le X\le X_{max}$$

代入 ($X=\mathcal{A}{x}_k+\mathcal{B}U$)：

$$X_{min}\le \mathcal{A}{x}_k+\mathcal{B}U\le X_{max}$$

整理成对 ($U$) 的线性不等式（两边拆开）：

$$\mathcal{B}U\le X_{max}-\mathcal{A}{x}_k$$
$$-\mathcal{B}U\le -X_{min}+\mathcal{A}{x}_k$$

3.3 输入变化率约束（抑制抖动/让动作更“丝滑”）

定义：

$$\Delta u_{k+i}=u_{k+i}-u_{k+i-1}$$

把全序列的 ($\Delta U$) 写成矩阵形式：

$$\Delta U=DU+d_0$$

其中 (D) 是“差分矩阵”（尺寸 ($mN\times mN$) 的块矩阵），结构相当于：

• 第一块：($\Delta u_k=u_k-u_{k-1}$) 依赖上一次实际输入 ($u_{k-1}$)
• 后面块：($\Delta u_{k+i}=u_{k+i}-u_{k+i-1}$)

写成块矩阵更清楚（以标量输入为例，m=1）：

$$D=\left[\begin{array}{ccccccccccccccccccccc}1& 0& 0& \cdots & 0-1& 1& 0& \cdots & 00& -1& 1& \cdots & 0⋮& ⋮& ⋮& \ddots & ⋮0& 0& 0& \cdots & 1\end{array}\right]$$

多输入（m>1）就是把每个 “1/-1” 替换成 ($I_m$)。

而 ($d_0$) 把 ($u_{k-1}$) 塞进去：

$$d_0=\left[\begin{array}{c}{u}_{k-1}0⋮0\end{array}\right](\text{对应 m=1；m>1 用向量})$$

约束：

$$\Delta u_{min}\le \Delta U\le \Delta u_{max}$$

也就是：

$$\Delta u_{min}\le DU+d_0\le \Delta u_{max}$$

同样是线性不等式。

要“无震动、无超调”的手感，(\Delta u) 约束 + 代价惩罚几乎是刚需。

---

4) 得到完整的标准 QP

最终每个时刻 (k) 要解：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+f^{T}U$$

subject to（把所有线性约束堆起来）：

$$GU\le h$$
以及可能的 box constraints（可并入 GU≤h 或用求解器专门接口）。

解出 ($U^{\ast }$)，只执行第一步：

$$u_k=[I_{m}0\cdots 0]U^{\ast }$$

下一时刻测量新状态，再滚动求解——这就是 receding horizon MPC。

---

5) 把“jerk / 平滑”直接放进 MPC：两种标准做法

做法A：惩罚 ($\Delta u$)（最简单也最常用）

把代价改成：

$J=(X-X^{ref}{)}^T\bar{Q}(X-X^{ref})+(U-U^{ref}{)}^T\bar{R}(U-U^{ref})+(\Delta U{)}^T\bar{S}(\Delta U)$

其中 ($\bar{S}=\mathrm{blkdiag}(S,\dots ,S)$)。

代入 ($\Delta U=DU+d_0$)，仍是 QP（H 和 f 会多出 ($D^T\bar{S}D)、(D^T\bar{S}{d}_0)$ 这些项）。

做法B：把 jerk 当决策变量（更“轨迹规划化”）

令优化变量是 ($\Delta u$) 或 (${\Delta }^{2}u$)，用积分关系把 (u) 恢复出来。这样可以更直接控制“动作丝滑度”，常用于高观感运动。

---

6) LTV / NMPC 的位置（如果要做机械臂动力学）

• LTV-MPC：每步用当前点线性化得到 ($A_k,B_k$)，然后按上面同样方法构造 ($\mathcal{A},\mathcal{B}$) 解 QP。
• NMPC：直接用非线性 ($x_{k+1}=f(x_k,u_k)$) 做非线性规划（NLP），更强但更贵。

对机械臂“优雅”而言，工程上常见的是：LTV-MPC/轨迹MPC + 内环 PD/阻抗。

---

7) 整体流程总结如下

1. 给定 ($A,B,Q,R,Q_f,N$)
2. 构造 ($\mathcal{A},\mathcal{B}$)
3. 构造 ($\bar{Q},\bar{R}$)
4. 生成 ($H,f$)
5. 构造约束（$Ubox、Xbox、\Delta Ubox$）
6. 解 QP 得到 ($U^{\ast }$)，取第一步执行