Control-模型预测控制(MPC)：从理论推导到MATLAB代码实现

1. 模型预测控制（MPC）基础入门

第一次接触模型预测控制（MPC）时，我完全被那些数学公式吓到了。但后来发现，它的核心思想其实特别直观——就像开车时不断调整方向盘一样简单。MPC本质上是一种"边走边看"的控制策略，它会根据当前状态预测未来几步的系统行为，然后选择最优的控制动作。

举个生活中的例子：当你开车转弯时，眼睛会不断观察前方路况，大脑预测车辆未来几秒的轨迹，然后双手微调方向盘。MPC就是把这个过程数学化了。它包含三个关键步骤：

• 预测模型：相当于你的驾驶经验，知道打多少方向盘会带来怎样的转向效果
• 滚动优化：就像大脑在多个可能的转向方案中选择最安全平稳的那一个
• 反馈校正：每次转动方向盘后，都会根据车辆实际反应调整下一步动作

在工业领域，MPC应用非常广泛。比如化工生产中，需要精确控制反应釜的温度；自动驾驶汽车需要平稳地保持车道；无人机要稳定悬停。这些场景都需要在满足各种约束（如温度上限、转向角度限制）的前提下，实现最优控制。

2. 从连续系统到离散模型

实际工程中，我们处理的都是数字系统，所以需要先把连续的状态空间方程离散化。这就像把连续的视频转换成离散的帧画面。常用的离散化方法有三种：

matlab复制% 欧拉前向差分法
Ad = eye(size(A)) + A*Ts;
Bd = B*Ts;
Dd = D*Ts;

% 后向差分法（更稳定）
Ad = inv(eye(size(A)) - A*Ts);
Bd = Ad*B*Ts;
Dd = Ad*D*Ts;

% 双线性变换（精度最高）
Ad = inv(eye(size(A)) - A*Ts/2) * (eye(size(A)) + A*Ts/2);
Bd = inv(eye(size(A)) - A*Ts/2) * B*Ts;
Dd = inv(eye(size(A)) - A*Ts/2) * D*Ts;

选择哪种方法？根据我的经验：

• 当采样时间很短时，用简单的欧拉法就够了
• 系统刚性较大（特征值差异大）时，后向差分更稳定
• 对精度要求高的场合，双线性变换是首选

离散化后的状态方程变为：

code复制x(k+1) = Ad*x(k) + Bd*u(k) + Dd*d(k)
y(k) = Cd*x(k) + Dd*u(k)

这个方程将成为我们预测未来的基础。

3. 构建预测方程

预测是MPC的核心能力。假设当前时刻为k，我们需要预测未来Np个时刻的系统状态。这就像下棋时，高手会提前计算后面几步的走法。

推导预测方程时，我常用"递推展开法"：

matlab复制% 初始化预测矩阵
F = zeros(Np*nx, nx);    % 状态预测矩阵
Phi = zeros(Np*nx, Nc*nu); % 控制输入矩阵
E = zeros(Np*nx, 1);     % 扰动矩阵

% 构建F矩阵
F(1:nx,:) = Ad;
for i = 2:Np
    F((i-1)*nx+1:i*nx,:) = F((i-2)*nx+1:(i-1)*nx,:) * Ad;
end

