企业官网建设流程全解析

做欠驱动AUV的轨迹跟踪和路径跟随仿真，最头疼的事情往往不是数学推导本身，而是你手上同时拿着Matlab脚本和Simulink模型两套工具，却搞不清楚“哪种问题该用哪种分析方法”。我在这上面反复折腾过很久，踩过不少坑，也总结出一些可以少走弯路的经验，这篇内容就是把“欠驱动AUV”的轨迹跟踪和路径跟随问题从模型搭建、算法设计、代码实现到Simulink仿真验证的完整链路拆开讲一遍。尤其想给正在做课设、毕设或者刚开始入门水下机器人控制方向的朋友们，提供一个可以直接对着抄作业的参考框架。

先说清楚这篇文章适合谁。如果你对AUV的运动学动力学还停留在“知道有6个自由度”的阶段，那本文从数学模型开始会帮你慢慢梳理；如果你已经写过一版简单的PID轨迹跟踪代码、但效果始终不太理想，那反步法和滑模控制的实现细节应该能让你找到问题所在；如果你正准备把算法从纯Matlab脚本迁移到Simulink环境、但不知道怎么搭模块和配参数，本文对应章节也给了具体的搭建思路。总之，这篇内容不是一篇纯概念科普，而是带着“拿来就能跑”的仿真逻辑写的。

这里要先把一种常见的误解纠正掉：轨迹跟踪和路径跟随，这两个词在AUV控制里是两类不同的问题，解法逻辑也有本质差别。很多人论文里把两者混着写，但真到了仿真阶段，设定不一样、控制器结构不一样、评价指标也不一样。所以本文会先讲清楚两者的核心差异，再分别给出可落地的算法设计方法，最后回到Matlab代码和Simulink模型这两套实现路径上做对比验证。

1. 欠驱动AUV的数学本质与建模要点

1.1 欠驱动到底“欠”在哪里

AUV在水下运动通常被描述为6自由度空间中的运动，包括沿三个坐标轴的线运动和绕三个坐标轴的角运动。理想情况下，如果一个航行器在每个自由度上都有对应的推力器或舵面提供控制力，它就是一个全驱动系统。但实际工程中，为了降低功耗、减少阻力、压缩成本，绝大多数AUV并不会在每个自由度上都安装推进器。

以最常见的水平面运动为例，AUV一般只在纵向（前进方向）配备螺旋桨提供推力，在偏航方向配备艉舵或差动推进提供转艏力矩，而横荡方向（侧向）往往没有任何直接执行机构。这就是“欠驱动”的本质：控制输入的数量少于被控自由度的数量。一个典型的欠驱动水平面模型有3个自由度，但只有2个控制输入：纵向推力τ_u和转艏力矩τ_r。你没法直接给这个AUV一个“横向推力”，让它像全向移动机器人那样横着挪动。这意味着系统的横荡速度是由纵向速度、偏航角速度和水动力耦合间接产生的，控制设计只能借助这种耦合关系来完成期望运动。

这里可以用一个特别生活化的类比帮助你建立直觉：欠驱动AUV很像你在停车场倒车入库的场景。你的车不能直接横着平移，只能靠油门和方向盘配合，前后移动中不断转向来实现侧向位置的调整。AUV的水平面欠驱动控制思路在本质上和这个很像——利用纵向推力和转艏力矩的组合，让航行器在向前/向后运动的过程中逐渐修正横向偏差。

1.2 数学模型怎么搭——从Fossen方程到仿真状态方程

做Matlab和Simulink仿真，第一步是把AUV的数学模型落实到代码上。如果一上来就建6自由度的完整模型，水动力系数一大堆，还没开始调控制器就先被参数搞崩溃了。我个人的建议是：先做水平面3自由度简化模型，把控制算法的逻辑跑通之后，再根据需要往上扩展。

水平面欠驱动AUV的运动学方程如下：

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

其中(x,y)是航行器在惯性坐标系下的位置，(\psi)是偏航角，(u)是纵向速度（随体坐标），(v)是横荡速度，(r)是偏航角速度。

动力学方程用Fossen模型进行简化：

