如果你正在复现一篇无人船轨迹跟踪方向的论文大概率会遇到和我类似的情况参考轨迹画得干干净净控制器却把船带进障碍物里或者NMPC求解器哼哧哼哧算半天结果第一步就数值发散再或者明明把论文里的公式抄下来了代价函数也写了代码一跑就是不符合预期明明有避碰项船还是硬生生穿过了障碍物。这篇博文就把这套基于NMPC的无人船USV轨迹跟踪与避碰Matlab程序彻底拆开讲透——模型怎么建、代价函数怎么配权重、避碰约束怎么写才不会把优化问题搞死、求解器选fmincon还是CasADi、以及我从发散到收敛的完整调试路线。适合正在做无人船方向的研究生或者想把非线性模型预测控制用于轨迹规划和避障的工程师。先说一句总结性的判断NMPC在无人船上的应用价值不是因为它比PID“高级”而是因为它把轨迹跟踪和避碰统一到了一个带约束的优化框架里这是传统控制方法很难做到的事。下面从建模、控制器设计、避碰实现、代码架构到调参经验一层层拆开讲。1. 项目概览与设计思路1.1 为什么轨迹跟踪必须考虑约束无人船USV的轨迹跟踪任务表面上看是“船沿着参考路径走”但实际工程里难点在三个地方第一船是典型的欠驱动系统——没有侧向推进器只能控制纵向推力和艏摇力矩横向运动是被动的耦合运动第二船体模型有非线性项包括阻尼、科氏力、舵力饱和等第三任务里同时存在跟踪精度、控制量平滑度、避碰安全距离等多个目标这些目标之间本身就是相互冲突的。传统方案里LOS导引法加PID是经典组合但LOS有一个致命问题它本质上只做了视线角跟踪避障必须单独设计一个“导引重规划层”两层之间一旦切换逻辑不严密船在障碍物附近会出现剧烈转向或者反复横摆。而NMPC的优势在于它直接在当前时刻预测未来一段时域内船的运动在预测里同时考虑“跟踪参考轨迹”和“避开障碍物”两个目标滚动求解、滚动施加。也就是说障碍物信息不是事后介入的而是从一开始就参与控制的实时决策。我在复现时采用的思路是按照论文里的整体框架用三自由度船体模型做被控对象用参考轨迹生成器提供直线/曲线/正弦等路径NMPC控制器在每个采样周期内求解一个有限时域最优控制问题再把第一个控制量施加给被控对象到下一拍重新滚动。这个框架非常经典几乎是所有NMPC类USV控制论文的公共底盘。1.2 系统层级与仿真闭环整个仿真程序的闭环流程大概是这样的先初始化船位、船速、障碍物位置和参考轨迹然后进入主循环每个循环内采集当前状态量把参考轨迹在预测时域内的目标状态截取出来连同障碍物信息一起送入NMPC求解器求解器输出未来Np步的控制序列取第一项施加到船上用动力学模型积分更新状态最后记录数据并在结束时绘制轨迹、跟踪误差、控制量等曲线。这个结构里有一个容易被忽略的点参考轨迹在每一步需要“向前看”一段。如果参考轨迹是一条全局路径那么控制器在每个时刻要取出预测时域内对应的子序列也就是从k1到kNp的参考状态。这一步做错了会出现一种非常典型的现象船明明跟着轨迹走得好好的但在转弯的时候总是“抄近路”或者“冲过头”因为参考信号一直取的是当前时刻最近点控制器压根看不到未来的弯道。大家拿到代码后第一件事建议先把参考轨迹截取逻辑看明白。1.3 与原论文方法的关系这篇程序复现的核心方法与近年来无人船NMPC方向的主流做法是一致的采用非线性三自由度船体模型代价函数包含轨迹跟踪误差、控制量惩罚、控制增量惩罚并把避碰作为软约束或硬约束加入优化问题。跟某些简化方法比原方法保留了模型的非线性动力学特征没有把模型线性化后再用线性MPC这是NMPC的优势所在也是复现时最容易出问题的地方——非线性优化每一步都要调用求解器实时性比线性MPC差很多但换来的控制效果和约束满足能力是值得的。2. 被控对象建模与参考轨迹设计2.1 三自由度运动学与动力学模型无人船建模最常用的是三自由度模型包括纵向速度u、横向速度v、艏摇角速度r位置由大地坐标系下的x、y和艏摇角ψ描述。运动学部分写成$\dot{x} u\cos\psi - v\sin\psi$$\dot{y} u\sin\psi v\cos\psi$$\dot{\psi} r$这个式子本身不难但它是整个仿真的基础因为控制力最终输出的是船体坐标系下的推力和力矩而轨迹误差必须在地球坐标系下计算两者之间的坐标转换全靠运动学。动力学部分我用的是典型的刚体模型加阻尼项形式如下$m_{11}\dot{u} \tau_u - d_{11}u - c_{13}vr$$m_{22}\dot{v} - d_{22}v - c_{23}ur$$m_{33}\dot{r} \tau_r - d_{33}r$注意横向方程里没有直接控制力这体现了前面说的欠驱动特性。在实际复现中我用的模型参数来自原论文的仿真部分船长约1米质量约20公斤惯性矩约4kg·m²阻尼系数根据水动力经验公式估算得到。这里有一个重要提醒模型参数直接影响NMPC的预测精度如果你自己有一条实船需要通过参数辨识获取实际的水动力系数不能随便往代码里套数字。仿真的目的只是验证算法可行性参数差别不是致命的但如果想移植到实船这一步必须重做。2.2 动力学模型离散化与仿真步长NMPC的预测模型和仿真模型实际上可以不是同一个离散化方式。仿真模型通常用四阶龙格库塔积分步长选择0.01到0.02秒精度高而NMPC内部的预测模型为了兼顾求解速度可以用欧拉法离散预测步长用0.05到0.2秒。两者的一致性分析是个关键话题预测模型越精确控制器对系统行为的预判就越准但求解越慢预测模型过于粗糙控制器看到的“未来”和实际发生的“未来”对不上就会出现模型失配导致控制性能退化。在我的程序里仿真步长0.01秒NMPC预测步长0.1秒预测时域20步即总共向前看2秒。这个配置是我跑了很多组参数之后的一个权衡方案Np过大求解太慢实时性不够Np过小控制器看不到足够远的未来遇到障碍物时来不及规划绕行路径。实测下来2秒的预测时域配合1m/s左右的船速给船预留了约2米的决策空间对于小型USV的避碰需求足够了。2.3 参考轨迹的类型与生成参考轨迹我提供了三种方便验证控制器的不同能力。直线轨迹最简单用于验证基础的跟踪性能圆形轨迹检验稳态跟踪精度S形曲线轨迹则能很好地检验控制器在连续大角度机动时的响应能力。轨迹生成的核心是给出一组带时间参数的参考状态包括参考位置和参考航向角。位置直接由曲线方程给出速度可以取恒定值航向角可以由位置路径的切线方向计算得到。有一个细节值得注意参考轨迹的光滑度直接影响控制器跟踪效果。如果参考轨迹本身有突变比如折线拐角处航向角突变NMPC的预测时域有限它无法做到“瞬间转过弯”就会表现为跟踪滞后甚至出现大超调。所以在生成折线轨迹时我特意用圆弧过渡或者在轨迹方程里加入平滑滤波让航向角连续变化。这个操作在论文里往往一句带过但实际复现时它决定了对比实验的第一张图好不好看。3. NMPC控制器核心设计与代价函数调参3.1 滚动时域优化的三个核心要素NMPC的本质是在每个控制周期内求解一个以当前状态为初值、以模型递推为约束的有限时域最优控制问题。它的核心要素有三个预测模型、代价函数、时域长度。预测模型解决“系统未来怎么演化”的问题代价函数解决“什么样的控制行为算好”的问题时域长度解决“控制器能看多远、能提前多少步规划”的问题。三者缺一不可。把这个思路用公式写出来每个周期的优化问题是$\min_{u_0,\dots,u_{N_p-1}} ; J \sum_{k0}^{N_p-1} \left[ e_k^\top Q e_k \Delta u_k^\top R \Delta u_k P_{obs,k} \right]$约束条件包括$x_{k1} f(x_k, u_k)$$u_{\min} \le u_k \le u_{\max}$$d_{safe} - |p_k - p_{obs}| \le s_k$最后一条里的s_k是松弛变量后面避碰部分会详细讲。这里先记住代价函数里每一项都对应一个控制目标权重矩阵Q和R决定了这些目标之间的优先级。3.2 误差项与控制项的权重博弈代价函数里的误差项Q、控制量惩罚R是我调试时花时间最多的地方。第一次复现时我直接把论文里写的权重抄过来结果控制器要么太“软”船慢吞吞地靠向参考轨迹完全没有机动性要么太“猛”控制量频繁饱和船体在轨迹附近抖动。后来我才意识到论文里的权重是为那个特定的船体参数和速度范围设计的换了自己的模型参数必须重新标定。直观地说位置误差权重大船会“拼命”靠向参考轨迹但代价是控制动作猛烈、容易激发船体高频运动控制量权重R大控制器倾向于“少出力、稳着来”但跟踪响应会变慢。我最终采用的权重是位置误差权重40航向角权重10控制量权重2。这个配置下船的机动性和平滑性相对平衡。调权重的建议是先把Q里的位置权重设为1其他权重都先暂时设为0跑一个最简单直线轨迹观察船能否基本贴合轨迹再把航向角权重逐步加上去直到转向不出现明显滞后最后再加R一点点增大直到控制曲线平滑。这个过程顺序很重要跳着调往往会浪费大量时间在一堆参数之间反复横跳。3.3 非线性约束与求解器选型NMPC的“非线性”体现在两个层面一是预测模型是非线性的二是代价函数里的避碰项是非线性的。这决定了不能直接用线性二次调节器那套解析解法而必须依赖数值优化算法。Matlab里最常见的两个选择是fmincon和CasADi。fmincon是Matlab自带的非线性优化求解器不需要额外安装环境用起来最方便CasADi是专门做最优控制的工具箱支持自动微分配合IPOPT求解器性能上比fmincon好不少但需要额外安装配置。我最初的版本用fmincon跑通全流程求解耗时大约每步80毫秒对于仿真验证完全够用后面为了展示更接近实时的效果换成了CasADi每步降到20毫秒左右。说实话如果你只是做算法验证和论文复现fmincon完全够用没必要一上来就折腾CasADi的环境配置。但如果你后续打算做硬件在环、实时控制那CasADi几乎是必须的选择。这里记录一个我用fmincon时踩过的坑fmincon的默认终止条件和步长设置经常导致NMPC在状态更新后计算出“原地踏步”的控制量表现为船不动或者控制力异常小。解决办法是在fmincon的options里把OptimalityTolerance调大到1e-3把MaxFunctionEvaluations调大同时把上一步的最优解作为本次求解的初始值也就是热启动。热启动不仅能加速收敛还能避免优化器在每步都从头搜索引起的控制量突变。4. 障碍物避碰的实现路径4.1 静态障碍物建模与惩罚函数避碰的基础是障碍物的数学描述。最简单的做法是把障碍物视为一个圆形区域圆心坐标和半径代表障碍物位置和影响范围。在NMPC里避碰可以通过硬约束或软约束两种方式实现。硬约束的写法是在整个预测时域内每个预测点的位置必须位于障碍物圆半径之外。这个约束是可行但有风险的如果船当前已经进入障碍物安全半径内或者前方障碍物分布复杂导致可行域非凸硬约束会让整个优化问题直接不可行求解器报错退出。我在调试时遇到过多次“当前位置就在障碍物边缘导致求解失败”的情况非常头疼。所以我在最终代码里用的是软约束加惩罚函数也就是在代价函数里加入避碰惩罚项$P_{obs}$。一种常见的写法是$P_{obs,k} \rho \cdot \sum_{j} \max\left(0, ; d_{safe} - |p_k - p_{obs,j}|\right)^2$这个惩罚函数是一个外点罚函数只要预测点到障碍物中心的距离小于安全半径就产生一个正惩罚值距离越近惩罚越大反之如果距离足够远惩罚为零。它的好处是即使初始状态就在障碍物附近优化问题也不会立刻不可行控制器会“感觉到”危险并尝试把船推离障碍物而不是直接拒绝给出解。4.2 动态障碍物的预测处理真实场景里障碍物往往是移动的。移动障碍物的处理方式比静态障碍物多一个维度控制器不仅要考虑船本身未来的位置还要考虑障碍物未来时刻的位置。我的做法是假设障碍物在短时间内做匀速直线运动根据障碍物的速度和方向在预测时域的每个时间步上估算出它未来时刻的位置。这样惩罚函数在每个预测时刻都利用该时刻障碍物的预测位置来计算距离。这个近似在船载传感器更新率较高的情况下是足够准确的因为预测时域通常只有两三秒障碍物位置变化有限。如果障碍物运动轨迹本身是未知的那问题就变成不确定性下的避碰问题通常需要引入障碍物轨迹预测器或者鲁棒MPC这里先不展开讲。对于复现IEEE论文来说匀速运动假设已经是一个相当标准的设定了。4.3 避碰项与跟踪项的权重平衡避碰项权重ρ的选择是另一个大坑。ρ设得太大船会在离障碍物还很远的地方就跑开轨迹绕得非常大跟踪性能大打折扣ρ设得太小船会“无视”障碍物直接穿过去避碰形同虚设。我实测下来避碰罚的权重通常要比跟踪权重高一个数量级才能保证基本不撞障碍物但又不能无限大——当ρ太大时船一旦进入罚函数显著的区域内控制器会为了逃离罚函数区而输出猛烈的转向指令导致船体剧烈摇摆。一个稳妥的策略是使用距离相关权重当船距离障碍物较远时避碰项权重较小主要精力放在跟踪上当船接近障碍物安全半径时避碰项权重自动增大控制优先级切换到安全通道。这种加权方式在工程上看着比固定权重自然而且能有效缓解“绕大弯”的问题。我的最终程序里固定权重取ρ600但代码里保留了分段加权的接口方便做改进实验。5. Matlab代码架构与核心函数实现5.1 程序文件组织与数据流整套程序我按功能模块划分共六个主要文件主脚本负责仿真流程控制模型文件负责运动学和动力学更新参考轨迹生成器负责曲线数据和参考状态计算代价函数文件负责计算NMPC目标值约束函数文件负责处理控制量和安全约束绘图脚本负责结果可视化。代码不复杂但每一个文件都只有一个明确职责这样后续改参数、加障碍物、换轨迹都非常方便。在我的代码里主循环的核心结构大致如下% 主仿真循环 for k 1 : N_sim % 获取当前状态 x_now x_history(:, k); % 截取预测时域内的参考轨迹 ref_seq ref_gen(x_now, k, Np, dt_ref); % 调用NMPC求解器 u_opt nmpc_solve(x_now, ref_seq, obstacles, param); % 施加第一个控制量并积分 u_now u_opt(:, 1); x_next usv_dynamics_rk4(x_now, u_now, dt_sim); % 数据记录 x_history(:, k1) x_next; u_history(:, k) u_now; end数据流的逻辑很简单状态从历史数组流出经过参考轨迹截取和求解器变成控制量控制量积分出下一时刻状态再写回历史。只要不在这条流水线上插入额外逻辑整个仿真就是确定性可复现的。5.2 关键代码片段讲解第一个关键函数是参考轨迹截取。这里最容易出错的是索引偏移因为我用的预测步长与仿真步长不一定相等所以参考轨迹需要按时间对齐。我的做法是仿真运行到第k步时实际时间为k乘以仿真步长NMPC预测的第j步对应时间偏移为j乘以预测步长所以参考轨迹索引需要做时间转换而不能简单写成ref_seq ref_traj(k1:kNp)。第二个关键函数是代价函数。代价函数要输入预测状态序列和控制序列输出标量代价值。我按照前面提到的公式逐项求和其中避碰项需要循环遍历每一个障碍物和每一个预测步。为了提高效率我提前把所有障碍物的位置和半径整理成结构化数组在代价函数中直接访问。第三个关键函数是约束函数。控制量约束被写成线性不等式控制增量约束需要利用上一步的控制值做差分。这里有个小细节控制增量的计算需要把上一步的实际控制量传入约束函数如果遗漏很容易导致控制量虽然限制在上下限之内但变化率过大的问题。5.3 仿真参数配置与可视化仿真参数我用一个struct集中管理包括仿真总时间、采样周期、预测时域、控制时域、权重矩阵、安全半径、船体参数等。这样修改参数时不需要在多个脚本之间来回找写论文实验记录也方便。可视化部分我画四张图第一张是全局轨迹图同时绘制参考轨迹、实际轨迹和障碍物区域第二张是位置跟踪误差曲线第三张是控制量推力和艏摇力矩变化曲线第四张是船与障碍物的最小距离随时间的变化曲线。这四张图基本覆盖了论文里需要的仿真验证内容。其中最小距离曲线是审稿人很爱看的图它能直观证明“控制器确实没有穿越障碍物”。我在画这张图的时候专门计算了每个时刻船体到每个障碍物的几何距离取最小值画成曲线同时把安全半径画成一条水平虚线两条线之间的距离一眼就能看出安全裕度。6. 调参流程与常见问题实录6.1 从发散到收敛的调试路线我拿到一套新NMPC程序时的习惯是第一步先把避碰相关的项全部关掉只保留轨迹跟踪把跟踪效果调好第二步加入静态障碍物调整避碰参数第三步再加入动态障碍物最后才考虑加扰动、模型失配等鲁棒性问题。这个分阶段推进的好处是每一步的参数问题都能被独立识别不至于出现“轨迹和避碰混在一起拐不清是谁的问题”。在第一步里如果轨迹跟踪都发散优先检查三个地方初始状态是否合理、权重的量级是否夸张、离散化是否稳定。我遇到过一次发散是因为把位置误差权重设置成1000控制器在第一步就输出了超出物理极限的控制量船直接冲出了仿真边界。调整思路是把所有量纲归一化位置误差以米为单位航向误差以弧度为单位控制量以牛顿和牛米为单位权重设置要大致平衡这些量纲的量级差异。在第二步里如果船会撞障碍物大概率是避碰权重太小或者安全半径设得过小。如果船在障碍物远处就大幅度绕行那说明避碰权重偏大或者障碍物影响范围太大。这个阶段要有点耐心绕行轨迹的形状是权重比例的直观体现。6.2 常见问题速查表我把调试过程中遇到的典型问题整理成了表格遇到类似情况可以直接对照排查。现象可能原因解决对策仿真初期就发散初始状态不合理或权重量级过大检查初始船位船速降低Q权重提高R权重船在参考轨迹附近抖动控制量惩罚R过小或预测时域过短增大R增大Np检查控制增量约束船转弯时大幅超调参考轨迹不光滑或航向权重低平滑参考轨迹增加航向角误差权重船直接穿过障碍物避碰权重小或安全半径小增大ρ增大安全距离检查松弛变量船在障碍物附近反复摆动避碰权重过大降低ρ或改用距离相关权重fmincon求解超时决策变量过多或初值不当缩短预测时域启用热启动控制量每次跳变剧烈缺少控制增量约束在约束中加入Δu限制船长时间偏离轨迹但不远离参考轨迹截取索引错误检查时间对齐逻辑6.3 实时性优化与工程扩展如果仿真里的每一步求解耗时太长可以考虑的方向包括缩短预测时域、减少控制时域、使用CasADi自动微分替代手写雅可比、简化模型例如忽略部分非线性阻尼项、用并行计算加速多个障碍物的距离计算。需要说明的是这些优化方向往往是在控制性能和求解速度之间做取舍没有免费的午餐。我实测过把预测时域从30步缩到15步求解时间大约减少一半但避碰路径会显得更急躁因为控制器“看”得更近了。往船端移植时还需要考虑几个工程问题。第一控制量输出往往要经过推力分配环节把NMPC解出的纵向推力和艏摇力矩转换为实际执行器的转速指令第二船体状态反馈通常来自传感器融合存在噪声和延迟NMPC的性能会受到影响需要加入状态估计器第三真实环境的障碍物信息来自雷达或视觉带有不确定性和量测噪声这时的避碰问题就升级为概率约束下的鲁棒NMPC属于更前沿的研究方向了。6.4 参数整定的一点个人经验最后分享一个我自己的调参习惯。NMPC的权重矩阵Q和R我不会一次性精确到某组数值而是先用数量级粗调再用细调。粗调阶段把Q的位置权重从1开始每次乘10递增跑一次仿真看效果R同理。找到“大方向没问题”的组合后再在这个组合附近做小范围微调比如把位置权重从40调到50观察跟踪误差均方根值的变化是否明显。如果变化不大就说明当前参数组合已经处于一个性能平坦区不需要再纠结更细的数值。避碰参数ρ的调整也有类似规律。我通常会先用一个比较大的ρ值让船无论如何都不敢撞障碍物然后再逐渐减小直到船在安全的前提下路径尽量贴近理想轨迹。这个过程本质上是在安全性边界上反复试探找到“刚好不出事”的临界值再把权重稍微取大一点作为安全冗余。整套程序跑通之后你会发现在无人船这样一个涉及建模、优化、约束处理的综合问题里NMPC的潜力远远不止于“能跟着轨迹走”。它把复杂的多目标控制问题变成了一个可调整、可分析、可复现的数值优化问题这也是它能成为近年来无人船控制研究主线的根本原因。如果你正在复现类似的算法希望这份从建模到调参的记录能帮你少走几条弯路。 基于NMPC的无人船轨迹跟踪与避碰Matlab实现与调参全解析
如果你正在复现一篇无人船轨迹跟踪方向的论文大概率会遇到和我类似的情况参考轨迹画得干干净净控制器却把船带进障碍物里或者NMPC求解器哼哧哼哧算半天结果第一步就数值发散再或者明明把论文里的公式抄下来了代价函数也写了代码一跑就是不符合预期明明有避碰项船还是硬生生穿过了障碍物。这篇博文就把这套基于NMPC的无人船USV轨迹跟踪与避碰Matlab程序彻底拆开讲透——模型怎么建、代价函数怎么配权重、避碰约束怎么写才不会把优化问题搞死、求解器选fmincon还是CasADi、以及我从发散到收敛的完整调试路线。适合正在做无人船方向的研究生或者想把非线性模型预测控制用于轨迹规划和避障的工程师。先说一句总结性的判断NMPC在无人船上的应用价值不是因为它比PID“高级”而是因为它把轨迹跟踪和避碰统一到了一个带约束的优化框架里这是传统控制方法很难做到的事。下面从建模、控制器设计、避碰实现、代码架构到调参经验一层层拆开讲。1. 项目概览与设计思路1.1 为什么轨迹跟踪必须考虑约束无人船USV的轨迹跟踪任务表面上看是“船沿着参考路径走”但实际工程里难点在三个地方第一船是典型的欠驱动系统——没有侧向推进器只能控制纵向推力和艏摇力矩横向运动是被动的耦合运动第二船体模型有非线性项包括阻尼、科氏力、舵力饱和等第三任务里同时存在跟踪精度、控制量平滑度、避碰安全距离等多个目标这些目标之间本身就是相互冲突的。传统方案里LOS导引法加PID是经典组合但LOS有一个致命问题它本质上只做了视线角跟踪避障必须单独设计一个“导引重规划层”两层之间一旦切换逻辑不严密船在障碍物附近会出现剧烈转向或者反复横摆。而NMPC的优势在于它直接在当前时刻预测未来一段时域内船的运动在预测里同时考虑“跟踪参考轨迹”和“避开障碍物”两个目标滚动求解、滚动施加。也就是说障碍物信息不是事后介入的而是从一开始就参与控制的实时决策。我在复现时采用的思路是按照论文里的整体框架用三自由度船体模型做被控对象用参考轨迹生成器提供直线/曲线/正弦等路径NMPC控制器在每个采样周期内求解一个有限时域最优控制问题再把第一个控制量施加给被控对象到下一拍重新滚动。这个框架非常经典几乎是所有NMPC类USV控制论文的公共底盘。1.2 系统层级与仿真闭环整个仿真程序的闭环流程大概是这样的先初始化船位、船速、障碍物位置和参考轨迹然后进入主循环每个循环内采集当前状态量把参考轨迹在预测时域内的目标状态截取出来连同障碍物信息一起送入NMPC求解器求解器输出未来Np步的控制序列取第一项施加到船上用动力学模型积分更新状态最后记录数据并在结束时绘制轨迹、跟踪误差、控制量等曲线。这个结构里有一个容易被忽略的点参考轨迹在每一步需要“向前看”一段。如果参考轨迹是一条全局路径那么控制器在每个时刻要取出预测时域内对应的子序列也就是从k1到kNp的参考状态。这一步做错了会出现一种非常典型的现象船明明跟着轨迹走得好好的但在转弯的时候总是“抄近路”或者“冲过头”因为参考信号一直取的是当前时刻最近点控制器压根看不到未来的弯道。大家拿到代码后第一件事建议先把参考轨迹截取逻辑看明白。1.3 与原论文方法的关系这篇程序复现的核心方法与近年来无人船NMPC方向的主流做法是一致的采用非线性三自由度船体模型代价函数包含轨迹跟踪误差、控制量惩罚、控制增量惩罚并把避碰作为软约束或硬约束加入优化问题。跟某些简化方法比原方法保留了模型的非线性动力学特征没有把模型线性化后再用线性MPC这是NMPC的优势所在也是复现时最容易出问题的地方——非线性优化每一步都要调用求解器实时性比线性MPC差很多但换来的控制效果和约束满足能力是值得的。2. 被控对象建模与参考轨迹设计2.1 三自由度运动学与动力学模型无人船建模最常用的是三自由度模型包括纵向速度u、横向速度v、艏摇角速度r位置由大地坐标系下的x、y和艏摇角ψ描述。运动学部分写成$\dot{x} u\cos\psi - v\sin\psi$$\dot{y} u\sin\psi v\cos\psi$$\dot{\psi} r$这个式子本身不难但它是整个仿真的基础因为控制力最终输出的是船体坐标系下的推力和力矩而轨迹误差必须在地球坐标系下计算两者之间的坐标转换全靠运动学。动力学部分我用的是典型的刚体模型加阻尼项形式如下$m_{11}\dot{u} \tau_u - d_{11}u - c_{13}vr$$m_{22}\dot{v} - d_{22}v - c_{23}ur$$m_{33}\dot{r} \tau_r - d_{33}r$注意横向方程里没有直接控制力这体现了前面说的欠驱动特性。在实际复现中我用的模型参数来自原论文的仿真部分船长约1米质量约20公斤惯性矩约4kg·m²阻尼系数根据水动力经验公式估算得到。这里有一个重要提醒模型参数直接影响NMPC的预测精度如果你自己有一条实船需要通过参数辨识获取实际的水动力系数不能随便往代码里套数字。仿真的目的只是验证算法可行性参数差别不是致命的但如果想移植到实船这一步必须重做。2.2 动力学模型离散化与仿真步长NMPC的预测模型和仿真模型实际上可以不是同一个离散化方式。仿真模型通常用四阶龙格库塔积分步长选择0.01到0.02秒精度高而NMPC内部的预测模型为了兼顾求解速度可以用欧拉法离散预测步长用0.05到0.2秒。两者的一致性分析是个关键话题预测模型越精确控制器对系统行为的预判就越准但求解越慢预测模型过于粗糙控制器看到的“未来”和实际发生的“未来”对不上就会出现模型失配导致控制性能退化。在我的程序里仿真步长0.01秒NMPC预测步长0.1秒预测时域20步即总共向前看2秒。这个配置是我跑了很多组参数之后的一个权衡方案Np过大求解太慢实时性不够Np过小控制器看不到足够远的未来遇到障碍物时来不及规划绕行路径。实测下来2秒的预测时域配合1m/s左右的船速给船预留了约2米的决策空间对于小型USV的避碰需求足够了。2.3 参考轨迹的类型与生成参考轨迹我提供了三种方便验证控制器的不同能力。直线轨迹最简单用于验证基础的跟踪性能圆形轨迹检验稳态跟踪精度S形曲线轨迹则能很好地检验控制器在连续大角度机动时的响应能力。轨迹生成的核心是给出一组带时间参数的参考状态包括参考位置和参考航向角。位置直接由曲线方程给出速度可以取恒定值航向角可以由位置路径的切线方向计算得到。有一个细节值得注意参考轨迹的光滑度直接影响控制器跟踪效果。如果参考轨迹本身有突变比如折线拐角处航向角突变NMPC的预测时域有限它无法做到“瞬间转过弯”就会表现为跟踪滞后甚至出现大超调。所以在生成折线轨迹时我特意用圆弧过渡或者在轨迹方程里加入平滑滤波让航向角连续变化。这个操作在论文里往往一句带过但实际复现时它决定了对比实验的第一张图好不好看。3. NMPC控制器核心设计与代价函数调参3.1 滚动时域优化的三个核心要素NMPC的本质是在每个控制周期内求解一个以当前状态为初值、以模型递推为约束的有限时域最优控制问题。它的核心要素有三个预测模型、代价函数、时域长度。预测模型解决“系统未来怎么演化”的问题代价函数解决“什么样的控制行为算好”的问题时域长度解决“控制器能看多远、能提前多少步规划”的问题。三者缺一不可。把这个思路用公式写出来每个周期的优化问题是$\min_{u_0,\dots,u_{N_p-1}} ; J \sum_{k0}^{N_p-1} \left[ e_k^\top Q e_k \Delta u_k^\top R \Delta u_k P_{obs,k} \right]$约束条件包括$x_{k1} f(x_k, u_k)$$u_{\min} \le u_k \le u_{\max}$$d_{safe} - |p_k - p_{obs}| \le s_k$最后一条里的s_k是松弛变量后面避碰部分会详细讲。这里先记住代价函数里每一项都对应一个控制目标权重矩阵Q和R决定了这些目标之间的优先级。3.2 误差项与控制项的权重博弈代价函数里的误差项Q、控制量惩罚R是我调试时花时间最多的地方。第一次复现时我直接把论文里写的权重抄过来结果控制器要么太“软”船慢吞吞地靠向参考轨迹完全没有机动性要么太“猛”控制量频繁饱和船体在轨迹附近抖动。后来我才意识到论文里的权重是为那个特定的船体参数和速度范围设计的换了自己的模型参数必须重新标定。直观地说位置误差权重大船会“拼命”靠向参考轨迹但代价是控制动作猛烈、容易激发船体高频运动控制量权重R大控制器倾向于“少出力、稳着来”但跟踪响应会变慢。我最终采用的权重是位置误差权重40航向角权重10控制量权重2。这个配置下船的机动性和平滑性相对平衡。调权重的建议是先把Q里的位置权重设为1其他权重都先暂时设为0跑一个最简单直线轨迹观察船能否基本贴合轨迹再把航向角权重逐步加上去直到转向不出现明显滞后最后再加R一点点增大直到控制曲线平滑。这个过程顺序很重要跳着调往往会浪费大量时间在一堆参数之间反复横跳。3.3 非线性约束与求解器选型NMPC的“非线性”体现在两个层面一是预测模型是非线性的二是代价函数里的避碰项是非线性的。这决定了不能直接用线性二次调节器那套解析解法而必须依赖数值优化算法。Matlab里最常见的两个选择是fmincon和CasADi。fmincon是Matlab自带的非线性优化求解器不需要额外安装环境用起来最方便CasADi是专门做最优控制的工具箱支持自动微分配合IPOPT求解器性能上比fmincon好不少但需要额外安装配置。我最初的版本用fmincon跑通全流程求解耗时大约每步80毫秒对于仿真验证完全够用后面为了展示更接近实时的效果换成了CasADi每步降到20毫秒左右。说实话如果你只是做算法验证和论文复现fmincon完全够用没必要一上来就折腾CasADi的环境配置。但如果你后续打算做硬件在环、实时控制那CasADi几乎是必须的选择。这里记录一个我用fmincon时踩过的坑fmincon的默认终止条件和步长设置经常导致NMPC在状态更新后计算出“原地踏步”的控制量表现为船不动或者控制力异常小。解决办法是在fmincon的options里把OptimalityTolerance调大到1e-3把MaxFunctionEvaluations调大同时把上一步的最优解作为本次求解的初始值也就是热启动。热启动不仅能加速收敛还能避免优化器在每步都从头搜索引起的控制量突变。4. 障碍物避碰的实现路径4.1 静态障碍物建模与惩罚函数避碰的基础是障碍物的数学描述。最简单的做法是把障碍物视为一个圆形区域圆心坐标和半径代表障碍物位置和影响范围。在NMPC里避碰可以通过硬约束或软约束两种方式实现。硬约束的写法是在整个预测时域内每个预测点的位置必须位于障碍物圆半径之外。这个约束是可行但有风险的如果船当前已经进入障碍物安全半径内或者前方障碍物分布复杂导致可行域非凸硬约束会让整个优化问题直接不可行求解器报错退出。我在调试时遇到过多次“当前位置就在障碍物边缘导致求解失败”的情况非常头疼。所以我在最终代码里用的是软约束加惩罚函数也就是在代价函数里加入避碰惩罚项$P_{obs}$。一种常见的写法是$P_{obs,k} \rho \cdot \sum_{j} \max\left(0, ; d_{safe} - |p_k - p_{obs,j}|\right)^2$这个惩罚函数是一个外点罚函数只要预测点到障碍物中心的距离小于安全半径就产生一个正惩罚值距离越近惩罚越大反之如果距离足够远惩罚为零。它的好处是即使初始状态就在障碍物附近优化问题也不会立刻不可行控制器会“感觉到”危险并尝试把船推离障碍物而不是直接拒绝给出解。4.2 动态障碍物的预测处理真实场景里障碍物往往是移动的。移动障碍物的处理方式比静态障碍物多一个维度控制器不仅要考虑船本身未来的位置还要考虑障碍物未来时刻的位置。我的做法是假设障碍物在短时间内做匀速直线运动根据障碍物的速度和方向在预测时域的每个时间步上估算出它未来时刻的位置。这样惩罚函数在每个预测时刻都利用该时刻障碍物的预测位置来计算距离。这个近似在船载传感器更新率较高的情况下是足够准确的因为预测时域通常只有两三秒障碍物位置变化有限。如果障碍物运动轨迹本身是未知的那问题就变成不确定性下的避碰问题通常需要引入障碍物轨迹预测器或者鲁棒MPC这里先不展开讲。对于复现IEEE论文来说匀速运动假设已经是一个相当标准的设定了。4.3 避碰项与跟踪项的权重平衡避碰项权重ρ的选择是另一个大坑。ρ设得太大船会在离障碍物还很远的地方就跑开轨迹绕得非常大跟踪性能大打折扣ρ设得太小船会“无视”障碍物直接穿过去避碰形同虚设。我实测下来避碰罚的权重通常要比跟踪权重高一个数量级才能保证基本不撞障碍物但又不能无限大——当ρ太大时船一旦进入罚函数显著的区域内控制器会为了逃离罚函数区而输出猛烈的转向指令导致船体剧烈摇摆。一个稳妥的策略是使用距离相关权重当船距离障碍物较远时避碰项权重较小主要精力放在跟踪上当船接近障碍物安全半径时避碰项权重自动增大控制优先级切换到安全通道。这种加权方式在工程上看着比固定权重自然而且能有效缓解“绕大弯”的问题。我的最终程序里固定权重取ρ600但代码里保留了分段加权的接口方便做改进实验。5. Matlab代码架构与核心函数实现5.1 程序文件组织与数据流整套程序我按功能模块划分共六个主要文件主脚本负责仿真流程控制模型文件负责运动学和动力学更新参考轨迹生成器负责曲线数据和参考状态计算代价函数文件负责计算NMPC目标值约束函数文件负责处理控制量和安全约束绘图脚本负责结果可视化。代码不复杂但每一个文件都只有一个明确职责这样后续改参数、加障碍物、换轨迹都非常方便。在我的代码里主循环的核心结构大致如下% 主仿真循环 for k 1 : N_sim % 获取当前状态 x_now x_history(:, k); % 截取预测时域内的参考轨迹 ref_seq ref_gen(x_now, k, Np, dt_ref); % 调用NMPC求解器 u_opt nmpc_solve(x_now, ref_seq, obstacles, param); % 施加第一个控制量并积分 u_now u_opt(:, 1); x_next usv_dynamics_rk4(x_now, u_now, dt_sim); % 数据记录 x_history(:, k1) x_next; u_history(:, k) u_now; end数据流的逻辑很简单状态从历史数组流出经过参考轨迹截取和求解器变成控制量控制量积分出下一时刻状态再写回历史。只要不在这条流水线上插入额外逻辑整个仿真就是确定性可复现的。5.2 关键代码片段讲解第一个关键函数是参考轨迹截取。这里最容易出错的是索引偏移因为我用的预测步长与仿真步长不一定相等所以参考轨迹需要按时间对齐。我的做法是仿真运行到第k步时实际时间为k乘以仿真步长NMPC预测的第j步对应时间偏移为j乘以预测步长所以参考轨迹索引需要做时间转换而不能简单写成ref_seq ref_traj(k1:kNp)。第二个关键函数是代价函数。代价函数要输入预测状态序列和控制序列输出标量代价值。我按照前面提到的公式逐项求和其中避碰项需要循环遍历每一个障碍物和每一个预测步。为了提高效率我提前把所有障碍物的位置和半径整理成结构化数组在代价函数中直接访问。第三个关键函数是约束函数。控制量约束被写成线性不等式控制增量约束需要利用上一步的控制值做差分。这里有个小细节控制增量的计算需要把上一步的实际控制量传入约束函数如果遗漏很容易导致控制量虽然限制在上下限之内但变化率过大的问题。5.3 仿真参数配置与可视化仿真参数我用一个struct集中管理包括仿真总时间、采样周期、预测时域、控制时域、权重矩阵、安全半径、船体参数等。这样修改参数时不需要在多个脚本之间来回找写论文实验记录也方便。可视化部分我画四张图第一张是全局轨迹图同时绘制参考轨迹、实际轨迹和障碍物区域第二张是位置跟踪误差曲线第三张是控制量推力和艏摇力矩变化曲线第四张是船与障碍物的最小距离随时间的变化曲线。这四张图基本覆盖了论文里需要的仿真验证内容。其中最小距离曲线是审稿人很爱看的图它能直观证明“控制器确实没有穿越障碍物”。我在画这张图的时候专门计算了每个时刻船体到每个障碍物的几何距离取最小值画成曲线同时把安全半径画成一条水平虚线两条线之间的距离一眼就能看出安全裕度。6. 调参流程与常见问题实录6.1 从发散到收敛的调试路线我拿到一套新NMPC程序时的习惯是第一步先把避碰相关的项全部关掉只保留轨迹跟踪把跟踪效果调好第二步加入静态障碍物调整避碰参数第三步再加入动态障碍物最后才考虑加扰动、模型失配等鲁棒性问题。这个分阶段推进的好处是每一步的参数问题都能被独立识别不至于出现“轨迹和避碰混在一起拐不清是谁的问题”。在第一步里如果轨迹跟踪都发散优先检查三个地方初始状态是否合理、权重的量级是否夸张、离散化是否稳定。我遇到过一次发散是因为把位置误差权重设置成1000控制器在第一步就输出了超出物理极限的控制量船直接冲出了仿真边界。调整思路是把所有量纲归一化位置误差以米为单位航向误差以弧度为单位控制量以牛顿和牛米为单位权重设置要大致平衡这些量纲的量级差异。在第二步里如果船会撞障碍物大概率是避碰权重太小或者安全半径设得过小。如果船在障碍物远处就大幅度绕行那说明避碰权重偏大或者障碍物影响范围太大。这个阶段要有点耐心绕行轨迹的形状是权重比例的直观体现。6.2 常见问题速查表我把调试过程中遇到的典型问题整理成了表格遇到类似情况可以直接对照排查。现象可能原因解决对策仿真初期就发散初始状态不合理或权重量级过大检查初始船位船速降低Q权重提高R权重船在参考轨迹附近抖动控制量惩罚R过小或预测时域过短增大R增大Np检查控制增量约束船转弯时大幅超调参考轨迹不光滑或航向权重低平滑参考轨迹增加航向角误差权重船直接穿过障碍物避碰权重小或安全半径小增大ρ增大安全距离检查松弛变量船在障碍物附近反复摆动避碰权重过大降低ρ或改用距离相关权重fmincon求解超时决策变量过多或初值不当缩短预测时域启用热启动控制量每次跳变剧烈缺少控制增量约束在约束中加入Δu限制船长时间偏离轨迹但不远离参考轨迹截取索引错误检查时间对齐逻辑6.3 实时性优化与工程扩展如果仿真里的每一步求解耗时太长可以考虑的方向包括缩短预测时域、减少控制时域、使用CasADi自动微分替代手写雅可比、简化模型例如忽略部分非线性阻尼项、用并行计算加速多个障碍物的距离计算。需要说明的是这些优化方向往往是在控制性能和求解速度之间做取舍没有免费的午餐。我实测过把预测时域从30步缩到15步求解时间大约减少一半但避碰路径会显得更急躁因为控制器“看”得更近了。往船端移植时还需要考虑几个工程问题。第一控制量输出往往要经过推力分配环节把NMPC解出的纵向推力和艏摇力矩转换为实际执行器的转速指令第二船体状态反馈通常来自传感器融合存在噪声和延迟NMPC的性能会受到影响需要加入状态估计器第三真实环境的障碍物信息来自雷达或视觉带有不确定性和量测噪声这时的避碰问题就升级为概率约束下的鲁棒NMPC属于更前沿的研究方向了。6.4 参数整定的一点个人经验最后分享一个我自己的调参习惯。NMPC的权重矩阵Q和R我不会一次性精确到某组数值而是先用数量级粗调再用细调。粗调阶段把Q的位置权重从1开始每次乘10递增跑一次仿真看效果R同理。找到“大方向没问题”的组合后再在这个组合附近做小范围微调比如把位置权重从40调到50观察跟踪误差均方根值的变化是否明显。如果变化不大就说明当前参数组合已经处于一个性能平坦区不需要再纠结更细的数值。避碰参数ρ的调整也有类似规律。我通常会先用一个比较大的ρ值让船无论如何都不敢撞障碍物然后再逐渐减小直到船在安全的前提下路径尽量贴近理想轨迹。这个过程本质上是在安全性边界上反复试探找到“刚好不出事”的临界值再把权重稍微取大一点作为安全冗余。整套程序跑通之后你会发现在无人船这样一个涉及建模、优化、约束处理的综合问题里NMPC的潜力远远不止于“能跟着轨迹走”。它把复杂的多目标控制问题变成了一个可调整、可分析、可复现的数值优化问题这也是它能成为近年来无人船控制研究主线的根本原因。如果你正在复现类似的算法希望这份从建模到调参的记录能帮你少走几条弯路。