% 构建Phi矩阵（关键步骤）
for i = 1:Np
    for j = 1:min(i,Nc)
        Phi((i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu) = ...
            F((i-j)*nx+1:(i-j+1)*nx,:) * Bd;
    end
end

% 构建E矩阵（考虑扰动）
E(1:nx,:) = Dd;
for i = 2:Np
    E((i-1)*nx+1:i*nx,:) = F((i-1)*nx+1:i*nx,:)*Dd + E((i-2)*nx+1:(i-1)*nx,:);
end

最终得到紧凑的预测方程：

code复制X = F*x0 + Phi*U + E

其中X是未来状态序列，U是待求的控制序列。这个方程告诉我们，未来状态取决于当前状态x0、控制输入U和扰动E。

4. 设计代价函数与约束

好的代价函数就像精准的导航仪，能引导系统到达理想状态。最常见的形式是二次型代价：

code复制J = (Y-Yref)'*Q*(Y-Yref) + U'*R*U

这里有个实用技巧：Q和R矩阵的对角元素取值很关键。根据我的项目经验：

• Q矩阵中，对应重要状态（如倒立摆的角度）的权重可以设大些
• R矩阵控制输入权重，太大会导致响应迟缓，太小则控制过于剧烈
• 通常先设R=eye()，然后调整Q使各项达到平衡

约束处理是MPC的另一大优势。常见的约束包括：

matlab复制% 控制量幅值约束
u_min <= u <= u_max

% 控制增量约束
delta_u_min <= u(k)-u(k-1) <= delta_u_max

% 状态约束（如位置限制）
x_min <= x <= x_max

在MATLAB中，这些约束可以转化为QP问题的标准形式：

matlab复制Aineq = [eye(Nc); -eye(Nc)];
bineq = [repmat(u_max, Nc,1); repmat(-u_min, Nc,1)];

5. MATLAB实现技巧

经过前面的理论准备，现在可以动手写代码了。分享几个我在实现过程中总结的实用技巧：

1. 代码结构优化

matlab复制function [u_opt, status] = MPC_Solver(x0, u_prev, ref, params)
    % 参数解包
    Ad = params.Ad; Bd = params.Bd; 
    Q = params.Q; R = params.R;
    Np = params.Np; Nc = params.Nc;
    
    % 构建预测矩阵
    [F, Phi, E] = build_prediction_matrices(Ad, Bd, Np, Nc);
    
    % 构造QP问题
    H = Phi'*Q*Phi + R;
    f = (F*x0 + E - ref)'*Q*Phi;
    
    % 求解QP
    options = optimoptions('quadprog','Display','none');
    [u_seq, ~, exitflag] = quadprog(H, f', Aineq, bineq, [], [], [], [], [], options);
    
    % 返回第一个控制量
    u_opt = u_seq(1:size(Bd,2));
    status = exitflag > 0;
end

2. 数值稳定性处理

• 添加正则化项：H = H + 1e-6*eye(size(H))避免奇异
• 缩放变量：把不同量纲的状态归一化到相近范围
• 使用更稳定的QP求解器，如OSQP

3. 实时性优化

• 预先计算不变的部分（如H矩阵中Phi'QPhi）
• 使用热启动（warm start），用上一周期的解作为初始猜测
• 对于快速系统，可以适当减小预测时域Np

6. 完整案例：倒立摆控制

让我们用一个经典案例把前面内容串起来。倒立摆系统状态包括：

• 小车位置x
• 小车速度v
• 摆杆角度θ
• 摆杆角速度ω

步骤1：建模

matlab复制% 连续状态空间模型
A = [0 1 0 0;
     0 0 -m*g/M 0;
     0 0 0 1;
     0 0 (M+m)*g/(M*l) 0];
B = [0; 1/M; 0; -1/(M*l)];
C = eye(4);
D = zeros(4,1);

% 离散化
Ts = 0.05; % 采样时间50ms
sysd = c2d(ss(A,B,C,D), Ts);
Ad = sysd.A; Bd = sysd.B;

步骤2：MPC参数设置

matlab复制params.Np = 20;   % 预测时域
params.Nc = 5;    % 控制时域
params.Q = diag([10,1,100,10]); % 重点控制角度
params.R = 0.1;   % 控制权重
params.u_max = 10; % 最大推力
params.u_min = -10;

步骤3：仿真循环

matlab复制x = [0;0;0.1;0]; % 初始小角度偏移
for k = 1:100
    ref = zeros(4*params.Np,1); % 目标：全零状态
    
    [u, status] = MPC_Solver(x, u_prev, ref, params);
    
    % 系统仿真
    x = Ad*x + Bd*u;
    
    % 记录数据
    X_log(:,k) = x;
    U_log(k) = u;
end

调试技巧：

1. 先去掉所有约束，检查无约束控制效果
2. 逐步添加约束，观察系统响应变化
3. 如果出现震荡，适当增大R或减小Q
4. 对于不稳定系统，先用LQR稳定后再加MPC

7. 进阶话题与避坑指南

在实际项目中，我遇到过不少坑，这里分享几个典型问题及解决方案：

问题1：计算延迟影响
真实系统存在计算延迟，我的处理方法是：

matlab复制% 在预测时考虑一步延迟
x0 = Ad*x_actual + Bd*u_previous;

问题2：模型失配
当模型不准确时，可以：

1. 增加状态扰动观测器
2. 自适应地在线更新模型参数
3. 加强反馈校正环节的权重

问题3：实时性不足
对于计算复杂的系统：

• 使用显式MPC（预先计算好控制律）
• 采用次优解，提前终止QP迭代
• 考虑降阶模型简化计算

问题4：参数整定困难
我总结的参数调试流程：

1. 先调Q矩阵，使状态收敛
2. 再调R矩阵，平滑控制输入
3. 最后调整预测时域Np和控制时域Nc
4. 必要时加入终端代价(terminal cost)

对于想深入研究的同学，推荐尝试：

• 非线性MPC（需要更复杂的优化算法）
• 鲁棒MPC（考虑模型不确定性）
• 分布式MPC（大型系统分解）

在无人机项目中，我们使用MPC实现了精确轨迹跟踪。开始时遇到控制滞后问题，后来通过调整预测时域和加入风速观测器，最终实现了厘米级定位精度。这让我深刻体会到，好的控制算法需要理论与实践反复迭代。