MATLAB实现模型预测控制的船舶艏向控制：从原理到代码

前阵子在调船舶自动舵算法，连续几个晚上对着Simulink里的PID参数反复折腾——超调压下去了响应又变慢，响应提上来舵角又开始高频抖。后来我把MPC（模型预测控制）真正跑起来做船舶艏向控制，才意识到之前的纠结大多来自控制器本身的结构限制，而不是参数没调好。这篇就用一个完整的MATLAB实现，把MPC的模型建立、原理推导、代码编写到优化改进，逐层拆开讲清楚。

1. 为什么偏偏是MPC：船舶艏向控制的真实约束与选型逻辑

1.1 船舶自动舵面对的不是"调参"问题，而是"约束"问题

很多刚接触船舶运动控制的朋友会有一个直觉：艏向控制不就是把PID调好吗？实际跑过仿真或者看过实船数据就会明白，船舶自动舵是个被物理约束卡得死死的系统。舵角不是你想要多少就给多少，液压舵机有机械限位，一般就是正负35度；舵速率也有限制，常见的航速条件下舵机打舵速度大概在每秒5到7度。这两条限制在PID框架里属于"事后处理"——PID先把控制量算出来，再靠限幅模块硬截断。一旦控制器输出长时间顶在限幅上，积分项就会越积越多，等偏差反向时控制器还反应不过来，这就是典型的积分饱和。

除了执行机构约束，船舶本身还是个大惯性、大滞后的对象。一条几万吨的散货船，艏向对舵角的响应时间常数可能会到几十秒。PID本质上只根据当前偏差做比例、积分、微分运算，它看不到"我这一舵打下去，十秒之后船会转到哪里"。所以在强风浪工况下，PID很容易出现来回修正、航向偏差波动幅度大的问题。

1.2 PID和MPC的本质差异：看眼前偏差还是看未来轨迹

我用一个表格把两者的差异摊开讲，这样最直观：

从工程角度看，MPC真正打动我的不是"优化"这个概念本身，而是它把"舵角有限幅、舵速有限速、要提前规划转向轨迹"这类实际需求直接放进了问题描述里。你不需要在控制器外面再挂一堆抗积分饱和、微分先行之类的补偿逻辑，这些在MPC框架里都是约束条件和预测模型的一部分。

1.3 什么船舶场景最适合MPC

MPC有它的适用边界，不是所有舱段控制都要上MPC。以我的实测体验来说，这三类场景尤其适合：

• 大角度航向改变，比如转向30度以上，需要在舵角限幅下规划出一条平滑转向轨迹。
• 航向保持与抗扰并存，既要求稳态精度，又要求对风浪扰动有主动的、预见性的补偿。
• 执行机构有明确物理限制且希望延长舵机寿命的场景，MPC能够主动避免频繁打满舵。

如果你的工况只是平静海况下的小角度航向修正，PID完全够用。MPC的优势在"约束明显、滞后明显、扰动明显"的组合工况下才会充分体现。

2. Nomoto模型与MPC数学机制：控制方案落地的第一块基石

2.1 用Nomoto模型描述船舶艏向运动

做MPC第一步是拿到一个"能用来预测"的数学模型。实船水动力模型用Abkowitz或者MMG那套太复杂，适合做仿真验证，但不适合直接嵌进控制器做在线预测。工程上最常用的是Nomoto模型，把船舶从舵角到艏摇角速度的响应简化成一阶或二阶惯性环节。

一阶Nomoto模型的微分方程是：

T * ψ̈ + ψ̇ = K * δ

其中：

• ψ是艏向角，单位rad
• δ是舵角，单位rad
• K是回转性指数，描述舵角引起的稳态艏摇角速度增益
• T是追随性指数，描述艏向对舵响应的快慢，T越大惯性越大

一艘典型货船的K值大约在0.05到0.3之间，T值在20到80秒范围内。我这篇代码里取的参数是K = 0.16，T = 40，代表一条中等吨位的训练仿真船。注意代码中一定要统一单位，如果模型中用的角度单位是弧度，参考输入和约束也要全部用弧度，否则算出来的控制量会差得很离谱。

