企业官网建设流程全解析

花了两周把一篇顶刊论文里的水下机器人轨迹规划与控制链路完整复现出来，最后看到仿真曲线和论文里的趋势对上的那一刻，说实话挺解压。这里说的不是简单“跑通代码”，而是把 AUV 路径规划这一块的全局路径生成，和 MPC 模型预测控制跟踪控制这一块的局部控制器，从模型、算法到 MATLAB 实现一步步重新搭出来，并让两者在同一个仿真环境里协同工作。这篇文章就写给准备啃这类复现任务、又不想在细节上踩太多坑的朋友，尤其是刚接触水下机器人、对 MPC 只停留在理论层面的读者。

复现一篇论文，最忌讳一上来就抄代码。把任务拆清楚，把每段代码背后的数学对象对应上，后面才能改参数、换工况、做对比实验。我会按我实际动手的顺序来讲，从拆论文、建模、规划、控制到联调，最后专门说几个折磨过我的坑。

1. 复现前的拆解：这篇论文其实是两个独立项目

1.1 为什么路径规划与跟踪控制必须分成两条线

我第一次看那篇论文时，觉得标题把“路径规划”和“MPC跟踪控制”放在一起，应该是一个端到端的完整系统。后来真正动手才发现，这两个模块在代码里几乎是完全解耦的：路径规划器负责在地图或连续空间里生成一条参考路径，输出是路径点序列；跟踪控制器只负责让 AUV 沿着这条参考路径走，它根本不关心路径是通过 A*、RRT* 还是人工势场得到的。

这个拆分非常关键。如果你把规划器和控制器揉在同一个脚本里，一旦仿真结果不对，你根本分不清是路径没规划好，还是控制器没跟踪好。我会在工程上先定义两个独立函数：planPath()和mpcController()，前者输入地图、起点、终点，输出参考路点；后者输入当前状态和参考路点，输出控制力和力矩。两个函数分别调试，都通过后再放进主循环。

1.2 从摘要里提炼必须复现的数学对象

论文摘要通常只给结论，但真正复现需要的是“数学对象清单”。我建议拿张表格把论文里的关键信息列出来，对照着逐项实现。我当时列的是这样：

这一步看起来简单，但能避免很多返工。我见过有人复现时把控制器的预测模型直接用了论文里的连续模型，没有做离散化，结果控制器输出一直在震荡。问题就出在“论文给的是微分方程，代码需要的是差分方程”这个转化上，而摘要里通常不会写这一层。

1.3 搭建一个“可复现”的 MATLAB 工程目录

复现不是写完一个脚本就完事。我习惯把工程按模块拆成目录，这样后期改参数、画图、对比实验都会轻松很多。结构大概是这样：

project_root/ main_sim.m params/ auv_params.m mpc_params.m map_params.m planner/ plan_path.m smooth_path.m controller/ mpc_setup.m mpc_step.m model/ auv_dynamics.m discretize_model.m utils/ plot_results.m normalize_angle.m

main_sim.m只做三件事：加载参数、初始化状态、跑循环。所有模块都通过函数接口通信，不要在脚本里到处复制代码。这个习惯在复现复杂系统时特别重要，因为你经常需要停下来检查某一个中间变量，如果代码都挤在一起，排查成本会翻倍。

2. AUV 运动学与动力学模型：复现的第一道坎

2.1 六自由度太复杂，三自由度恰好够用

水下机器人的完整模型是六自由度，三个位置自由度（x、y、z）加上三个姿态自由度（横摇、纵摇、艏摇），动力学里还有科氏力、水动力阻尼、浮力、重力等一大堆项。但绝大部分路径规划和水平面跟踪控制的顶刊论文，用的都是三自由度水平面模型：忽略深度变化，认为 AUV 在定深平面内运动，横摇和纵摇近似为零。只有当论文明确研究变深度任务时，才需要把垂向自由度加进来。

三自由度模型的状态通常取：

[ \eta = [x, y, \psi]^T,\quad \nu = [u, v, r]^T ]

其中 x、y 是大地坐标系下的位置，ψ 是艏向角，u 是纵向速度，v 是横向速度，r 是艏摇角速度。运动学方程是：

