简介：面向船舶工程与控制工程学习者的动力定位仿真资源，基于MATLAB与Simulink搭建，可用于理解动力定位系统如何通过推进器抵消风、浪、流影响并保持船位与姿态。压缩包共13个文件、约58KB，以mat数据文件、m脚本、mdl与slx仿真模型为主，另含旧版本工程文件，便于在MATLAB环境中直接加载、运行和修改。已有1305人学习，适合课程设计、毕业课题与入门级科研复现。读者可从中获得六自由度船舶运动模型、控制器建模思路，以及叠加海浪干扰和白噪声的仿真设置，并通过示波器模块观察各自由度位移、速度与反馈信号，评估定位精度、响应时间和稳定性。它也可作为测试与优化PID等控制策略的实验底稿，帮助把控制理论与海洋工程场景对应起来。

1. 从一次码头试航说起：为什么要先在一套模拟器里把闭环跑通

去年帮一个做海上施工支持的朋友看控制参数。他要验证一条供给船在定点作业时的艏向保持能力，真船出海一个航次的钱够买十台工作站，而且风浪不等人。退一步的方案是在岸上先把整套闭环压到一台机器里跑：船舶动力定位模拟器，MATLAB 打底，Simulink 搭闭环，船模、环境、传感器、估计器、控制器、推力分配全在一张图上。这套东西的价值不在于“仿真好看”，而在于你可以在十分钟内把风速从 5 m/s 拉到 15 m/s、把一台艏侧推强制失效，然后看控制器到底是稳住了还是开始画圈。适合三类人：做 DP 控制算法验证的、要交课程设计或论文实验的、以及想弄明白“为什么真船上的推力分配会抖”的工程开发。

2. 项目全景与技术栈：一张 Simulink 图里塞进了几层东西

拿到压缩包先别急着双击 .slx。这类项目最容易翻车的地方不是模型跑不起来，而是你根本不知道哪一层在算什么，改了一个增益之后曲线变了，但你不知道是控制器变强了还是被控对象被你改错了。先把分层看清楚，后面所有调试都会顺。

2.1 解压后的目录组织与文件职责

这类 DP 仿真项目的组织方式我见过好几种，常见做法是把“参数、模型、算法、绘图”四件事分开，而不是把所有东西塞进一个 .m 文件。下面这套结构是拆开之后最舒服的一种，你可以对照自己手里的包逐个对号：

DP_Simulator/ ├── main.m % 仿真入口：装载参数、调用 Simulink 或纯脚本积分 ├── config/ │ ├── vessel_params.m % 船体质量矩阵 M、阻尼矩阵 D、推进器布置 │ └── sim_config.m % 步长、仿真时长、环境工况、初始状态 ├── models/ │ ├── dp_closed_loop.slx % 顶层闭环：船模 + 环境 + 传感器 + 估计 + 控制 + 分配 │ ├── vessel_3dof.slx % 三自由度船模子系统 │ └── env_load.slx % 风、流、二阶波浪慢漂力 ├── src/ │ ├── vessel_dynamics.m % 运动方程右端项 │ ├── env_wind.m / env_current.m / env_wave.m │ ├── sensor_model.m % 位置参考、电罗经、风速仪的量测与噪声 │ ├── ekf_dp.m % 状态估计 + 慢变偏差估计 │ ├── controller_pid.m / controller_lqr.m │ └── thruster_alloc.m % 推力分配（伪逆或 QP） ├── utils/ │ ├── plot_results.m % 轨迹、推力、误差曲线 │ └── dp_metrics.m % 定点精度、能耗、推力利用率统计 └── docs/ └── 参数说明与工况表

main.m一般只有二三十行，它的职责是“把配置读进来、把模型跑起来、把结果画出来”，不该出现任何控制律。这一点很重要：如果你发现控制参数是硬编码在main.m里而不是在vessel_params.m或sim_config.m里，那这个包大概是被改过很多手的，先做一次参数归拢再动手。

