接下来三个例子教你入门mpc，为了体现视频的高质量，在倒立摆和二自由度模型预测控制方面，我会给出一种基于状态变量微分的实时线性化策略，经过这样处理的mpc实际可以看作是nmpc。

1.一阶倒立摆MPC

1.1倒立摆状态方程

1.2倒立摆状态方程线性化

为了将上述方程写成的形式对上式进行整理可得：

F对X求偏导可得A：

F对u求偏导可得B：

剩余项：

注意：此时已经完成了状态方程的线性化，这是实时线性化的重要步骤，并不需要带入特殊点（如目标点）得到线性化的具体值，我们只需要把A和B的表达式存起来，然后在控制过程中，带入当前状态量进去，在当前状态下调用表达式得到具体线性化的值即可，这样就能够避免出现由于倒立摆初始状态不在目标点附近导致的不可控问题（后续视频将会展示）。

转化为matlab代码的形式如下：

syms mc mp L q dq x dx f g realM=[mc+mp,mp*L*cos(q);
mp*L*cos(q),mp*L*L];
C=[0,-mp*L*sin(q)*dq;
0,0];
G=[0;-mp*g*L*sin(q)];
tao=[f;0];Z=[x;q;dx;dq];
F=[dx;dq;simplify(inv(M)*(tao-G-C*[dx;dq]))];%[dx;dq;ddx;ddq]A=[diff(F(1),Z(1)),diff(F(1),Z(2)),diff(F(1),Z(3)),diff(F(1),Z(4));
diff(F(2),Z(1)),diff(F(2),Z(2)),diff(F(2),Z(3)),diff(F(2),Z(4));
diff(F(3),Z(1)),diff(F(3),Z(2)),diff(F(3),Z(3)),diff(F(3),Z(4));
diff(F(4),Z(1)),diff(F(4),Z(2)),diff(F(4),Z(3)),diff(F(4),Z(4))];B=simplify(diff(F,f));

1.3状态方程离散化

 采用前向欧拉法将状态方程离散化：

 2.二自由度MPC

略。

3.六自由度mpc

4.视频效果

技术/代码交流邮箱/企鹅（欢迎交流、讨论、私信）：3531225003@qq.com