[ m_{11}\dot{u} = m_{22}vr - d_{11}u + \tau_u + w_u ] [ m_{22}\dot{v} = -m_{11}ur - d_{22}v + w_v ] [ m_{33}\dot{r} = (m_{11} - m_{22})uv - d_{33}r + \tau_r + w_r ]

这里的(m_{11}, m_{22}, m_{33})是包含附加质量的惯性系数，(d_{11}, d_{22}, d_{33})是水动力阻尼系数，(\tau_u)和(\tau_r)是控制输入，(w_u, w_v, w_r)是外界扰动的简化表达。

注意一个关键点：在第二个方程里，横荡方向没有控制输入(\tau_v)，这就是欠驱动约束的数学体现。横荡速度(v)本质上是被前两个通道的状态耦合间接驱动起来的。很多初始版本的控制设计失败，问题不在于增益没调好，而是设计者没有理解“横荡速度不能直接控制，它只能被间接影响”这个约束条件。

在Matlab代码里，我习惯将状态向量写成(\mathbf{x} = [x, y, \psi, u, v, r]^T)，然后在一个函数文件（AUV_model.m）中定义状态的导数。这样无论是后面用ode45做脚本仿真，还是用S-Function接入Simulink，模型部分代码都可以复用，不用重写。

1.3 轨迹跟踪与路径跟随：核心差异与仿真设定

轨迹跟踪（Trajectory Tracking）和路径跟随（Path Following）在数学设定上的区别，我用一个生活化的场景来展开。想象你要去接女朋友/男朋友下班：轨迹跟踪就像你必须时刻保持“跟她同步行走”，每个时刻她走到哪里，你也必须在哪里；而路径跟随则更像是“她要求你沿着某条固定的路线走到一个指定地点”，至于你中途走快一点还是慢一点，并不会影响“你是否沿着这条路线走”这件事本身。

进一步从数学角度拆解这个差异：

• 轨迹跟踪的参考量是与时间相关的三维/二维信号(\mathbf{q}_d(t) = [x_d(t), y_d(t), \psi_d(t)]^T)。控制器必须在每一个时刻跟踪这个时变参考，任何时间上的滞后都会被视为跟踪误差。因此仿真中通常会评估时间相关的跟踪误差，并要求初始误差尽量小，否则容易产生较大超调甚至失稳。
• 路径跟随的参考量是一条与时间无关的几何路径，比如直线(\mathcal{P} = {(x_p(s), y_p(s)) | s \in [0, s_f]})。控制器要解决的核心问题是让AUV恢复到这条路径上，并且沿路径方向以期望速度前进。它不需要严格匹配某个时间表，因此系统有时间余量去修正初始横向偏差，收敛过程也通常更平缓。

下表整理了两者在仿真设计中的关键差异：

在做仿真研究时，我强烈建议把这两类问题分开建模、分开设计控制器，即便你最后写论文时会把它们放在一章里对比分析。因为将一套控制器直接从一个场景搬到另一个场景，往往结果很差。你在轨迹跟踪中精心调节的反步法控制器，直接拿到路径跟随场景未必能快速消除大的横向偏差；反过来，LOS引导律用在轨迹跟踪场景中，也难以满足严格的时域要求。

2. 不同控制策略的对比分析与选型逻辑

2.1 反步法（Backstepping）的级联结构与设计步骤

反步法是欠驱动AUV轨迹跟踪控制中最常用的方法之一，几乎可以说是这类问题的“标准答案”。它的设计思想基于系统的级联（cascade）结构：系统可以拆成“位置运动学子系统”和“速度动力学子系统”两部分，控制器先设计虚拟控制量来稳定外层相对简单的位置环，再反推回内层速度环设计真实控制输入。

以水平面轨迹跟踪为例，假设期望轨迹为(\mathbf{q}_d(t) = [x_d(t), y_d(t)]^T)，定义位置跟踪误差：

[ e_x = x - x_d, \quad e_y = y - y_d ]

为了避免直接对(e_x, e_y)求导时出现非线性的三角函数耦合，实际操作中更推荐将误差转换到艇体坐标系下：