2.2 状态空间描述与离散化

控制器的实现要在离散时间域进行，所以先把连续模型改写成状态空间形式。取状态向量x = [ψ, r]T，其中r = ψ̇是艏摇角速度，控制输入u = δ。

连续状态空间为：

A = [0 1; 0 -1/T] B = [0; K/T] C = [1 0]

离散化我推荐直接用零阶保持器（ZOH）假设，然后用MATLAB的c2d函数处理。为什么用ZOH而不是简单的欧拉法？因为真实舵机在每个控制周期内保持舵角不变，这个行为本质上就是零阶保持。欧拉法在采样周期小时误差不大，但采样周期一大，离散模型和实际系统会明显偏移。

% 连续模型 K = 0.16; % 回转性指数 T = 40; % 追随性指数 Ac = [0 1; 0 -1/T]; Bc = [0; K/T]; Cc = [1 0]; Dc = 0; sys_c = ss(Ac, Bc, Cc, Dc); % 离散化，采样周期Ts=1s Ts = 1; sys_d = c2d(sys_c, Ts, 'zoh'); Ad = sys_d.A; Bd = sys_d.B; Cd = sys_d.C;

2.3 MPC三大机制：预测、滚动优化、反馈校正

MPC为什么能"看未来"？因为它手里拿着一张预测模型这张地图。在每个采样时刻，控制器利用当前状态和未来的控制序列，把未来Np步的输出全部算出来，这Np步就是预测时域。

光有预测还不够，控制器需要在这Np步里选出一组最优的控制序列，让预测输出尽量贴近参考轨迹，同时满足约束。这个求解过程叫滚动优化。注意"滚动"这两个字很关键——我们不是把未来Np步的控制量全部执行掉，而是只执行当前这一步，下一个采样周期到来后，用新的测量状态重新做一遍预测和优化。这样反复滚动的机制，天然赋予了MPC反馈校正的能力：模型预测得再准，也不可能完全等于真实系统，滚动更新机制让误差不会长时间累积。

打个比方，MPC就像一个每走一步棋都要重新看三步的棋手，而不是开局算好十步就走到底的莽夫。它牺牲了一部分计算效率，换来了对模型误差和外部扰动的克制能力。

2.4 代价函数与约束的物理含义

MPC的核心是每个采样周期求解一个有约束的优化问题。艏向控制里，我用的代价函数是：

J = Σ || ψ(k+i|k) - ψ_ref(k+i)||²_Q + Σ || Δδ(k+i)||²_R

第一项衡量未来预测艏向和期望航向的偏差，Q越大，控制器越"急切"地把航向拉到参考值；第二项衡量舵角的增量变化，R越大，舵机动作越平缓。这里的Δδ是舵角增量，不是舵角本身。

为什么控制量用增量而不是舵角的绝对值？有两个原因。第一，增量形式本身包含积分作用，能够让系统在存在常值扰动时消除静态误差；第二，舵速限制本质上是舵角增量限制，用增量作为优化变量可以直接把舵速约束写成上下界形式。这一点是我从最初用舵角绝对值做控制量踩坑之后，改造成增量形式才彻底理顺的。

约束方面，我把舵角限幅和舵速限幅都写进优化问题：

• 舵角限幅：-35° ≤ δ ≤ 35°，换算成弧度约±0.6109
• 舵速限幅：-5°/s ≤ Δδ ≤ 5°/s，换算成弧度为±0.0873

3. Matlab代码实现：从预测矩阵生成到quadprog在线求解

3.1 建立增广状态空间，把舵角放进状态里

既然控制量要用舵角增量Δδ，那就不能继续用只有ψ和r的两维状态了。我把舵角δ也扩充为状态，新增的控制输入是舵角增量u = Δδ。

增广后的连续状态空间为：

x_aug = [ψ; r; δ]

dot_x_aug = [Ac Bc; zeros(1,2) 0] * x_aug + [0; 0; 1] * u

输出矩阵取C_aug = [1 0 0]，因为艏向角是第一个状态。