.slx是二进制文件，跨 MATLAB 版本打开会有兼容问题。包里如果有.mdl老格式，说明它的原始版本比较早；如果只有.slx，用比它更新的版本打开基本没问题，反过来往下打开经常报“模型由更新版本创建”。这不是模型坏了，是文件格式的事。

2.2 三层结构：被控对象、估计器、控制器与分配器

把闭环拆开看，其实就三层在互相喂数据。

第一层是被控对象，也就是船本身加环境。输入是推进器合力矩tau（船体坐标系下的纵荡力、横荡力、艏摇力矩三个分量），输出是船在大地坐标系下的位置和艏向eta = [x; y; psi]，中间还夹着船体坐标系速度nu = [u; v; r]。环境力在这里以“附加输入”的方式叠加进来，所以船模子系统的输入端口通常是两路：推力一路、环境力一路。

第二层是量测与估计。真实 DP 船上你不会直接拿到nu，甚至eta也是带噪声的：位置来自位置参考系统，艏向来自电罗经，而且都带慢变偏差。所以仿真里必须有一个传感器模块把“真值”污染成“量测”，再送进估计器。估计器输出的是滤波后的位置、速度和偏差估计，控制器只吃估计值，不吃真值——如果你的控制器直接接了船模输出的eta，那这个仿真就没意义了，等于开卷考试。

第三层是控制与分配。控制器根据“期望位置 − 估计位置”算出大地坐标系下的期望合力，再旋转到船体坐标系，交给推力分配器拆成每台推进器各自的推力指令。分配器是整条链路里最容易被低估的一环，它要处理推力上限、变化率、方位角、死区，很多时候控制器没问题，是分配器在抖。

三层之间的信号流向是单向的，没有代数环。如果你的模型跑起来报代数环错误，八成是传感器模块里混进了当前步的推力计算，或者推力分配把上一拍的推力又送回控制器做了反馈。

2.3 工具箱依赖与第一次打开模型

这类项目对工具箱的依赖主要集中在三块：Simulink 本体、Control System Toolbox（lqr、ss、tf都在里面）、Optimization Toolbox（quadprog做推力分配的二次规划）。如果推力分配用的是伪逆而不是 QP，Optimization Toolbox 可以不要；如果控制器只用 PID，Control System Toolbox 也不是必须的，但lqr那条路走不了。

第一次打开的正确顺序是：先把当前文件夹切到项目根目录，运行一次config下的参数脚本，让base workspace里有p这个结构体；再打开models/dp_closed_loop.slx。因为 Simulink 里的模块参数很多是写成p.M、p.u_max这种引用形式的，工作区里没有p就会一片红。

% 在项目根目录执行，检查依赖是否齐全 cd DP_Simulator p = vessel_params(); % 船体与推进器参数 sc = sim_config(); % 步长、时长、工况 fprintf('M 矩阵条件数: %.3e\n', cond(p.M)); fprintf('推进器数量: %d\n', size(p.thruster,1)); fprintf('仿真步长: %.3f s，总时长: %.0f s\n', sc.Ts, sc.t_end); % 检查关键函数是否在路径上 for f = {'ekf_dp','controller_pid','thruster_alloc','vessel_dynamics'} fprintf('%-20s %s\n', f{1}, which(f{1})); end

这段脚本干的事很朴素：确认参数被正确构造、确认函数在搜索路径上。cond(p.M)那行是给自己看的，三自由度质量矩阵里纵荡方向的量级是 1e6，艏摇方向是 1e9，条件数在 1e3 量级属于正常；如果跑出来是 1e8 以上，说明参数表里某个附加质量填错了量级。which的循环是血泪经验：MATLAB 路径里如果存在同名函数（比如你自己以前写过另一个thruster_alloc.m），它会静默调用错的那个，曲线怎么调都不对，你还以为是算法问题。

3. 核心模块拆解：三自由度船模、环境载荷与传感器