[ e_1 = e_x\cos\psi + e_y\sin\psi ] [ e_2 = -e_x\sin\psi + e_y\cos\psi ]

这里(e_1)是纵向位置误差，(e_2)是横向位置误差。这个坐标变换是反步法设计中很关键的一步，很多新手直接对惯性坐标系下的误差做反步设计，结果推导出的控制量和系统实际动态不匹配，仿真就容易发散。

接下来引入虚拟控制量(\alpha_u)和(\alpha_r)。(\alpha_u)用来稳定纵向位置误差通道，可以设计为：

[ \alpha_u = u_d\cos e_{\psi} - k_1 e_1 ]

其中(u_d)是期望纵向速度，(e_{\psi} = \psi - \psi_d)是偏航误差。而(\alpha_r)的设计要稳定横向误差通道，通常采用：

[ \alpha_r = -k_2 e_2 + \dot{\psi}_d ]

之后的步骤是定义速度误差变量(\tilde{u} = u - \alpha_u)，(\tilde{r} = r - \alpha_r)，再对速度环动力学设计真实的控制输入(\tau_u)和(\tau_r)。最终控制律中会包含期望轨迹的二阶导数项、模型中的科里奥利项和阻尼项，以及若干整定增益(k_1, k_2, k_3, k_4)。设计过程中每一层都要对应一个Lyapunov函数，确保整个闭环系统的稳定性可以由多个二次型项叠加证明。

在Matlab实现中，反步控制律的代码结构大致如下：

function [tau_u, tau_r] = backstepping_controller(state, ref, K) % state: [x; y; psi; u; v; r] % ref: 期望状态结构体，包含 xd, yd, xd_dot, yd_dot, xd_ddot, yd_ddot x = state(1); y = state(2); psi = state(3); u = state(4); v = state(5); r = state(6); % 期望偏航与期望偏航速率 psi_d = atan2(ref.yd_dot, ref.xd_dot); psi_d_dot = (ref.xd_dot * ref.yd_ddot - ref.yd_dot * ref.xd_ddot) / (ref.xd_dot^2 + ref.yd_dot^2); % 位置误差（惯性系 -> 艇体系） ex = x - ref.xd; ey = y - ref.yd; e1 = ex * cos(psi) + ey * sin(psi); e2 = -ex * sin(psi) + ey * cos(psi); epsi = psi - psi_d; % 期望纵向速度（可根据轨迹动态给定） ud = sqrt(ref.xd_dot^2 + ref.yd_dot^2); % 虚拟控制 alpha_u = ud * cos(epsi) - K(1) * e1; alpha_r = -K(2) * e2 + psi_d_dot; % 速度误差 u_tilde = u - alpha_u; r_tilde = r - alpha_r; % 最终控制律（这里的模型参数需要从全局或参数结构体读取） tau_u = m11 * (d_alpha_u - m22/m11 * v * r + d11/m11 * u) - K(3) * u_tilde; tau_r = m33 * (d_alpha_r - (m11-m22)/m33 * u * v + d33/m33 * r) - K(4) * r_tilde; end

代码中(d_alpha_u)和(d_alpha_r)是虚拟控制量的导数，工程上可以用解析求导，也可以用数值差分近似代替。我个人建议先用数值差分快速验证逻辑是否通顺，再回到纸上进行解析推导，因为解析推导一旦符号出错，查错成本很高。

2.2 滑模控制（SMC）的抗扰优势与抖振处理

滑模控制在欠驱动AUV路径跟随中的应用也非常常见，尤其是当外部海流扰动不可忽略时，滑模的鲁棒性优势很明显。滑模设计包含两个阶段：设计滑模面使系统状态在滑模面上满足期望的动态特性，以及设计趋近律使得状态在有限时间内到达滑模面。

这里以横向位置误差的收敛问题为例。对于路径跟随，我们可以定义滑模面为：

[ s = \dot{e}_y + \lambda e_y ]

其中(e_y)是到参考路径的横向偏差，(\lambda)是滑模面参数，它直接决定了状态在滑模面上收敛的快慢。(\lambda)越大收敛越快，但过大也会放大噪声并增加控制量峰值。

