MPC模型预测控制原理与工程实践

1. MPC模型预测控制基础理论

模型预测控制(Model Predictive Control, MPC)作为现代控制理论中的重要分支，其核心思想是通过在线求解有限时域内的最优控制问题来实现系统的控制目标。与传统的PID控制相比，MPC具有处理多变量、约束和时滞等复杂情况的天然优势。

1.1 MPC基本原理与工作流程

MPC的工作流程可以概括为"预测-优化-执行"的循环：

1. 预测模型：基于当前系统状态和数学模型，预测未来一段时间内的系统行为
2. 优化求解：在预测时域内求解最优控制序列，使性能指标最小化
3. 滚动实施：仅执行最优控制序列的第一个控制量，下一时刻重新开始整个过程

这种滚动时域优化策略使MPC能够不断修正预测误差，适应系统变化。在实际工程中，MPC控制器通常以10-100Hz的频率运行，具体取决于被控对象的动态特性。

关键点：MPC的预测时域和控制时域是两个重要参数。预测时域决定了控制器"看多远"，而控制时域决定了优化变量的数量。通常预测时域应覆盖系统的主要动态响应时间。

1.2 数学建模与离散化处理

建立准确的数学模型是MPC实现的基础。对于连续时间系统，我们通常采用状态空间描述：

code复制dx/dt = Ax + Bu
y = Cx + Du

在实际应用中，我们需要将其离散化为：

code复制x(k+1) = A_d x(k) + B_d u(k)
y(k) = C_d x(k) + D_d u(k)

离散化方法有多种选择，最常用的是零阶保持法(ZOH)：

matlab复制% MATLAB中的离散化示例
sys = ss(A,B,C,D);
Ts = 0.1; % 采样时间
sysd = c2d(sys,Ts,'zoh');
[A_d,B_d,C_d,D_d] = ssdata(sysd);

对于非线性系统，通常采用线性化或直接使用非线性模型。在倒立摆案例中，我们可以在平衡点附近进行线性化处理，得到局部线性模型。

2. MPC核心算法实现

2.1 二次规划问题构建

MPC的核心是将控制问题转化为二次规划(QP)问题。考虑如下代价函数：

code复制J = Σ [x'(k)Qx(k) + u'(k)Ru(k)] + x'(N)Px(N)

其中Q、R分别为状态和控制量的权重矩阵，P为终端代价。通过预测模型，我们可以将所有未来状态表示为初始状态和控制量的线性组合：

code复制X = Fx0 + GU

其中X是状态序列，U是控制序列，F和G是由系统矩阵构成的预测矩阵。最终QP问题形式为：

code复制min U' H U + 2f' U
s.t. LU ≤ b

2.2 MATLAB实现详解

在提供的SolveLinearMPC.m函数中，关键步骤包括：

1. 预测矩阵构建：通过嵌套循环计算状态转移矩阵

matlab复制for i = 1:N
    for j = 1:N
        if i < j
            k{i, j} = b*0;
        else
            k{i,j} = a^(i-j)*b;
        end
    end
end
K = cell2mat(k);

2. QP问题构建：将MPC问题转化为标准QP形式

