前阵子在调船舶自动舵算法连续几个晚上对着Simulink里的PID参数反复折腾——超调压下去了响应又变慢响应提上来舵角又开始高频抖。后来我把MPC模型预测控制真正跑起来做船舶艏向控制才意识到之前的纠结大多来自控制器本身的结构限制而不是参数没调好。这篇就用一个完整的MATLAB实现把MPC的模型建立、原理推导、代码编写到优化改进逐层拆开讲清楚。1. 为什么偏偏是MPC船舶艏向控制的真实约束与选型逻辑1.1 船舶自动舵面对的不是调参问题而是约束问题很多刚接触船舶运动控制的朋友会有一个直觉艏向控制不就是把PID调好吗实际跑过仿真或者看过实船数据就会明白船舶自动舵是个被物理约束卡得死死的系统。舵角不是你想要多少就给多少液压舵机有机械限位一般就是正负35度舵速率也有限制常见的航速条件下舵机打舵速度大概在每秒5到7度。这两条限制在PID框架里属于事后处理——PID先把控制量算出来再靠限幅模块硬截断。一旦控制器输出长时间顶在限幅上积分项就会越积越多等偏差反向时控制器还反应不过来这就是典型的积分饱和。除了执行机构约束船舶本身还是个大惯性、大滞后的对象。一条几万吨的散货船艏向对舵角的响应时间常数可能会到几十秒。PID本质上只根据当前偏差做比例、积分、微分运算它看不到我这一舵打下去十秒之后船会转到哪里。所以在强风浪工况下PID很容易出现来回修正、航向偏差波动幅度大的问题。1.2 PID和MPC的本质差异看眼前偏差还是看未来轨迹我用一个表格把两者的差异摊开讲这样最直观维度PIDMPC控制依据当前/历史偏差模型预测的未来一段输出约束处理外部限幅事后截断优化问题内显式包含约束多步前瞻无有预测时域内统一规划调参方式Kp/Ki/Kd直接调权重矩阵、时域参数对模型依赖弱强需要较准确的预测模型算力需求极低较高每个周期要解QP问题从工程角度看MPC真正打动我的不是优化这个概念本身而是它把舵角有限幅、舵速有限速、要提前规划转向轨迹这类实际需求直接放进了问题描述里。你不需要在控制器外面再挂一堆抗积分饱和、微分先行之类的补偿逻辑这些在MPC框架里都是约束条件和预测模型的一部分。1.3 什么船舶场景最适合MPCMPC有它的适用边界不是所有舱段控制都要上MPC。以我的实测体验来说这三类场景尤其适合大角度航向改变比如转向30度以上需要在舵角限幅下规划出一条平滑转向轨迹。航向保持与抗扰并存既要求稳态精度又要求对风浪扰动有主动的、预见性的补偿。执行机构有明确物理限制且希望延长舵机寿命的场景MPC能够主动避免频繁打满舵。如果你的工况只是平静海况下的小角度航向修正PID完全够用。MPC的优势在约束明显、滞后明显、扰动明显的组合工况下才会充分体现。2. Nomoto模型与MPC数学机制控制方案落地的第一块基石2.1 用Nomoto模型描述船舶艏向运动做MPC第一步是拿到一个能用来预测的数学模型。实船水动力模型用Abkowitz或者MMG那套太复杂适合做仿真验证但不适合直接嵌进控制器做在线预测。工程上最常用的是Nomoto模型把船舶从舵角到艏摇角速度的响应简化成一阶或二阶惯性环节。一阶Nomoto模型的微分方程是T * ψ̈ ψ̇ K * δ其中ψ是艏向角单位radδ是舵角单位radK是回转性指数描述舵角引起的稳态艏摇角速度增益T是追随性指数描述艏向对舵响应的快慢T越大惯性越大一艘典型货船的K值大约在0.05到0.3之间T值在20到80秒范围内。我这篇代码里取的参数是K 0.16T 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); % 离散化采样周期Ts1s 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 Σ || ψ(ki|k) - ψ_ref(ki)||²_Q Σ || Δδ(ki)||²_R第一项衡量未来预测艏向和期望航向的偏差Q越大控制器越急切地把航向拉到参考值第二项衡量舵角的增量变化R越大舵机动作越平缓。这里的Δδ是舵角增量不是舵角本身。为什么控制量用增量而不是舵角的绝对值有两个原因。第一增量形式本身包含积分作用能够让系统在存在常值扰动时消除静态误差第二舵速限制本质上是舵角增量限制用增量作为优化变量可以直接把舵速约束写成上下界形式。这一点是我从最初用舵角绝对值做控制量踩坑之后改造成增量形式才彻底理顺的。约束方面我把舵角限幅和舵速限幅都写进优化问题舵角限幅-35° ≤ δ ≤ 35°换算成弧度约±0.6109舵速限幅-5°/s ≤ Δδ ≤ 5°/s换算成弧度为±0.08733. 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参数。第二类舵角约束需要额外组装因为舵角是优化变量Δδ的积分δ(ki) δ(k-1) Σ Δδ(kj)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问题的决策变量变多求解时间明显上升。参数设置过小设置过大Np预测不充分超调明显计算慢远端预测无意义Nc控制自由度不足响应迟缓计算量增大收益甚微Q艏向偏差收敛慢舵角容易饱和、抖舵R舵机动作频繁转向迟钝跟踪滞后4.3 软约束给QP问题留条退路这是我从一次仿真崩溃中深刻体会到的问题。某次我把参考航向直接设成从当前艏向跳变60度在某个中间时刻预测轨迹显示无论怎么打舵都会超出舵角约束quadprog直接返回空解导致控制输出跳变。后来我意识到约束从数学上看是硬性的但工程上应该给调节器留出退路。做法是引入松弛变量ε把舵角约束从必须不超过限幅改成允许短时超过限幅但重罚代价函数中增加一项ρ*ε²。这样在极端工况下求解器至少能返回一个可行解而不是直接罢工。代价是控制器偶尔允许舵角超限一瞬间换取系统的连续运行。4.4 抗浪干扰的实用改进扰动估计补偿MPC的预测模型里没有风浪扰动项所以当外界扰动持续作用时预测轨迹会偏离实际轨迹产生稳态误差和周期性振荡。一个工程上很有效的改进是为模型增加一个扰动估计项。我用的方法是在每步滚动优化前把上一步的预测误差实际测量艏向 - 预测艏向作为当前的等效扰动叠加到预测模型的输出上。这样等效于对模型做了在线修正不需要额外建模风浪特性。具体实现很轻量只需要在预测输出上加上一个由误差滤波得到的修正项即可。这样做之后在有持续侧风或海流的海况下艏向偏差能明显压低。核心原理就是MPC天然支持模型误差校正你只需要把反馈环节做好。5. 仿真验证与调参避坑几组实测数据和踩坑记录5.1 大角度转向测试MPC的实际表现我用上面的代码做了一次航向改变仿真初始艏向0度参考艏向在第10秒阶跃到30度海况静水。MPC参数的Q1、R0.1、Np20、Nc5。仿真的艏向曲线显示MPC大概在30秒左右完成转向最大超调几乎为零舵角在整个过程中始终处在35度限幅以内舵速也稳定在每秒5度限幅以下。而同样的系统用PID控制我把PID调成响应速度相近转向过程出现了约2度的超调后续还带两次小幅振荡。这个对比很能说明问题。5.2 强烈浪扰动下的航向保持测试在静水模型上加了一个周期为8秒、幅值为0.05 rad的波浪扰动力矩模拟中等海况下的航向保持。PID在扰动下艏向偏差峰值约1.8度且偏差曲线上有明显的周期性纹波MPC由于有未来预测和滚动修正偏差峰值压低到1度以内而且舵角动作更平滑。这说明MPC虽然不能完全抵消扰动但能有效避免扰动被控制器本身放大让舵机工作得更从容。5.3 新手最容易踩的坑逐个说明第一个坑是采样周期Ts取得太大。Ts1秒对这条船合适但如果换成一条快速小艇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会为了尽快跟上而在一开始输出较大舵角。如果你希望转向轨迹更平滑可以在参考轨迹生成时加一个斜率限制变成斜坡参考让控制器有更多前瞻规划的余地。问题现象解决Ts过大离散模型失真、控制抖振减小采样周期满足Ts T/10初始舵角错误启动即打满舵用实际舵角初始化delta_prevquadprog不可用Undefined function报错安装Optimization Toolbox参考航向跳变起动舵角冲击参考轨迹加斜坡限制权重Q过大舵角饱和、抖舵降低Q或增大R5.4 我自己的调参心得最后说点实在的调参心得。我建议顺序是先把Np定下来再调Q和R最后看约束是否被激活。先给一个大一点的Q让系统尽快跟上参考如果舵角抖动再逐步加大R抑制舵速冲击。Q和R的比值比它们的绝对值更重要Q/R在10附近是一个比较常见的起点然后根据响应调整到10到100之间某一个值。如果你发现MPC控制下的舵角频谱里高频分量明显不一定是权重问题也可能只是测量噪声太大。加一阶滤波在反馈回路上比单纯增大R更有效。我最后的实践体会这套基于MPC的船舶艏向控制代码我从模型搭建到优化改进反复迭代了很长时间。最大的体会是MPC不是万能药它要求你对控制对象的模型有清晰认识也要舍得在预测矩阵推导上花功夫——但一旦把模型、预测矩阵、约束这几块理顺它在约束处理和多步前瞻上的优势是PID难以比拟的。代码本身不需要多复杂的技巧关键是理解每一步在干什么。如果你也准备从PID往MPC迁移我的建议是先用这篇文章里的框架把基础版本跑通再去动约束和扰动补偿的扩展。相信我当你第一次看到MPC在舵角限幅下依然平滑地完成大角度转向曲线时你会觉得之前的推导和调试都值得。