趋近律可以设计为等速趋近律：

[ \dot{s} = -\eta \cdot \text{sgn}(s) ]

其中(\eta)是趋近增益。这里的(\text{sgn})符号函数会带来高频抖振（chattering），这在实际系统和仿真中都表现为控制量在高频振荡。如果直接把这个信号输入给执行机构模型，仿真步长不够小的话很容易出现数值抖动甚至发散。

解决抖振的经典办法是把符号函数替换成饱和函数或连续化近似：

[ \text{sat}(s) = \begin{cases} \text{sgn}(s), & |s| > \Phi \ s/\Phi, & |s| \leq \Phi \end{cases} ]

(\Phi)是边界层厚度。这个参数很关键：(\Phi)太小，抖振抑制效果差；(\Phi)太大，滑模控制等同于高增益比例控制，鲁棒性下降。我仿真中常用的经验值是(\Phi = 0.02\sim0.05)，具体需要配合控制量限幅来整定。

2.3 LOS引导律：路径跟随的核心引导方法

视线法（Line-of-Sight，LOS）之所以在路径跟随中几乎成为默认标配，是因为它提供了一种清晰直观的偏航角指令生成方式。假设AUV当前位置为((x,y))，当前跟踪的目标点为路径上的期望点((x_p(s), y_p(s)))，定义一个前视距离(\Delta > 0)，则期望偏航角为：

[ \psi_{LOS} = \text{atan2}(-e_y, \Delta) ]

其中(e_y)是横向偏差。这个公式的几何含义很直观：如果AUV在路径右侧（(e_y > 0)），期望偏航角就指向左侧，引导航行器朝路径修正回来；前视距离(\Delta)越大，期望偏航角越接近路径方向，修正越平缓；(\Delta)越小，航行器越“急切”地朝路径点转向，收敛越快但振荡也越明显。

在仿真中，我通常会同时引入一个与路径切线方向对齐的前馈偏航指令(\psi_p(s))，最终的期望偏航角为：

[ \psi_d = \psi_p(s) + \text{atan2}(-e_y, \Delta) ]

这里(\psi_p(s))是参考路径在当前参数(s)处的切线方向。如果不加这个前馈项，AUV在直线路径上会先大幅度摆向切线方向，再慢慢修正横向偏差，收敛过程会多出一段无效机动。我见过很多LOS代码写成只补偿横向偏差、完全不管路径方向变化的版本，在曲线路径跟踪时跟踪误差大得离谱，问题就出在这里。

2.4 算法选型建议：什么场景用什么方法

做仿真的最终目标是服务实际应用或者论文论证，不是把算法堆得越复杂越好。结合我自己的仿真经验，下表给出了一个基于不同场景的选型参考：

另外提一句，如果你的场景同时需要轨迹跟踪和路径跟随能力，我建议不要强行用一个控制器包打全场。更好的做法是设计一个混合框架：当横向误差较大时切换为路径跟随模式先将航行器拉回参考走廊，误差较小时再切换回轨迹跟踪模式进行精确跟踪。这个思路在Simulink中实现起来并不复杂，只需要在状态机里根据误差阈值做切换。

3. Matlab代码实现的核心细节与参数整定

3.1 仿真主循环怎么写——一个可以直接跑的框架

许多新手拿到AUV模型后第一反应是“用Simulink拉一个模型”，但我个人强烈建议先在Matlab脚本里用数值积分跑通整个流程。因为脚本方式更方便打印中间变量、快速调节参数、用绘图函数逐帧分析问题，比在Simulink里拖拉模块再调试要快得多。

一个最朴素的仿真主循环框架如下：

