MPC与MHE集成在无人艇控制中的Matlab实现-代码聚汇网

MPC与MHE集成在无人艇控制中的Matlab实现

楚予微茫·2026-08-11·阅读 23 GEO检测（免费）

模型预测控制滚动时域估计MPC

|难度：普通|阅读约 16 分钟

1. 项目概述

在控制工程领域，实现系统对目标点的精确镇定一直是个经典而富有挑战性的课题。最近我在一个无人艇控制项目中，尝试将模型预测控制(MPC)与滚动时域估计(MHE)这两种先进控制方法进行集成，取得了不错的实验效果。这个方案特别适合处理存在模型不确定性和外部干扰的动态系统控制问题。

传统PID控制在简单系统中表现良好，但在复杂非线性系统或存在显著延迟的场合就显得力不从心。MPC通过在线求解有限时域内的最优控制问题，能够显式处理多变量约束；而MHE则提供了对系统状态和未知扰动的实时估计能力。两者的结合形成了一个强大的"预测-校正"闭环框架。

需要模型API调用？免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。

2. 核心原理与技术路线

2.1 模型预测控制(MPC)基础

MPC的核心思想可以用"边走边看"来形象理解。它不像传统控制器那样一次性给出全部控制序列，而是：

在当前时刻基于系统模型预测未来有限时域内的系统行为
通过优化算法计算最优控制序列
只实施序列中的第一个控制量
到下一时刻重新进行预测和优化

这种滚动优化的策略使MPC具有处理约束和适应模型误差的天然优势。在Matlab中，我们可以使用Model Predictive Control Toolbox提供的函数快速搭建MPC控制器。

2.2 滚动时域估计(MHE)原理

MHE可以看作是MPC的"逆向问题"——它通过最近一段时域内的测量数据，反向估计系统的状态和扰动。与卡尔曼滤波相比，MHE的优势在于：

能显式处理状态和参数约束
对模型误差更鲁棒
可以同时估计状态和未知输入

在实现上，MHE也需要求解一个优化问题，计算量通常比MPC更大。这也是为什么在实际应用中我们往往会给MHE设置比MPC更短的时域。

2.3 MPC-MHE集成框架

将两者集成的关键点在于：

MHE为MPC提供准确的状态估计和扰动预测
MPC基于这些信息计算最优控制量
新获得的测量数据又反馈给MHE进行下一轮估计

这种双向数据流形成了一个强大的闭环系统。在我的实现中，两者的采样周期设为相同，通过共享内存交换数据。

3. Matlab实现细节

3.1 系统建模

首先需要建立被控对象的数学模型。以无人艇为例，其动力学模型可以表示为：

matlab复制function dx = shipModel(x,u)
    % 状态x = [位置x; 位置y; 航向角; 速度; 角速度]
    % 控制u = [推进力; 转向力矩]
    
    m = 100; % 质量
    I = 50;  % 转动惯量
    beta = 0.1; % 阻尼系数
    
    dx = zeros(5,1);
    dx(1) = x(4)*cos(x(3)); % x方向速度
    dx(2) = x(4)*sin(x(3)); % y方向速度
    dx(3) = x(5);           % 角速度
    dx(4) = (u(1)-beta*x(4))/m; % 加速度
    dx(5) = (u(2)-beta*x(5))/I; % 角加速度
end

3.2 MPC控制器设计

使用MPC工具箱创建控制器：

matlab复制% 创建MPC对象
mpcobj = mpc(ssModel,Ts,PredictionHorizon,ControlHorizon);

% 设置约束
mpcobj.MV(1).Min = -50;  % 推进力下限
mpcobj.MV(1).Max = 50;   % 推进力上限
mpcobj.MV(2).Min = -20;  % 转向力矩下限
mpcobj.MV(2).Max = 20;   % 转向力矩上限

% 设置权重矩阵
mpcobj.Weights.OutputVariables = [10 10 1 0.1 0.1];
mpcobj.Weights.ManipulatedVariables = [0.1 0.1];
mpcobj.Weights.ManipulatedVariablesRate = [0.01 0.01];

3.3 MHE估计器实现

由于Matlab没有现成的MHE工具箱，我们需要手动实现：

matlab复制function [x_est, d_est] = MHE_estimator(y_hist, u_hist, x_guess)
    % y_hist: 历史测量数据
    % u_hist: 历史控制输入
    % x_guess: 状态初始猜测
    
    options = optimoptions('fmincon','Display','off');
    cost_func = @(x) MHE_cost(x, y_hist, u_hist);
    
    % 求解优化问题
    x_opt = fmincon(cost_func, x_guess, [], [], [], [], lb, ub, [], options);
    
    % 提取估计结果
    x_est = x_opt(1:nx);
    d_est = x_opt(nx+1:end);
end

function J = MHE_cost(x, y, u)
    % 计算MHE代价函数
    J = 0;
    for k = 1:length(y)
        x_pred = shipModel(x(:,k), u(:,k)) + x(nx+1:end,k);
        J = J + (y(:,k)-x_pred(1:ny))'*Q*(y(:,k)-x_pred(1:ny));
    end
    J = J + x(:)'*R*x(:); % 正则化项
end

3.4 闭环仿真框架

主仿真循环结构如下：

matlab复制% 初始化
x_true = x0; x_est = x0; u = [0;0];
N = 1000; % 总步数