[ \dot{x} = u\cos\psi - v\sin\psi ] [ \dot{y} = u\sin\psi + v\cos\psi ] [ \dot{\psi} = r ]

动力学方程写成矩阵形式：

[ M\dot{\nu} + C(\nu)\nu + D(\nu)\nu = \tau + \tau_d ]

M 是包含附加质量的惯性矩阵，C 是科氏力矩阵，D 是阻尼矩阵，τ 是推进器输入，τ_d 是外界扰动（海流、未建模力）。复现时最稳妥的做法是先把 C 和 D 的次要耦合项忽略，只保留对角主导项，让系统先跑通，再加扰动看鲁棒性。

2.2 水动力参数怎么处理

水动力参数是论文里最容易被忽略、又最容易坑人的部分。很多论文的表格里会给 M 和 D 的具体数值，比如：

[ M = \begin{bmatrix} m-X_{\dot{u}} & 0 & 0 \ 0 & m-Y_{\dot{v}} & 0 \ 0 & 0 & I_z-N_{\dot{r}} \end{bmatrix} ]

这些附加质量系数如果你没有实验数据，就只能按论文给定值来。复现时遇到的一个常见问题是：论文没有给全所有参数。这时候不要自己瞎猜，可以先参考同类型 AUV 的公开模型参数，我一般用某经典欠驱动水下航行器的参数表作为兜底，然后根据论文仿真结果中的速度范围反推数量级是否匹配。

另一个容易被坑的是坐标系。导航坐标系（NED）和载体坐标系（body）之间的转换矩阵必须写对。很多人复现出来轨迹抖得很厉害，最后发现是运动学方程里 sin/cos 的位置写反了，或者 v 的符号没对齐。建议先用最简单的常速直线运动验证模型：给一个恒定的纵向推力，看 x 是否匀速增加、ψ 是否保持不变。这一步通过，再继续往下走。

2.3 建模错误导致的“假复现”

我管一种情况叫“假复现”：仿真曲线看着没问题，但本质是错的。最典型的例子是 MPC 控制器里用的预测模型和仿真模型不一致，导致控制器在模型失配下依然能工作，但一旦加扰动就崩。复现时一定要保证 MPC 内部的预测模型与实际仿真模型来自同一个函数，或者至少是同一个参数文件。

我之前踩过一次：仿真模型用的阻尼是线性的，MPC 预测模型里却写成了二次阻尼，结果小扰动下没区别，把海流扰动一加上，跟踪误差直接发散。这个问题的隐蔽性在于，你很难从一张图里看出“预测模型错了”，只能从理论推导和代码细节里排查。所以建模阶段多花半小时做模型一致性检查，比后期调参省一天。

3. 全局路径规划层：地图和路径平滑直接影响控制效果

3.1 栅格地图 vs 连续空间，怎么选

路径规划的第一步是明确规划空间。论文里常见两种：栅格地图和连续空间。栅格地图实现简单、可复现性强，适合做对比实验；连续空间（比如 RRT*）更接近真实环境，但随机性会让复现结果不稳定，每次跑出来的路径都不一样。

我复现的这篇论文用的是栅格地图上的 A* 算法，所以我先从 A* 入手。栅格地图的精度取决于栅格尺寸，一般来说栅格越小路径越精细，但计算量也越大。我习惯把地图尺寸设成 100m × 100m，栅格边长 1m，组成 100×100 的代价矩阵。障碍物区域直接设为无穷大，可通行区域设置一个基础代价，还可以根据任务需求给靠近障碍物的栅格加惩罚，让路径不要太贴边。

3.2 A* 作为基准路径生成器

A* 的核心是维护 open list 和 closed list，每次从 open list 里取 f = g + h 最小的节点扩展。MATLAB 里用结构体数组存节点，用循环实现扩展。下面是我常用的代码骨架：