控制算法再花哨，被控对象错了就全是空谈。这一章把船模这一侧拆开：运动方程怎么来的、Simulink 里怎么落地、风和流怎么加、传感器噪声怎么造。这四件事做对，后面估计和控制才有讨论的基础。

3.1 三自由度低频运动方程与参数文件

DP 关心的频段是低频。一阶波浪引起的往复运动周期在 5 到 15 秒，控制器根本追不上也不该追，追了就是白白烧油还磨损推进器。所以船模一般只保留三个自由度：纵荡、横荡、艏摇，也就是纵荡位移x、横荡位移y、艏向角psi。

运动学部分是把船体坐标系速度转到大地坐标系：

eta_dot = R(psi) * nu R(psi) = [ cos(psi) -sin(psi) 0 sin(psi) cos(psi) 0 0 0 1 ]

动力学部分是低频的刚体加附加质量模型，写成M * nu_dot + D * nu + D_nl * nu .* |nu| = tau + tau_env。M里已经包含了附加质量，D是线性阻尼，D_nl是二次阻尼项，低速时二次项几乎不起作用，但横荡方向被流推着走的时候它能吃掉不少能量。

参数文件是个结构体，量级按一条七十多米级海上供给船来给：

function p = vessel_params() % 三自由度（纵荡/横荡/艏摇）船舶参数，量级参考 70 m 级海上供给船 p.L = 76.2; % 垂线间长 [m] p.rho = 1025; % 海水密度 [kg/m^3] % 刚体质量 + 附加质量矩阵 M（3x3，已含附加质量） p.M = [5.3122e6, 0, 0; 0, 8.2831e6, 0; 0, 0, 3.7454e9]; % 线性阻尼矩阵 D（含横荡-艏摇耦合项） p.D = [5.0242e4, 0, 0; 0, 2.7229e5, -4.3933e6; 0, -4.3933e6, 4.1894e8]; % 二次阻尼，按对角近似 [N·s^2/m^2] p.Dnl = diag([1.2e3, 8.0e3, 3.0e7]); % 推进器布置：每行 [lx, ly, 方位角偏置(rad)]，x 向船首为正 p.thruster = [ -32, 5, 0; % T1 尾部主推（左） -32, -5, 0; % T2 尾部主推（右） 30, 4, pi/2; % T3 艏侧推（左） 30, -4, pi/2]; % T4 艏侧推（右） p.u_max = [800e3; 800e3; 400e3; 400e3]; % 单机推力上限 [N] p.u_min = -p.u_max; end

参数说明上，M的三个对角元分别对应纵荡、横荡、艏摇的有效惯量，横荡方向的数值比纵荡大一截是正常的，因为船体横向的附加质量远大于纵向。D里非对角的那两项是横荡与艏摇的耦合阻尼，删掉它们仿真照样跑，但船在斜向流里的漂移会偏大。p.thruster的第三列是方位角偏置，主推固定朝船首方向所以是 0，艏侧推是pi/2表示推力沿横向。p.u_max里主推 800 kN、侧推 400 kN，量级和实际船对得上，改船型时这一行是必改项。

方程右端项写成一个函数，Simulink 里和纯脚本积分都能复用：