% 增广系统 A_aug = [Ac, Bc; zeros(1,2), 0]; B_aug = [0; 0; 1]; C_aug = [1 0 0]; D_aug = 0; % 离散化 sys_aug = ss(A_aug, B_aug, C_aug, D_aug); sys_d_aug = c2d(sys_aug, Ts, 'zoh'); Ad_aug = sys_d_aug.A; Bd_aug = sys_d_aug.B; Cd_aug = sys_d_aug.C;

3.2 预测矩阵的构建：离线部分要做扎实

预测模型的核心公式是：

Y = Ψ * x_aug(k) + Θ * ΔU

其中Y是未来Np步艏向预测，ΔU是未来Nc步舵角增量序列。Ψ和Θ是两个预测矩阵，它们只和离散系统矩阵、时域参数有关，在仿真开始前可以一次性离线算好，没必要在实时循环里反复计算。

Np = 20; % 预测时域 Nc = 5; % 控制时域 % 计算Psi矩阵 (Np x 3) Psi = zeros(Np, 3); for i = 1:Np Psi(i, :) = Cd_aug * (Ad_aug^i); end % 计算Theta矩阵 (Np x Nc) Theta = zeros(Np, Nc); for i = 1:Np for j = 1:Nc if i >= j Theta(i, j) = Cd_aug * (Ad_aug^(i-j)) * Bd_aug; else Theta(i, j) = 0; end end end

我想强调一下，这段循环只是教学清晰起见这么写。实际工程里，Theta矩阵的计算可以写成双层循环，也可以用Toeplitz结构来做，性能差异不大，但可读性这样最高。

3.3 把代价函数化成标准QP问题

下一步要构造Hessian矩阵H和线性项系数f。前面代价函数展开后，可以整理成标准的QP形式：

min 0.5 * ΔUᵀ * H * ΔU + fᵀ * ΔU

其中：

H = Θᵀ * Q_bar * Θ + R_bar f = Θᵀ * Q_bar * (Ψ * x_aug(k) - Y_ref)

Q_bar是预测时域内艏向偏差的权重矩阵，用kron函数从标量Q扩展成对角块；R_bar同理。

Q = 1; % 艏向偏差权重 R = 0.1; % 舵角增量权重 Q_bar = Q * eye(Np); R_bar = R * eye(Nc); H = Theta' * Q_bar * Theta + R_bar; H = (H + H') / 2; % 强制对称，避免quadprog数值警告

H对称化这行很多人忽略。quadprog对Hessian矩阵的对称性很敏感，数值误差导致的不对称会让求解器报警告甚至退出。我在自己代码里加这行之后，求解器再也没出过奇怪警告。

3.4 约束矩阵的组装：舵角累积约束和舵速约束

约束方面有两类。第一类舵速约束最简单，直接是优化变量Δδ的上下界，交给quadprog的lb和ub参数。第二类舵角约束需要额外组装，因为舵角是优化变量Δδ的积分：δ(k+i) = δ(k-1) + Σ Δδ(k+j)，j从0到i。这是关于Δδ的线性不等式约束。

% 舵速约束，单位换算成弧度 du_max = deg2rad(5); lb = -du_max * ones(Nc, 1); ub = du_max * ones(Nc, 1); % 舵角累积约束 delta_max = deg2rad(35); A_ineq = tril(ones(Nc, Nc)); % 下三角矩阵，用于累加舵角增量 b_ineq_max = (delta_max - delta_prev) * ones(Nc, 1); b_ineq_min = (-delta_max - delta_prev) * ones(Nc, 1); Aineq = [A_ineq; -A_ineq]; bineq = [b_ineq_max; b_ineq_min];

A_ineq是Nc阶下三角全1矩阵，乘上ΔU之后得到的就是每一步的舵角相对当前舵角的累积变化量。加两个方向的不等式就同时限住了上界和下界。

3.5 在线求解与主循环

每个控制周期里，只需要根据当前状态重新计算f，然后调用quadprog求解：