for k = 1:N
    % 获取测量值(加入噪声)
    y = x_true(1:3) + 0.01*randn(3,1);
    
    % MHE估计
    [x_est, d_est] = MHE_estimator(y_hist, u_hist, x_est);
    
    % MPC控制
    u = mpcmove(mpcobj, x_est, [], y_ref);
    
    % 系统演化
    x_true = shipModel(x_true, u) + [0;0;0;d_est];
    
    % 更新历史数据
    y_hist = [y_hist(:,2:end), y];
    u_hist = [u_hist(:,2:end), u];
end

4. 关键参数调试经验

4.1 时域长度选择

MPC和MHE的时域长度对性能影响很大：

MPC预测时域：通常选择3-5倍的系统主导时间常数
MHE估计时域：一般比MPC时域短，以平衡计算量和估计精度

在无人艇案例中，经过多次试验，我最终设置为：

matlab复制MPC.PredictionHorizon = 20; % 对应10秒
MHE.WindowSize = 10;        % 对应5秒

4.2 权重矩阵调整

权重矩阵的调节需要反复试验。一些经验法则：

先调整输出权重使系统稳定
再调节控制量权重避免过大控制动作
最后微调控制变化率权重使控制更平滑

典型的权重设置过程：

matlab复制% 第一轮：保证跟踪性能
mpcobj.Weights.OutputVariables = [10 10 5 1 1]; 

% 第二轮：限制控制幅值
mpcobj.Weights.ManipulatedVariables = [0.5 0.5];

% 第三轮：平滑控制
mpcobj.Weights.ManipulatedVariablesRate = [0.1 0.1];

4.3 实时性优化技巧

为提高实时性，我采用了以下优化：

热启动：使用上一时刻的解作为当前优化的初始猜测
提前终止：设置优化器最大迭代次数(通常5-10次已足够)
简化模型：在MPC中使用线性化模型，而非完整非线性模型

对应的代码修改：

matlab复制options = optimoptions('fmincon','MaxIterations',8,...
                      'Algorithm','interior-point');
mpcobj.Optimizer.CustomSolver = true;
mpcobj.Optimizer.Solver = @(f,x0,A,b,Aeq,beq,lb,ub)...
    fmincon(f,x0,A,b,Aeq,beq,lb,ub,[],options);

5. 典型问题与解决方案

5.1 估计发散问题

症状：MHE估计的状态逐渐偏离真实值
可能原因：

过程噪声协方差设置不当
时域长度太短
约束条件不合理

解决方案：

matlab复制% 增加过程噪声权重
Q = diag([1,1,1,0.5,0.5]); 

% 延长MHE时域
MHE.WindowSize = 15;

% 放松状态约束
lb = [-inf;-inf;-inf;-inf;-inf];
ub = [inf;inf;inf;inf;inf];

5.2 控制振荡问题

症状：控制量在高频小幅振荡
可能原因：

控制变化率权重太小
采样时间过短
测量噪声过大

调试方法：

matlab复制% 增加控制变化率权重
mpcobj.Weights.ManipulatedVariablesRate = [1 1];

% 调整采样时间
Ts = 0.1; % 从0.05调整到0.1秒

% 添加测量滤波器
y_filtered = filter(ones(1,3)/3, 1, y);

5.3 实时性不足问题

症状：控制器无法在一个采样周期内完成计算
优化策略：

减少优化变量维度
使用更高效的求解器
采用显式MPC

具体实施：

matlab复制% 使用qpOASES求解器(需安装)
mpcobj.Optimizer.Solver = @qpoases;

% 降低MPC时域
mpcobj.PredictionHorizon = 10;

% 离线计算显式MPC
expmpc = getExplicitMPC(mpcobj);

6. 进阶应用与扩展

6.1 参数自适应机制

为应对系统参数变化，可以增加在线参数估计：

matlab复制function [x_est, param_est] = adaptive_MHE(y_hist, u_hist)
    % 将关键参数作为扩展状态估计
    extended_state = [x; m; I; beta];
    
    % 修改模型函数
    function dx = extended_model(z,u)
        m = z(6); I = z(7); beta = z(8);
        % ...其余与shipModel相同
    end
    
    % 执行MHE
    z_est = MHE_estimator(y_hist, u_hist, extended_model);
    x_est = z_est(1:5);
    param_est = z_est(6:8);
end

6.2 多速率采样策略

为减轻计算负担，可以采用：

MHE执行频率低于MPC(如MPC 100Hz，MHE 20Hz)
使用更简单的模型进行高频MPC计算

实现代码：

matlab复制mhe_counter = 0;
for k = 1:N
    % MPC每步都执行
    u = mpcmove(mpcobj, x_est, [], ref);
    
    % MHE每5步执行一次
    if mod(mhe_counter,5)==0
        [x_est, ~] = MHE_estimator(...);
    end
    mhe_counter = mhe_counter + 1;
end

6.3 硬件部署考虑

将算法部署到实际硬件时需要注意：

代码生成：使用Matlab Coder将关键函数转为C代码
定点化：对嵌入式平台，需将浮点运算转为定点
异常处理：增加传感器失效检测和容错逻辑

代码生成示例：

matlab复制% 配置代码生成选项
cfg = coder.config('lib');
cfg.TargetLang = 'C';

% 生成C代码
codegen('MHE_estimator','-config','cfg','-args',...
        {coder.typeof(y_hist,[3,10]),...
         coder.typeof(u_hist,[2,10]),...
         coder.typeof(x0,[5,1])})