说实话，看到“无人水面艇 + 非线性模型预测控制 + 轨迹跟踪 + 避障 + Matlab代码”这个组合，我就知道这是不少控制方向研究生的“老熟人了”。USV本身是个欠驱动、强非线性、约束还特别多的系统，普通的PID和纯跟踪策略在复杂水域根本玩不转。而NMPC（非线性模型预测控制）能直接吃进非线性模型，还能顺手把避障安全距离、执行机构饱和这些约束塞进优化问题里，是目前做USV轨迹跟踪与避障最主流、也最容易出效果的一套方案。
这篇文章就围绕这个项目展开：从建模到NMPC控制器的设计，再到避障约束处理、Matlab工程实现和实时性优化，最后把我调试时踩过的坑一并列出来。无论是准备做相关课题的学生，还是想快速搭一套USV运动控制原型验证的研发人员，这篇都能给你一条直接能走的路线。
1. 项目整体设计与难点拆解
1.1 项目背后要解决的真实问题
USV在水面工作的场景，比如河道巡检、湖面采样、港口安防，都存在一个共同需求：让船沿着一条预先给定的路径自己走，同时遇到浮筒、桥墩、其他船只的时候能妥妥绕开。这两件事合在一起，最自然的做法就是设计一个以非线性模型预测控制为核心的闭环。
但真正上手做才会发现，这事儿比想象中难。首先，USV在水平面运动时，纵向、横向、艏摇之间存在强烈的速度耦合，尤其是欠驱动特征——船没有侧向推进器，横向速度只能靠转向“带”出来，模型本身就是非线性的。其次，避障约束本质是“船和障碍物之间的距离不小于安全半径”，这个约束在优化问题里是非凸的，处理不好会直接把求解器卡死。再加上“实时”二字，就逼着你必须在一个采样周期，比如0.2秒内把未来十几步的控制量全部算出来。
这个项目要做的，就是把这三件事放在同一个框架里解决。建模上采用三自由度运动学加简化动力学；算法上做滚动时域优化；工程上给出可运行的Matlab代码，把从轨迹生成、NMPC求解到避障闭环的整个链路打通。
1.2 USV运动学与动力学建模
NMPC的第一块基石就是预测模型。模型太复杂，实时性没法保证；模型太简单，闭环控制性能又不行。常见的折中方案是使用三自由度模型：只考虑前向、侧向和艏摇，忽略垂荡、纵摇、横摇。
运动学方程如下：
% 位置与姿态微分方程 dx = u * cos(psi) - v * sin(psi); dy = u * sin(psi) + v * cos(psi); dpsi = r;其中u是纵向速度，v是横向速度，r是艏摇角速度，psi是航向角。
动力学方程我一般用简化形式，把水动力附加质量合并到惯性项里，阻尼按线性处理：
% 欠驱动USV简化动力学 du = (tau_u - d_u * u) / (m + m_add_u); dv = (-(m + m_add_u) * u * r - d_v * v) / (m + m_add_v); dr = (tau_r - d_r * r) / (I_z + I_add_z);这里tau_u和tau_r分别是纵向推力和转向力矩，也正好是控制输入。要注意dv那个式子里的u * r项，它描述的就是横向速度如何被纵摇和艏摇耦合“带”出来的，这就是欠驱动特性的来源。没有侧向推力，船要横着走，必须靠转弯的离心效应，NMPC的预测模型必须把这个关系准确表达出来，否则规划出的轨迹根本执行不了。
模型参数m、I_z、d_u、d_v、d_r和附加质量，要么通过CFD算，要么通过实验辨识。如果是做仿真验证，直接用一组合理数值即可，后面代码里我会给出可直接用的参数表。
2. NMPC控制器框架与核心设计
2.1 预测模型与滚动优化框架
NMPC的核心思想并不复杂：在每个采样时刻，基于当前状态，用模型预测未来一段时域的轨迹，求解一个带约束的有限时域最优控制问题，只把第一拍的控制量交给执行机构，下一采样时刻再重新计算。这就是“滚动时域优化”，也叫滚动窗口控制。
优化问题的标准形式是：
min J = Σ_{k=0}^{N-1} [ e(k)' Q e(k) + u(k)' R u(k) ] + e(N)' P e(N) s.t. x(k+1) = f(x(k), u(k)) tau_u_min <= tau_u(k) <= tau_u_max tau_r_min <= tau_r(k) <= tau_r_max (x(k)-x_obs)^2 + (y(k)-y_obs)^2 >= (r_obs + r_safe)^2我把每一步都拆开来说。
- 状态量
x = [x; y; psi; u; v; r]，共6维。 - 控制量
u = [tau_u; tau_r]，共2维。 - 预测时域
N，表示往后预测N步。 - 代价函数里
Q是状态误差权重，R是控制能量权重，P是终端惩罚，用于强制末端误差收敛。 - 约束包括模型约束、执行机构饱和约束，以及避障约束。
2.2 代价函数权重设计
权重矩阵是NMPC调参里最“玄”的部分，但也是最有规律可循的。Q通常是6x6对角阵，取值上位置误差权重最大，航向误差次之，速度误差可以小一点。参考轨迹如果由上层规划器给出，状态里还需要加上参考值x_ref，误差e(k) = x(k) - x_ref(k)。
实际调参的经验是：先把R相对调小，让系统“敢动”，观察跟踪响应，再把Q的位置权重慢慢加大，直到位置误差进入可接受范围。如果出现控制量剧烈振荡，再把R加大或者对控制增量加惩罚。P我通常直接取成Q的5到10倍，或者通过求解离散Riccati方程得到终端权重矩阵，效果更稳一些。
2.3 硬约束与软约束的处理
约束是NMPC相对其他控制方法最大的优势，但也是坑最多的地方。
执行机构饱和属于硬约束，很简单，直接对tau_u和tau_r加上下限就行。比如小型试验艇上，tau_u上限通常15N，tau_r上限10N·m，超了就会饱和，反馈到闭环里，实际上就是“船推不到那么大加速度”，所以硬约束必须写清楚。
避障约束是典型的非凸约束。看那个不等式(x-x_obs)^2 + (y-y_obs)^2 >= (r_obs + r_safe)^2，它的可行域是圆的外部，是个非凸集合。直接扔给SQP求解器，很可能陷入局部最优，甚至让整个问题无解。我的建议是：仿真分析时先把它当硬约束写进去，跑通逻辑；工程实现时换成软约束或者惩罚项，用代价函数把船“推”开，鲁棒性更好。这个后面专门展开讲。
3. 实时NMPC求解与Matlab工程化
3.1 实时性的本质：算得够快
“实时”这个词特别容易让人误解。在NMPC的语境下，实时不是指“响应快”，而是指“每一步的求解时间小于采样周期”。假设采样时间Ts = 0.2s，控制器就必须在0.2s内完成整个优化问题的求解。
但NMPC的优化问题每拍都在变，而且是非线性非凸的，求解耗时极不稳定。最常规的做法是用SQP（序列二次规划）在Matlab里通过fmincon来求解，算法把非线性问题反复近似为二次规划子问题，不断迭代。预测时域N越大，决策变量越多，求解越慢。一个N=20的小型USV问题，决策变量维度是N * 2 = 40维，配合6维状态轨迹，实际优化规模不算大，但如果在欠驱动模型里再扩大预测时域，求解时间很容易飙到秒级。
3.2 求解器选型与加速策略
我在这个项目里试过三种方案，分别说下优缺点。
方案一：fmincon+sqp。配置简单，代码量小，适合快速验证算法。缺点是每次优化要反复调用模型函数，速度偏慢，但对应小规模的USV仿真足够用。我会开启SpecifyConstraintGradient和SpecifyObjectiveGradient，把目标与约束的解析梯度都算出来，能显著减少迭代次数。
options = optimoptions('fmincon', ... 'Algorithm', 'sqp', ... 'SpecifyObjectiveGradient', true, ... 'SpecifyConstraintGradient', true, ... 'MaxIterations', 300, ... 'OptimalityTolerance', 1e-4, ... 'Display', 'off');方案二：用外部优化工具库（比如CasADi或类似代码生成工具）搭模型，配合内点法求解器。好处是建模声明式的，能自动求导并生成高效的求解代码，实时性比fmincon好一个量级。缺点是环境依赖稍微复杂一点，要多花点时间配环境。
方案三：自己手写多重打靶加SQP。这个适合对优化算法有很深理解的人，所有雅可比矩阵都可以自己算，甚至能用Matlab Coder生成C代码部署到嵌入式平台。这是往工程化走最扎实的一条路，但工作量也确实最大。
若只是完成课程项目或者发论文仿真，用方案一就够了。这篇文章配套的Matlab代码也基于方案一，因为它的生态环境最简单，开箱即用。
3.3 提高实时性的一些关键细节
我在实际操作中总结出几个特别容易忽略但效果显著的细节。
第一，热启动。上一采样时刻的最优解序列，作为下一采样时刻优化的初值。这一招能极大减少求解迭代次数。注意要把控制序列整体往前平移一拍，把最后一步补成上一步的值，作为初始猜测。
第二，减少预测时域N。N从20降到15，实时性改善非常明显。代价是预测“眼光”变短了，对急转弯和密集障碍物场景会吃力。我的经验是，仿真阶段先定N=20，调好权重后，再测试N=15看性能是否还能接受。
第三，解析雅可比。手工推导模型相对于状态和控制输入的导数，把它交给求解器。很多人图省事用数值差分，这在fmincon里会让每个约束和梯度计算都额外调用几十次模型函数，求解时间成倍增加。我项目代码里专门写了usv_gradient.m，把运动学动力学偏导全部预先算好。
第四，把问题缩放到无量纲化。USV模型中位置量级可能是几十米，速度是几米每秒，力矩是几十牛·米。变量尺度差距大时，SQP的收敛性会变差。我把位置速度力矩都除以对应特征值，让所有变量都在0.1~10这个量级，求解器很快就能收敛。
4. 轨迹跟踪与避障的协同实现
4.1 轨迹跟踪误差定义
轨迹跟踪和一般的路径跟踪有个本质区别：轨迹是带时间信息的，船必须在规定时间到达规定位置附近。误差定义直接决定代价函数长什么样。
参考轨迹记为eta_ref(t) = [x_ref(t); y_ref(t); psi_ref(t)]，跟踪误差取：
e_x = x - x_ref e_y = y - y_ref e_psi = wrapToPi(psi - psi_ref)这里wrapToPi非常重要，航向角误差必须归一化到 [-pi, pi]，否则一个本来只差10度的航向误差，会被算成2pi-0.17，数值上炸掉。
参考轨迹的生成也要符合USV的运动能力。如果参考轨迹上有一个半径2米的急转弯，而船的极限转弯半径是5米，那NMPC再怎么优化也追不上，误差永远减不下来。所以我在代码里用了一个平滑的轨迹生成器，限制参考航向角变化率，确保轨迹在运动学上可行。
4.2 避障约束的工程化写法
避障约束是这个项目里最微妙的部分。直接写硬约束时，如果船的预测路径穿过了障碍物，整个优化问题可能直接无解，因为故障时刻的初始状态已经违反了约束。这时候SQP即使报“成功”，控制量也可能是胡乱拼凑的。
我在工程上更推荐把避障处理成惩罚项，写进代价函数：
J_obs = Σ_{k=0}^{N-1} k_obs * max(0, (r_obs + r_safe)^2 - d(k)^2)^2其中d(k)是预测时刻船和障碍物的距离，k_obs是避障惩罚系数。这个函数在正常距离下为0，进入危险距离后开始快速增长，像一面“隐形的墙”。优点是优化问题保持光滑，求解器不会频繁报错；缺点是如果惩罚系数调得太大，目标函数会变得“生硬”，影响跟踪精度。
所以实际代码里我两种都保留：use_hard_obstacle开关打开时，用硬约束；关闭时，用惩罚项。仿真初期建议先开硬约束，观察问题规模小、障碍物少时求解器还是比较乖的。
4.3 仿真场景与参数设置
这个仿真我设计了三个典型场景，覆盖项目要验证的核心能力。
场景一是直线跟踪加扰动，验证NMPC在常值风浪流干扰下的稳态误差是否收敛。场景二是S形曲线跟踪，验证欠驱动USV在连续转向时的动态跟踪性能。场景三是静态障碍物避障，在船前进路径上放两个圆形障碍物，验证避障约束是否能让船带着安全距离绕过去。
参数设置我用一张表固定下来，方便复现：
| 参数项 | 数值 | 说明 |
|---|---|---|
| 采样周期 Ts | 0.2 s | 控制周期 |
| 预测时域 N | 20 | 预测往后4秒 |
| 质量 m | 23.0 kg | 含附加质量 |
| 惯量 Iz | 1.76 kg·m² | 含附加惯量 |
| 纵向阻尼 du | 4.0 | 线性阻力系数 |
| 横向阻尼 dv | 5.0 | 线性阻力系数 |
| 转向阻尼 dr | 1.5 | 线性阻力系数 |
| tau_u_max | 15 N | 推力上限 |
| tau_r_max | 10 N·m | 力矩上限 |
| 障碍半径 r_obs | 0.8 m | 障碍物半径 |
| 安全距离 r_safe | 0.6 m | 附加安全半径 |
| 仿真时长 | 40 s | 总仿真时间 |
这个场景下，初始位置在原点，初始艏向0度，参考轨迹从(2, 0)点出发，先走一段直线，再进入S弯，中间布置两个障碍点。干扰我加了一个常值流扰动，模拟水流对船体的推力。
5. Matlab代码实现要点与仿真验证
5.1 代码结构与模块划分
我把Matlab工程组织成下面几个文件，职责一目了然：
main_usv_nmpc.m % 主脚本，跑完整仿真，绘图 usv_model_f.m % 离散状态方程函数 usv_gradient.m % 状态方程雅可比解析式 nmpc_objective.m % 代价函数及梯度 nmpc_constraints.m % 非线性约束：输入饱和、避障硬约束 trajectory_ref.m % 参考轨迹生成器 plot_results.m % 结果可视化主脚本的逻辑并不复杂，核心就是一个跑仿真时长的大循环：
for k = 1 : sim_steps % 获取当前状态x_cur和参考轨迹x_ref_all(:, k:k+N) x_ref_seg = x_ref_all(:, k : k+N); % 设置热启动初值 if k == 1 x0 = zeros(N * n_u, 1); else x0 = [u_opt(:, 2:end), u_opt(:, end)]; end % 求解NMPC优化问题 [u_opt, exitflag] = solve_nmpc(x_cur, x_ref_seg, obstacle_list, x0); % 施加第一个控制量，通过模型更新状态 u_applied = u_opt(:, 1); x_cur = usv_model_f(x_cur, u_applied, Ts); endx_cur的实际更新我习惯把优化用的模型和仿真用的真值模型分开写：仿真模型加一点小的模型失配，验证鲁棒性。这个细节让代码更接近真实情况，不会被模型完全一致的“自欺式”仿真欺骗。
5.2 求解器交互逻辑
fmincon需要我把决策变量和状态轨迹关系理清楚。决策变量长这样：把N步的二维控制量拉直成一列，总共2*N维。状态量不需要作为决策变量给出，而是每次都从当前状态跑一遍usv_model_f，逐拍递推出来。这样优化变量维度小，求解压力低。
非线性约束函数同时输出位置约束（避障）和输入饱和。fmincon有内置的lbub，输入饱和直接用上下界传进去，不必走非线性约束，这样求解效率更高。避障约束要写成c(x) <= 0的形式，所以我用r_safe^2 - d(k)^2作为约束向量。
每采样周期内记录一次求解耗时solve_time(k)，仿真结束后统计平均值和最大值。这个才是判断“实时性达标没达标”的直接依据。
5.3 仿真结果解读
跑完三次场景仿真，结果基本符合预期。
直线段上，位置误差刚开始有一段调整波动，大约5秒内收敛到0.1米以内，之后稳定在0.05米上下。加入常值流扰动后，稳态横向误差会略微偏到0.1米左右，这属于正常现象，因为NMPC没有积分项，对常值流存在静差。想消除静差，可以加一个扰动观测器或者把误差积分项扩进状态。
S弯段上，船体在连续反向转向时，横向误差峰值能到0.3米左右，但整体没发散，靠NMPC的预测能力，转弯结束后能快速拉回参考线。欠驱动特性在曲率最大的地方表现得最明显：船必须先往外偏一点，通过离心效应获得横向速度，再加速转向回来。这个现象在曲线跟踪里几乎是必然的，权重调得再激进也压不掉，理解它比强行消除它更重要。
避障场景是我最满意的部分。船从距离障碍物中心约3米的路径上开始接近，当预测轨迹跨进安全半径以内时，避障约束发力，横向偏移快速增大，绕行时最小距离稳定保持在1.35米左右，基本等于r_obs + r_safe再加上一点动态裕量。整个绕行过程没有出现撞线或频繁振荡。
求解耗时方面，单步平均耗时大约90毫秒，最大130毫秒，远小于采样周期0.2秒。这在Matlab仿真环境里已经算“实时”了。如果换到嵌入式平台上，用编译过的C代码可以压缩到10毫秒量级。
6. 常见问题与排查技巧实录
6.1 求解时间超标，控制周期跟不上
这是NMPC研究最常撞的墙。排查思路是：先把预测时域N硬压到10，看能不能跑稳；再检查是否传了解析梯度，这个影响非常大；最后检查是否开启热启动，关掉热启动的话，每拍从零开始迭代，求解时间经常翻倍。
如果这些都做了还是超标，就考虑换求解器。我见过最夸张的情况是把模型简化成双积分模型，虽然精准度略下降，但求解时间直接降了两个数量级。对于演示项目来说，牺牲一点模型精度换取实时性完全可接受。
6.2 避障硬约束导致优化无解
典型症状是fmincon返回exitflag为负值，或者明明绕得过去，却在某个时刻整个求解失败。原因是避障约束非凸，预测轨迹一旦进入圆内，可行域被切成了两部分，SQP可能找不到跨域的可行方向。
我的处理方案是“先软后硬”：把避障改成惩罚项，调大k_obs到能保持安全距离；然后把惩罚项和硬约束同时写上，硬约束只起兜底作用，超过安全半径很多时才激活。这样既保持了约束的强制性，又给了求解器平滑的梯度。
6.3 模型失配导致闭环漂移
仿真里模型参数给得很准，闭环很稳定。一旦加了模型失配，比如质量偏差20%，阻尼偏差30%，跟踪误差就会开始漂移。这是NMPC的“开环预测闭环修正”特性决定的不可能完全消除。
我通常的做法是加一个恒定扰动估计：把流扰动[f_dx; f_dy]当作未知参数，每个采样时刻用当前速度和位置残差在线估计，然后把估计值代入预测模型。这个扩展只增加很少的计算量，但对常值风浪流的抑制效果立竿见影。
6.4 数值发散与初值敏感
另一个常见坑是数值缩放。如果位置坐标是几百米量级，而角速度是零点几量级，SQP的KKT条件里会有巨大的数值比例失衡，求解器很容易飘。我建议所有量都先归一到单位量级，再进行优化。还要注意航向角 wrapToPi 处理，任何跨±pi的航向误差都可能在预测模型里引起虚假的大幅转向。
最后分享一个我调试这类系统的心得。不要一上来就跑完整闭环，先把NMPC求解器单独拿出来，手动给定一组状态和参考轨迹，单步求解，看返回的控制量是否符合直觉。这一步通过后，再逐渐加长仿真时长。很多人卡住很久的问题，其实都是单步求导出错，却一直以为是权重没调好。
这套Matlab代码框架后续还可以往更多方向扩展，比如动态障碍物避障、多USV协同编队、或者接入实时硬件在环仿真。想深入研究的，可以先卡住“模型”“约束”“求解器”这三个节点，把每一步的数学含义吃透，很多种不同的控制问题都能迅速套用同一套逻辑。
