简介：面向水面无人艇（USV）自主航行研究方向的学习者与开发者，MPC（模型预测控制）仿真资料包提供了从船舶运动建模到避碰决策的完整实现思路。水面无人艇需同时应对设定航线循迹、复杂环境中静态与动态障碍物规避等挑战，资料以航迹偏移、航速变化、碰撞危险度和规则符合度构建评价函数，并以执行机构动力为约束，设计了无人艇自主航行模型。压缩包共11个文件，包括8个Matlab脚本，覆盖模型建模、碰撞规避、约束处理、成本计算、四阶龙格库塔积分等核心模块，另有1份PDF研究报告、1个README说明文件及1张示意图，整包大小仅871KB。已有126人学习，适合具备一定控制理论基础、希望使用MPC进行USV航行仿真的研究人员参考。读者可借助源码梳理模型预测控制的预测、优化与反馈流程，并结合PDF与示意图复现仿真实验，验证自主循迹与避障效果。
1. 基于MPC的USV自主航行仿真：为什么先仿真再上船
无人水面艇（USV）的自主航行，本质上是在风浪流干扰下让船体持续追踪一条参考轨迹。MPC（模型预测控制）能天然地把路径跟踪、执行机构限幅和避障约束一起揉进优化问题里，所以在这类应用里明显比PID和纯几何追踪更稳。但MPC的另一面是“依赖模型预测未来”，参数和数值细节一旦出错，仿真里就能亲眼看到轨迹发散或优化无解的翻车现场。先做仿真，把模型、约束和代价函数在源码层面理顺，再谈实船部署，这才是性价比最高的路线。本文的读者定位是正在做USV控制相关课题、准备把MPC作为核心控制器落地的人，目标是让你顺着代码骨架把整套仿真复现出来，并把参数调整变成一门可以有依据的手艺。
2. USV运动学与动力学模型：从三自由度方程到预测机制
2.1 三自由度USV模型：忽略垂荡后的等效表示
水面艇的完整运动其实是六自由度的：纵荡、横荡、垂荡、横摇、纵摇、艏摇。如果每条USV仿真都要处理完整的水动力系数表，工作量会非常大，而且很多系数在缺少船模实验的情况下只能靠经验公式乱猜，精度并不高。工程上做MPC预测模型时，普遍做法是把垂荡、横摇、纵摇折合成附加质量系数吸收入三自由度模型，只留下纵荡、横荡和艏摇。这个处理的合理性在于：水面航行的USV主要任务是平面路径跟踪，垂向运动对位置和艏向的耦合在多数场景下可以当扰动处理，交给反馈环节去抑制。
用符号来说明，状态向量取 (\eta = [x, y, \psi]^T) 表示大地坐标系下的位置和艏向角，速度向量取 (\nu = [u, v, r]^T) 表示船体坐标系下的纵荡速度、横荡速度和艏摇角速度。运动学部分是一个简单的旋转矩阵转换，它把船体坐标系下的速度投影到大地坐标系：
[ \dot{x} = u\cos(\psi) - v\sin(\psi) ]
[ \dot{y} = u\sin(\psi) + v\cos(\psi) ]
[ \dot{\psi} = r ]
这里有个很容易被忽略的点：(v) 不是零。很多从陆上轮式机器人转过来的开发者总习惯把 (v) 当成侧滑角速度直接清零，这在USV上会带来明显模型失配，因为水面船在转弯时横漂非常明显。保留 (v) 这个自由度，MPC在预测弯道轨迹时才能“预料到”船会往外飘，从而提前打舵。
动力学部分我常用一阶惯性加线性阻尼来近似，而不是直接上复杂的MMG模型。原因是MPC本质是滚动优化，每一拍都会用实际状态做反馈校正，模型只要抓住主导动态就够了。控制量取纵向推力 (\tau_u) 和艏摇力矩 (\tau_r)，模型写成：
[ \dot{u} = (\tau_u - X_u u) / m_{11} ]
[ \dot{v} = (-Y_v v - m_{11} u r) / m_{22} ]
[ \dot{r} = (\tau_r - N_r r) / m_{33} ]
其中 (m_{11}, m_{22}, m_{33}) 是各自由度上的惯性，里面已经把附加质量算进去了；(X_u, Y_v, N_r) 是线性阻尼系数。这套模型只有五个物理参数，对于仿真验证足够，如果后面要做硬件在环，也可以用同样的结构去辨识实船参数，不需要换模型形式。
2.2 预测控制机制：滚动优化与反馈校正
MPC和普通PID的区别，在于控制器本身内嵌了一个“未来时域”的视角。每一控制周期，它都会从当前状态出发，用预测模型外推出未来 (N) 步的状态轨迹，然后在一个有限时域优化问题里寻找最优控制序列。目标函数最常见的形式是：
[ J = \sum_{k=0}^{N-1} \left( (\eta_k - \eta_{ref,k})^T Q (\eta_k - \eta_{ref,k}) + \tau_k^T R \tau_k \right) ]
这里 (N) 是预测时域，(Q) 是状态误差权重矩阵，(R) 是控制量权重矩阵。注意，真正施加到推进器上的并不是整段最优控制序列，而是序列里的第一个元素，等下一控制周期到来时再重新优化一遍。这个“只取第一步、每步重算”的机制就是滚动优化，也是MPC能对抗模型误差和外部扰动的底气所在。
反馈校正环节看着不起眼，却经常决定仿真能不能收敛。每次优化开始前，必须把当前传感器测到的实际位置、实际艏向作为初始状态塞进优化问题，而不是拿上一拍预测出的状态来续算。水面船会被浪推着走，如果初始状态不更新，预测轨迹会一步步偏离真实轨迹，最后优化器面对的是一个根本不可能满足的参考值序列，必然报不可行。
这套机制的设计取舍，解释了为什么MPC在USV上是个合理选择：船的动态响应比汽车慢不少，预测时域可以拉到5秒甚至10秒，优化求解的时间窗口足够；同时水面环境复杂，滚动优化又能持续修正扰动带来的偏差。相比之下，纯追踪算法只看到当前点前的一段几何关系，对前方急转弯和限幅约束没有前瞻能力；PID就更不谈约束问题了，参数适应不了大幅工况变化。MPC把路径跟踪、执行机构限幅、避障距离约束统一成一个优化问题，这也是它在USV自主航行研究中越来越主流的原因。
3. 仿真环境搭建与MPC控制器实现：一份可直接跑通的代码骨架
3.1 最小依赖环境搭建
做USV的MPC仿真，我建议直接用Python加CasADi的组合。CasADi做非线性MPC非常顺手，它能对动态方程做自动求导，也能直接调用成熟的优化求解器，不需要手推梯度；另外它是开源库，没有授权问题，换机器跑也方便。对研究型验证来说，仿真环境的最小依赖是Python 3.9以上、numpy、matplotlib和casadi。
conda create -n usv_mpc python=3.9 conda activate usv_mpc pip install numpy matplotlib casadi第一行创建独立的conda环境，-n后面是环境名，建议直接叫usv_mpc，这样和项目名对应，后面跑别的仿真时不会把依赖混在一起。第三行安装三个核心包，casadi会默认安装当前稳定版本，对大多数NMPC场景都够用。
装完后先验证一下安装是否正常：
python -c "import casadi; print(casadi.__version__)"能打印出版本号，说明环境已经通了。这一步的花费时间不到一分钟，但能避免后面把环境问题误判成代码问题。
3.2 核心MPC控制器的代码实现
接下来把模型落到CasADi的符号表达里。我建议把USV的状态方程单独写成一个函数，方便在预测积分和文档公式之间一一对应。
import casadi as ca import numpy as np def usv_dynamics(s, a): """USV三自由度状态方程 s = [x, y, psi, u, v, r] a = [tau_u, tau_r] """ x, y, psi, u, v, r = s[0], s[1], s[2], s[3], s[4], s[5] tau_u, tau_r = a[0], a[1] # 模型参数，按一艘中型USV的仿真量级给 m11 = 20.0 m22 = 25.0 m33 = 7.0 Xu = 2.5 Yv = 7.0 Nr = 1.0 # 运动学：把船体速度投影到大地坐标 xdot = u * ca.cos(psi) - v * ca.sin(psi) ydot = u * ca.sin(psi) + v * ca.cos(psi) psidot = r # 动力学：一阶惯性加线性阻尼 udot = (tau_u - Xu * u) / m11 vdot = (-Yv * v - m11 * u * r) / m22 rdot = (tau_r - Nr * r) / m33 return ca.vertcat(xdot, ydot, psidot, udot, vdot, rdot)这段代码里的ca.cos和ca.sin是CasADi的符号函数，和numpy里同名函数看起来一样，但返回的是符号表达式而不是数值，这是后面给优化器求导的基础。模型参数里，(m_{11})和(m_{22})的比值会对转弯半径产生明显影响，如果后面要复现某条实船，优先调整这两个值。
然后写MPC主体，用Opti栈来组装优化问题：
def solve_mpc(s0, ref_trajectory, N_horizon, dt): """求解一个N步有限时域最优控制问题""" opti = ca.Opti() a_seq = opti.variable(2, N_horizon) s_current = ca.MX.sym("s0", 6) opti.subject_to(s_current == s0) cost = 0 # Q、R在真实项目里应该作为参数传入，这里为演示直接定义 Q = ca.diag([10.0, 10.0, 5.0, 0.0, 0.0, 0.0]) R = ca.diag([0.1, 0.1]) for k in range(N_horizon): s_next = s_current + dt * usv_dynamics(s_current, a_seq[:, k]) err = s_next[0:3] - ref_trajectory[k] cost += ca.mtimes(err.T, ca.mtimes(Q, err)) cost += ca.mtimes(a_seq[:, k].T, ca.mtimes(R, a_seq[:, k])) # 执行机构限幅，先把约束加上 opti.subject_to(opti.bounded(-10.0, a_seq[:, k], 10.0)) s_current = s_next opti.minimize(cost) opts = {"ipopt.print_level": 0, "print_time": 0} opti.solver("ipopt", opts) sol = opti.solve() return sol.value(a_seq)这段代码的核心逻辑是：从当前状态出发，用欧拉法逐步外推(N)步状态轨迹，每一拍计算状态误差的二次型代价和控制量的二次型代价，同时把执行机构上下限作为约束塞进优化器。Q矩阵前三个对角元对应位置和艏向误差，后三个是速度项，这里置零是因为速度由参考轨迹隐式约束；R矩阵两个对角元分别控制推力和艏摇力矩的惩罚强度。
opti.solver("ipopt", opts)表示选择ipopt作为非线性优化求解器，print_level=0是关闭求解过程的刷屏信息，仿真里每步都打印的话根本看不完。求解完成后返回的是完整最优控制序列，但仿真循环里只取第一列。
最后把控制器放进仿真循环：
def run_simulation(): dt = 0.2 Tf = 60.0 steps = int(Tf / dt) N_horizon = 20 s = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) log = [] # 生成一条参考轨迹：纵向推进为0.5m/s的正弦路径 ref = [] for k in range(steps): t = k * dt ref.append(np.array([0.5 * t, 5.0 * np.sin(0.1 * t), 0.3 * np.cos(0.1 * t)])) for k in range(steps): # 截取从当前时刻开始的N步参考轨迹 horizon_ref = [ref[min(k + i, steps - 1)] for i in range(N_horizon)] a_opt = solve_mpc(s, horizon_ref, N_horizon, dt) # 施加第一个控制量并更新状态 s = s + dt * usv_dynamics(ca.DM(s), ca.DM(a_opt[:, 0])).full().ravel() log.append(s.copy()) return np.array(log)horizon_ref这一步很关键：它决定了MPC“看多远”。参考轨迹必须按当前仿真时刻往后截取(N)步，而不是把整条轨迹都传给优化器。ca.DM(s)是把numpy数组转成CasADi的稠密矩阵，因为usv_dynamics里的符号运算需要统一类型；.full().ravel()又转回numpy一维数组，方便更新状态。
实际运行后，把log里的[x, y]画出来，就能看到一条贴着参考轨迹前进的曲线。如果曲线能跟上，说明这套代码骨架已经跑通；如果出现振荡或发散，下一章要讲的参数调整就是关键了。
4. MPC三个核心参数的调优经验：预测时域、控制时域与权重矩阵
4.1 预测时域N与控制时域的选择
预测时域(N)决定了MPC在前面看的距离，我的习惯是在采样时间(dt=0.2)秒的前提下取15到25，对应3到5秒的未来视野。这个数字不是拍脑袋来的：(N)太小，控制器只看到眼前一小段路，高速航行时容易冲到参考轨迹附近再急刹车；(N)太大，优化变量增多，ipopt单步求解时间会明显上升，而且船速高时同样的(N)会让控制器“看得太远”，反而觉得没必要提前转向。
控制时域(N_c)是另一个容易混淆的概念。很多文档把控制时域和预测时域混着写，其实控制时域只决定“未来几步的控制量是需要优化的”。对USV来说，控制量是推力和舵角，执行机构本身有响应带宽，不需要未来20步每一步都自由变化。我通常把(N_c)取为5，也就是优化前1秒的操控动作，后面15步的控制量保持最后一步的值不再变化。这个技巧能大幅减少优化变量个数，求解速度提升非常明显，而跟踪效果几乎不受影响。
另一个值得记下的经验是船速变化时要回头重新检查(N)。有人在低速场景把(N)调到30，轨迹很平滑；换到船速3米/秒以上的场景，同一个(N)会让控制器反应迟钝，原因就是预测时域对应的物理时间变短了，船往前冲得快，控制器还没“看到”远处的转弯就已经到了跟前。遇到这种情况，要么增大(N)，要么把采样时间(dt)调小。
4.2 权重矩阵Q、R的设计原则
权重矩阵是MPC里最容易被乱调的部分。Q矩阵对角元素对应位置误差和艏向误差，我用的基准值是(Q = \mathrm{diag}([10, 10, 5]))，位置权重是艏向权重的两倍。原因是USV路径跟踪最终考核的是横向偏差，艏向偏差虽然也是代价的一部分，但为了强行把船头掰到和参考航线完全平行，控制器会频繁打舵，反而让轨迹左右振荡。
R矩阵针对控制量幅值，基准值是(R = \mathrm{diag}([0.1, 0.1]))。把R调大会让控制器舍不得用舵，轨迹响应变慢；调小则响应更快，但执行机构可能出现高频抖振。一个工程化的调参流程是：先把R固定为0.1，Q里位置项固定为10，只调(N)让直线跟踪稳定；然后再逐步增大R，看到轨迹出现轻微延迟时回调一档。这个过程比同时乱试几个参数容易诊断。
这里必须提醒一个量纲陷阱：位置误差单位是米，艏向误差单位是弧度。如果Q里位置和艏向直接给同样的数值，优化器会认为1弧度的偏差和1米的偏差同等重要，但实际上0.5弧度的船头偏转就会让横漂加速。我一般把艏向权重取为位置权重的0.3到0.5倍，并且在用真实船参数时检查一下船体长度：船越长，同样的艏向误差造成的横向偏差越大，权重应该相应提高。
4.3 约束处理：硬约束转软约束
USV航行里常见的约束有执行机构饱和、艏摇角速度限制和动态避障距离约束。执行机构饱和必须写进优化问题，否则控制器会解出一个推力幅值远大于实际执行器的控制量，上船后直接饱和。但直接把这些写成硬约束，非线性优化非常容易找不到可行解。
我的做法是引入松弛变量，把硬约束转成软约束。以执行机构限幅为例，把(-10 \le \tau \le 10)改成：
[ \tau = \tau_{des} + s_{relax} ]
[ -10 \le \tau \le 10 ]
[ s_{relax} \ge 0 ]
然后在代价函数里加上一个对(s_{relax})的大惩罚项。这样优化器在极端情况下可以“借”一点超出边界的松弛量来保证问题可解，但正常工况下惩罚项会迫使松弛量回到零，约束仍然有效。这个方案在我做过的模拟项目里，多次避免了无解翻车，而且比手动调节约束边界要省心得多。
5. 避坑排查：NMPC求解失败、轨迹发散与模型失配的六类翻车现场
5.1 现象：ipopt一上来就报infeasible problem
这是NMPC最常见的第一道坎。我遇到的现象是仿真跑不了几步，控制周期里直接抛出不可行解异常。原因多半有两个：一是约束设置过紧，执行机构限幅给得很窄，而参考轨迹第一步就要求一个大幅转向，优化器找不到满足全部约束的控制序列；二是初始状态本身已经在约束边缘，加上预测时域太长，未来几步的轨迹对控制量的需求变化太剧烈。
解决方法是先把所有执行机构限幅放宽到原来的两倍，并逐步增大R，让控制器对控制量的惩罚也变大，这样优化器会更倾向于“少用力”。如果放宽后还是不可行，那就检查初始状态是否正常，尤其是艏向角(\psi)有没有混入度数单位。这个看似低级的问题在工程里反复出现，角度和弧度混用会让模型退化得很离谱。
5.2 现象：轨迹发散，船在参考轨迹两侧左右乱窜
这种表现为船头来回摆动，实际轨迹像锯齿一样贴在参考线两边。最常见的原因是Q矩阵里位置权重过大、R矩阵里控制权重也过大，两者共同作用让控制器既想拼命追点又不敢用舵，结果就是相位滞后加持续振荡。
我一般会把Q位置项设为10，R设为0.1，先跑一条匀速直线参考轨迹，确认系统稳定后再换成带转弯的路径。如果直线还是振荡，就把控制时域(N_c)从5减到3，同时检查参考轨迹是否连续。很多人把航点直接连成折线丢给控制器，MPC对折线非常敏感，因为参考速度方向突变会让优化问题来回切换，正确做法是用B样条把航点平滑成连续曲线。
5.3 现象：仿真中分段跑，前面航段稳定，转到新航段就卡住或发散
这个坑藏在横漂速度(v)的初始化上。USV在转弯时(v)并不为零，如果上一个航段结束时船体还带着一个明显的横向速度，而新航段的参考轨迹是从零开始规划，MPC会试图在极短时间内把(v)拉回零，这个约束条件太苛刻，优化器自然就卡住了。
解决方式是做航段切换时不把整条旧轨迹硬塞给控制器，而是在新航段开始时用当前位置和艏向作为初始状态，重新规划一条平滑的过渡参考轨迹。文档里应该把这段逻辑单独拿出来写，因为它属于“仿真框架”问题而不是“控制器”问题，不解决的话项目一换场景就会复发。
5.4 现象：实时性不够，每个控制周期计算时间超预算
仿真时觉得无所谓，但要往实船上走的话，这个问题迟早会撞上。原因主要有三个：预测时域(N)取得太大、采样时间(dt)太短、ipopt的求解精度设置太高。我见过有人把ipopt的tolerance设为1e-10，每步求解耗时直接超过一个控制周期，这是完全没必要的精度浪费。
常规做法是把求解精度降到1e-4，这已经比模型本身误差高两个数量级了。同时把(N)从30减到15，并把控制时域设为5，优化变量从60个降到15个，求解时间往往能下降一半以上。如果还嫌慢，可以把求解器从ipopt换成sqpmethod，在非线性程度不高的场景下，SQP方法比内点法通常快不少。
5.5 现象：输入数据里有隐藏的NaN，导致优化器行为怪异
这一条放在后面，是因为它最隐蔽。某次我在课题里排查了一整天，所有打印都显示状态正常，但控制器偶尔会解出异常控制量。最后把参考轨迹数据逐个打印才发现，某几个时间点的数据在生成过程中被除零操作染成了NaN。NaN不会让程序崩溃，但会顺着自动求导一路扩散到梯度里，优化器得到的结果就好像中邪一样，毫无规律。
解决方法是给所有进入MPC的数组做一次显式检查，用numpy的np.isnan().any()来判断，一旦发现NaN就停住并打印数据来源。这个习惯应该写进代码模板里，它花费的时间不到一毫秒，却能省下大量排查时间。
6. 验证方法进阶：从误差统计到动画回放的工程化习惯
6.1 把跟踪误差量化成指标
仿真跑通后，不要只盯着图形看个大概，把横向偏差和艏向误差按时间序列记录下来，画出累计分布曲线。横向偏差定义为参考轨迹到当前位置的最短距离，在直线段可以用最近参考点近似，但在急转弯处建议做分段投影，否则误差值会偏大。记录误差均值、最大误差和最后的能量消耗，这样每次改参数后能有一个可对比的数字，而不是“感觉好一点”。
6.2 动画回放比静态曲线更容易发现问题
把每一拍的状态存下来，用matplotlib做一个带船体姿态的动画回放，船的朝向用箭头标识。这个方法能直观看到控制器的行为模式，比如是否在某个特定转向角度附近抖动，是否每次过弯都是先超调再拉回来。动画结束后自动打印误差统计数据，这样一遍回放就能同时观察到轨迹形态和量化指标。
一个习惯建议：每次调参后都保存一份对应的动画和数据文件，并给文件名加上参数摘要，比如N20_Q10_R01.mp4。这个习惯看似占了一点存储空间，但做参数对比时能省下大量重复跑仿真的时间。回头看我自己做过的某个仿真项目，最耗时的不是写MPC代码，而是反复对比不同参数组合下的表现；有了这套数据归档，几组参数的效果一眼就能看清楚。
最后说一条个人教训：那次被NaN坑了两天后，我给自己定了一条规矩——任何输入MPC的数据，先做数值检查再交给优化器；任何调参实验，必须留下一份可视化结果和参数记录。这两条习惯让后续所有控制相关的开发都顺畅了很多。希望帮到你。
本文还有配套的精品资源，点击获取
