无人船（USV）这几年在水面巡检、水文采样、应急救援这些场景里越来越常见，但真正把自主控制做稳、做准，我踩过的坑比收获的经验还多。这个项目用模型预测控制（MPC）给USV做自主控制，用Matlab实现了一套从建模、仿真到控制器设计的完整流程——文章会把核心思路、模型方程、代码架构、调参经验和踩坑记录都摊开来讲，适合正在做水面机器人控制、或者刚入门MPC想找真实场景练手的同学。项目本身不复杂，但涉及的细节相当多：如何把无人船的数学离散模型写成代码、预测时域和控制时域怎么搭配、约束条件怎么设才不会让优化器罢工、权重矩阵调成什么量级才能兼顾跟踪精度和执行机构寿命。这些内容我会逐一拆开说明，并给出可以直接复用的Matlab实现思路。
模型预测控制的本质就是在每个采样时刻，基于当前状态预测未来一段时间的系统行为，然后在满足约束的前提下求解一个有限时域最优控制问题，最后只把第一个控制量施加给对象，到下一个采样时刻再重复这个过程。这种“边走边看、滚动优化”的机制，天然适合USV这种存在强扰动、执行机构受限、运动状态之间相互耦合的欠驱动系统。相比PID面对多变量时那一堆增益矩阵的调试噩梦，MPC把约束和预测都放进了一个统一的优化框架，控制效果和可解释性都要好一个档次。
1. 为什么USV自主控制选MPC而不是PID
1.1 USV控制面临的核心难题
先说说USV的控制难点在哪。绝大多数无人船采用“推进器+舵”的驱动方式，船体只有纵荡方向（前进）和艏摇方向（转向）有直接的驱动力，横荡方向（侧向漂移）没有独立的推进器，这种系统在控制理论里叫欠驱动系统。欠驱动意味着你不能简单地把横向误差单独闭环，而要通过协调纵荡推力和艏摇力矩，让船体走出一条合理的“曲线路径”来消除横向位置偏差。
第二个大问题是干扰。水面环境的风、浪、流是持续存在的，而且是非线性的、有时变的。风对船体的作用力、浪带来的周期波动作用力、水流引起的整体漂移，都会叠加到被控系统上。你想让船精准走直线，实际上船一直在被环境推着走，控制器必须持续输出修正量对抗这些扰动。
第三个问题是执行机构约束。推进器和舵都有物理限制——推力有最大值、舵角有最大转角、舵机转速有极限。很多控制算法在推导时根本不管这些约束，仿真里调得好好的，一上真机就崩，就是因为在极限工况下算法输出的控制量远超执行机构能给出的范围。
第四个问题是多变量耦合。USV的运动自由度不是独立的：纵荡速度会改变回转特性，横向漂移速度会影响航向变化率，航向角又反过来影响位置更新的几何关系。PID要处理这种耦合，往往要专门去整定多个通道之间的交叉项，工程上非常麻烦。
1.2 几种主流控制方案的横向对比
我把做USV控制经常用到的几类方法放在一起比一比，这样你能直观看出MPC的优缺点：
| 方案 | 建模要求 | 约束处理 | 多变量特性 | 实时性 | 工程难度 |
|---|---|---|---|---|---|
| PID | 低 | 无 | 需解耦整定 | 高 | 低 |
| LQR | 线性模型 | 无 | 支持，但约束受限 | 高 | 中 |
| 反步法 | 非线性模型 | 无 | 支持，但设计复杂 | 高 | 高 |
| 滑模控制 | 非线性模型 | 无 | 支持，但抖振问题 | 高 | 高 |
| 模型预测控制 | 线性或非线性 | 显式支持 | 天然支持 | 中 | 中偏高 |
PID在简单工况下完全够用，但它本质上是一个“当前误差驱动”的控制器，没有对未来的预判能力。实际调试的时候你会发现，USV这种惯性很大的对象，PID对误差的反应总是慢半拍，调快了容易震荡，调慢了跟踪误差又大。LQR比PID高级一些，能处理多变量线性系统，但没有约束处理能力。反步法和滑模控制理论深度高、抗干扰性强，但设计过程对数学功底要求高，而且滑模的抖振问题在处理执行机构饱和时特别容易激发高频振荡。
MPC之所以能在这几年的USV控制研究里成为主流方向，核心就一句话：它把约束、预测和多变量建模统一在了一个框架里。你不需要像PID那样为了处理饱和单独设计抗积分饱和结构，不需要像LQR那样事后验证约束是否越界，不需要像滑模那样用不连续项去对抗不确定项——你只需要把约束条件写进优化问题，求解器会帮你算出既满足约束、又让代价函数最小的控制序列。
1.3 适合用MPC的具体场景
MPC不是万能的，它在USV上的优势场景非常明确。轨迹跟踪是典型的场景：给定一条由路径规划模块生成的参考轨迹，MPC通过预测模型和滚动优化，让船按照参考轨迹低速或中速行驶，同时保证转角不超过舵机物理极限。路径跟踪（Path Following）略有不同——它不要求严格“定时定点”到达，而是让船沿着一条几何路径走，强调空间上的收敛而非时间上的对齐，MPC同样适用。
避障是另一个典型场景。USV在水面作业时经常遇到浮标、桥墩、来往船只等障碍物，你可以在MPC框架里把时空障碍物约束写成状态方程的不等式约束，控制器在滚动优化过程中会主动寻找一条不触碰障碍物、同时尽量接近目标路径的可行轨迹。相比传统的“先规划、再跟踪”架构，MPC天然具备某种“局部重规划”的能力，规划和控制之间的界线是模糊的，这对于动态环境尤为珍贵。
2. 建模与MPC控制核心设计
2.1 USV运动学与动力学模型
做MPC第一步就是建立被控对象的数学模型。USV常用模型是三自由度水面船模型，即忽略船体的横摇、纵摇和垂荡，只保留纵荡（surge）、横荡（sway）和艏摇（yaw）三个自由度。运动学方程描述的是大地坐标系下位置与船体坐标系下速度之间的关系：
x_dot = u*cos(psi) - v*sin(psi) y_dot = u*sin(psi) + v*cos(psi) psi_dot = r其中x和y是大地坐标系下的位置坐标，psi是航向角，u是纵荡速度，v是横荡速度，r是艏摇角速度。动力学方程描述的是速度与外力之间的动态关系，形式上一般是：
u_dot = (1/m11) * (m22*v*r - d11*u + tau_u) v_dot = (1/m22) * (-m11*u*r - d22*v) r_dot = (1/m33) * ((m11-m22)*u*v - d33*r + tau_r)m11、m22、m33是包含附加质量的惯性系数，d11、d22、d33是线性阻尼系数，tau_u是纵向推力，tau_r是艏摇控制力矩。这个模型是文献里最常用的简化形式——不考虑非线性阻尼、不考虑风浪流扰动项，但对控制器设计验证来说足够。要注意横荡方向上没有控制输入tau_v，这就是欠驱动特性的数学体现：你没法直接给横荡方向加力，只能通过纵向推力和艏摇力矩的配合间接到这个方向。
2.2 MPC的三要素：预测模型、滚动优化、反馈校正
MPC的原理骨架是三个支柱，理解这三件事就能抓住MPC的本质。
预测模型负责“预见未来”。在k时刻，控制器拿到当前状态x(k)，根据系统模型外推未来Np个采样时刻的状态序列——这个过程需要知道控制输入序列怎么取值，因为不同的输入序列导致不同的未来轨迹。在这个项目里，预测模型就是离散化后的USV模型。你可以用欧拉法做离散化：x(k+1)=x(k)+Ts*f(x(k),u(k))，Ts是采样时间。
滚动优化负责“选最优”。在k时刻，控制器求解一个有限时域最优控制问题，找到一组控制输入序列u(k),...,u(k+Nc-1)，让预测轨迹对参考轨迹的跟踪误差最小、控制能耗最小，同时满足约束。这个优化问题写出来一般是这样：
min sum_{i=0}^{Np-1} (x(k+i)-x_ref(k+i))'Q(x(k+i)-x_ref(k+i)) + u(k+i)'R*u(k+i)s.t. 状态方程约束、输入约束、状态约束。Q是状态权重矩阵，R是控制权重矩阵。这里Nc是控制时域，表示只优化前Nc步的控制量，Nc到Np之间的控制量保持不变（或按某种策略保持）,这样能减少决策变量数目、降低计算量。
最微妙的是“只施加第一步”。滚动优化求出的是一整段控制序列，但控制器真正发给执行机构的只有当前时刻的控制量u(k)。到了下一个采样时刻，系统会重新测量或估计当前状态，把新的状态作为起始点再重新求解一遍。这种“滚动”机制赋予了MPC反馈校正的能力，即使模型有误差、外界有扰动，控制器也不会越飘越远——它每个周期都在做一次基于真实状态的修正。
2.3 约束与权重设计
约束是MPC的看家本领。在USV控制中，最常用的约束包括：
- 输入幅值约束：推力上限tau_u_max、舵角/力矩上限tau_r_max，这对应物理执行机构的饱和特性。
- 输入变化率约束：舵角和推力的变化不能太剧烈，对应舵机转速限制。
- 状态约束：比如航向角范围、位置范围，或者在避障场景下把障碍物表示为位置约束。
权重的设计关系到控制品质。一般Q矩阵中位置误差的权重取大一些，航向误差权重次之，速度误差权重可以相对小一些；R矩阵用于限制控制输入的幅度，取值太大会导致控制量过于保守、跟踪变慢，太小则会让控制量抖动剧烈。实际调试中，位置误差权重给1到10的量级、控制权重给0.1到1的量级，是一个比较常见的起点，然后再根据仿真效果逐步调整。
3. Matlab代码实现过程
3.1 代码总体架构
做这个项目时我没有用现成的MPC Toolbox，而是自己写了基于fmincon的求解器封装，这样能更清楚地看到MPC内部的每一步发生了什么，也方便调整模型或约束。整体代码结构分四层：
- 主脚本：定义仿真参数、参考轨迹、初始条件，循环调用控制器。
- 模型函数：描述USV的离散状态方程，用于预测。
- 控制器函数：在每一步封装代价函数、约束条件、求解器配置。
- 可视化：绘制轨迹、航向、速度、控制量曲线。
3.2 参数设置与初始化
采样时间的选取需要结合平台动态特性。USV的运动周期通常较慢，我会先设Ts=0.5秒左右：太大会丢失系统的动态过程，太小则优化求解次数暴增导致实时性压力大。预测时域Np给20步，也就是说控制器“看”未来10秒左右的轨迹，这个前瞻时间足够让船提前感知弯道和路径偏移。控制时域Nc给5步，减少优化变量以提升求解速度。
初始条件一般设置船在原点、初始航向为0，参考轨迹用直线或者圆。直线适合验证基本的跟踪能力，圆形轨迹适合检验控制器对航向和横漂的协调能力。下面是主脚本里的初始化和参数定义方式：
Ts = 0.5; % 采样时间，单位秒 Np = 20; % 预测时域 Nc = 5; % 控制时域 Q = diag([10, 10, 1, 1, 1]); % 状态权重：位置误差权重最大 R = diag([0.1, 0.1]); % 控制权重：要求控制量比较平滑 % 状态量顺序：x, y, psi, u, v, r（根据实际建模确定） % 约束条件 tau_u_max = 500; % 纵向推力上限 tau_r_max = 200; % 艏摇力矩上限 delta_tau_max = 50; % 控制变化率上限 % 参考轨迹：一条直线，期望速度恒定 x_ref = linspace(0, 200, 2000); y_ref = zeros(1, 2000); psi_ref = zeros(1, 2000);3.3 核心代码逻辑：模型离散化
模型函数是整个预测的基础，必须写成符合预测循环的输入输出接口。我采用欧拉法离散化，尽管精度不如Runge-Kutta，但对于常规仿真和MPC预测来说精度已经足够。下面是模型的Matlab函数实现，描述了完整的状态更新过程：
function x_next = usv_model(x, u, Ts, param) % x = [x_pos; y_pos; psi; u; v; r] % u = [tau_u; tau_r] m11 = param.m11; m22 = param.m22; m33 = param.m33; d11 = param.d11; d22 = param.d22; d33 = param.d33; x_pos = x(1); y_pos = x(2); psi = x(3); u_vel = x(4); v_vel = x(5); r_rate = x(6); tau_u = u(1); tau_r = u(2); dynamics = [ u_vel*cos(psi) - v_vel*sin(psi); u_vel*sin(psi) + v_vel*cos(psi); r_rate; (m22*v_vel*r_rate - d11*u_vel + tau_u) / m11; (-m11*u_vel*r_rate - d22*v_vel) / m22; ((m11-m22)*u_vel*v_vel - d33*r_rate + tau_r) / m33; ]; x_next = x + Ts * dynamics; end注意第三行到第六行的动力学方程中，横荡速度v的导数是‑(m11uv+d22*v)/m22，没有控制输入项，这正是欠驱动特性的体现。仿真时如果发现船在转弯时横向漂移很大，这是物理上的正常现象，控制器需要靠航向角的预偏来压制它，而不是期望横向速度直接为零。
3.4 核心代码逻辑：代价函数与约束写进优化问题
在每一时刻，控制器需要求解一个带约束的优化问题。fmincon是Matlab自带的非线性优化求解器，对于本项目的变量维度（Nc*2个控制变量）来说速度可以接受。代价函数是指标的核心：把预测的轨迹与参考的偏差加权求和，同时把控制量的幅值作为惩罚项加入。计算代价函数时需要从当前状态出发、用候选控制序列去逐一模拟未来Np步的过程：
function cost = cost_function(u_seq, x0, x_ref_traj, Np, Nc, Ts, Q, R, param) x_pred = x0; cost = 0; for i = 1:Np if i <= Nc u_now = u_seq(2*i-1:2*i); % 当前控制量 else u_now = u_seq(2*Nc-1:2*Nc); % 超出控制时域后保持末段控制量 end x_pred = usv_model(x_pred, u_now, Ts, param); ref = x_ref_traj(i,:)'; cost = cost + (x_pred-ref)' * Q * (x_pred-ref); if i <= Nc cost = cost + u_now' * R * u_now; end end end这里有个细节值得专门提醒：超出控制时域Nc之后，控制量保持为最后一段值，这在长期预测中会产生一定的误差。如果你的系统不稳定特性比较强，建议把Nc稍微加大，或者对末段控制量施加一个小的惩罚，避免预测轨迹在末端出现不实际的发散。
约束条件在fmincon里通过非线性约束函数传入。输入的幅值约束可以直接用上下界参数处理，变化率约束需要写成非线性不等式约束的形式：
function [c, ceq] = constraints(u_seq, u_prev, Nc) ceq = []; c = []; delta = zeros(Nc, 2); for i = 1:Nc if i == 1 delta(i,:) = u_seq(2*i-1:2*i) - u_prev; else delta(i,:) = u_seq(2*i-1:2*i) - u_seq(2*i-3:2*i-2); end end c = [c; abs(delta(:,1)) - delta_tau_max; abs(delta(:,2)) - delta_tau_max]; end3.5 每一步的控制循环与仿真推演
仿真的主循环体现了MPC“滚动优化”的节奏。每一步优化求解出控制序列后，只取第一个量赋给被控对象，然后将对象推进一个采样周期；对象的新状态随后被带回控制器，作为下一步优化的初始状态。主循环的核心代码如下：
x_history = zeros(6, N); u_history = zeros(2, N); x_current = x_init; u_prev = zeros(2, 1); for k = 1:N-1 % 生成当前时刻的参考轨迹序列 x_ref_traj = ...; % 从全局参考中截取预测时域范围内的参考状态 % 求解最优控制序列 u_opt = fmincon(@(u) cost_function(u, x_current, x_ref_traj, Np, Nc, Ts, Q, R, param), ... u_init, [], [], [], [], lb, ub, ... @(u) constraints(u, u_prev, Nc), options); % 只施加当前时刻的第一个控制量 u_current = u_opt(1:2); x_current = usv_model(x_current, u_current, Ts, param); x_history(:,k+1) = x_current; u_history(:,k) = u_current; u_prev = u_current; end我实际运行时发现，fmincon对初值比较敏感。建议用上一个时刻的最优解作为本次优化的初值（也就是热启动），这样不但收敛快，还能减少数值抖动。第一次迭代没有上一时刻解时，就用当前稳态控制量作为初值。
3.6 从线性MPC到非线性MPC的演进
做这个项目时我们可以先从线性MPC入手，把USV动力学方程在工作点附近线性化，然后用quadprog求解二次规划问题。线性MPC的优势是求解速度快、算法稳定性好，Matlab的MPC Toolbox可以直接用。但USV的动力学具有很强的非线性特性——大角度转弯时，动力学方程中的哥式力项uv和vr会发生显著变化，线性化模型的预测会失真。该项目的进阶方向是实现非线性MPC（NMPC），直接用完整非线性模型做预测，然后通过fmincon或专用NMPC求解器求解。
非线性MPC的求解速度是瓶颈，对于USV这种慢动态系统，采样时间0.5秒给求解留出了足够的运行窗口，因此使用fmincon是可行的。如果你后续要上真机、采样时间需要压缩到10毫秒量级，可以考虑更高效的求解器如内置的MpcActiveSetSolver或者第三方工具YALMIP配合OSQP、IPOPT等，并使用C代码生成来加速。
4. 常见问题与排查技巧
4.1 仿真发散或轨迹震荡
我在最初跑仿真时遇到最多的问题是结果发散：船在追踪直线时竟然越跑越偏，最后飞出了坐标系。排查下来主要原因有三类。第一类是采样时间Ts设置过大。如果Ts太大，欧拉离散化对连续系统的近似误差就会剧增，预测结果失真。解决方法是把Ts调小，但也要注意不能太小——太小会让控制频率过高，单步优化时间不足。第二类是预测时域Np太短。如果Np过短，控制器只能“看到”眼前几秒的路径，来不及对弯道或大角度修正做出预判，自然容易发散。建议Np至少覆盖系统主导动态周期的2至3倍。第三类是权重矩阵Q、R比例失衡。Q过大、R过小会让控制量猛打猛冲，激励出系统的未建模动态，反而造成震荡。
我踩过的一个比较隐蔽的坑是：初值给得太离谱。如果初始状态离参考轨迹非常远，fmincon可能在可行域边缘找不到合适的下降方向。解决办法是先用一个简单的LIN-MPC或者给一个较大的初试控制量做开环推进，让船先靠近参考轨迹，然后再切换到MPC闭环。
4.2 约束不可行问题
MPC最折磨人的问题就是优化器返回“约束不可行”——无解。我遇到过的情况是：预测时域内设置了一个很苛刻的位置约束，同时控制输入的幅值上限又太保守，优化器在有限步数内无论如何都无法把轨迹拉回允许区域内。这种情况在避障场景里尤其常见。
解决思路有两个方向。第一个是增加软约束。把硬约束改写成带有松弛变量的软约束，代价函数中额外惩罚松弛变量的大小，这样即使约束暂时无法满足，优化器也会给出一个“尽可能不侵犯约束”的折中解，而不是直接宣告无解。这在工程上非常实用，因为水面环境的不确定性决定了你不可能永远保证硬约束可满足。第二个是调整约束范围或时域。适当放宽输入变化率约束、增大推力上限或加长预测时域，让优化器有更多自由度寻找可行解。
4.3 计算时间过长与实时性问题
MPC在每一个采样时刻都需要求解一个优化问题，如果求解时间超过采样周期，控制器就变成了“延迟控制”，这在真实系统中是不可接受的。我的经验是先用Profile函数查看fmincon的调用耗时，如果发现单次求解超过0.1秒而采样时间是0.5秒，其实还有余量，但如果你觉得紧张，可以从以下方面优化：
- 缩短控制时域Nc。Nc从5减到3，决策变量就是4个，求解时间可以下降接近一半。
- 减少预测时域Np。Np从20减到15，预测循环变短，代价函数计算更快。
- 简化代价函数。去除不必要的矩阵乘法，直接写标量展开式。
- 提供解析梯度。fmincon默认用有限差分计算梯度，如果自己提供梯度函数，收敛速度往往成倍提升。
- 使用热启动初值，避免每次从零开始搜索。
4.4 调参经验的实操总结
调参这件事，说难也难，说简单也简单。我自己的经验是：先固定一个合理的权重比例，再把预测时域调好，最后才动权重细调。
具体来说，先设定Q中位置误差权重为10、航向为1、速度为1，R中两个控制权重为0.1。跑一次直线跟踪，看看稳态误差和控制量是否在一个合理范围。如果控制量震荡，就把R调大，比如0.5或1；如果跟踪太慢，就把Q中的位置权重调大，或适当缩小R。这样反复迭代几次，就能找到比较满意的参数组。为了更高效地做这件事，可以写一个参数扫描脚本，对几组参数自动仿真并画出误差指标对比图，一眼就能看出趋势。
另外一个小技巧：在USV这种慢动态系统里，不要把控制权重调得太小。R太小虽然能带来更快的跟踪响应，但控制量会非常“冲”，一会儿正向满推、一会儿反向满推，推进器和舵机都受不了。我在调参时会把控制变化率约束调得比较紧，保证控制量有物理可实现性——宁愿跟踪误差稍微大一点，也不能让执行机构做无效的高频摆动。
4.5 模型失配时的鲁棒性讨论
仿真模型和真实系统总有差距，因此模型失配的鲁棒性很重要。我在项目中特意做了这样一个实验：在仿真对象里故意加上一个随时间变化的未知水流漂移项，但控制器里的预测模型并不知道这个漂移。结果MPC依然能保持大致正确的航向，但位置会出现持续的稳定偏差。这说明MPC的反馈校正能力可以对抗扰动，但它默认把扰动解释为状态偏差，而不是主动估计扰动项。
如果希望更精确地补偿扰动，可以考虑两个方向。一是采用扩展状态MPC，把未知扰动扩展为系统状态，用观测器对扰动进行估计，然后再补偿。二是将MPC与扰动观测器结合，在控制量前馈通道中加上估计的扰动补偿。这个方向不算复杂，但能大幅提升真实水面环境下的控制精度。
结尾
这个项目做下来，我个人最大的收获不是把MPC调通，而是真正理解了“预测控制”这四个字的工程含义——预测不是算得准、算得多，而是在模型不完美、环境不确定的前提下，通过滚动修正始终保持对真实状态的跟随。我在实际调试中反复踩进过发散、约束不可行、求解超时这些坑，但每次把问题拆解开之后，发现根源往往不是算法本身，而是参数配置和模型离散化这类看似“不太配得上论文版面”的基础细节。
另外想分享一个小技巧：做USV的MPC仿真时，把仿真环境稍微“弄脏”一点——加一点随机的风浪干扰、加一点模型参数摄动、加一点测量噪声，这样调试出来的控制器才可能在接近真实的工况下依然稳得住。如果只在理想模型上跑得完美，那只是玩具，不是控制方案。后续如果想在这个方向上继续扩展，可以把避障约束加进MPC的优化问题，或者用分布式MPC做多船协同控制，这些都是很好的进阶方向。