% 初始化参数 Params = AUV_parameters(); % 包含模型参数和控制器增益 dt = 0.01; % 仿真步长 T_end = 100; % 仿真时长 t = 0:dt:T_end; N = length(t); % 状态初始化 state = zeros(6, N); state(:,1) = [0; -2; 0; 0.5; 0; 0]; % 初始位置在参考轨迹外侧，验证收敛能力 % 预分配记录数组 tau_u_rec = zeros(1, N); tau_r_rec = zeros(1, N); for k = 1:N-1 ref = trajectory_reference(t(k)); % 计算期望轨迹/路径参数 [tau_u, tau_r] = backstepping_controller(state(:,k), ref, Params.K); % 控制量限幅 tau_u = max(min(tau_u, Params.tau_u_max), -Params.tau_u_max); tau_r = max(min(tau_r, Params.tau_r_max), -Params.tau_r_max); % 使用ode45做单步积分，返回下一步状态 [~, x_temp] = ode45(@(t, x) AUV_model(t, x, tau_u, tau_r, Params), [0 dt], state(:,k)); state(:, k+1) = x_temp(end, :)'; tau_u_rec(k) = tau_u; tau_r_rec(k) = tau_r; end

这里用ode45做单步积分，本质上等价于一个变步长的离散递推，比直接使用欧拉积分要稳。欧拉积分在AUV模型的强非线性作用下，如果步长取大了很容易出现数值不稳定，尤其是横荡速度和偏航角速度的耦合项在小初始状态下有可能被数值误差放大。

3.2 增益调节的经验法则

反步法或滑模控制器中有多个可调增益，有的新手一上来就把所有增益都调得很大，认为“增益越大误差收敛越快”，仿真结果往往是系统发生剧烈振荡甚至发散。这里有一个基本的整定逻辑：

• 先固定内层增益（速度环），从较小的值开始，逐步增大直到跟踪响应不再明显变快；
• 再调外层增益（位置环），从内层增益的1/3~1/2开始，避免内外环带宽重叠过多；
• 如果系统出现持续振荡，优先减小位置环的增益，而不是继续加阻尼项。

以我的测试经验，当AUV模型参数在实验室小型AUV量级（质量约30kg，长度约1.2米）时，位置环增益(k_1, k_2)在0.5~1.5之间，速度环增益(k_3, k_4)在1~3之间，是比较合理的起点。滑模中的趋近增益(\eta)从0.1到0.5之间尝试，(\lambda)从0.5到2之间调。

3.3 仿真结果怎么评价——误差积分与性能指标

评价AUV跟踪效果，除了画轨迹图，更科学的做法是计算累积误差指标。我常用的几个指标包括：

[ \text{IAE} = \int_0^T |e(t)| dt \quad \text{（绝对误差积分）} ]

[ \text{ITAE} = \int_0^T t |e(t)| dt \quad \text{（时间加权绝对误差积分）} ]

ITAE对后期的稳态误差更加敏感，能够反映出控制器是否真正消除了长尾偏差。如果你的两个算法对比结果在轨迹图上“肉眼看差别不大”，但ITAE数值相差很大，那说明一个控制器可能在快速收敛后残留了小稳态误差，而另一个则始终稳定贴合参考。这种情况在论文写作中也是很有价值的分析切入点。

4. Simulink仿真框架搭建与联合验证

4.1 S-Function还是纯Simulink模块？我的选择标准

很多初学者纠结于Simulink建模时该用S-Function还是用纯模块搭建。我的经验是：如果模型本身包含大量耦合的非线性动力学方程，直接用纯Simulink模块搭建会非常繁琐，调试也不方便。比如(m_{22}vr - d_{11}u)这种耦合项，用模块搭虽然可行，但一旦需要修改模型结构，就要重新梳理线和模块。更推荐的做法是使用Level-2 MATLAB S-Function来承载动力学模型，把控制器部分用纯Simulink模块或另一个S-Function实现。

这样做的好处是模型代码和控制器代码分离，如果你要切换不同控制器做对比，只需要在Simulink中更换控制器S-Function或Controller模块即可，无需改动底层模型的任何连线。这也是我做“不同分析方法对比仿真”时的基本架构方案。

S-Function的基本模板如下：