function dx = vessel_dynamics(~, x, tau, tau_env, p) % 状态 x = [x; y; psi; u; v; r]，输入为推进器合力与环境力（均在船体系） psi = x(3); R = [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; nu = x(4:6); eta_dot = R * nu; % 运动学 nu_dot = p.M \ (tau + tau_env - p.D*nu - p.Dnl*nu.*abs(nu)); % 动力学 dx = [eta_dot; nu_dot]; end

这段的关键在p.M \ (...)，用左除而不是inv(p.M)*(...)，一是数值上更稳，二是 MATLAB 会自己选分解方式。p.Dnl*nu.*abs(nu)那个写法要小心优先级：先算矩阵乘再逐元素乘绝对值，效果等价于diag(p.Dnl).*nu.*abs(nu)，如果你把Dnl存成向量了就得改成后一种写法。~占位是因为这个函数不依赖时间，但如果后面要加时变环境力，这个位置就得改回t。

3.2 Simulink 侧的三种落地方式与选择

运动方程写好了，放进 Simulink 有三种常见路子，各有各的适用范围。

第一种，用State-Space模块。适合纯线性、参数不随时间变的情况。但vessel_dynamics里那个R(psi)是状态相关的，整个系统是非线性的，所以State-Space只能覆盖“把船在某个工作点线性化之后”的场合，做小信号分析够用，做完整闭环仿真不够。

第二种，用MATLAB Function模块，把上面那个函数体贴进去。这是我最常用的一种。优点是代码可读、能直接调p结构体、调试时可以单步；缺点是每个仿真步都要过一遍 MATLAB 解释器，长仿真会慢。缓解办法是把p在Simulink里做成Simulink.Parameter或者Simulink.Bus，避免每步都在工作区里做结构体拷贝。

第三种，Level-2 MATLAB S-Function，或者干脆用Integrator加Fcn模块手搭。前者性能最好，后者最直观，适合教学演示。手搭的方式是三个积分器分别积eta_dot和nu_dot，把R(psi)用一个MATLAB Function算出来，剩下全是连线。

不管哪种，有个约定必须守住：状态顺序固定为[x; y; psi; u; v; r]。仿真里最隐蔽的翻车就是“某一次重构把顺序改了”，控制器那边还按老顺序取psi，结果拿r当艏向用，曲线看着像在发散，其实是索引错位。

3.3 环境载荷：风、流与二阶慢漂力怎么加

环境力是 DP 仿真里最容易糊弄、也最容易失真的一块。真实海况里有三样东西对船起作用：平均风、平均流、二阶波浪慢漂力。一阶波浪力会造成高频往复运动，我们靠波浪滤波和死区把它挡在控制器外面，不参与低频力平衡。

风的作用写成相对风速的二次型：

function tau_w = env_wind(t, nu, p) % 平均风 + 阵风，输出船体系风力/力矩 V_w = [10; 0]; % 大地系风速 [m/s]，10 m/s 约 5 级风 psi_w = deg2rad(30); % 风来向（相对大地系） Vw_g = V_w * [cos(psi_w); sin(psi_w)]; % 船速转到大地系后求相对风速 psi = 0; % 由调用方传入当前艏向，这里简化 R = [cos(psi) -sin(psi); sin(psi) cos(psi)]; V_r = Vw_g - R * nu(1:2); V_r_mag = norm(V_r); gamma = atan2(V_r(2), V_r(1)) - psi; % 相对风舷角 % 简化系数：实际项目应查风阻系数表或用 Isherwood 拟合 Cx = 0.6; Cy = 0.8; Cn = 0.5; A_F = 300; A_L = 800; % 正面/侧面受风面积 [m^2] tau_w = 0.5 * 1.225 * V_r_mag^2 * ... [ Cx*A_F; Cy*A_L; Cn*A_L*p.L ]; end

逻辑上分三步：先算出相对风速矢量，再用相对风舷角查（或拟合）风阻系数，最后乘0.5*rho_a*V^2*面积得到力。gamma那一行是整个函数的核心，风舷角变了系数就变，这也是为什么船在转向过程中风力矩会突然变号。系数用常数是最粗的做法，工程上一般用按风舷角分段的系数表，或者用interp1做插值；Cx/Cy/Cn这三个常数的取值和上层建筑面积强相关，改船型时必须重算。

流的作用形式一样，只是密度换成海水、速度换成相对流速，而且流速通常比风速小一个量级，但方向稳定，对定点精度的影响反而更持久：

function tau_c = env_current(nu, p) V_c = 0.8; % 流速 [m/s] psi_c = deg2rad(-60); Vc_g = V_c * [cos(psi_c); sin(psi_c)]; V_r = Vc_g - nu(1:2); % 船体系相对流速（艏向已在小角度下近似） Vr = norm(V_r); ang = atan2(V_r(2), V_r(1)); Cx = 0.5; Cy = 1.2; Cn = 0.3; A_Fs = 200; A_Ls = 600; % 水下受流面积 tau_c = 0.5 * p.rho * Vr^2 * ... [ Cx*A_Fs; Cy*A_Ls; Cn*A_Ls*p.L ]; end

二阶波浪慢漂力不要用正弦硬凑。常见做法是白噪声过一个二阶低通成形滤波器，截止频率设在 0.1 到 0.2 rad/s，幅值按有效波高标定：

function tau_w2 = env_wave_drift(t, p) % 二阶慢漂力：白噪声经二阶低通成形（简化近似） persistent w1 w2 if isempty(w1) rng(42); % 固定随机种子，保证可复现 w1 = randn; w2 = randn; end wn = 0.15; zeta = 0.7; % 截止频率与阻尼比 tau_w2 = 3e4 * [1; 1.5; 0.2*p.L] * sin(wn*t + 0.3*w1) * (1 + 0.1*w2); end

rng(42)那一行是后悔药。不加种子，两次跑出来的曲线不一样，你就没法判断“改了参数之后变好”到底是改对了还是随机数碰巧。wn决定慢漂力的主频，zeta决定它的带宽，这两个参数需要对着有效波高和平均周期标定，不要随手填。

3.4 传感器模型：把真值污染成量测

传感器模块的输入是真值eta和nu，输出是量测y。位置量测一般是位置参考系统的组合，噪声标准差 1 到 3 米，还带一个几十米量级的慢变偏差——这个偏差可能来自位置参考基准的漂移，也可能来自船体坐标系的安装偏差。艏向量测来自电罗经，噪声 0.2 到 0.5 度，偏差 0.5 度上下。

function y = sensor_model(eta, bias, p) % 输出量测 y = [x; y; psi] + 偏差 + 噪声 persistent first if isempty(first) rng(7); first = false; end sig_pos = 2.0; % 位置噪声标准差 [m] sig_hdg = deg2rad(0.3); % 艏向噪声标准差 [rad] v = [sig_pos*randn; sig_pos*randn; sig_hdg*randn]; y = eta + bias + v; y(3) = wrapToPi(y(3)); % 艏向必须绕回 ±pi end

wrapToPi这一行不是可选项。少了它，量测里的艏向会在+pi和-pi之间跳变，EKF 的新息序列会出现几弧度的野值，滤波直接发散。偏差bias单独作为一个参数传入，是为了后面在 EKF 里把它当成状态估出来。

4. 估计与控制链路：EKF、PID/LQR 与推力分配

船模对了、量测有了，接下来是把“带噪声的位置”变成“能拿去做控制的干净信号”，再变成“每台推进器该出多少力”。这一段是整份代码里改动最频繁的部分，也是二次开发最常动刀的地方。

4.1 九阶 EKF：把位置、速度、慢变偏差一起估出来

DP 的估计器要做三件事：滤掉一阶波浪引起的高频位移、估出船体速度（因为速度通常没有直接量测）、估出量测里的慢变偏差。标准做法是扩展卡尔曼滤波，状态取[eta; nu; b]共九维。

预测步用上一步的推力做输入，状态转移写成离散形式：

function [z, P] = ekf_dp(z, P, tau, y, T, p) % 一步 EKF，状态 z = [x;y;psi; u;v;r; bx;by;bpsi] n = 9; eta = z(1:3); nu = z(4:6); b = z(7:9); psi = eta(3); R = [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; % ---------- 预测 ---------- f = [eta + T*(R*nu); nu + T*(p.M \ (tau - p.D*nu)); b]; dRnu_dpsi = [-sin(psi)*nu(1) - cos(psi)*nu(2); cos(psi)*nu(1) - sin(psi)*nu(2); 0]; A = eye(n); A(1:3,3) = A(1:3,3) + T*dRnu_dpsi; % 注意：不能直接赋值，要累加 A(1:3,4:6) = T*R; A(4:6,4:6) = eye(3) - T*(p.M \ p.D); Q = diag([0.5^2, 0.5^2, deg2rad(0.1)^2, ... 0.02^2, 0.02^2, deg2rad(0.005)^2, ... 1e-8, 1e-8, 1e-8]); P = A*P*A' + Q; z = f; % ---------- 更新 ---------- H = [eye(3), zeros(3,3), eye(3)]; % y = eta + b + v Rm = diag([2.0^2, 2.0^2, deg2rad(0.3)^2]); S = H*P*H' + Rm; K = P*H'/S; z = z + K*(y - H*z); z(3) = wrapToPi(z(3)); % 更新后艏向也要绕回 P = (eye(n) - K*H)*P; P = (P + P')/2; % 强制对称，防数值漂移 end

逐段说。f里第一行是运动学推进，第二行是动力学推进，第三行是偏差的随机游走模型——偏差不主动变化，靠过程噪声Q最后三项的小值让它慢慢跟着量测走。A(1:3,3)那一行是最容易写错的地方：状态转移矩阵对psi的偏导要累加到原来的单位阵上，直接赋值会把psi自身的 1 抹掉，滤波在艏向通道上会失去跟踪能力。P = (P+P')/2那行看着像洁癖，实际上跑几千步之后协方差矩阵会因为浮点误差失去对称性，chol分解失败、增益算出复数，都是从不加这一行开始的。

Q和Rm的比例决定了滤波带宽。Q里位置的三项（0.5 米）比量测噪声（2 米）小，滤波器就会更相信模型、更狠地压制高频，这正是我们想要的波浪滤波效果。如果你把Q位置项调到 3 米以上，滤波器带宽拉高，一阶波浪会原封不动地漏进控制器，推力就开始高频抖。

九阶模型是简化版。完整版会把三阶波浪模型的状态也扩进来，状态维数到 15 阶左右，好处是波浪抑制有明确的频域依据，坏处是参数整定工作量翻倍。做算法验证的话九阶够用，要做产品级对比再考虑扩维。

4.2 控制器：从 PID 起步，用 LQR 给增益找初值

控制器要输出的是大地坐标系下的期望合力，再转到船体系。PID 的形式写成这样：

function tau_c = controller_pid(eta, nu, eta_d, e_int, p) % 大地系 PID，输出船体系期望推力 e = eta - eta_d; e(3) = wrapToPi(e(3)); % 艏向误差绕回 ±pi R = [cos(eta(3)) -sin(eta(3)) 0; sin(eta(3)) cos(eta(3)) 0; 0 0 1]; tau_d = -p.Kp*e - p.Kd*(R*nu) - p.Ki*e_int; tau_c = R' * tau_d; % 大地系 → 船体系 end

三个增益矩阵都是 3×3 对角阵就够了，非对角项在对称船上没有物理意义，加了反而难调。Kd作用在R*nu上，也就是大地系速度，而不是船体系速度——这点很多实现会搞混，用船体系速度做微分反馈在船转向时会引入额外耦合。

增益初值不建议手工试。用 LQR 在线性化模型上算一遍，把结果当成 PID 的初值，收敛速度快得多：

function p = controller_gain(p) % 用 LQR 在平衡点线性化模型上求增益，作为 PID 初值 A = [zeros(3), eye(3); zeros(3), -p.M \ p.D]; B = [zeros(3); p.M \ eye(3)]; % Q 的对角元 = 1/(允许偏差)^2，物理含义直观 Q = diag([1/3^2, 1/3^2, 1/deg2rad(1)^2, ... 1/0.5^2, 1/0.5^2, 1/deg2rad(0.2)^2]); Rr = diag([1/1e6^2, 1/1e6^2, 1/1e8^2]); % 力的代价，量级决定推力激进程度 K = lqr(A, B, Q, Rr); p.Kp = K(:,1:3); p.Kd = K(:,4:6); p.Ki = 0.02 * p.Kp; % 积分项先给个小值，避免一上来就饱和 end

Q的对角元写法是有讲究的：它等于“允许偏差平方的倒数”，位置允许偏 3 米就写1/3^2，艏向允许偏 1 度就写1/deg2rad(1)^2。这样调参时你调的是工程指标，不是抽象数字。Rr的量级决定控制器有多“舍得”用推力，把它调小，推力变激进、响应变快，但更容易撞上限幅。

Ki给0.02*Kp是个保守起手式。积分项在 DP 里主要用来吃掉风和流的常值扰动，不需要快，给大了就是超调和振荡。

4.3 推力分配：伪逆够用，但 QP 才是工程做法

分配器要解的方程是B(alpha) * u = tau_c，其中B是推进器配置矩阵，每一列对应一台推进器在当前方位角下产生的力和力矩。

function B = thruster_config(p, alpha) % alpha: 各推进器当前方位角增量（固定桨传全零） n = size(p.thruster,1); B = zeros(3,n); for i = 1:n a = p.thruster(i,3) + alpha(i); lx = p.thruster(i,1); ly = p.thruster(i,2); % 该列 = [Fx; Fy; N]，N = lx*Fy - ly*Fx B(:,i) = [cos(a); sin(a); lx*sin(a) - ly*cos(a)]; end end

伪逆的写法是u = pinv(B) * tau_c，五行代码搞定，教学演示够用。但它有三个绕不过去的问题：一是不考虑推力上限，算出来 2000 kN 你也只能给 800 kN，实际合力就对不上；二是四台推进器解三个方程，属于欠定问题，伪逆给的是最小范数解，能耗不一定最优；三是推力变化率完全不受控，仿真里看着没事，真船上液压系统跟不上。

工程做法是转成带约束的二次规划：

function [u, flag] = thruster_alloc(tau_c, u_prev, p) % min u'*W*u + s'*Q*s % s.t. B*u - s = tau_c, u_min <= u <= u_max, |u - u_prev| <= du n = size(p.thruster,1); B = thruster_config(p, zeros(n,1)); W = diag(1./p.u_max.^2); % 按额定推力归一化，防止大桨吃掉全部权重 Q = 1e3 * eye(3); % 松弛变量罚因子，越大越优先保证合力 H = blkdiag(2*W, 2*Q); f = zeros(n+3,1); Aeq = [B, -eye(3)]; % B*u - s = tau_c beq = tau_c; lb = [p.u_min; -inf(3,1)]; ub = [p.u_max; inf(3,1)]; du = 0.1 * p.u_max; % 每步允许的推力变化 Aineq = [ eye(n), zeros(n,3); -eye(n), zeros(n,3)]; bineq = [ u_prev + du; -u_prev + du]; opts = optimoptions('quadprog','Display','off', ... 'Algorithm','interior-point-convex'); [z, ~, flag] = quadprog(H, f, Aineq, bineq, Aeq, beq, lb, ub, [], opts); u = z(1:n); end

W = diag(1./u_max.^2)这一步是把不同量级的推进器拉到同一个尺度上比较，不归一化的话 800 kN 的主推在代价函数里的权重只有侧推的四分之一，QP 会倾向让主推使劲、侧推摸鱼。s是松弛变量，允许合力与期望值有偏差，Q越大越不允许偏，但在推进器全部饱和的极端工况下，有松弛变量至少能返回一个“最接近”的解而不是exitflag = -2。du约束的就是变化率，取额定推力的 10% 每步，配合 0.1 秒步长相当于每秒最多变化一个额定值。

flag要检查。flag = 1是收敛，2是可行但精度不够，0是迭代超限，负数是不可行或无界。负数出现时先看tau_c是不是本身就超出了所有推进器合力能覆盖的范围——那不是分配器的问题，是控制器给的目标不合理。

4.4 结果怎么看：三个指标定生死

跑完之后plot_results.m一般会画四张图：大地系轨迹、位置误差随时间、艏向误差、各推进器推力。光看图不够，得有三个数：

本文还有配套的精品资源，点击获取