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);

但实际跑起来会发现，当摆杆从下垂状态启动时，这个约束会让求解器直接摆烂。这时候得在目标函数里加个松弛变量，就像给控制量穿上弹性裤衩。

MPC+NMPC模型预测控制从原理与代码实现组合装。 MPC包括： mpc模型预测控制详细原理推导 matlab和c++两种编程实现 四个实际控制工程案例： 双积分控制系统 倒立摆控制系统 车辆运动学跟踪控制系统 车辆动力学跟踪控制系统 NMPC包括： NMPC模型预测控制详细原理推导 平行泊车轨迹规划 倒立摆Swing up控制 车辆运动学模型轨迹跟踪 四旋翼无人机轨迹跟踪 包含上述所有的文档和代码。

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和能跑通的版本）