options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex'); for k = 1:sim_steps % 计算线性项，参考航向设为常值ref_psi f = Theta' * Q_bar * (Psi * x_aug_current - ref_psi * ones(Np, 1)); % 求解QP du_opt = quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options); % 取当前步控制增量 delta_cmd = delta_prev + du_opt(1); delta_prev = delta_cmd; % 将舵角送入船舶模型，更新真实状态 % 这里需要你自己根据Nomoto模型写状态更新 % 得到新的艏向角psi_new和艏摇角速度r_new % 更新增广状态x_aug_current x_aug_current = [psi_new; r_new; delta_cmd]; % 记录数据 psi_history(k) = psi_new; delta_history(k) = delta_cmd; end

注意一个细节：quadprog的调用格式里，我给的是lb和ub分别约束所有Nc个Δδ，但实际舵角约束已经写在Aineq里了。这样分开处理的好处是舵速约束用边界约束表达更高效，舵角约束用线性不等式表达更灵活，两者互不干扰。

3.6 一个小问题：状态怎么获取

仿真环境里状态是已知的，直接拿来用就行。但如果你要把这套代码往实船或者半实物仿真上迁移，ψ可以从电罗经或者光纤罗经拿到，r可以从垂直参考单元或者GNSS航向变化率推算，δ直接从舵角反馈传感器读。真正困难的是艏摇角速度r有噪声，建议加一阶低通滤波或者用卡尔曼滤波做状态估计，否则MPC的预测起点会抖。

4. 代码优化的几个方向：算得快、响应稳、抗扰强

4.1 离线计算和在线计算的边界要清晰

我见过很多人写的MPC代码，把Psi和Theta矩阵放在主循环里每步重新算一遍。这么做在仿真里能跑，但实时性一测就露馅。正确的做法是把所有只依赖固定参数的计算全部挪到循环外：Psi、Theta、H矩阵、约束矩阵这些在仿真开始前就算好，循环里只做状态更新、f向量计算和quadprog求解。

如果追求极致效率，可以用MATLAB Coder把MPC函数转成C代码。不过前提是代码风格要对，比如避免动态变量维度变化、避免在循环内使用eval这类动态执行函数。我自己用MATLAB Coder导出过一版，在x86工控机上单步求解时间从几毫秒降到了零点几毫秒，性能提升明显。

4.2 时域参数的选取经验

预测时域Np和控制时域Nc的选择直接影响控制效果和计算量。我的经验参数如下：

Np = 20; % 预测20秒 Nc = 5; % 控制5步

Np太小，控制器看不到系统惯性的后续影响，容易出现过调；Np太大，计算量上去了，而且远处的预测精度没有意义，反而可能引入模型误差。Nc一般取Np的五分之一到四分之一就够了，更大的Nc对响应速度的提升非常有限，但QP问题的决策变量变多，求解时间明显上升。

4.3 软约束：给QP问题留条退路

这是我从一次仿真崩溃中深刻体会到的问题。某次我把参考航向直接设成从当前艏向跳变60度，在某个中间时刻，预测轨迹显示无论怎么打舵都会超出舵角约束，quadprog直接返回空解，导致控制输出跳变。后来我意识到，约束从数学上看是硬性的，但工程上应该给调节器留出退路。

做法是引入松弛变量ε，把舵角约束从"必须不超过限幅"改成"允许短时超过限幅但重罚"，代价函数中增加一项ρ*ε²。这样在极端工况下，求解器至少能返回一个可行解，而不是直接罢工。代价是控制器偶尔允许舵角超限一瞬间，换取系统的连续运行。

4.4 抗浪干扰的实用改进：扰动估计补偿

MPC的预测模型里没有风浪扰动项，所以当外界扰动持续作用时，预测轨迹会偏离实际轨迹，产生稳态误差和周期性振荡。一个工程上很有效的改进是为模型增加一个扰动估计项。

我用的方法是，在每步滚动优化前，把上一步的预测误差（实际测量艏向 - 预测艏向）作为当前的等效扰动，叠加到预测模型的输出上。这样等效于对模型做了在线修正，不需要额外建模风浪特性。具体实现很轻量，只需要在预测输出上加上一个由误差滤波得到的修正项即可。

