写在前面
pid作为一种通用的泛用的控制方案，在某些工况下还是难以满足需求，比如动作模仿、轨迹规划、调速控制等，只要控制目标涉及到运动的动态过程，pid的效果总是差强人意；
因此，需要引入MPC(模型预测控制)，直接针对运动过程中的加速度/速度/位置等状态量直接控制，在更大程度上控制运动的动态。
具体推导
以最常用的离散线性 MPC / LTV-MPC为例，从模型到 QP（二次规划）
问题设置
离散时间线性系统（也可视作每步线性化后的 LTV）：
x k + 1 = A x k + B u k x_{k+1}=A x_k + B u_k xk+1=Axk+Buk
- ( x k ∈ R n ) (x_k\in\mathbb{R}^{n}) (xk∈Rn)：状态
- ( u k ∈ R m ) (u_k\in\mathbb{R}^{m}) (uk∈Rm)：控制输入
- 预测时域 (N)：向前看 N 步
- 参考轨迹： ( x r e f ∗ k + i , u r e f ∗ k + i ) (x^{ref}*{k+i}, u^{ref}*{k+i}) (xref∗k+i,uref∗k+i)
定义误差变量（更方便写代价）：
x ~ k = x k − x k r e f , u ~ k = u k − u k r e f \tilde x_{k}=x_k-x^{ref}_k,\quad \tilde u_k=u_k-u^{ref}_k x~k=xk−xkref,u~k=uk−ukref
若参考是常值或零，直接把 ref 省略即可。
1) 预测模型展开：把未来状态写成"当前状态 + 未来输入"的线性函数
定义未来控制序列（决策变量）：
U ≜ [ u k u k + 1 ⋮ u k + N − 1 ] ∈ R m N U \triangleq \begin{bmatrix} u_{k} \ u_{k+1}\ \vdots \ u_{k+N-1} \end{bmatrix}\in\mathbb{R}^{mN} U≜[uk uk+1 ⋮ uk+N−1]∈RmN
未来状态堆叠向量：
X ≜ [ x k + 1 x k + 2 ⋮ x k + N ] ∈ R n N X \triangleq \begin{bmatrix} x_{k+1}\ x_{k+2}\ \vdots\ x_{k+N} \end{bmatrix}\in\mathbb{R}^{nN} X≜[xk+1 xk+2 ⋮ xk+N]∈RnN
逐步展开：
- ( x k + 1 = A x k + B u k ) (x_{k+1}=A x_k + B u_k) (xk+1=Axk+Buk)
- ( x k + 2 = A x k + 1 + B u k + 1 = A 2 x k + A B u k + B u k + 1 ) (x_{k+2}=A x_{k+1}+B u_{k+1}=A^2 x_k + AB u_k + B u_{k+1}) (xk+2=Axk+1+Buk+1=A2xk+ABuk+Buk+1)
- ( x k + 3 = A 3 x k + A 2 B u k + A B u k + 1 + B u k + 2 ) (x_{k+3}=A^3 x_k + A^2B u_k + AB u_{k+1}+B u_{k+2}) (xk+3=A3xk+A2Buk+ABuk+1+Buk+2)
- ...
于是得到紧凑形式：
X = A x k + B U X = \mathcal{A}x_k + \mathcal{B}U X=Axk+BU
其中
A = [ A A 2 ⋮ A N ] ∈ R n N × n \mathcal{A}= \begin{bmatrix} A\ A^2\ \vdots \ A^N \end{bmatrix} \in\mathbb{R}^{nN\times n} A=[A A2 ⋮ AN]∈RnN×n
B = [ B 0 0 ⋯ 0 A B B 0 ⋯ 0 A 2 B A B B ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ A N − 1 B A N − 2 B A N − 3 B ⋯ B ] ∈ R n N × m N \mathcal{B}= \begin{bmatrix} B & 0 & 0 & \cdots & 0\ AB & B & 0 & \cdots & 0\ A^2B & AB & B & \cdots & 0\ \vdots & \vdots & \vdots & \ddots & \vdots\ A^{N-1}B & A^{N-2}B & A^{N-3}B & \cdots & B \end{bmatrix} \in\mathbb{R}^{nN\times mN} B=[B00⋯0 ABB0⋯0 A2BABB⋯0 ⋮⋮⋮⋱⋮ AN−1BAN−2BAN−3B⋯B]∈RnN×mN
（这是 MPC 推导里最核心的一步：预测矩阵。）
如果是 LTV（每步 ( A i , B i A_i,B_i Ai,Bi) 不同），形式仍是 ( X = A x k + B U X=\mathcal{A}x_k+\mathcal{B}U X=Axk+BU)，只不过 ( A , B \mathcal{A},\mathcal{B} A,B) 由链式乘积构造（代码里按递推填块矩阵即可）。
2) 代价函数：二次型 + 终端项（标准 LQR 风格）
最常见的 MPC 目标是"跟踪 + 控制努力"：
J = ∑ i = 1 N ( x k + i − x r e f ∗ k + i ) T Q ( x ∗ k + i − x k + i r e f ) J = \sum_{i=1}^{N} (x_{k+i}-x^{ref}*{k+i})^T Q (x*{k+i}-x^{ref}_{k+i}) J=i=1∑N(xk+i−xref∗k+i)TQ(x∗k+i−xk+iref)
$ ∑ i = 0 N − 1 ( u k + i − u r e f ∗ k + i ) T R ( u ∗ k + i − u k + i r e f ) \sum_{i=0}^{N-1} (u_{k+i}-u^{ref}*{k+i})^T R (u*{k+i}-u^{ref}_{k+i}) i=0∑N−1(uk+i−uref∗k+i)TR(u∗k+i−uk+iref)
再加终端权重（可选但常用）：
( x k + N − x r e f ∗ k + N ) T Q f ( x ∗ k + N − x k + N r e f ) (x_{k+N}-x^{ref}*{k+N})^T Q_f (x*{k+N}-x^{ref}_{k+N}) (xk+N−xref∗k+N)TQf(x∗k+N−xk+Nref)
为了写成堆叠形式，定义块对角矩阵：
Q ˉ = b l k d i a g ( Q , Q , ... , Q , Q f ) ∈ R n N × n N \bar Q=\mathrm{blkdiag}(Q,Q,\dots,Q,Q_f)\in\mathbb{R}^{nN\times nN} Qˉ=blkdiag(Q,Q,...,Q,Qf)∈RnN×nN
R ˉ = b l k d i a g ( R , R , ... , R ) ∈ R m N × m N \bar R=\mathrm{blkdiag}(R,R,\dots,R)\in\mathbb{R}^{mN\times mN} Rˉ=blkdiag(R,R,...,R)∈RmN×mN
以及参考堆叠：
X r e f = [ x r e f ∗ k + 1 ⋮ x r e f ∗ k + N ] , U r e f = [ u r e f ∗ k ⋮ u r e f ∗ k + N − 1 ] X^{ref}= \begin{bmatrix} x^{ref}*{k+1}\ \vdots\ x^{ref}*{k+N} \end{bmatrix},\quad U^{ref}= \begin{bmatrix} u^{ref}*{k}\ \vdots\ u^{ref}*{k+N-1} \end{bmatrix} Xref=[xref∗k+1 ⋮ xref∗k+N],Uref=[uref∗k ⋮ uref∗k+N−1]
则代价可写成：
J = ( X − X r e f ) T Q ˉ ( X − X r e f ) + ( U − U r e f ) T R ˉ ( U − U r e f ) J = (X-X^{ref})^T\bar Q (X-X^{ref}) + (U-U^{ref})^T\bar R (U-U^{ref}) J=(X−Xref)TQˉ(X−Xref)+(U−Uref)TRˉ(U−Uref)
把 ( X = A x k + B U X=\mathcal{A}x_k+\mathcal{B}U X=Axk+BU) 代入，得到只关于 ( U U U) 的二次函数：
J = ( A x k + B U − X r e f ) T Q ˉ ( A x k + B U − X r e f ) + ( U − U r e f ) T R ˉ ( U − U r e f ) 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}) J=(Axk+BU−Xref)TQˉ(Axk+BU−Xref)+(U−Uref)TRˉ(U−Uref)
展开并整理成标准 QP 形式：
J = 1 2 U T H U + f T U + const J = \frac12 U^T H U + f^T U + \text{const} J=21UTHU+fTU+const
其中（常用写法，注意 QP 的 (1/2) 约定）
H = 2 ( B T Q ˉ B + R ˉ ) H = 2(\mathcal{B}^T\bar Q\mathcal{B}+\bar R) H=2(BTQˉB+Rˉ)
f = 2 B T Q ˉ ( A x k − X r e f ) − 2 R ˉ U r e f f = 2\mathcal{B}^T\bar Q(\mathcal{A}x_k - X^{ref}) - 2\bar R U^{ref} f=2BTQˉ(Axk−Xref)−2RˉUref
常数项与优化无关可丢掉。
到这里，已经把 MPC 的"最优控制"变成了一个凸二次规划：H 半正定（通常正定），很好解。
3) 约束：把输入/状态/变化率都写成线性不等式
3.1 输入约束
每步：
u m i n ≤ u k + i ≤ u m a x u_{min}\le u_{k+i}\le u_{max} umin≤uk+i≤umax
堆叠成：
U m i n ≤ U ≤ U m a x U_{min}\le U\le U_{max} Umin≤U≤Umax
这是一组简单的 box constraints。
3.2 状态约束
每步：
x m i n ≤ x k + i ≤ x m a x x_{min}\le x_{k+i}\le x_{max} xmin≤xk+i≤xmax
堆叠为：
X m i n ≤ X ≤ X m a x X_{min}\le X\le X_{max} Xmin≤X≤Xmax
代入 ( X = A x k + B U X=\mathcal{A}x_k+\mathcal{B}U X=Axk+BU)：
X m i n ≤ A x k + B U ≤ X m a x X_{min}\le \mathcal{A}x_k+\mathcal{B}U \le X_{max} Xmin≤Axk+BU≤Xmax
整理成对 ( U U U) 的线性不等式（两边拆开）：
B U ≤ X m a x − A x k \mathcal{B}U \le X_{max}-\mathcal{A}x_k BU≤Xmax−Axk
− B U ≤ − X m i n + A x k -\mathcal{B}U \le -X_{min}+\mathcal{A}x_k −BU≤−Xmin+Axk
3.3 输入变化率约束（抑制抖动/让动作更"丝滑"）
定义：
Δ u k + i = u k + i − u k + i − 1 \Delta u_{k+i}=u_{k+i}-u_{k+i-1} Δuk+i=uk+i−uk+i−1
把全序列的 ( Δ U \Delta U ΔU) 写成矩阵形式：
Δ U = D U + d 0 \Delta U = D U + d_0 ΔU=DU+d0
其中 (D) 是"差分矩阵"（尺寸 ( m N × m N mN\times mN mN×mN) 的块矩阵），结构相当于：
- 第一块：( Δ u k = u k − u k − 1 \Delta u_k = u_k - u_{k-1} Δuk=uk−uk−1) 依赖上一次实际输入 ( u k − 1 u_{k-1} uk−1)
- 后面块：( Δ u k + i = u k + i − u k + i − 1 \Delta u_{k+i} = u_{k+i} - u_{k+i-1} Δuk+i=uk+i−uk+i−1)
写成块矩阵更清楚（以标量输入为例，m=1）：
D = [ 1 0 0 ⋯ 0 − 1 1 0 ⋯ 0 0 − 1 1 ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 ⋯ 1 ] D= \begin{bmatrix} 1 & 0 & 0 & \cdots & 0\ -1& 1 & 0 & \cdots & 0\ 0 & -1& 1 & \cdots & 0\ \vdots & \vdots & \vdots & \ddots & \vdots\ 0 & 0 & 0 & \cdots & 1 \end{bmatrix} D=[100⋯0 −110⋯0 0−11⋯0 ⋮⋮⋮⋱⋮ 000⋯1]
多输入（m>1）就是把每个 "1/-1" 替换成 ( I m I_m Im)。
而 ( d 0 d_0 d0) 把 ( u k − 1 u_{k-1} uk−1) 塞进去：
d 0 = [ u k − 1 0 ⋮ 0 ] ( 对应 m=1；m>1 用向量 ) d_0= \begin{bmatrix} u_{k-1}\ 0\ \vdots \ 0 \end{bmatrix} \quad(\text{对应 m=1；m>1 用向量}) d0=[uk−1 0 ⋮ 0](对应 m=1；m>1 用向量)
约束：
Δ u m i n ≤ Δ U ≤ Δ u m a x \Delta u_{min}\le \Delta U \le \Delta u_{max} Δumin≤ΔU≤Δumax
也就是：
Δ u m i n ≤ D U + d 0 ≤ Δ u m a x \Delta u_{min}\le DU+d_0 \le \Delta u_{max} Δumin≤DU+d0≤Δumax
同样是线性不等式。
要"无震动、无超调"的手感，(\Delta u) 约束 + 代价惩罚几乎是刚需。
4) 得到完整的标准 QP
最终每个时刻 (k) 要解：
min U 1 2 U T H U + f T U \min_{U}\ \frac12 U^T H U + f^T U Umin 21UTHU+fTU
subject to（把所有线性约束堆起来）：
G U ≤ h G U \le h GU≤h
以及可能的 box constraints（可并入 GU≤h 或用求解器专门接口）。
解出 ( U ∗ U^* U∗)，只执行第一步：
u k = [ I m 0 ⋯ 0 ] U ∗ u_k = [I_m\ 0\ \cdots\ 0]\ U^* uk=[Im 0 ⋯ 0] U∗
下一时刻测量新状态，再滚动求解------这就是 receding horizon MPC。
5) 把"jerk / 平滑"直接放进 MPC：两种标准做法
做法A：惩罚 ( Δ u \Delta u Δu)（最简单也最常用）
把代价改成：
J = ( X − X r e f ) T Q ˉ ( X − X r e f ) + ( U − U r e f ) T R ˉ ( U − U r e f ) + ( Δ U ) T S ˉ ( Δ 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) J=(X−Xref)TQˉ(X−Xref)+(U−Uref)TRˉ(U−Uref)+(ΔU)TSˉ(ΔU)
其中 ( S ˉ = b l k d i a g ( S , ... , S ) \bar S=\mathrm{blkdiag}(S,\dots,S) Sˉ=blkdiag(S,...,S))。
代入 ( Δ U = D U + d 0 \Delta U = D U + d_0 ΔU=DU+d0)，仍是 QP（H 和 f 会多出 ( D T S ˉ D ) 、 ( D T S ˉ d 0 ) D^T\bar S D)、(D^T\bar S d_0) DTSˉD)、(DTSˉd0) 这些项）。
做法B：把 jerk 当决策变量（更"轨迹规划化"）
令优化变量是 ( Δ u \Delta u Δu) 或 ( Δ 2 u \Delta^2 u Δ2u)，用积分关系把 (u) 恢复出来。这样可以更直接控制"动作丝滑度"，常用于高观感运动。
6) LTV / NMPC 的位置（如果要做机械臂动力学）
- LTV-MPC ：每步用当前点线性化得到 ( A k , B k A_k,B_k Ak,Bk)，然后按上面同样方法构造 ( A , B \mathcal{A},\mathcal{B} A,B) 解 QP。
- NMPC ：直接用非线性 ( x k + 1 = f ( x k , u k ) x_{k+1}=f(x_k,u_k) xk+1=f(xk,uk)) 做非线性规划（NLP），更强但更贵。
对机械臂"优雅"而言，工程上常见的是：LTV-MPC/轨迹MPC + 内环 PD/阻抗。
7) 整体流程总结如下
- 给定 ( A , B , Q , R , Q f , N A,B,Q,R,Q_f,N A,B,Q,R,Qf,N)
- 构造 ( A , B \mathcal{A},\mathcal{B} A,B)
- 构造 ( Q ˉ , R ˉ \bar Q,\bar R Qˉ,Rˉ)
- 生成 ( H , f H,f H,f)
- 构造约束（ U b o x 、 X b o x 、 Δ U b o x U box、X box、ΔU box Ubox、Xbox、ΔUbox）
- 解 QP 得到 ( U ∗ U^* U∗)，取第一步执行