function path = plan_path(map, start, goal) % map: 2D matrix, inf for obstacles % start, goal: [row, col] [rows, cols] = size(map); openList = struct('pos', start, 'g', 0, 'h', 0, 'f', 0, 'parent', -1); closedList = false(rows, cols); % 8-connected neighbors neighbors = [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; while ~isempty(openList) [~, idx] = min([openList.f]); current = openList(idx); openList(idx) = []; if isequal(current.pos, goal) % reconstruct path break; end closedList(current.pos(1), current.pos(2)) = true; for i = 1:size(neighbors, 1) npos = current.pos + neighbors(i, :); if npos(1) < 1 || npos(1) > rows || npos(2) < 1 || npos(2) > cols continue; end if closedList(npos(1), npos(2)) || isinf(map(npos(1), npos(2))) continue; end % diagonal movement cost if abs(neighbors(i,1)) + abs(neighbors(i,2)) == 2 stepCost = 1.414; else stepCost = 1; end g = current.g + stepCost; h = norm(npos - goal); f = g + h; % check if already in open list with lower f % ... update or add node end end end

实际运行时，open list 的节点查找和更新是性能瓶颈。如果地图是 100×100，纯 MATLAB 循环还能接受；再大就要考虑用二叉堆或者优先队列。不过复现论文，优先保证逻辑正确，性能优化放到后面再说。

3.3 路径平滑：从折线到可跟踪的参考轨迹

A* 直接输出的路径是折线，包含大量 45 度转弯和直角弯。如果把这样的路径直接喂给 MPC，控制器会频繁产生大幅转向，跟踪误差也会忽大忽小。所以必须在规划器和控制器之间加一个平滑模块。

我用的方法是三次样条插值加路径点重采样：

1. 把 A* 输出的路径点按顺序累积计算累计弧长。
2. 用csape或spline对 x、y 分别关于弧长做三次样条插值。
3. 在总弧长上等间距重采样，得到密化的路径点。
4. 对每个路径点分配期望速度，比如在转弯曲率大的地方降低期望速度，直道上恢复巡航速度。

平滑后的路径和时间参数化就变成了控制器的参考轨迹。这一步是论文里经常不细写、但在实际仿真中非常影响效果的地方。很多复现项目“路径规划成功但跟踪失败”，问题往往不在 MPC，而在参考路径本身有太多不可跟踪的突变点。

4. MPC 跟踪控制器的设计与求解：从公式到可运行代码

4.1 为什么跟踪控制选 MPC 而不是 PID

AUV 是典型的多输入多输出、强耦合、有约束的系统。PID 在简单工况下也能用，但它很难显式处理输入饱和约束和执行机构变化率约束。MPC 的优势在于：它把“未来一段时间内的跟踪误差最小化”变成一个在线优化问题，在每一步都考虑预测、约束和代价，然后只执行第一个控制量，下一时刻重新滚动。

用一个生活化的类比：PID 像你开车只看当前离车道线的距离，猛打方向修正；MPC 像你看着前方两百米的道路走势，提前规划方向盘转角，还不能超过方向盘最大转速。后者自然会开得更稳，也更符合 AUV 这种大惯性、慢响应的平台。

4.2 预测模型线性化和离散化

MPC 内部需要足够快的预测运算，一般不会直接用非线性模型做滚动优化，除非论文明确说用了 NMPC。那篇论文用的是线性时变 MPC，做法是先把三自由度模型在当前工作点线性化，再离散化。

状态取 ( x_k = [x, y, \psi, u, v, r]^T )，控制输入取 ( u_k = [\tau_u, \tau_r]^T )，也就是纵向推力和艏摇力矩。线性化参考点通常在期望速度 ( u_d ) 和零侧滑角处展开，得到连续状态空间形式：

[ \dot{x}(t) = A_c x(t) + B_c u(t) + d_c(t) ]

然后用零阶保持法离散化，采样周期我取 ( T_s = 0.1s )：

sys_d = c2d(ss(Ac, Bc, eye(6), zeros(6,2)), Ts, 'zoh'); Ad = sys_d.A; Bd = sys_d.B;

离散化的意义在于，预测模型里的每一步递推都能直接写成矩阵算子，不需要在每个预测步重新解微分方程。这也是 MPC 能在 MATLAB 里高效运行的原因。

4.3 代价函数、约束与权重设计

MPC 的优化问题可以写成：

[ \min_{u} \sum_{k=0}^{N-1} \left( e_k^T Q e_k + \Delta u_k^T R \Delta u_k \right) + e_N^T P e_N ]

其中 ( e_k = x_k - x_{ref,k} ) 是状态跟踪误差，N 是预测时域，Q 是状态权重矩阵，R 是控制增量权重矩阵，P 是终端代价矩阵。终端代价的作用是保证稳定性，复现时可以用离散代数黎卡提方程求解 P，也可以简单取一个比 Q 大几倍的对角矩阵。

约束至少要有：

• 控制输入饱和：( |\tau_u| \leq \tau_{u,max} )，( |\tau_r| \leq \tau_{r,max} )
• 控制增量约束：( |\Delta \tau| \leq \Delta\tau_{max} )，对应推进器响应速度限制
• 状态输出约束（可选）：如速度不超过某上限，或到障碍物距离不小于安全值

权重的整定经验是：Q 中位置误差（x、y）权重最大，航向误差次之，速度误差可以小一些；R 不要设得太小，否则控制量抖动剧烈，真实推进器受不了。初始值我一般设：

Q = diag([10, 10, 5, 1, 1, 1]); R = diag([0.5, 0.5]);

然后再根据仿真结果微调。

4.4 MATLAB 里的 MPC 求解实现

在不使用 Model Predictive Control Toolbox 的情况下，我常用quadprog把 MPC 每一步的优化问题转成标准二次规划。先把预测模型写成批量矩阵形式：

[ X = \Phi x_0 + \Psi U ]

其中 X 是未来 N 步的状态序列，U 是未来 N 步的控制增量序列。代价函数展开后得到标准二次型：

[ J = \frac{1}{2} U^T H U + g^T U ]

然后调quadprog求解：

options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex'); [U_opt, ~, exitflag] = quadprog(H, g, A_ineq, b_ineq, [], [], lb, ub, [], options);

注意这里的 U_opt 是未来 N 步的控制增量序列，但 MPC 只取第一步用于实际控制，下一步重新求解。这个“滚动”过程就是整个控制器的核心循环。我建议把批量矩阵的构造放在仿真循环之前完成，在线只做quadprog求解，能显著降低每步耗时。

一个常见的坑：quadprog在半正定 H 矩阵下不一定收敛，所以 R 矩阵不能设成零。另一个坑是变量缩放，位置是以米为单位，输入是以牛为单位，两者数量级差很多，如果不做归一化，优化器很容易因为数值病态而求解失败。我会先把状态和控制输入按最大值归一化，求解后再反归一化回物理单位。

5. 闭环联调：规划器输出的路点，MPC 能不能跟得上

5.1 串接整个仿真链路

每个模块单独跑通后，我开始把它们串起来。整个仿真主循环的数据流是：

1. 路径规划器生成平滑参考路点 ( [x_{ref}, y_{ref}, \psi_{ref}, u_{ref}] )。
2. 在当前时刻，从参考路径中插值得到当前参考状态 ( x_{ref}(k) )。
3. MPC 根据当前状态和参考状态计算控制量 ( \tau_u, \tau_r )。
4. AUV 动力学模型接收控制量，更新下一时刻状态。
5. 记录状态和误差，进入下一步。

我建议先用纯 MATLAB 循环跑通，这个循环的代码量不大，而且每一步的状态都可以打印检查。等确认控制效果合理后，再考虑要不要搬到 Simulink 做更复杂的可视化。这里有一个容易犯的错误：参考路径的索引和采样时间没对齐，导致控制器的参考状态突然跳变，跟踪误差出现脉冲尖峰。解决办法是让规划器输出一个连续可插值的时间序列，控制器按当前仿真时间查表取参考值。

5.2 仿真步长与离散化一致性

复现时要特别留意三个时间尺度：

物理步长必须远小于控制周期，否则动力学积分不准确。MPC 预测模型里的离散化步长要和控制周期一致，不能一边用 0.1s 预测、一边用 0.05s 执行。路径重规划大多数场景下一次就够了，如果环境动态变化，可以把重规划周期设成几秒一次，但每次重规划后要平滑衔接，避免参考轨迹突变。

5.3 参数整定的实际手感

MPC 参数多，整定起来要有顺序。我的经验是分三步走：

• 第一步只调 Q 的位置权重，把 R 固定，让跟踪误差先减小，观察有没有超调。位置权重过大会导致控制量接近饱和。
• 第二步调 R 的控制增量权重，抑制控制量的高频抖动。R 太小，推力会像噪声一样高频变化；R 适中，推力曲线会平滑很多。
• 第三步调预测时域 N。N 太小，MPC 只看得到眼前几步，容易出现振荡；N 太大，计算量变大，而且远离当前状态的预测精度下降。折中取值通常在 10 到 30 之间。

这个过程中要盯着两个指标：横向跟踪误差（路径上每个点对应的法向偏差）和航向误差。理想情况下两者都收敛到零附近，有扰动时能在 3 到 5 秒内恢复。如果横向误差消不掉，先检查参考路径的速度分配，很多情况下是期望速度设得太高，超出了推进器能力范围。

5.4 结果怎么看才算复现成功

不要只看一个“轨迹贴着路径走”的图。我复现时至少画三张图：

• 平面轨迹图：路径与 AUV 实际轨迹的对比，直观看出跟踪效果。
• 跟踪误差曲线：x、y、ψ 三个通道的误差随时间变化。
• 控制输入曲线：两个控制量是否频繁饱和、是否抖动。

论文里通常会给平均跟踪误差和最大误差，我用同等工作量算出来后，只要数量级一致，就算复现成功。比如论文说平均横向误差 0.3m 到 0.5m，我复现出来 0.2m 到 0.6m，这基本是合理的；如果差了一个数量级，那一定是某个环节的参数不对，不是运气问题。

6. 复现中真正折磨人的坑：单位、初始猜测与约束归一化

6.1 单位不统一是“隐形杀手”

AUV 领域经常混用国际单位制、论文自定义单位和坐标系单位。我在一次调试中遇到过位置单位被写成“格数”、速度单位却是“米/秒”的离谱情况，导致 MPC 参考值完全对不上。复现的第一步就应该把论文里所有物理量换算成标准单位，尤其是地图栅格和米之间的换算。如果地图是 1m 栅格，那每个格点的位置乘上栅格尺寸就是实际坐标；如果漏乘，跟踪误差直接大几十倍。

6.2 初始可行解对 MPC 求解的影响

quadprog求解时如果不给初始可行解，某些内点法实现会从默认初始点开始迭代，在约束较强时可能求不到最优解。我建议每次滚动求解时，把上一次的 U_opt 作为下一次的初始猜测传入：

[x_opt, ~, ~] = quadprog(H, g, A_ineq, b_ineq, [], [], lb, ub, prev_u, options); prev_u = x_opt;

这个小改动对求解速度和稳定性提升很明显。还有一点，如果某一步求解失败（exitflag 不为正数），不要继续用上一次的控制量硬跑，应该做一次保守处理，比如把控制增量归零，等下一次求解恢复。真实系统里这叫“控制保护”，复现时也能帮你定位到底是模型问题还是求解器问题。

6.3 验证“复现成功”的标准必须量化

我见过不少复现项目，最后只说“能跑”，但拿不出量化指标。顶刊论文通常有实验对比表格，比如不同场景下的平均跟踪误差、规划算法耗时、与基线方法的优劣比较。复现时至少要做到：

• 与论文中某个典型仿真场景做数值对比；
• 复现控制器的平均误差与论文同量级；
• 加一组论文没做的扰动实验，证明控制器鲁棒性。

做到这三条，才算真正把论文里的方法“消化”了，而不只是把代码跑通。如果论文没给全参数，我会在代码里把所有猜测的参数集中放在一个配置文件中，并加注释说明依据，方便后续调整。

最后再分享一个小技巧：我每次跑完整的复现仿真前，都会先用一个简化场景做“冒烟测试”——直线路径、无扰动、初始状态和参考路径偏差很小。如果这个场景都不能通过，说明某个基础环节有问题，没必要立刻上复杂地图。等直线跟踪稳定之后，再把地图换成论文里的障碍物环境、加上海流扰动、调大初始偏差，一步步逼近论文的实验条件。这样排查问题会快很多，也不会因为一堆现象同时出现而手忙脚乱。复现的价值不在于把别人的结果抄一遍，而是通过重走整个推导和实现过程，真正理解每个模块为什么这么设计。等你能在论文没写全的细节里补出自己的合理假设并跑通结果，这篇论文在你眼里就没有秘密了。