无人水面艇NMPC轨迹跟踪与避障的Matlab实现

说实话看到“无人水面艇 非线性模型预测控制 轨迹跟踪 避障 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 Σ_{k0}^{N-1} [ e(k) Q e(k) u(k) R u(k) ] e(N) P e(N) s.t. x(k1) 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上限通常15Ntau_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越大决策变量越多求解越慢。一个N20的小型USV问题决策变量维度是N * 2 40维配合6维状态轨迹实际优化规模不算大但如果在欠驱动模型里再扩大预测时域求解时间很容易飙到秒级。3.2 求解器选型与加速策略我在这个项目里试过三种方案分别说下优缺点。方案一fminconsqp。配置简单代码量小适合快速验证算法。缺点是每次优化要反复调用模型函数速度偏慢但对应小规模的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实时性改善非常明显。代价是预测“眼光”变短了对急转弯和密集障碍物场景会吃力。我的经验是仿真阶段先定N20调好权重后再测试N15看性能是否还能接受。第三解析雅可比。手工推导模型相对于状态和控制输入的导数把它交给求解器。很多人图省事用数值差分这在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 Σ_{k0}^{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在连续转向时的动态跟踪性能。场景三是静态障碍物避障在船前进路径上放两个圆形障碍物验证避障约束是否能让船带着安全距离绕过去。参数设置我用一张表固定下来方便复现参数项数值说明采样周期 Ts0.2 s控制周期预测时域 N20预测往后4秒质量 m23.0 kg含附加质量惯量 Iz1.76 kg·m²含附加惯量纵向阻尼 du4.0线性阻力系数横向阻尼 dv5.0线性阻力系数转向阻尼 dr1.5线性阻力系数tau_u_max15 N推力上限tau_r_max10 N·m力矩上限障碍半径 r_obs0.8 m障碍物半径安全距离 r_safe0.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:kN) x_ref_seg x_ref_all(:, k : kN); % 设置热启动初值 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协同编队、或者接入实时硬件在环仿真。想深入研究的可以先卡住“模型”“约束”“求解器”这三个节点把每一步的数学含义吃透很多种不同的控制问题都能迅速套用同一套逻辑。