MPC+NMPC模型预测控制从原理与代码实现组合装。 MPC包括： mpc模型预测控制详细原理推导 matlab和c++两种编程实现 四个实际控制工程案例： 双积分控制系统 倒立摆控制系统 车辆运动学跟踪控制系统 车辆动力学跟踪控制系统 NMPC包括： NMPC模型预测控制详细原理推导 平行泊车轨迹规划 倒立摆Swing up控制 车辆运动学模型轨迹跟踪 四旋翼无人机轨迹跟踪 包含上述所有的文档和代码。
你盯着屏幕上疯狂抖动的倒立摆仿真动画，手心微微出汗。这已经是本周第七次尝试复现论文里的MPC控制效果，每次仿真到第8秒摆杆就给你表演自由落体。别慌，今天咱们换个姿势——直接把手插进代码里看看控制律到底怎么算的。
MPC：把未来装进矩阵的男人
模型预测控制的核心骚操作在于把未来N步的状态预测写成一个超大的矩阵方程。假设咱有个离散状态空间模型x(k+1)=Ax(k)+Bu(k)，把这玩意儿展开成预测形式：
% 构建预测矩阵（Matlab版） function [Phi, Gamma] = build_prediction_matrix(A,B,N) Phi = zeros(size(A,1)*N, size(A,1)); Gamma = zeros(size(A,1)*N, size(B,2)*N); for i=1:N rows = (i-1)*size(A,1)+1 : i*size(A,1); Phi(rows,:) = A^i; for j=1:i cols = (j-1)*size(B,2)+1 : j*size(B,2); Gamma(rows,cols) = A^(i-j)*B; end end end这代码干了个狠活——把未来N步的状态x(k+1)...x(k+N)全部表示成当前状态x(k)和控制量序列U的线性组合。当你用quadprog求解二次规划时，目标函数里的H矩阵实际上藏着系统动态的DNA。
C++老哥的暴力美学
切换到C++实现，Eigen库处理矩阵像切黄油：
// 预测方程构建（C++片段） MatrixXd build_cost_H(const MatrixXd& Q, const MatrixXd& R, const MatrixXd& Gamma, int N) { MatrixXd H = MatrixXd::Zero(N*u_dim, N*u_dim); // 填充Q矩阵块 for(int i=0; i<N; ++i){ H.block(i*u_dim, i*u_dim, u_dim, u_dim) = R; if(i < N-1) H.block(i*x_dim, i*x_dim, x_dim, x_dim) += Q; } return Gamma.transpose() * H * Gamma; // 这才是真正的技术 }这里在玩一个矩阵俄罗斯方块——把权重矩阵Q和R按时间步拼装成块对角矩阵，再和预测矩阵Gamma做三明治乘法。当你在调试时看到H矩阵突然变得正定，那种快感堪比游戏通关。
倒立摆：在悬崖边跳华尔兹
拿倒立摆的案例开刀，约束处理是灵魂操作。假设电机最大扭矩5N·m，代码里要这么搞：
% 输入约束设置 u_min = -5*ones(N,1); u_max = 5*ones(N,1); A_ineq = []; b_ineq = []; A_eq = []; b_eq = []; options = optimoptions('quadprog','Algorithm','active-set'); [U_opt,~,exitflag] = quadprog(H,f,A_ineq,b_ineq,A_eq,b_eq,u_min,u_max,[],options);但实际跑起来会发现，当摆杆从下垂状态启动时，这个约束会让求解器直接摆烂。这时候得在目标函数里加个松弛变量，就像给控制量穿上弹性裤衩。
NMPC：非线性系统的变形金刚
切到NMPC画风突变。平行泊车问题中，车辆模型是妥妥的非线性：
# 车辆运动学模型（CasADi版） import casadi as ca x = ca.MX.sym('x'); y = ca.MX.sym('y'); theta = ca.MX.sym('theta') v = ca.MX.sym('v'); phi = ca.MX.sym('phi') dt = 0.1 L = 2.5 # 轴距 # 微分方程 dxdt = ca.vertcat( v*ca.cos(theta), v*ca.sin(theta), v*ca.tan(phi)/L ) F = ca.Function('f', [x,y,theta,v,phi], [dxdt])用CasADi做自动微分时，它会偷偷生成C代码来加速计算。当你在实时测试中看到求解时间从200ms优化到20ms，会感觉像给算法打了类固醇。
无人机：在四维空间绣花
四旋翼轨迹跟踪的代价函数设计是个艺术活。不仅要惩罚位置误差，还得让横滚/俯仰角变化温柔点：
// 代价函数计算（C++片段） double cost = 0; for(int i=0; i<N; ++i){ // 位置误差 cost += 10*pow(x[3*i] - ref_x[i],2); cost += 10*pow(x[3*i+1] - ref_y[i],2); // 姿态角变化率惩罚 if(i>0){ cost += 0.1*pow((x[3*i+2] - x[3*(i-1)+2])/dt,2); } }调参时发现10这个权重系数能让无人机在风扰下稳如老狗，但再大会导致电机过热——这参数是拿炸机次数换来的经验值。
从仿真到实战的黑暗森林
在车辆动力学跟踪中，当实车测试时发现MPC在80km/h时开始画蛇，原来是轮胎模型在代码里被简化成了线性。赶紧掏出魔术贴——在预测模型里塞入Pacejka轮胎公式，这时NMPC的求解时间飙升。最后祭出大杀器：用C++重写求解器核心，加上warm start技巧，终于让控制频率达标。
（代码仓库见评论区，包含带血丝的调试log和能跑通的版本）