function AUV_sfun(block) setup(block); end function setup(block) block.NumInputPorts = 2; % 控制输入 tau_u, tau_r block.NumOutputPorts = 1; % 状态向量 [x; y; psi; u; v; r] block.SetPreCompPortInfoToDefaults; block.NumContStates = 6; block.SampleTimes = [0 0]; % 连续采样时间 block.RegDlgParams.Name = 'Params'; block.DialogPrMasks = {'struct'}; block.SimStateIntegrationMethod = 'ForwardEuler'; block.SetAcceleratorRunOnTLC(false); block.SimStateIntegrationMethod = 'Internal'; block.RegBlockMethod('InitializeConditions', @InitConditions); block.RegBlockMethod('Derivatives', @Derivatives); block.RegBlockMethod('Outputs', @Outputs); end function InitConditions(block) block.ContStates.Data = [0; -2; 0; 0.5; 0; 0]; end function Derivatives(block) x = block.ContStates.Data; tau_u = block.InputPort(1).Data; tau_r = block.InputPort(2).Data; xd = AUV_model(0, x, tau_u, tau_r, block.DialogPrm(1).Data); block.Derivatives.Data = xd; end function Outputs(block) block.OutputPort(1).Data = block.ContStates.Data; end

在编写S-Function时，有一个坑必须提醒：S-Function中的连续状态会由Simulink的求解器进行积分，这里模型中的AUV_model函数里不要自己再写一次积分器。很多新手在Derivatives回调里把(x)积分了一下再输出，结果就是状态被重复积分，仿真结果完全错误。

4.2 Simulink模型结构怎么搭才算清晰

一个清晰可维护的Simulink模型应该包含下面几个核心子系统：

• Reference Generator（参考轨迹生成子系统）：根据时间或者路径参数(s)生成期望位置、速度及加速度。这里可以用MATLAB Function Block实现，也可以直接用Clock信号带入S-Function。
• Controller（控制器子系统）：输入当前状态和参考信号，输出控制量(\tau_u, \tau_r)。里面可以嵌入S-Function或者MATLAB Function实现反步/滑模/LOS等不同算法。
• AUV Dynamics（AUV动力学模型）：用S-Function实现，输入控制量，输出6维状态。
• Saturation与限幅模块：控制量限幅是必须加的，不然滑模控制器很容易把控制量推到几十上百牛，远超实际执行机构的物理能力。
• Scope/To Workspace（数据记录模块）：把状态和控制量记录到工作区，用于后续绘图和分析。

模块之间的信号线建议都用总线（Bus）组织，避免乱七八糟的线条交叉。Simulink中总线信号虽然初次理解有些门槛，但一旦用习惯，模型的可读性会提升一个量级。

4.3 求解器配置与参数对拍

Simulink连续模型的求解器配置直接影响仿真精度和速度。欠驱动AUV模型包含快速动态和慢速动态的耦合，我通常使用ode45（默认）配合Max step size为0.01s。如果滑模控制中频繁触发符号函数切换，建议改为ode15s刚性求解器，否则可能因为太小的积分步长而把仿真时间拖到不可接受。

还有一个容易被忽视的点：使用S-Function时，如果模型中有连续状态和MATLAB Function之间循环代数环，Simulink会报编译错误。这时候需要在控制器输出加一个Unit Delay模块来打破代数环。代数环问题在AUV仿真中经常出现，尤其是控制器用到状态的微分时。我给控制器输出加一个非常小的惯性环节（一阶低通滤波），既能打破代数环，又能模拟实际执行机构的动态特性，一举两得。

比较规范的“对拍”流程是：先用Matlab脚本仿真得到一组结果，保存为mat文件；然后用Simulink模型跑同样的场景，将To Workspace输出的结果与mat文件中的数据做误差对比。如果两组结果的轨迹误差在数值积分误差范围内一致，那么就可以确信你的Simulink模型和脚本模型在数学上是等价的，后续可以放心用Simulink做更复杂的场景扩展。对拍这一步非常重要，否则你无法判断Simulink调试中出现的怪异现象是控制器问题还是建模差异。

5. 常见问题与排查技巧实录

5.1 轨迹振荡、超调明显

