前阵子接了一个无人船相关的仿真项目，要求把轨迹跟踪、非线性模型预测控制、障碍物避碰揉在一个Matlab程序里，还得对标某篇IEEE论文的复现结果。说实话，刚拿到这个任务的时候心里是有点发怵的——NMPC本身就是控制领域公认的“效果上限高、落地门槛也高”的代表，再叠加水面无人船这种强非线性、欠驱动、还带环境扰动的对象，模型稍微给得不合适，后面的优化求解分分钟教你重新做人。不过等整个项目跑通，回头再看，其实NMPC用在USV轨迹跟踪这件事上，思路非常清晰，难点主要集中在模型怎么建、约束怎么设、求解器怎么调这三件事上。这篇文章就是把我从模型搭建到仿真验证的完整过程记录下来，包括踩过的坑和改过的参数，希望能给正在啃无人船控制方向的朋友一点参考。
先说清楚这个程序能做什么：目标船按照一条给定的参考轨迹（比如正弦路径、调头路径）运动，无人船在不知道完整未来轨迹信息（或者只知道参考轨迹）的情况下，通过NMPC在每个控制周期实时求解有限时域最优控制问题，输出推进力与转艏力矩，让无人船既跟踪参考轨迹，又能避开设定的静态障碍物。程序基于三自由度水面船模型，用Matlab的fmincon做滚动优化求解，仿真环境里放了几个圆形障碍物，验证避碰效果。这套东西复现出来以后，基本就是IEEE论文里比较典型的那种“NMPC + USV轨迹跟踪 + 避碰”实验框架，适合作为算法对比的基线，或者后续往编队、动态避障方向扩展的底盘。
整个过程里我最大的感受是：NMPC的程序实现其实没有想象中那么玄乎，真正需要花心思的地方，一是动力学模型与仿真步长怎么匹配，二是避碰约束怎么从“看起来合理”变成“求解器能处理”，三是SQP求解器在非线性约束下的数值稳定性。这三块搞定了，剩下的就是调参和跑结果。
1. 为什么无人船轨迹跟踪会选中NMPC：不是炫技，是任务需求
1.1 轨迹跟踪的底层矛盾：跟踪精度与执行机构响应
水面无人船的运动控制，从任务层面拆开来看其实就两个目标：第一个是“船能不能按照指定的路线走”，第二个是“在走的过程中遇到障碍物能不能绕开”。前者是跟踪精度问题，后者是避碰安全问题。两者单独拿出来，PID、LQR这类线性控制方法其实都能凑合——反正给定一条参考轨迹，用线性化模型在工作点附近设计控制器，小扰动的场景下表现也够看。但问题在于，水面船的运动本质上是强非线性的，尤其是航向变化大的时候（比如大角度转弯、调头），横漂、艏摇与纵荡之间的耦合效应会非常明显，用线性模型近似出来的控制效果会随着偏离工作点越来越差。
1.2 为什么选NMPC而不是传统PID或LQR
NMPC的核心优势在于“带着未来看现在”。每个控制周期，控制器会基于当前状态预测未来一段时间内系统的运动轨迹，然后找出一个控制序列，让预测轨迹在满足约束的前提下尽量逼近参考轨迹。这里面的关键点有两个：一是预测时域带来的“前瞻性”——它能看到未来几秒内的约束条件变化，因此可以在还没撞上障碍物之前就主动调整航向；二是约束处理能力——控制量幅值限制、控制量变化率限制、避碰障碍物距离限制，这些物理约束可以直接写进优化问题中，而不是靠限幅器事后截断。
打个比方，PID控制像是一个“只看眼前三步”的司机，看到前面有弯道了才打方向盘，遇到坑洼了才刹车；而NMPC像是一个“边开边看导航路况”的司机，在入弯前几百米就开始规划路线，提前把车速和方向盘角度调整到合理范围内。这种前瞻性对无人船尤其重要——船舶惯性大、执行机构响应慢，晚一秒动作可能就多冲出去好几米。
当然，NMPC的代价也很直观：运算量大。每个控制周期都要求解一个非线性约束优化问题，实时性压力比传统控制器大得多。这也是为什么很多论文里NMPC的仿真步长都取在0.2秒到0.5秒之间，而且被控对象模型都会做一定的简化处理。
1.3 整体方案框架：模型、控制器、仿真三件套
整个程序可以拆成三个模块来理解。首先是对象模型模块，负责描述无人船的运动学与动力学特性——包含参考坐标系定义、三自由度运动方程、控制量与状态量的映射关系。其次是控制器模块，这是核心，负责在每个采样时刻构建NMPC优化问题——包括预测模型离散化、代价函数设计、约束条件构建，然后调用MATLAB优化工具箱求解控制量。最后是仿真环境模块，负责整合对象模型和控制器，完成离散时间循环——包括参考轨迹生成、状态更新、数据记录、结果可视化。
这三个模块在代码层面要尽量解耦。我在最开始写程序的时候偷懒，把模型方程直接写进了控制器里，结果后面想改一下模型参数，控制器和仿真部分都要同步改，出了好几次低级错误。后来老老实实拆成独立函数，模型统一由model参数结构体传入，改参数只需改一处，省心很多。
2. 船舶模型：三自由度方程与离散化处理
2.1 坐标系定义与运动学方程
无人船运动模型通常采用两个坐标系来描述：大地固定坐标系（北东坐标系）和船体固定坐标系。大地坐标系用来描述船舶的位置与航向，是“外面的观察者”看到的轨迹；船体坐标系则固定在船体上，用来描述船体自身的线速度与角速度，是“船上的传感器”感受到的运动。
用公式来表示的话，状态量通常取为η = [x, y, ψ]^T，其中x和y是大地坐标系下的位置，ψ是艏向角；控制相关的速度量取为ν = [u, v, r]^T，对应纵荡速度（surge）、横荡速度（sway）和艏摇角速度（yaw rate）。运动学方程本质上是船体坐标系速度向大地坐标系的投影转换：
x_dot = ucos(ψ) - vsin(ψ) y_dot = usin(ψ) + vcos(ψ) ψ_dot = r
这组方程看起来简单，但它是整个仿真系统的基础。船体坐标系下的运动速度u和v是“船自己在水里的感觉”，但当它被投影到大地坐标系时，就变成了外面人看到的x和y方向位移变化率。这里面最容易出错的地方是符号：有些论文把运动学方程写成x_dot = ucos(ψ) + vsin(ψ)，这取决于v的正方向定义。建议在写代码前先明确v的正向定义是向右舷为正，然后严格按照转换矩阵来写，不要凭感觉。
2.2 动力学方程的选择：简化Nomoto模型还是完整力矩模型
动力学方程描述的是“力和力矩如何产生加速度”，这是NMPC预测模型中最耗计算量的部分，也是建模误差的主要来源。完整的水面船动力学模型是高度耦合的，包含附加质量、科氏力、阻尼力、流体动力等十几个参数，参数整定麻烦不说，每个控制周期都要在数十次甚至上百次优化迭代中反复调用，计算开销实在太大。
在实际复现IEEE的NMPC轨迹跟踪程序时，更常见的做法是采用简化的三自由度动力学模型，把纵荡、横荡和艏摇之间的耦合关系用一个较简洁的矩阵形式表达。一种典型形式是：
M * ν_dot = -C(ν) * ν - D(ν) * ν + τ
其中M是包含附加质量的惯性矩阵，C(ν)是科氏力和向心力矩阵，D(ν)是阻尼矩阵，τ是控制输入（推进力和转艏力矩）。这个模型保留了水面船运动的主要非线性特征，又不像全参数模型那样需要大量的流体动力学参数，平衡了精度与计算效率。
不过这里有个隐藏的问题：这个简化模型虽然物理上合理，但在NMPC里面，预测部分的计算量依然不小。因为每次求解优化问题时，fmincon都要在迭代过程中反复调用动力学方程进行数值积分（就是离散化状态转移）。如果离散化步长取得太小、预测时域取得太长，一次预测就要调用几十次动力学函数，整个优化循环的计算时间会爆炸式增长。
所以我在程序设计里额外加了一个模型离散化参数项：仿真步长Ts仿真取0.1秒，但NMPC预测模型内部的离散化步长可以独立设置。这样仿真精度和控制器计算效率可以分开调节，不会一改仿真步长就影响控制器的收敛性。
2.3 离散化方法与仿真步长设计
连续时间的动力学方程没法直接在优化问题中使用，必须按采样周期离散化。最常用的方法是一阶欧拉法：
x(k+1) = x(k) + Ts * f(x(k), u(k))
如果你觉得欧拉法精度不够，也可以换用四阶Runge-Kutta（RK4）做离散，但计算量会明显上升。就我复现的这套场景来说，控制周期取0.2秒、预测时域取10到20步时，欧拉法的精度已经足够——只要障碍物安全距离不要卡得太极限，预测误差带来的偏差完全可以通过滚动优化来修正，这就是MPC容错性的体现：每步都在更新，早期误差会在后续校正中被消化掉。
仿真步长的选取有几个经验值可供参考：无人船运动速度一般在1到3 m/s之间（本文仿真实验设定为约1.2 m/s），控制周期（即NMPC每次求解的间隔）取0.2秒，预测时域取10到20秒（对应预测步数Np = 10到20步）。这样整个预测窗口能够覆盖前方几米的距离，足够让控制器提前感知障碍物并调整航向。如果你把控制周期取到1秒，控制器反应速度会明显变慢，遇到急转弯或近距离障碍物时容易失真。
3. 避碰约束的建模：距离不等式、凸化处理与软约束
这一节是整个NMPC程序中最让我纠结的部分。跟踪参考轨迹本身是“在约束下找最优控制量”，但避碰约束一旦加入，优化问题的可行域会发生结构性变化——有些时候，控制器甚至会在“无法同时满足跟踪精度和避碰安全”的情况下做出危险的取舍。所以避碰约束怎么建模，直接影响最终仿真的安全性和程序稳定性。
3.1 障碍物表示方法与距离约束
在IEEE论文复现场景中，障碍物通常被简化为以某个坐标为中心的圆形区域，无人船被看作一个质点或者带半径的圆盘。这个假设在开阔水面场景下是合理的，而且方便计算。设障碍物中心坐标为(x_obs, y_obs)，安全半径（无人船半径 + 障碍物半径 + 额外安全距离）为d_safe，那么避碰约束可以写为：
(x(k) - x_obs)^2 + (y(k) - y_obs)^2 ≥ d_safe^2
这个约束的物理含义是：在预测时域内的每一个时刻，船与障碍物中心的距离都不能小于安全半径。它保证了船不会进入障碍物“势力范围”。
但问题在于，这个约束对优化求解器来说并不友好——它在几何上定义的是一个“禁区圆的外部”，也就是整个可行域是一个非凸区域。一个非凸约束扔给SQP算法，求解器的收敛性会受到很大影响，甚至可能在不同迭代步之间反复振荡，导致解出来的控制量一抖一抖的。
3.2 距离约束的非凸性与凸化处理
为了绕开非凸约束带来的求解困难，很多论文采用的方式是“避碰惩罚项”而不是“避碰硬约束”。具体做法是把距离约束转化为代价函数中的惩罚项——当船距离障碍物较远时，惩罚项几乎为零；当船靠近障碍物时，惩罚项迅速增大，迫使控制器选择远离障碍物的控制量。
我采用的惩罚项形式是：
J_obs = Σ w_obs * exp(-((x(k)-x_obs)^2 + (y(k)-y_obs)^2) / σ^2)
其中w_obs是避碰惩罚权重，σ控制惩罚的作用范围。这个高斯型的惩罚项非常实用——它在优化问题中是光滑的、可导的，不会给SQP算法带来非光滑性，同时又能比较自然地模拟“斥力场”的效果。船离障碍物越近，惩罚越大，控制器自然会被“推”开。
不过要注意，惩罚项与硬约束是两种不同的机制。惩罚项不能保证“绝对不碰撞”，只能保证“尽可能不碰撞”。如果w_obs取得足够大、σ取得足够合理，仿真结果可以做到船与障碍物保持安全距离，但从优化理论的角度讲，它不能像硬约束那样提供一个数学上绝对安全的边界。为了弥补这个短板，我在这套程序里实际采用了“惩罚项为主、硬约束兜底”的组合方案：代价函数中加避碰惩罚项，同时在约束函数里也保留距离不等式，但给不等式加了松弛变量。
3.3 软约束与松弛变量：给求解器留一条活路
硬约束在NMPC中带来的另一个经典问题是“无解”。考虑一个极端的场景：船已经非常靠近障碍物，按照当前速度继续前进，即使打满舵也无法在预测时域内把距离拉开到安全值以上。这时候，如果避碰距离不等式是硬约束，优化问题就会找不到可行解——fmincon直接报错退出，整个控制器就“死机”了。
解决这个问题的标准做法是引入松弛变量，把避碰约束写成软约束：
(x(k) - x_obs)^2 + (y(k) - y_obs)^2 + ε ≥ d_safe^2, ε ≥ 0
同时，在代价函数里增加松弛变量的二次惩罚项w_ε * ε^2。这样一来，当约束可以满足时，ε取0，约束等价于原来的硬约束；当约束无法满足时（比如异常接近障碍物），ε会被取成一个正数，让约束在数学上依然可行，代价是代价函数增大。控制器就会在“加大避碰效果”和“减少惩罚代价”之间做一个权衡，而不是直接崩溃。
松弛变量听起来像是一个“作弊”技巧，但它在工程上是极其实用的方法论。实际复现中，我建议把松弛变量的惩罚权重取到足够大（比如避碰权重的10到20倍），确保正常情况下ε真的会趋于零，避免“软约束妥协”过多导致实际避碰距离不足。
避碰约束的整体结构搞清楚了，后面还有一个隐藏难点：预测时域内的每一个时刻对应的船的位置，都依赖于状态转移矩阵，而状态转移矩阵又是控制序列的函数。所以避碰约束本质上是一系列嵌套了控制量的复杂非线性不等方程组。这就是为什么NMPC比线性MPC麻烦——每一步约束的雅可比矩阵算出来都很费劲，而且很难保证雅可比矩阵的数值条件足够好。这个问题我会在第五节的“求解器调试”部分详细展开。
4. 代价函数设计与求解器配置：Matlab实现细节
4.1 代价函数各项的物理意义
NMPC从算法框架上来讲不复杂：给定当前状态，找一个控制序列，让从当前时刻到未来Np步的“累计代价”最小，同时满足约束条件。代价函数的每一项都有明确的物理或工程意义：
- 轨迹跟踪误差项：衡量预测轨迹与参考轨迹的偏差，一般用二次型表示。比如对位置跟踪误差(x(k)-x_ref(k))^2 + (y(k)-y_ref(k))^2加权重，对艏向跟踪误差(ψ(k)-ψ_ref(k))^2加权重。这部分的权重矩阵Q越大，控制器越“较真”地跟踪参考轨迹。
- 控制量惩罚项：衡量控制能耗和幅度。推进力τ_u和转艏力矩τ_r（有些论文用舵角δ）本身是有限的物理量，但优化求解器不一定知道“你希望它尽可能小”。为了模拟真实的能耗效率和执行机构平滑性，代价函数里对控制量及其变化率做了二次惩罚。最经典的改进是惩罚控制增量Δu(k) = u(k) - u(k-1)，这样能够显著减少控制量的抖动。
- 避碰惩罚项：如上一节所述，高斯型的障碍物距离惩罚。
- 松弛变量惩罚项：软约束对应的代价。
综合起来，代价函数的典型形式如下：
J = Σ_{k=1}^{Np} [ (η(k)-η_ref(k))^T Q (η(k)-η_ref(k)) ]
- Σ_{k=0}^{Nc-1} [ u(k)^T R u(k) + Δu(k)^T S Δu(k) ]
- Σ_{k=1}^{Np} [ w_obs * exp(-d_k^2 / σ^2) ]
- w_ε * ε^2
权重矩阵的选取是有讲究的。如果用比例太高，控制器会为了提高跟踪精度而不惜大幅打舵，控制量变化率会很陡，对执行机构不友好——所以S矩阵的存在非常必要。实际调参时，我习惯从“控制量权重大于跟踪误差权重”开始，先压住控制量的抖动，再逐步提高Q来改善跟踪性能。一组合理的初值可以是：
- Q = diag([30, 30, 5])（对x、y、ψ的跟踪误差）
- R = diag([0.5, 0.5])（对推进力和转艏力矩的幅值惩罚）
- S = diag([5, 5])（对控制增量的惩罚）
- w_obs = 50, σ取6到10（具体取决于安全距离）
- w_ε = 500
注意，这组初值不一定适配所有场景，但拿来跑通第一遍仿真验证闭环逻辑是没问题的。
4.2 从代价函数到fmincon的接口
有了代价函数和约束，下一步就是写Matlab代码调用求解器。我使用的是最通用的fmincon，优化的决策变量为控制序列：将预测时域内所有控制步的控制量拼接成一个大向量。这里给出一个简化的代码接口思路：
% 决策变量定义：U = [tau_u(0), tau_r(0), tau_u(1), tau_r(1), ..., tau_u(Nc-1), tau_r(Nc-1)] % 当前系统状态x_current，之前一步的控制量u_prev % 代价函数句柄 cost_func = @(U) nmpc_cost(U, x_current, ref_trajectory, obstacles, params); % 约束函数句柄（同时返回非线性约束初值，线性约束可写空） constraint_func = @(U) nmpc_constraints(U, x_current, obstacles, params); % 求解 options = optimoptions('fmincon', 'Algorithm', 'sqp', ... 'MaxIterations', 100, 'MaxFunctionEvaluations', 20000, ... 'OptimalityTolerance', 1e-4, 'ConstraintTolerance', 1e-4, ... 'Display', 'off'); U_opt = fmincon(cost_func, U_init, [], [], [], [], ... lb, ub, constraint_func, options); % 只取第一个采样周期的控制量施加给被控对象 u_applied = U_opt(1:2);需要注意的是，fmincon优化问题的求解质量受初值影响很大。在实际仿真中，我把上一周期求出的最优控制序列“向前挪一步”作为本轮迭代的初值——这叫做热启动。热启动能够大幅减少迭代次数，而且能够提高收敛到全局最优的可能性。如果你不热启动，每个周期都从零开始猜一个初始控制序列，SQP很容易卡在局部最优或者直接因为初始解不可行而报错。
4.3 求解器参数与性能权衡
fmincon里有几个关键参数直接影响求解性能：
- 算法选择：Matrix里NMPC最常用的是sqp（序列二次规划）或interior-point（内点法）。我在这个场景下倾向于sqp，它在非线性约束下表现更稳定，而且对热启动初值利用得更好。内点法在多约束场景下有时候更慢，但收敛性质理论上更好。你要是碰到SQP反复失败，可以换成interior-point救急。
- MaxIterations和MaxFunctionEvaluations：这两个参数决定了最坏情况下的耗时上限。预测时域取15步、决策变量维度是2 * 15 = 30时，MaxIterations取100、MaxFunctionEvaluations取20000基本够用。如果你发现求解时间过长，检查一下有没有卡在100步迭代还没收敛的情况。
- OptimalityTolerance和ConstraintTolerance：收敛精度，一般取1e-4到1e-6之间。注意这两个容差不是越小越好，太小的容差会导致fmincon死磕微小梯度，单周期求解时间成倍增加。在控制器仿真场景，1e-4的精度完全够用。
- 上下界：控制量幅值限幅，这个必须写。无人船的推进力和转艏力矩不可能无限大，一般根据船型参数设一个合理的范围，比如推进力上限为5N、转艏力矩上限为2N·m（基于一个小型无人船模型的典型值）。
求解器的单周期耗时直接决定了整个仿真能否实时跑起来。我实测过，预测步数Np = 15时，单周期求解时间大约在0.2到0.5秒之间（CPU是普通的i5处理器），满足控制周期0.2秒的要求有一段距离，但作为离线仿真复现已经够用。如果你后续要跑半实物仿真或者实时控制，建议换用C++或CasADi这类更底层的优化求解方案。
5. 仿真复现与参数调优：把论文里没写的坑踩平
5.1 仿真场景设置与参考轨迹生成
整个仿真环境搭建好之后，第一件事不是直接跑完整轨迹，而是先生成一个足够有挑战性的参考轨迹。我复现时用的是典型的正弦参考轨迹加调头组合：
% 参考轨迹：x方向匀速，y方向正弦，中间插入一段调头 t_ref = 0:Ts:T_total; x_ref = 0.6 * t_ref; y_ref = 10 * sin(0.1 * t_ref) + 5; psi_ref = atan2(cos(0.1*t_ref) * 10 * 0.1, 0.6); % 参考艏向由轨迹方向给出参考轨迹生成后，需要预先把整条轨迹按时间索引存放。NMPC在每个控制周期要查询“未来Np步的参考轨迹点”，所以不能只给当前时刻的参考点，而要维护一个轨迹序列索引。这是一个容易忽略的细节——如果你只把当前时刻的参考点传给控制器，控制器就会“迷茫”，不知道未来要往哪走，跟踪效果会大打折扣。
障碍物的放置也需要讲究。我放了三类障碍物：一类在参考轨迹的直线段附近偏一侧，考验避碰时的跟踪偏差恢复能力；一类在参考轨迹的转弯处内侧，考验突然出现的近距离避障；还有一类放在远处的开阔水域，用来检验控制器是否会误触发避碰惩罚（正常情况下距离远，惩罚应为零，不会影响跟踪）。
5.2 参数整定的顺序与经验值
参数整定的顺序非常重要。我最开始上来就同时调Q、R、S和避碰权重，结果一锅粥，完全看不出哪个参数主导了哪个现象。后来按下面的顺序调：
- 先把避碰功能关掉（把w_obs设成0，或者让障碍物坐标设置在很远的位置），只调跟踪性能。调Q和R，让船在无障碍情况下能平滑地跟踪参考轨迹。这时候重点观察偏差是否收敛、控制量是否平滑。
- 再加上避碰惩罚项和软约束，把避碰权重从零开始逐渐增大，观察船接近障碍物时是否主动绕行，以及绕行后是否能重新回到参考轨迹上。
- 最后调节松弛变量的权重和预测时域，观察极端场景下的鲁棒性。
调参过程中有一个经验值得特别记录：避碰权重并不是越大越好。w_obs取太大时，船会离障碍物“远远地”就绕行，绕行轨迹幅度巨大，跟踪误差也跟着放大；w_obs取太小时，船会贴着障碍物边缘擦过去，虽然没碰撞但安全余量太小。需要找到一个“刚好在安全距离之外绕行，且绕行幅度不过分大”的折中点。
5.3 典型失败现象与排查方法
复现过程中，我遇到过三个典型的失败现象，这里如实记录：
第一个是“控制器输出剧烈振荡”。现象是船在靠近障碍物时，控制量在正负最大值之间来回跳，船体抖动非常明显。排查后发现，原因是控制增量惩罚系数S取得太小，导致控制器“肆无忌惮”地利用大幅控制变化来迎合代价函数。把S提高到5以后，振荡明显缓解。另一个可能原因是预测步数太少（Np只有5），控制器对未来看得太短，无法形成平滑的控制趋势。
第二个是“避碰约束整天报无解”。fmincon经常报“No feasible solution found at initial point”。这个坑的原因很典型：我设置的初始控制序列是零，而船刚好在障碍物附近，零控制序列预测出来的轨迹会直接闯进障碍物区域，导致初始解不满足避碰约束。解决办法是给初始控制序列一个合理的猜测——用当前控制量的值填充整个预测时域，并且在求解前先调用一次约束函数检查初始可行度，有条件的可以把避碰距离放宽一点生成初始可行解，然后再用正式的约束去求解。
第三个是“船绕过障碍物后回不到参考轨迹”。现象是避碰很成功，船绕行得很漂亮，但绕过去之后无法收敛回参考轨迹，而是沿着一条平行的偏离轨迹一直走下去。这个问题的根源在于参考轨迹是“随时间变化的曲线”，而不是“固定终点”。船绕行期间，跟踪误差已经累积，绕行结束后控制器试图跟踪的参考点已经跑到了船的前面（或侧面），如果不给跟踪误差项足够大的权重，船就一直追不上。解决方法是适当调大Q中位置误差的权重，并且稍微缩短预测时域（让控制器更“短视”地关注近期误差，而不是长远规划），可以在绕行结束后更快地“拉回来”。
5.4 扩展方向：动态障碍物与环境扰动
写到这里，基础的静态障碍物避碰NMPC框架已经完整了。如果要做动态障碍物避碰，其实只需要把障碍物坐标从常数改成“随预测时域变化的序列”即可，也就是说，在代价函数和约束中，把(x_obs, y_obs)换成(x_obs(k), y_obs(k))。最常见的方式是假定动态障碍物保持恒定速度航行，用简单的线性外推预测未来Np步的位置：
obs_x_predict = obs_x + obs_vx * (k * Ts); obs_y_predict = obs_y + obs_vy * (k * Ts);这种做法的效果在MATLAB仿真中非常直观——无人船能在障碍物还没到眼前时就提前转向，形成一段平滑的“避让弧线”。另外，如果你不想局限于圆形障碍物，也可以把障碍物建模为椭圆形或者多边形，只是距离计算和非线性约束的复杂度会进一步上升，求解时间也会变长，需要谨慎评估实时性。
再进一步，如果要在程序中加入海流或者风浪扰动，可以在动力学方程里增加一个扰动项，或在仿真模型中增加干扰矩阵。这部分会影响NMPC预测模型与实际被控对象的一致性——如果扰动项只是加在仿真里，而没有体现在预测模型里，控制器会通过反馈校正来逐步修正偏差，整体鲁棒性依然可控；如果扰动项同时加进预测模型，就需要在每个控制周期更新扰动的估计值，这又牵扯到状态估计的问题了。
6. 复现IEEE论文时容易遗漏的细节：从公式到程序的“翻译”难点
6.1 论文里不会明说的离散化与初始化假设
IEEE论文由于篇幅限制，数学推导通常直接从连续时间模型跳到控制器设计，很少会把“仿真中用什么步长”“初始状态怎么取”“预测模型的离散化方式是什么”这些工程细节写全。所以复现的第一步其实是“锁定”这些隐含假设。
我踩过的一个具体坑是：论文中写的动力学模型里，M矩阵和D矩阵的单位可能是“标准国际单位”，但仿真中如果船体尺度很小（比如只长1米的小型USV），M矩阵的数量级只有个位数甚至不到1。如果照搬论文里的控制量限幅值（比如推进力上限80N），控制器会在绝对值意义上根本不可能达到这个值——因为船体尺寸对应的推进力合理范围可能只有5N。所以复现时务必要根据模型参数自行核算控制量限幅，不要照抄论文里的数值。
另外，论文中的状态反馈通常默认“全部状态可测”。仿真中这没问题，直接把真实状态喂给控制器就行。但如果你后续要往实验平台迁移，就要增加一个状态估计环节（比如卡尔曼滤波去估计位置、速度和艏向角），这是另一个议题了。
6.2 代价函数中隐藏的约束处理技巧
很多IEEE论文的公式中会写“s.t. 避碰约束”，但实际仿真代码往往不会真的一板一眼去求解这个约束，而是像我前面那样用惩罚项加软约束的组合来代替。为什么要这样？除了非凸约束的求解困难，还有一个原因是：硬约束会让SQP算法在每次迭代时都需要进行昂贵的“可行性恢复”，这在滚动优化的高实时性要求下并不划算。
所以复现时，如果你发现某些论文的NMPC“看起来特别丝滑”，大概率人家在代码里动了巧——用复杂的惩罚项结构替代了公开公式中的硬约束。这不是作弊，而是工程上非常合理的取舍。你在自己的博文或者项目报告里，也完全可以这样写：约束形式的理论分析是硬约束，但工程实现采用惩罚与软约束的组合，既保证接近论文的理论框架，又保证程序的数值稳定性。
6.3 结果可视化：复现成功与否的判定标准
复现完成后，判定“成功”不能只靠肉眼看轨迹图“好像差不多”。我建议至少画出这三张图：
- 实际轨迹与参考轨迹的对比图：直观看出跟踪偏差的分布区间。
- 跟踪误差随时间的变化曲线：定量分析最大误差、稳态误差，能直接对比论文中的实验结果。
- 避碰过程的最小距离曲线：计算整个仿真过程中船与障碍物的最近距离，验证是否始终大于设定的d_safe。
如果论文中有具体的误差性能指标（比如均方根误差、最大绝对误差），可以直接按同样的指标计算对比。我当时复现出来的均方根位置误差在0.3米以内，最大误差不超过0.6米，基本符合“小型USV在低速场景下NMPC跟踪效果良好”的结论。
7. 我自己的几点体会：关于NMPC的“能用”与“好用”
整套程序从零到跑通，前后花了一周多的时间。回头再看，NMPC + USV轨迹跟踪 + 避碰这套组合，在仿真环境下确实有着其他控制方法难以匹敌的灵活性和性能上限——尤其在多约束同时作用的情况下，NMPC的预测能力几乎像是“开了上帝视角”，它能在约束即将被违反之前就开始调整控制量，而不是等违规发生了再慌慌张张地补救。
不过也要泼一盆冷水：这套程序在Matlab环境下的实时性是有限的。仿真复现没问题，做算法对比也够用，但一旦要上真船，fmincon的求解速度很可能成为瓶颈。到时候你可以考虑几种优化路径：
- 用CasADi或ACADO生成更高效的C++求解器，把单周期求解时间从几百毫秒压缩到几十毫秒甚至几毫秒；
- 把预测时域缩短到5到8步，并降低控制步数Nc（比如Nc = 2到3），大幅度减少决策变量个数；
- 对动力学模型做进一步简化，比如把横荡速度v的动力学退化为稳态表达式，只保留纵荡和艏摇两个主自由度。
最后再分享一个很实用的调试技巧：在仿真循环里加一个定时器，实时打印每个控制周期的求解时间。当你调整预测时域或权重时，用这个时间数据来判断计算负荷是否在可接受范围内，比凭感觉优化要高效得多。另外，fmincon偶发不收敛的情况不用慌——可以在调用处加一个try-catch，把上一次的解作为回退方案继续推进仿真，先把仿真跑通调好参数，再回头解决求解器的收敛性问题。这种“先跑起来，再优化”的节奏，在整个NMPC复现过程中帮我省了大量时间。