matlab复制M1 = (K.')*Q*K+R;
M2 = (K.')*Q*(M-refs);

3. 约束处理：设置控制量的上下限

matlab复制[control,~,~,~,~] = quadprog(M1,M2,[],[],[],[],ll, uu);

实际工程中，我们还需要考虑状态约束、控制增量约束等更复杂的情况。这时需要扩展约束矩阵L和b。

2.3 C++实现要点

对于实时性要求高的应用，C++实现更为适合。关键步骤包括：

1. 使用Eigen等矩阵库高效实现矩阵运算
2. 采用qpOASES等专用QP求解器
3. 注意内存预分配避免动态内存分配
4. 实现定点数运算以提高确定性

一个简化的C++实现框架：

cpp复制class MPCController {
public:
    void setup(const MatrixXd& A, const MatrixXd& B, int horizon);
    VectorXd solve(const VectorXd& x0, const VectorXd& ref);
private:
    MatrixXd H, F;
    MatrixXd A_, B_;
    int horizon_;
    qpOASES::SQProblem qp_solver_;
};

3. 典型应用案例实现

3.1 双积分系统控制

双积分系统是最基础的MPC测试案例，其状态空间方程为：

code复制dx1/dt = x2
dx2/dt = u

在DoubleInt_mpc.m中的实现要点：

1. 系统矩阵定义：

matlab复制A = [0 1; 0 0];
B = [0; 1];

2. 权重矩阵设计：

matlab复制Q = diag([10, 1]); % 位置误差权重大于速度误差
R = 0.1; % 控制量权重

3. 约束设置：

matlab复制u_min = -2; % 最小控制量
u_max = 2;  % 最大控制量

调试技巧：当系统出现振荡时，可以适当增大R或减小Q中的速度权重。如果响应过慢，则可以反向调整。

3.2 倒立摆控制

倒立摆是非线性系统的典型代表。在InvertedPendulum_mpc.m中，我们采用线性化模型：

1. 线性化模型获取：

matlab复制[sys,~] = linearizeInvertedPendulum(m,M,l,g);
[A,B] = ssdata(sys);

2. 扩展状态空间（包含积分项）：

matlab复制A_ext = [A zeros(4,1); -C 0];
B_ext = [B; 0];

3. 抗饱和处理：

matlab复制function u = antiWindup(u_raw, u_prev, limit)
    if abs(u_raw) > limit
        u = sign(u_raw)*limit;
    else
        u = u_raw;
    end
end

实际调试中发现，倒立摆的MPC控制对采样时间非常敏感。建议采样时间在20-50ms之间，预测时域应覆盖至少一个摆动周期。

4. 车辆控制应用深入解析

4.1 车辆模型构建

在车辆控制案例中，我们实现了两种模型：

1. 运动学模型：适用于低速场景（<5m/s）

matlab复制function dx = vehicleKinematicModel(x, u)
    % x = [X; Y; phi; v]
    % u = [a; delta]
    dx = zeros(4,1);
    dx(1) = x(4)*cos(x(3));
    dx(2) = x(4)*sin(x(3));
    dx(3) = x(4)*tan(u(2))/L;
    dx(4) = u(1);
end

2. 动力学模型：考虑轮胎力、载荷转移等

matlab复制function dx = vehicleDynamicModel(x, u, mu)
    % x = [vx; vy; omega; e; theta; s]
    % u = [delta; a]
    Fxf = mu*m*g*(a/(a+b))*(u(2)/g);
    Fxr = mu*m*g*(b/(a+b))*(u(2)/g);
    % 详细力计算...
end

4.2 轨迹生成算法

GenRefLineSegment.m中实现的五次多项式轨迹生成：

1. 横向规划：

matlab复制function y = lateralPoly5(t, T, y0, dy0, ddy0, yT, dyT, ddyt)
    A = [1    0    0      0      0       0;
         0    1    0      0      0       0;
         0    0    2      0      0       0;
         1    T    T^2    T^3    T^4     T^5;
         0    1    2T     3T^2   4T^3    5T^4;
         0    0    2      6T     12T^2   20T^3];
    b = [y0; dy0; ddy0; yT; dyT; ddyt];
    coeff = A\b;
    y = coeff'*[1; t; t^2; t^3; t^4; t^5];
end

2. 纵向规划：

matlab复制function s = longitudinalPoly4(t, T, s0, ds0, dds0, sT, dsT)
    % 类似横向规划，但使用四次多项式
end

实际应用中发现，轨迹曲率的连续性对控制效果影响很大。建议曲率变化率不超过0.1-0.3 m^(-1)/s，具体取决于车辆性能和乘客舒适性要求。

4.3 MPC控制器实现细节

车辆MPC控制的关键实现环节：

1. 误差计算：

matlab复制function [err, psi_err] = calcError(veh_pos, ref_pos)
    % 计算横向误差和航向角误差
    dx = veh_pos(1) - ref_pos(1);
    dy = veh_pos(2) - ref_pos(2);
    psi_err = veh_pos(3) - ref_pos(3);
    err = -dx*sin(ref_pos(3)) + dy*cos(ref_pos(3));
end

2. 曲率前馈：

matlab复制delta_ff = atan2(L*k, 1);

3. 权重调整策略：

matlab复制if v > 10
    Q(1,1) = 5; % 高速时增大横向误差权重
else
    Q(1,1) = 2;
end

在实车测试中，我们发现MPC参数需要根据车速自适应调整。建议建立参数与车速的映射关系表，实现平滑过渡。

5. 工程实践中的关键问题

5.1 实时性保障

确保MPC能在规定时间内完成计算是关键挑战：

1. 代码优化：

   • 预计算不变矩阵
   • 使用更高效的QP求解器
   • 采用热启动技术

2. 降阶处理：

   • 模型降阶
   • 减少预测时域
   • 稀疏矩阵利用

3. 硬件加速：

   • GPU加速
   • FPGA实现
   • 多核并行计算

5.2 鲁棒性设计

提高MPC鲁棒性的常用方法：

1. 扰动估计与补偿：

matlab复制d_hat = observer(x_meas, u);
Bd = ...; % 扰动矩阵
x_pred = A*x + B*u + Bd*d_hat;

2. Tube MPC：
   设计鲁棒不变集，保证状态在允许范围内

3. 参数自适应：

matlab复制function updateModelParameters(v, mu)
    % 根据车速和路面摩擦系数更新模型
end

5.3 调试与验证

MPC系统的调试建议：

1. 分阶段验证：

   • 先开环验证预测模型
   • 然后测试QP求解
   • 最后闭环测试

2. 可视化工具：

matlab复制function plotMPCDebug(info)
    % 绘制预测轨迹、实际轨迹、控制量等
end

3. 参数整定步骤：
   1. 先调Q矩阵，确保状态收敛
   2. 再调R矩阵，平滑控制量
   3. 最后调整约束边界

在车辆控制项目中，我们开发了专门的参数整定GUI，可以实时调整参数并观察响应，大大提高了调试效率。

6. 进阶话题与扩展方向

6.1 非线性MPC实现

对于强非线性系统，可以考虑：

1. 连续线性化：在每个采样点重新线性化
2. 直接非线性MPC：使用非线性优化求解器
3. 神经网络近似：用NN表示非线性动态

示例代码结构：

matlab复制function u = nonlinearMPC(x0, ref)
    options = optimoptions('fmincon','Algorithm','sqp');
    cost = @(u) calcNMPCCost(x0,u,ref);
    u = fmincon(cost, u0,[],[],[],[],lb,ub,[],options);
end

6.2 分布式MPC

对于大规模系统，可采用分布式架构：

1. 分解协调法：将大系统分解为子系统
2. 一致性约束：处理子系统间耦合
3. 并行计算：加速求解过程

6.3 学习增强MPC

结合机器学习的方法：

1. 模型学习：用数据驱动方法改进模型
2. 参数自适应：在线调整权重和约束
3. 策略融合：MPC与强化学习结合

实际工程经验表明，纯数据驱动方法在安全性关键系统中风险较大，推荐采用"MPC为主，学习为辅"的架构。