现象：AUV沿着参考轨迹走的时候，位置曲线在参考线两侧来回摆动，幅度先大后小，像阻尼不足的弹簧。 排查思路：首先检查位置环增益是否过大。反步法中(k_1, k_2)与虚拟控制量的收敛速度直接相关，增益增大虽然让位置误差快速归零，但代价是期望偏航角速度和期望纵向速度被推到过大，产生超调。解决方法是把位置环增益减小到原来的1/3左右，再观察是否改善。 另一个可能原因是前视距离(\Delta)选得过小。路径跟随场景中，LOS期望偏航角会在横向偏差为零时依旧保留一个较大转向角度，导致转弯惯性把航行器推向另一侧。将(\Delta)增大到艇长的2~3倍，振荡现象往往明显缓解。

5.2 AUV在原地打转或偏航角快速旋转

现象：仿真过程中(\psi)值不断快速变化，轨迹出现螺旋甚至原地画圈。 排查思路：这是偏航角数据处理不当的经典问题。Matlab中角度绕圈时，atan2的输出范围为([-\pi, \pi])，但期望偏航角通常也被限制在这个区间。如果你直接使用(e_\psi = \psi - \psi_d)，当两者跨越(\pm\pi)边界时，误差会突然跳到接近(2\pi)，控制器就会误判需要猛烈转艏，导致打转。解决方法是把角度误差归一化到([-\pi, \pi])区间：

epsi = wrapToPi(psi - psi_d);

这一行代码看起来简单，但在实际仿真中能解决一半以上的“AUV原地画圈”问题。

5.3 收敛速度极慢，误差迟迟不减小

现象：轨迹跟踪中AUV能跟上参考形状，但总是滞后一大截，误差随时间推移始终无法降下来。 排查思路：这大概率是因为期望速度的生成逻辑有问题。在轨迹跟踪中，如果期望纵向速度(u_d)始终设为常数，而参考轨迹曲率很大，那么AUV在弯道处就会累积较大的位置滞后。正确的做法是让期望速度与轨迹曲率关联：曲率大时降低期望速度，曲线回到直道之后恢复巡航速度。一个简单的实现是：

[ u_d(t) = \frac{u_{max}}{1 + \beta |\kappa(t)|} ]

其中(\kappa(t))是参考轨迹的曲率，(\beta)是调节参数。这样虽然牺牲了一点平均速度，但跟踪误差会显著下降。

5.4 Simulink仿真速度极慢，甚至长时间无响应

现象：模型能运行，但仿真1秒需要真实时间好几分钟，或者步长被压缩到极小。 排查思路：最可能的原因是滑模控制器里的符号函数切换导致事件频繁触发，变步长求解器因此不断重试积分。解决办法是先把符号函数改为饱和函数，将积分器类型改回固定步长（如ode4，步长0.001s），再观察仿真速度。固定步长虽然在精度上略逊于变步长，但对工程调试和对照组实验来说已经足够。

5.5 参数完全相同，Matlab脚本和Simulink结果却对不上

现象：同一个控制器、同一组参数，脚本仿真稳定收敛，Simulink仿真发散或者误差很大。 排查思路：优先检查S-Function的状态初始化是否与脚本一致，特别是初始位置偏差和初始速度。很多对拍不一致是因为Simulink中的状态初始值忘记设置为脚本中非零初始状态，导致起点不同，动态过程自然不同。 另一个常见原因是S-Function中使用了全局变量或persistent变量存储状态，而Simulink在多次运行时会保留这些persistent变量，第二次运行用的还是上一次的残留值。解决方法是把persistent变量都清除，或者在模型初始化回调中重置。

5.6 问题速查表

结尾

最后分享一点个人体会。欠驱动AUV的轨迹跟踪和路径跟随仿真，真正考验人的地方不在“会不会写控制器公式”，而在于你能否把模型、算法、代码、仿真环境四者串成一条互相验证的链路。我最早做这个方向的时候，光是调试偏航角误差的跨边界问题就耗了将近一周，后来发现一个小小的wrapToPi就能解决。希望这篇内容能帮你把那些看似玄学的问题变成可以定位、可以复现、可以解决的工程问题，也让Matlab和Simulink真正成为你研究AUV控制算法的有力工具，而不是反复折腾你的耐心。