这样做之后，在有持续侧风或海流的海况下，艏向偏差能明显压低。核心原理就是MPC天然支持模型误差校正，你只需要把反馈环节做好。

5. 仿真验证与调参避坑：几组实测数据和踩坑记录

5.1 大角度转向测试：MPC的实际表现

我用上面的代码做了一次航向改变仿真：初始艏向0度，参考艏向在第10秒阶跃到30度，海况静水。MPC参数的Q=1、R=0.1、Np=20、Nc=5。

仿真的艏向曲线显示，MPC大概在30秒左右完成转向，最大超调几乎为零，舵角在整个过程中始终处在35度限幅以内，舵速也稳定在每秒5度限幅以下。而同样的系统用PID控制，我把PID调成响应速度相近，转向过程出现了约2度的超调，后续还带两次小幅振荡。这个对比很能说明问题。

5.2 强烈浪扰动下的航向保持测试

在静水模型上加了一个周期为8秒、幅值为0.05 rad的波浪扰动力矩，模拟中等海况下的航向保持。

PID在扰动下艏向偏差峰值约1.8度，且偏差曲线上有明显的周期性纹波；MPC由于有未来预测和滚动修正，偏差峰值压低到1度以内，而且舵角动作更平滑。这说明MPC虽然不能完全抵消扰动，但能有效避免扰动被控制器本身放大，让舵机工作得更从容。

5.3 新手最容易踩的坑，逐个说明

第一个坑是采样周期Ts取得太大。Ts=1秒对这条船合适，但如果换成一条快速小艇，T时间常数只有几秒，Ts还是1秒就会让离散模型严重失真。判断标准很简单：Ts至少要小于等效时间常数的十分之一。

第二个坑是初始舵角假设。很多代码在初始化时把delta_prev默认设为0，但仿真开始前船舶可能已经有一个保持航向的舵角。如果这个初值不对，第一个控制周期的舵角累积约束判断就会出错，导致一开始就打出满舵。启动时一定要把delta_prev初始化为当前实际舵角。

第三个坑是quadprog求解器版本和算法选项。R2020a之后interior-point-convex是默认推荐算法，处理中小规模QP又快又稳。如果你用的是老版本MATLAB，注意确认Optimization Toolbox已经安装，否则quadprog函数根本找不到。我在给同事排查时遇到过几次，报错信息是"Undefined function 'quadprog'"，基本就是工具箱缺失。

第四个坑是参考航向的跳变处理。参考航向从0度跳到30度，如果预测时域内参考值全部瞬间变成30度，MPC会为了尽快跟上而在一开始输出较大舵角。如果你希望转向轨迹更平滑，可以在参考轨迹生成时加一个斜率限制，变成斜坡参考，让控制器有更多前瞻规划的余地。

5.4 我自己的调参心得

最后说点实在的调参心得。我建议顺序是先把Np定下来，再调Q和R，最后看约束是否被激活。先给一个大一点的Q让系统尽快跟上参考，如果舵角抖动，再逐步加大R抑制舵速冲击。Q和R的比值比它们的绝对值更重要，Q/R在10附近是一个比较常见的起点，然后根据响应调整到10到100之间某一个值。

如果你发现MPC控制下的舵角频谱里高频分量明显，不一定是权重问题，也可能只是测量噪声太大。加一阶滤波在反馈回路上，比单纯增大R更有效。

我最后的实践体会

这套基于MPC的船舶艏向控制代码，我从模型搭建到优化改进反复迭代了很长时间。最大的体会是：MPC不是万能药，它要求你对控制对象的模型有清晰认识，也要舍得在预测矩阵推导上花功夫——但一旦把模型、预测矩阵、约束这几块理顺，它在约束处理和多步前瞻上的优势是PID难以比拟的。

代码本身不需要多复杂的技巧，关键是理解每一步在干什么。如果你也准备从PID往MPC迁移，我的建议是先用这篇文章里的框架把基础版本跑通，再去动约束和扰动补偿的扩展。相信我，当你第一次看到MPC在舵角限幅下依然平滑地完成大角度转向曲线时，你会觉得之前的推导和调试都值得。