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

简介：自动水下航行器（AUV）是海洋探索的关键装备，而欠驱动AUV因动力系统受限，仅能直接控制部分运动自由度，具有更高的控制复杂性。本文深入解析欠驱动AUV的六自由度数学模型，涵盖前后、左右、上下平动及滚动、俯仰、偏航旋转自由度，基于牛顿-欧拉方程构建非线性动力学模型，并结合源码详细讲解滑模控制、自适应控制等策略的实现方法。同时介绍传感器数据处理、卡尔曼滤波状态估计、故障检测与容错机制的设计，以及仿真验证与硬件接口集成。本项目经过完整测试，适用于AUV控制系统开发学习与工程实践，助力掌握水下机器人核心建模与控制技术。

1. 欠驱动AUV六自由度运动模型概述

1.1 欠驱动AUV的运动特性与建模意义

自主水下航行器（AUV）在海洋探测中通常面临推进器配置受限的问题，导致其控制自由度少于六自由度（6-DOF），形成典型的 欠驱动系统 。这类系统在横荡、垂荡和艏摇等方向缺乏直接驱动力，依赖非线性耦合实现间接控制，显著增加了动力学建模与控制器设计的难度。

建立精确的六自由度运动模型是实现高精度轨迹跟踪的前提，需综合描述刚体运动、流体动力效应及外部干扰力。该模型不仅反映位姿与速度间的微分关系，还需体现质量分布、阻尼力和恢复力等物理特性，为后续基于牛顿-欧拉法的动力学推导提供基础框架。

2. 牛顿-欧拉法在AUV动力学建模中的应用

2.1 牛顿-欧拉法的理论基础

2.1.1 刚体动力学基本原理

刚体动力学是研究物体在外力作用下运动规律的基础理论，广泛应用于航空航天、水下机器人和机械臂等领域。对于自主水下航行器（Autonomous Underwater Vehicle, AUV），其六自由度运动包括三个平动自由度（ Surge、Sway、Heave）和三个转动自由度（Roll、Pitch、Yaw）。由于AUV通常被视为刚体系统，在忽略结构形变的前提下，可采用牛顿-欧拉方程描述其动力学行为。

牛顿-欧拉法结合了牛顿第二定律与欧拉旋转方程，分别用于描述质心的线性运动和绕质心的角运动。设 $ \mathbf{F} $ 为作用于刚体质心的合外力，$ \mathbf{\tau} $ 为合外力矩，$ m $ 为质量，$ \mathbf{v} $ 为质心速度，$ \mathbf{\omega} $ 为角速度，$ \mathbf{I} $ 为惯性张量，则有如下基本动力学方程：

\begin{aligned}
\frac{d}{dt}(m\mathbf{v}) &= \mathbf{F} + \mathbf{F} {\text{Coriolis}} + \mathbf{F} {\text{centrifugal}} \
\frac{d}{dt}(\mathbf{I}\boldsymbol{\omega}) &= \boldsymbol{\tau} + \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega})
\end{aligned}

其中右侧额外项来源于非惯性坐标系下的科里奥利力与离心力效应。该公式适用于体固连坐标系（body-fixed frame），这是处理AUV动力学问题的标准选择，因为传感器数据（如IMU）多在此坐标系中输出。

值得注意的是，上述方程未考虑流体环境带来的附加质量与阻尼效应。实际建模中需引入“附加质量矩阵” $ \mathbf{M}_a $ 和“水动力阻尼矩阵” $ \mathbf{D}(\mathbf{v}) $ 来修正动力学模型。因此完整表达式应写为：

(\mathbf{M} + \mathbf{M} a)\dot{\mathbf{v}} + \mathbf{C}(\mathbf{v})(\mathbf{M} + \mathbf{M}_a)\mathbf{v} + \mathbf{D}(\mathbf{v})\mathbf{v} + \mathbf{g}(\boldsymbol{\eta}) = \boldsymbol{\tau} {\text{prop}}

其中：
- $ \mathbf{M} $：刚体质量与惯性矩阵；
- $ \mathbf{C}(\mathbf{v}) $：科里奥利与向心力矩阵；
- $ \mathbf{g}(\boldsymbol{\eta}) $：恢复力项（重力与浮力）；
- $ \boldsymbol{\tau}_{\text{prop}} $：推进器提供的控制输入力/力矩；
- $ \boldsymbol{\eta} $：位姿向量（位置与姿态角）。

此形式构成了现代AUV非线性动力学建模的核心框架。通过合理构造各项系数并结合实验辨识，可在高保真度下模拟真实航行器响应。

科里奥利项的物理意义解析

科里奥利矩阵 $ \mathbf{C}(\mathbf{v}) $ 源于运动学变换过程中对时间导数的展开。当在旋转坐标系中对速度进行微分时，必须计入坐标系自身旋转的影响。例如，即使没有外部施加力矩，若AUV处于高速转弯状态，其内部质量分布会产生惯性耦合效应，表现为一种“虚拟力”。这类效应在俯仰与偏航耦合运动中尤为显著。

以横荡（sway）速度影响滚转力矩为例，若AUV侧向移动同时发生滚动，则因质量相对于旋转轴的位置变化，将产生附加力矩。这正是 $ \mathbf{C}(\mathbf{v})\mathbf{v} $ 所捕捉的动力学特性。

数值稳定性考量

在数值仿真中，直接使用上述连续形式可能导致积分不稳定，尤其在大步长或强非线性条件下。为此常采用显式重构方式，将动力学方程改写为状态空间形式：

function dv = auv_dynamics(t, v, M_total, C_func, D_func, g_func, tau_prop)
    % 输入：
    %   v: 6x1 状态速度向量 [u,v,w,p,q,r]
    %   M_total: 6x6 总质量矩阵（含附加质量）
    %   tau_prop: 6x1 推进力向量
    % 输出：
    %   dv: 6x1 加速度向量

    Cv = C_func(v);           % 计算科里奥利矩阵
    Dv = D_func(v);           % 阻尼力
    g = g_func(v(4:6));       % 恢复力（依赖姿态）

    dv = M_total \ (tau_prop - Cv*v - Dv - g);
end

代码逻辑逐行分析：
1. 函数定义标准ODE接口，兼容 ode45 等求解器；
2. M_total 包含刚体与附加质量之和，确保惯性正确；
3. C_func 是预定义函数句柄，返回当前速度下的科里奥利矩阵；
4. 使用左除 \ 实现矩阵逆运算，比显式 inv() 更稳定；
5. 最终加速度由所有力共同决定，体现合力平衡思想。

该实现方式便于模块化封装，支持后续控制器集成与仿真闭环测试。

2.1.2 广义坐标与运动方程构建

在构建AUV动力学模型时，必须明确广义坐标的选取及其对应的广义速度与广义力。通常采用两组变量：
- 广义位置向量 $ \boldsymbol{\eta} = [\mathbf{x}, \boldsymbol{\Theta}]^T \in \mathbb{R}^6 $，其中 $ \mathbf{x} = [x,y,z]^T $ 表示惯性系下位置，$ \boldsymbol{\Theta} = [\phi,\theta,\psi]^T $ 为欧拉角（roll, pitch, yaw）；
- 广义速度向量 $ \boldsymbol{\nu} = [\mathbf{v}, \boldsymbol{\omega}]^T \in \mathbb{R}^6 $，其中 $ \mathbf{v} = [u,v,w]^T $ 为体坐标系下线速度，$ \boldsymbol{\omega} = [p,q,r]^T $ 为角速度。

两者之间的关系由雅可比矩阵 $ \mathbf{J}(\boldsymbol{\Theta}) $ 连接：

\dot{\boldsymbol{\eta}} = \mathbf{J}(\boldsymbol{\Theta}) \boldsymbol{\nu}

具体地，该雅可比矩阵分为平动与转动部分：

\mathbf{J}(\boldsymbol{\Theta}) =
\begin{bmatrix}
\mathbf{R}(\boldsymbol{\Theta}) & \mathbf{0} \
\mathbf{0} & \mathbf{T}(\boldsymbol{\Theta})
\end{bmatrix}

其中 $ \mathbf{R}(\boldsymbol{\Theta}) $ 为方向余弦矩阵（DCM），实现从体坐标到惯性坐标的矢量转换；$ \mathbf{T}(\boldsymbol{\Theta}) $ 为欧拉角速率到角速度的映射矩阵，其形式为：

\mathbf{T}(\boldsymbol{\Theta}) =
\begin{bmatrix}
1 & 0 & -\sin\theta \
0 & \cos\phi & \sin\phi\cos\theta \
0 & -\sin\phi & \cos\phi\cos\theta
\end{bmatrix}

⚠️ 注意：当俯仰角 $ \theta = \pm90^\circ $ 时，$ \mathbf{T} $ 奇异，出现“万向节锁”现象。实践中建议采用四元数替代欧拉角以避免奇异性。

参数	符号	单位	物理含义
质量	$ m $	kg	AUV总质量
惯性矩	$ I_{xx}, I_{yy}, I_{zz} $	kg·m²	绕主轴的转动惯量
附加质量	$ m_a^{ij} $	kg 或 kg·m²	流体跟随运动等效质量
阻尼系数	$ d_u, d_v, \dots $	N·s/m 或 N·m·s/rad	速度相关阻力
推进力	$ T_u, T_r $	N 或 N·m	可控推力/力矩

该表列出了关键参数类型及其单位，为后续参数辨识提供依据。

动力学方程的层次化构建流程

为了系统化建立AUV动力学模型，推荐采用以下步骤：

graph TD
    A[确定广义坐标] --> B[建立动能表达式]
    B --> C[计算拉格朗日函数 L=T-V]
    C --> D[应用拉格朗日方程]
    D --> E[引入耗散力与外力]
    E --> F[转化为牛顿-欧拉形式]
    F --> G[加入流体效应修正]

该流程体现了从能量法出发推导动力学方程的思想路径，最终仍归结为牛顿-欧拉框架下的矢量方程。优势在于能够自然包含复杂几何结构的质量分布信息，并便于扩展至柔性体或多体系统。

进一步，若采用拉格朗日方法，动能 $ T $ 可表示为：

T = \frac{1}{2} \boldsymbol{\nu}^T \mathbf{M} \boldsymbol{\nu}

势能 $ V $ 主要来自重力与浮力做功，即：

V = -(m - m_b)gz + \text{orientation-dependent terms}

则拉格朗日方程为：

\frac{d}{dt}\left( \frac{\partial L}{\partial \dot{q}_i} \right) - \frac{\partial L}{\partial q_i} = Q_i

其中 $ Q_i $ 为广义力，包含推进力、阻尼力与控制输入。经变分运算后，可得与牛顿-欧拉法一致的结果，验证了两种方法的等价性。

2.1.3 坐标系定义与变换关系（惯性系与体固连系）

精确的动力学建模依赖于清晰的坐标系定义。在AUV建模中，主要涉及两个坐标系：

• 惯性坐标系（Inertial Frame） $ {I} $：固定于地球表面某点，通常取NED（北-东-下）方向作为坐标轴，记作 $ X_IY_IZ_I $。
• 体固连坐标系（Body-fixed Frame） $ {B} $：原点位于AUV质心或几何中心，随AUV一起运动，轴向沿艇体主方向，记作 $ X_BY_BZ_B $。

两坐标系之间通过旋转矩阵 $ \mathbf{R}_{BI} \in SO(3) $ 关联。给定欧拉角序列（ZYX顺序，即偏航-俯仰-滚转），方向余弦矩阵为：

\mathbf{R}_{BI} =
\begin{bmatrix}
c\psi c\theta & c\psi s\theta s\phi - s\psi c\phi & c\psi s\theta c\phi + s\psi s\phi \
s\psi c\theta & s\psi s\theta s\phi + c\psi c\phi & s\psi s\theta c\phi - c\psi s\phi \
-s\theta & c\theta s\phi & c\theta c\phi
\end{bmatrix}

其中 $ c(\cdot)=\cos(\cdot), s(\cdot)=\sin(\cdot) $。

速度变换方面，线速度满足：

\mathbf{v}^I = \mathbf{R}_{BI} \mathbf{v}^B

而角速度关系更为复杂，因角速度是赝矢量，其在体坐标系中测量值 $ \boldsymbol{\omega}^B = [p,q,r]^T $ 直接用于动力学方程。

坐标变换误差来源分析

在实际仿真中，常见错误包括：
1. 忽略 $ \mathbf{T}(\boldsymbol{\Theta}) $ 的存在，误认为 $ [\dot{\phi}, \dot{\theta}, \dot{\psi}] = [p,q,r] $；
2. 错用旋转矩阵方向（应为 $ \mathbf{R}_{BI} $ 将体坐标向量转为惯性坐标）；
3. 混淆质心与浮心位置差异导致恢复力计算偏差。

为规避上述问题，建议采用标准化符号体系，如下表所示：

符号	含义	所在坐标系
$ \boldsymbol{\eta} $	位姿向量（位置+姿态）	惯性系
$ \boldsymbol{\nu} $	速度向量（线速+角速）	体固连系
$ \mathbf{M} $	质量与惯性矩阵	体固连系
$ \mathbf{g}(\boldsymbol{\eta}) $	恢复力向量	体固连系
$ \boldsymbol{\tau} $	外力与外力矩	体固连系

此外，可通过MATLAB脚本自动化生成旋转矩阵与雅可比矩阵：

function R = euler_to_dcm(yaw, pitch, roll)
    % 输入：欧拉角（弧度）
    % 输出：方向余弦矩阵 R_IB（将I->B）
    cy = cos(yaw); sy = sin(yaw);
    cp = cos(pitch); sp = sin(pitch);
    cr = cos(roll); sr = sin(roll);

    R = [
        cy*cp,     cy*sp*sr - sy*cr,     cy*sp*cr + sy*sr;
        sy*cp,     sy*sp*sr + cy*cr,     sy*sp*cr - cy*sr;
        -sp,       cp*sr,                cp*cr
    ];
end

参数说明：
- 输入角度须为弧度制；
- 返回矩阵 $ \mathbf{R}_{IB} $，注意是惯性到体坐标，若需反向则取转置；
- 此函数可用于实时姿态更新或轨迹可视化。

综上所述，准确理解并实现坐标系间的变换关系，是构建高精度AUV动力学模型的前提条件，也为后续控制律设计提供了可靠的反馈基础。

2.2 欠驱动系统的动力学特性分析

2.2.1 欠驱动与全驱动的本质区别

全驱动系统指在每一个自由度上均具备独立可控的执行机构，使得系统能够在任意方向施加所需力或力矩。相比之下，欠驱动系统是指控制输入维度小于系统自由度数量的情形。典型AUV配置中，仅有艏向推进器与水平舵面，仅能主动控制纵荡（surge）、偏航（yaw）及有限俯仰（pitch），而无法直接控制横荡（sway）、垂荡（heave）与滚转（roll），故属于典型的欠驱动系统。

数学上，设系统自由度为 $ n = 6 $，控制输入维数为 $ m < 6 $，则称其为欠驱动。令控制输入向量 $ \boldsymbol{\tau}_c \in \mathbb{R}^m $，通过输入矩阵 $ \mathbf{B} \in \mathbb{R}^{6\times m} $ 映射至广义力空间：

\boldsymbol{\tau} = \mathbf{B} \boldsymbol{\tau}_c

若 $ \mathbf{B} $ 的秩小于6，则系统不可在所有方向独立施控，构成欠驱动。

类型	控制输入	自由度	是否欠驱动
全驱动AUV	$ T_u, T_v, T_w, M_p, M_q, M_r $	6	否
传统鱼雷型AUV	$ T_u, M_q, M_r $	6	是
ROV（带多个推进器）	$ T_x,T_y,T_z,M_\phi,M_\theta,M_\psi $	6	否
滑翔机	无主动推进，仅调节重心	6	是

由此可见，大多数实用型AUV出于能耗与可靠性考虑，均设计为欠驱动结构。

欠驱动带来的挑战

最核心问题是 可控性受限 。虽然不能直接控制某些自由度（如 sway），但可通过动态耦合间接影响其行为。例如，利用偏航机动产生侧向力（通过舵面诱导水流），从而实现横向位移。这种“间接控制”机制依赖于系统内部动力学结构，要求控制器具备较强非线性处理能力。

更深层次的问题在于 可微分同胚不可行性 ：即无法通过坐标变换将欠驱动系统转化为完全线性可控形式。这意味着经典线性控制方法（如LQR）难以直接应用，必须借助非线性控制理论（如反馈线性化、滑模控制、backstepping）来实现有效调控。

2.2.2 自由度受限下的可控性与稳定性问题

欠驱动AUV的可控性分析需基于非线性系统理论。根据李括号（Lie Bracket）方法，判断系统是否能在小范围内到达任意邻近状态。考虑简化二维平面运动（surge, sway, yaw），动力学方程为：

\begin{cases}
\dot{x} = u\cos\psi - v\sin\psi \
\dot{y} = u\sin\psi + v\cos\psi \
\dot{\psi} = r \
\dot{u} = f_u(u,v,r) + \tau_u/m_{uu} \
\dot{v} = f_v(u,v,r) \
\dot{r} = f_r(u,v,r) + \tau_r/I_{zz}
\end{cases}

其中 $ \tau_u, \tau_r $ 为可控输入，其余状态受耦合影响。

系统向量场可表示为：

\dot{\mathbf{x}} = \mathbf{f}_0 + \tau_u \mathbf{g}_1 + \tau_r \mathbf{g}_2

通过计算李括号 $ [\mathbf{g}_1, \mathbf{g}_2], [\mathbf{g}_1, [\mathbf{g}_1, \mathbf{g}_2]] $ 等，可发现其张成空间覆盖全部状态方向，表明系统满足 局部弱可控性 。

然而，稳定性仍面临严峻挑战。由于缺少对 sway 和 heave 的直接阻尼控制，这些方向易受扰动积累影响，导致漂移甚至失稳。特别在强流环境下，横向漂移可能超出任务允许范围。

解决策略之一是设计 级联控制器 ：先稳定可控自由度（如 heading 和 speed），再利用其动态激励间接调控不可控自由度。例如，蛇形路径跟踪即利用周期性偏航振荡产生净侧向位移。

2.2.3 六自由度中未驱动方向的动力耦合影响

尽管某些自由度无直接控制输入，但由于非线性耦合项的存在，它们并非孤立。以 roll 方向为例，虽无专用滚转力矩发生器，但在高速转弯时，因离心力作用于非对称质量分布，会自发产生滚转力矩：

M_p = -m z_G (q w - r v)

其中 $ z_G $ 为质心相对于浮心的垂直偏移。若设计不当，可能导致“倾覆风险”。

类似地，heave 方向虽无垂向推进器，但通过俯仰角变化可改变升力分布，进而实现深度调节。这正是滑翔机的工作原理。

未驱动自由度	耦合源	可利用机制
Sway ($v$)	Yaw rate $r$ × surge speed $u$	舵面侧力诱导
Heave ($w$)	Pitch angle $\theta$	升力面攻角调节
Roll ($p$)	Asymmetry + angular rates	离心力矩利用

因此，建模时必须完整保留交叉项，否则将低估系统响应能力或误判稳定性边界。

2.3 动力学建模流程设计

2.3.1 系统受力分解：重力、浮力、水动力与推进力

AUV所受外力可分为四大类：

1. 重力 $ \mathbf{W} $ ：作用于质心，方向竖直向下，大小为 $ mg $；
2. 浮力 $ \mathbf{B} $ ：作用于浮心，方向竖直向上，大小等于排开水重；
3. 水动力 $ \mathbf{F}_h $ ：包括粘性阻尼、升力、附加质量效应；
4. 推进力 $ \mathbf{F}_p $ ：由螺旋桨或泵喷提供，可控。

在体坐标系中，合力为：

\mathbf{F}_{\text{total}} = \mathbf{F}_g + \mathbf{F}_b + \mathbf{F}_h + \mathbf{F}_p

其中恢复力项（gravity-buoyancy）为：

\mathbf{F} {\text{restoring}} = \mathbf{R} {BI}^{-1}
\begin{bmatrix}
0 \ 0 \ W - B
\end{bmatrix}
+
\begin{bmatrix}
(W-B)\sin\theta \
-(W-B)\sin\phi\cos\theta \
-(W-B)\cos\phi\cos\theta
\end{bmatrix}

详细推导可见于Fossen《Guidance and Control of Ocean Vehicles》。

2.3.2 外力矩与角动量变化的关系推导

根据欧拉方程：

\frac{d\mathbf{H}}{dt} = \boldsymbol{\tau} {\text{ext}}
\quad \Rightarrow \quad
\mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega}) = \boldsymbol{\tau} {\text{hydro}} + \boldsymbol{\tau} {\text{prop}} + \boldsymbol{\tau} {\text{weight/buoyancy}}

其中力矩来源包括：
- 水动力矩（舵面、壳体分离流）；
- 推进器偏置产生的力臂矩；
- 重力与浮力不在同一直线上形成的扶正力矩。

若质心 $ G $ 与浮心 $ B $ 不重合，距离为 $ \mathbf{d}_{GB} $，则恢复力矩为：

\boldsymbol{\tau} {\text{rb}} = \mathbf{R} {BI}^{-1} \left( \mathbf{d}_{GB} \times (B \hat{z}_I) \right)

该力矩具有自动回正特性，是静态稳定的来源。

2.3.3 运动方程的分步构建策略

推荐采用以下五步法构建完整动力学模型：

flowchart LR
    Step1[参数采集] --> Step2[坐标系设定]
    Step2 --> Step3[建立动能与势能]
    Step3 --> Step4[推导广义力]
    Step4 --> Step5[整合为状态方程]

每一步均可通过MATLAB/Simulink实现自动化建模，提升开发效率与复用性。

3. 非线性动力学方程编程实现

水下自主航行器（Autonomous Underwater Vehicle, AUV）的运动行为由其六自由度非线性动力学方程决定。这些方程描述了在复杂海洋环境中，AUV受重力、浮力、水动力、推进力等多重作用下的加速度响应与状态演化过程。将理论模型转化为可执行的数值仿真程序，是验证控制策略、优化系统设计和开展虚拟试验的关键步骤。本章聚焦于如何在MATLAB/Simulink环境下对欠驱动AUV的动力学方程进行结构化建模与高效编程实现，重点涵盖质量与惯性参数表达、水动力项数学建模以及核心代码模块的设计逻辑。

3.1 质量与惯性参数的数学表达

AUV作为刚体或准刚体系统，在牛顿-欧拉法框架下，其动力学方程依赖于精确的质量与惯性特性。这些参数不仅决定了系统的惯性响应，还深刻影响着姿态稳定性与轨迹跟踪性能。尤其对于欠驱动系统而言，由于部分自由度缺乏直接推力输入，惯性耦合效应更加显著，因此必须在建模阶段对质量矩阵、惯性张量及附加质量进行精细化处理。

3.1.1 质量矩阵的构成与对称性处理

在六自由度空间中，AUV的状态通常用广义坐标 $\boldsymbol{\eta} = [x, y, z, \phi, \theta, \psi]^T$ 表示位置与姿态，对应的广义速度为 $\boldsymbol{v} = [u, v, w, p, q, r]^T$，其中 $u,v,w$ 为平动速度分量，$p,q,r$ 为绕体轴的角速度。根据拉格朗日力学或牛顿-欧拉方法，系统动力学可写为：

\mathbf{M}(\boldsymbol{v})\dot{\boldsymbol{v}} + \mathbf{C}(\boldsymbol{v})\boldsymbol{v} + \mathbf{D}(\boldsymbol{v})\boldsymbol{v} + \mathbf{g}(\boldsymbol{\eta}) = \boldsymbol{\tau}

其中 $\mathbf{M}$ 是总质量矩阵，包含刚体质量和流体附加质量；$\mathbf{C}$ 为科里奥利/向心力矩阵；$\mathbf{D}$ 为阻尼矩阵；$\mathbf{g}$ 为恢复力矢量；$\boldsymbol{\tau}$ 为外部控制输入。

质量矩阵 $\mathbf{M}$ 可分解为两部分：

\mathbf{M} = \mathbf{M}_{RB} + \mathbf{M}_A

• $\mathbf{M}_{RB}$：刚体质量矩阵，源于AUV自身质量和质心分布；
• $\mathbf{M}_A$：附加质量矩阵，源于流体加速时产生的惯性反作用。

以标准形式表示，$\mathbf{M}_{RB}$ 的结构如下：

	u	v	w	p	q	r
u	m	0	0	0	-mz_g	my_g
v	0	m	0	mz_g	0	-mx_g
w	0	0	m	-my_g	mx_g	0
p	0	mz_g	-my_g	I_{xx}	-I_{xy}	-I_{xz}
q	-mz_g	0	mx_g	-I_{xy}	I_{yy}	-I_{yz}
r	my_g	-mx_g	0	-I_{xz}	-I_{yz}	I_{zz}

说明 ：$m$ 为总质量，$(x_g, y_g, z_g)$ 为质心相对于体坐标系原点的位置，$I_{ij}$ 为惯性张量元素。

该矩阵具有对称性，且当质心与体坐标系原点重合时（常见于对称设计），交叉项消失，简化为对角主导结构。

function M_RB = mass_matrix_rb(m, r_g, I)
% 计算刚体质量矩阵
% 输入：
%   m: 标量，总质量 (kg)
%   r_g: [x_g, y_g, z_g]，质心位置 (m)
%   I: [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]，惯性张量元素 (kg·m²)

xg = r_g(1); yg = r_g(2); zg = r_g(3);
Ixx = I(1); Iyy = I(2); Izz = I(3);
Ixy = I(4); Ixz = I(5); Iyz = I(6);

M_RB = zeros(6,6);
M_RB(1,1) = m;
M_RB(2,2) = m;
M_RB(3,3) = m;
M_RB(4,4) = Ixx; M_RB(4,5) = -Ixy; M_RB(4,6) = -Ixz;
M_RB(5,4) = -Ixy; M_RB(5,5) = Iyy; M_RB(5,6) = -Iyz;
M_RB(6,4) = -Ixz; M_RB(6,5) = -Iyz; M_RB(6,6) = Izz;

% 添加科里奥利耦合项（线角耦合）
M_RB(1,5) = -m*zg; M_RB(1,6) = m*yg;
M_RB(2,4) = m*zg;  M_RB(2,6) = -m*xg;
M_RB(3,4) = -m*yg; M_RB(3,5) = m*xg;

% 对称填充
M_RB(5,1) = M_RB(1,5);
M_RB(6,1) = M_RB(1,6);
M_RB(4,2) = M_RB(2,4);
M_RB(6,2) = M_RB(2,6);
M_RB(4,3) = M_RB(3,4);
M_RB(5,3) = M_RB(3,5);
end

逻辑分析 ：
- 第一行定义函数接口，接收质量、质心和惯性张量。
- 中间部分初始化零矩阵，并设置主对角块：平动质量 m 和转动惯量 I 。
- 随后添加因质心偏移引起的线角动量耦合项（如 -m*zg 等），这是刚体动力学中不可忽略的部分。
- 最后通过对称赋值确保 $\mathbf{M}_{RB}$ 是实对称矩阵，满足物理守恒要求。

此函数输出的矩阵将用于后续动力学积分计算，其准确性直接影响仿真结果的真实性。

3.1.2 惯性张量计算及坐标变换

惯性张量描述物体绕不同轴旋转时的抵抗能力，其值依赖于坐标系的选择。通常，AUV的设计图纸提供的是相对于形心或某个基准点的惯性数据，但在动力学建模中需将其转换至体固连坐标系（Body-fixed Frame）。若原始惯性张量已知于局部坐标系 $O’$，而体坐标系位于 $O$，两者之间存在位移 $\Delta \mathbf{r} = [\Delta x, \Delta y, \Delta z]$，则可通过平行轴定理进行变换：

\mathbf{I} O = \mathbf{I} {O’} + m \left( (\Delta \mathbf{r}^T \Delta \mathbf{r}) \mathbf{E} - \Delta \mathbf{r} \otimes \Delta \mathbf{r} \right)

其中 $\mathbf{E}$ 为单位矩阵，$\otimes$ 表示外积。

例如，假设某部件质量 $m=50kg$，相对于其质心的惯性张量为：

\mathbf{I}_{cm} = \begin{bmatrix}
8 & 0 & 0 \
0 & 10 & 0 \
0 & 0 & 7
\end{bmatrix} \text{kg·m}^2

现将其安装于整体AUV坐标系中偏移 $[0.2, -0.1, 0.3]$ 处，则新惯性张量为：

dx = 0.2; dy = -0.1; dz = 0.3;
dr_sq = dx^2 + dy^2 + dz^2; % = 0.14
delta_r_outer = [dx*dx, dx*dy, dx*dz;
                 dy*dx, dy*dy, dy*dz;
                 dz*dx, dz*dy, dz*dz];

I_shift = I_cm + m * (dr_sq * eye(3) - delta_r_outer);

该操作应在所有子部件组装完成后统一执行，最终累加得到整体惯性张量。

此外，在多体系统或倾斜安装传感器时，还需考虑坐标系之间的旋转关系。设从参考系 $A$ 到 $B$ 的旋转矩阵为 $\mathbf{R}_{AB}$，则惯性张量变换为：

\mathbf{I} B = \mathbf{R} {AB} \mathbf{I} A \mathbf{R} {AB}^T

这一变换常用于将IMU测得的数据映射到体坐标系中。

3.1.3 附加质量效应的近似建模方法

附加质量（Added Mass）是指流体随物体加速运动所产生的等效惯性力。对于水下航行器，特别是低速高密度环境中的AUV，附加质量可达自身质量的30%以上，必须予以建模。

对于理想流体中椭球形体，可用势流理论求解附加质量系数。常用经验公式如下：

自由度	附加质量表达式（近似）
X方向	$X_{\dot{u}} \approx -0.5 \rho \pi a b^2$
Y方向	$Y_{\dot{v}} \approx -0.5 \rho \pi a^2 b$
Z方向	$Z_{\dot{w}} \approx -0.5 \rho \pi a^2 b$
K方向（滚转）	$K_{\dot{p}} \approx -0.1 \rho \pi a^3 b$
M方向（俯仰）	$M_{\dot{q}} \approx -0.2 \rho \pi a b^4 / L$
N方向（偏航）	$N_{\dot{r}} \approx -0.2 \rho \pi a^4 b / L$

其中 $\rho$ 为海水密度（~1025 kg/m³），$a$ 为半长轴，$b$ 为半短轴，$L$ 为长度。

由此构建附加质量矩阵 $\mathbf{M}_A$，通常为对角阵（忽略交叉项）：

function M_A = added_mass_matrix(rho, a, b, L)
% 计算附加质量矩阵（对称简化模型）

Xdu = -0.5 * rho * pi * a * b^2;
Ydv = -0.5 * rho * pi * a^2 * b;
Zdw = -0.5 * rho * pi * a^2 * b;
Kdp = -0.1 * rho * pi * a^3 * b;
Mdq = -0.2 * rho * pi * a * b^4 / L;
Ndr = -0.2 * rho * pi * a^4 * b / L;

M_A = diag([Xdu, Ydv, Zdw, Kdp, Mdq, Ndr]);
end

参数说明 ：
- rho : 海水密度（默认1025 kg/m³）
- a , b : 几何半轴（m）
- L : 总长（m）

尽管此模型基于理想形状，但可用于初步设计。更精确的方法包括CFD仿真提取系数，或通过衰减试验辨识实际值。

整个质量建模流程可通过以下Mermaid流程图概括：

graph TD
    A[开始] --> B[读取几何尺寸与材料属性]
    B --> C[计算刚体质量与质心]
    C --> D[应用平行轴定理修正惯性张量]
    D --> E[调用经验公式估算附加质量]
    E --> F[合成总质量矩阵 M = M_RB + M_A]
    F --> G[输出至动力学求解器]

该流程体现了从物理参数到数学表达的完整链条，是高保真仿真的基础。

3.2 水动力阻尼与恢复力项建模

在真实海洋环境中，AUV持续受到粘性阻力和静力学恢复力的作用。这两类力共同构成了非线性动力学方程中的耗散与保守项，直接影响系统的稳定域与能耗特性。

3.2.1 二次型阻尼模型的选择与参数辨识

水动力阻尼主要来源于流体粘性摩擦与涡旋脱落，通常建模为速度的非线性函数。最广泛采用的形式是 线性+二次组合模型 ：

\mathbf{D}(\boldsymbol{v})\boldsymbol{v} =
\begin{bmatrix}
X_u u + X_{|u|u} |u| u \
Y_v v + Y_{|v|v} |v| v \
Z_w w + Z_{|w|w} |w| w \
K_p p + K_{|p|p} |p| p \
M_q q + M_{|q|q} |q| q \
N_r r + N_{|r|r} |r| r \
\end{bmatrix}

其中线性项主导低速段（如悬停），二次项在高速巡航时占优。

此类模型的优点在于易于辨识且计算效率高。参数可通过风洞/水槽实验、系统辨识算法（如最小二乘、递推辨识）获得。

以纵向阻力为例，编写通用阻尼函数：

function Dv = hydro_damping(v, coeffs)
% 输入：
%   v: [u,v,w,p,q,r] 当前速度矢量
%   coeffs: 结构体，含 Xu, Xuu, Yv, Yvv, ..., Nr, Nrr

u = v(1); v_y = v(2); w = v(3);
p = v(4); q = v(5); r = v(6);

Dv = zeros(6,1);
Dv(1) = coeffs.Xu * u + coeffs.Xuu * abs(u)*u;
Dv(2) = coeffs.Yv * v_y + coeffs.Yvv * abs(v_y)*v_y;
Dv(3) = coeffs.Zw * w + coeffs.Zww * abs(w)*w;
Dv(4) = coeffs.Kp * p + coeffs.Kpp * abs(p)*p;
Dv(5) = coeffs.Mq * q + coeffs.Mqq * abs(q)*q;
Dv(6) = coeffs.Nr * r + coeffs.Nrr * abs(r)*r;
end

逻辑分析 ：
- 使用 abs(x)*x 实现符号保持的平方项，确保阻力方向始终与速度相反。
- 参数组织为结构体便于管理，支持后期扩展为表格查找或神经网络替代。

该模型已在多个AUV项目中验证有效，尤其适用于Reynolds数较高的工况。

3.2.2 SNAME标准下水动力系数的经验取值

SNAME（Society of Naval Architects and Marine Engineers）发布的技术报告TR-12提供了典型无人潜器的无量纲水动力系数推荐值。这些系数可用于快速原型开发。

系数	典型值（细长体AUV）	物理意义
$C_{D0}$	0.01–0.03	基础阻力系数
$X_{\dot{u}}/m$	-0.05 to -0.1	附加质量比
$Y_{\dot{v}}/m$	-0.5 to -1.0	侧向附加质量
$N_{\dot{r}}/I_{zz}$	-0.8 to -1.2	偏航附加惯量
$Y_v/(0.5\rho V^2 S)$	-2.0 to -4.0	升力导数
$N_r/(0.5\rho V^2 S L)$	-0.5 to -1.0	方向稳定性导数

注：$V$ 为巡航速度，$S$ 为参考面积（如横截面），$L$ 为特征长度。

利用上述经验范围，结合具体尺寸可估算出有量纲系数：

% 示例：估算Zww
rho = 1025;     % kg/m³
V_cruise = 1.5; % m/s
S_ref = pi*(0.2)^2; % 圆形横截面
C_Zww = -1.8;   % 来自SNAME建议
Zww = C_Zww * 0.5 * rho * S_ref * V_cruise;

这种方式可在缺乏实验数据时提供合理初值。

3.2.3 静态恢复力（重力-浮力）项的非线性表达

恢复力主要由重力与浮力不平衡引起，其表达式为：

\mathbf{g}(\boldsymbol{\eta}) =
\begin{bmatrix}
(W - B) \sin\theta \
-(W - B) \cos\theta \sin\phi \
-(W - B) \cos\theta \cos\phi \
y_g W - y_b B)\cos\phi + (z_g W - z_b B)\sin\phi \
(z_g W - z_b B)\cos\phi \sin\theta - (x_g W - x_b B)\cos\theta \
(y_b B - y_g W)\sin\theta
\end{bmatrix}

其中 $W=mg$, $B=\rho g V$ 分别为重量与浮力，$(x_ , y_ , z_*)$ 为重心/浮心坐标。

注意：该向量依赖姿态角 $\phi,\theta$，故为非线性项。即使 $W=B$，只要质心与浮心不共线（常见情况），仍会产生恢复力矩。

实现代码如下：

function g_vec = restoring_force(eta, params)
% eta: [x,y,z,phi,theta,psi]
phi = eta(4); theta = eta(5);
W = params.W; B = params.B;
xg = params.xg; yg = params.yg; zg = params.zg;
xb = params.xb; yb = params.yb; zb = params.zb;

sg = sin(phi); cg = cos(phi);
st = sin(theta); ct = cos(theta);

g_vec = zeros(6,1);
g_vec(1) = (W - B) * st;
g_vec(2) = -(W - B) * ct * sg;
g_vec(3) = -(W - B) * ct * cg;

g_vec(4) = (yg*W - yb*B)*cg + (zg*W - zb*B)*sg;
g_vec(5) = (zg*W - zb*B)*cg*st - (xg*W - xb*B)*ct;
g_vec(6) = (yb*B - yg*W)*st;
end

此函数应每步仿真调用一次，因其依赖当前姿态，不能预先固化。

3.3 MATLAB/Simulink环境下的代码实现

完成各项子模型构建后，进入集成阶段。MATLAB以其强大的ODE求解能力和Simulink可视化工具链，成为AUV仿真的首选平台。

3.3.1 状态变量定义与ODE求解器接口设计

使用 ode45 或 ode15s 求解六自由度方程，需封装右端函数：

function dvdt = auv_dynamics(t, v, eta, params)
% ODE接口函数

M_total = mass_matrix_rb(params.m, params.rg, params.I) + ...
          added_mass_matrix(params.rho, params.a, params.b, params.L);

Dv = hydro_damping(v, params.damp_coeffs);
g_vec = restoring_force(eta, params);

% 假设控制输入为零（开环）
tau = [0; 0; 0; 0; 0; 0]; 

% 求解 dv/dt = M^{-1}(tau - Cv - Dv - g)
Cv = coriolis_matrix(v, params.m, params.rg, params.I) * v;
dvdt = M_total \ (tau - Cv - Dv - g_vec);
end

主循环中调用：

[t, v] = ode45(@(t,v) auv_dynamics(t, v, eta_current, params), ...
               tspan, v0);

注意：此处 eta 应通过积分同步更新，完整系统需联立 $\dot{\eta} = J(\eta)v$。

3.3.2 核心函数模块化封装

建议建立如下文件结构：

/models/
    mass_matrix_rb.m
    added_mass_matrix.m
    hydro_damping.m
    restoring_force.m
    coriolis_matrix.m
/main_sim.m

各函数独立测试，便于版本管理和多人协作。

3.3.3 实时仿真循环结构搭建与初始条件设置

完整仿真循环如下所示：

% 初始化
v0 = [0;0;0;0;0;0];
eta0 = [0;0;-10;0;0;0]; % 下潜10米
tspan = [0 100];

% 存储变量
ts = linspace(tspan(1), tspan(2), 1000);
Vs = zeros(length(ts), 6);
Etap = zeros(length(ts), 6);

% 主循环
for i = 1:length(ts)-1
    t = ts(i);
    v = Vs(i,:);
    eta = Etap(i,:);
    dvdt = auv_dynamics(t, v, eta, params);
    dette = jacobian_transformation(eta) * v; % 更新姿态
    Vs(i+1,:) = v + dvdt * dt;
    Etap(i+1,:) = eta + dette * dt;
end

配合动画或GUI界面，即可实现动态可视化。

整个系统的软件架构可通过以下Mermaid图展示：

graph LR
    A[用户输入参数] --> B[初始化模块]
    B --> C[调用质量矩阵]
    B --> D[加载阻尼系数]
    B --> E[设定初始状态]
    C --> F[动力学核心引擎]
    D --> F
    E --> F
    F --> G[ODE求解器]
    G --> H[状态输出与可视化]

该架构清晰分离关注点，具备良好的可扩展性和调试便利性。

4. 滑模控制算法设计与代码实现

滑模控制（Sliding Mode Control, SMC）作为一种强鲁棒性的非线性控制策略，在欠驱动自主水下航行器（AUV）的轨迹跟踪与姿态调节中展现出显著优势。面对海洋环境中广泛存在的模型不确定性、外部扰动（如海流、波浪）、参数摄动以及未建模动态，传统线性控制器往往难以维持高性能控制效果。而滑模控制通过构造一个具有不变性的滑动流形，迫使系统状态在有限时间内趋近并沿其滑动，从而实现对干扰的抑制和对参考轨迹的高精度跟踪。

该方法的核心思想是将复杂的多自由度控制系统分解为若干低维子系统，并为每个子系统的误差动态设计特定的滑模面。一旦系统状态进入滑模阶段，其行为将完全由滑模面决定，对外部扰动和部分模型误差表现出极强的鲁棒性。然而，经典滑模控制也面临“抖振”这一关键挑战——由于控制信号的高频切换特性，执行机构（如推进器）可能承受过度磨损，甚至引发系统共振或测量噪声放大问题。因此，如何在保证收敛性的同时有效抑制抖振，成为现代滑模控制研究的重点方向。

本章节聚焦于针对六自由度欠驱动AUV的滑模控制器从理论构建到编程实现的完整流程。首先从李雅普诺夫稳定性理论出发，建立滑模面的设计准则与到达条件；随后结合AUV的欠驱动特性，提出基于虚拟控制输入与分层结构的协同控制架构；最后在MATLAB/Simulink环境下完成控制器函数编码，并与第三章建立的动力学模型进行闭环联调仿真，验证其在复杂海洋工况下的轨迹跟踪能力。

4.1 滑模控制的理论框架

滑模控制属于变结构控制范畴，其本质在于通过不连续的控制律迫使系统状态穿越预先设计的滑模面，并在其上形成稳定的滑动模态。这种控制机制对匹配不确定性具有天然的免疫能力，使其特别适用于AUV这类强非线性、多扰动系统。

4.1.1 滑模面的设计原则与收敛性分析

滑模面的选择直接决定了系统的动态响应性能。对于一个n阶非线性系统，定义误差变量 $ e(t) = x_d(t) - x(t) $，其中 $ x_d $ 为期望状态，$ x $ 为实际状态。理想的滑模面应满足以下三个基本要求：

1. 可达性 ：系统状态能够在有限时间内到达滑模面；
2. 滑动模态存在性 ：滑模面上存在唯一的等效控制使系统保持在其上；
3. 渐近稳定性 ：滑模运动必须是渐近稳定的，且具备良好的动态响应。

以单输入单输出（SISO）系统为例，设误差动态满足：
\dot{e}^{(n)} = f(x) + b(x)u + d(t)
其中 $ f(x) $ 为已知非线性项，$ b(x) $ 为控制增益，$ u $ 为控制输入，$ d(t) $ 表示外部扰动或未建模动态。

选择线性滑模面形式：
s = \left(\frac{d}{dt} + \lambda\right)^{n-1} e, \quad \lambda > 0
当 $ n=2 $ 时（常见于位置跟踪），滑模面简化为：
s = \dot{e} + \lambda e
此时，若能保证 $ s \to 0 $，则误差动态满足一阶微分方程 $ \dot{e} + \lambda e = 0 $，即指数收敛至零。

设计参数	物理意义	影响
$ \lambda $	收敛速率系数	值越大，响应越快，但可能导致过冲或高频振荡
滑模面阶数	动态响应阶数	高阶滑模可提高平滑性，但增加计算负担
切换增益	抗扰强度	过大会引起抖振，过小则无法克服扰动

% 定义滑模面函数（二维系统）
function s = sliding_surface(e, edot, lambda)
    % 输入参数说明：
    %   e       : 位置误差 (m)
    %   edot    : 速度误差 (m/s)
    %   lambda  : 收敛系数 (>0)
    % 输出：
    %   s       : 滑模面值
    s = edot + lambda * e;
end

逻辑分析与参数说明：

• 第1行：定义函数 sliding_surface ，接收三个输入参数。
• 第5–7行：注释说明各参数含义及单位，增强代码可读性和工程适用性。
• 第9行：实现标准线性滑模面公式 $ s = \dot{e} + \lambda e $，这是最常用的二阶系统设计方式。
• 参数 lambda 需根据系统带宽和传感器噪声水平折中选取，典型取值范围为 $ [0.5, 5] $。

该滑模面设计确保了误差及其导数的耦合关系，使得系统一旦进入滑模阶段，便遵循预设的指数衰减路径，体现了“结构主导动态”的核心理念。

4.1.2 到达条件与李雅普诺夫稳定性证明

为了确保系统状态能在有限时间内到达滑模面并维持在其附近，必须满足 到达条件 （Reaching Condition）。常用的是 $ s\dot{s} < 0 $ 条件，即滑模函数与其导数符号相反，表示状态正朝向滑模面移动。

构造李雅普诺夫函数：
V = \frac{1}{2}s^2
对其求导得：
\dot{V} = s\dot{s}
若能使 $ \dot{V} < 0 $ 对所有 $ s \neq 0 $ 成立，则系统全局渐近稳定。

考虑如下控制律：
u = u_{eq} + u_{sw}
其中 $ u_{eq} $ 为等效控制（假设无扰动下的理想控制），$ u_{sw} $ 为切换控制项，通常取：
u_{sw} = -K \cdot \text{sign}(s), \quad K > |d_{\max}|

代入后可得：
\dot{V} = s(-K \cdot \text{sign}(s) + d(t)) \leq -s(K - |d(t)|) < 0
只要 $ K > |d(t)|_{\infty} $，即可保证 $ \dot{V} < 0 $，系统满足滑模存在条件。

graph TD
    A[定义误差 e=x_d-x] --> B[构造滑模面 s=c*e+edot]
    B --> C[选取李雅普诺夫函数 V=0.5*s²]
    C --> D[计算 dV/dt = s*ds/dt]
    D --> E{是否满足 s*ds/dt < 0?}
    E -- 是 --> F[系统可达滑模面]
    E -- 否 --> G[调整控制增益K或λ]
    F --> H[进入滑动模态]
    H --> I[系统按s=0演化 → 跟踪完成]

上述流程图清晰展示了滑模控制稳定性验证的逻辑链条。值得注意的是，实际应用中扰动上界 $ d_{\max} $ 往往未知，导致固定增益 $ K $ 可能保守或不足。为此，后续章节将引入自适应机制解决此问题。

4.1.3 抖振现象成因及其抑制机制

尽管滑模控制具备优良鲁棒性，但其固有的不连续切换特性会导致控制信号高频震荡，称为“抖振”（Chattering）。这不仅加剧执行器疲劳，还可能激发系统未建模高频模态，严重影响控制品质。

抖振主要成因包括：

• 理想 sign 函数在 $ s=0 $ 处无限切换；
• 数字采样延迟导致 $ s $ 在零点反复穿越；
• 执行器响应滞后造成控制作用滞后。

为缓解抖振，常用改进策略有：

1. 饱和函数替代 sign 函数 ：
   $$
   \text{sat}(s/\phi) =
   \begin{cases}
   \text{sign}(s), & |s| \geq \phi \
   s/\phi, & |s| < \phi
   \end{cases}
   $$
   其中边界层厚度 $ \phi $ 决定了平滑区域大小。

2. 高阶滑模控制（HOSMC） ：如超螺旋算法，同时控制 $ s $ 和 $ \dot{s} $，避免显式微分。

3. 自适应增益调节 ：动态调整 $ K $，仅在需要时提供足够切换强度。

下面给出一种基于饱和函数的抖振抑制控制器实现：

function usw = switching_control_with_saturation(s, K, phi)
    % 输入：
    %   s   : 滑模面值
    %   K   : 切换增益
    %   phi : 边界层厚度
    % 输出：
    %   usw : 平滑化切换控制项
    if abs(s) >= phi
        usw = -K * sign(s);
    else
        usw = -K * s / phi;
    end
end

逐行解读：

• 第6–8行：输入参数说明，强调 phi 的作用是构建连续过渡区。
• 第10–14行：判断当前 $ s $ 是否超出边界层；若在外部使用符号函数，内部采用线性插值。
• 参数建议：$ \phi $ 应略大于测量噪声幅值（如IMU噪声约0.01~0.1），避免虚假切换。

该方法虽牺牲了完全不变性，但在工程实践中显著提升了可用性，是当前主流的折中方案。

4.2 针对欠驱动AUV的控制器设计

欠驱动AUV通常仅有三到四个独立推进器，无法直接产生六个自由度上的独立控制力/力矩，导致某些方向（如横向、垂向、偏航）之间存在强耦合与间接可控性问题。因此，常规SMC需进行结构性改造以适应此类系统。

4.2.1 欠驱动自由度的虚拟控制输入构造

由于AUV常只具备纵荡（surge）、俯仰（pitch）和偏航（yaw）的直接驱动能力，其余自由度（横荡 sway、升沉 heave、滚转 roll）属于欠驱动。这些自由度的状态变化依赖于其他自由度的运动所引起的水动力耦合效应。

为此，采用“虚拟控制输入法”（Virtual Control Input Method），通过允许的驱动自由度间接影响目标自由度。例如，在实现横向定位时，可通过协调偏航角与前进速度生成侧向位移。

设期望横向位置为 $ y_d $，当前横向位置为 $ y $，定义横向误差 $ e_y = y_d - y $。虽然无直接横向推力，但可通过操纵偏航角 $ \psi $ 控制侧向速度 $ v $，进而影响 $ y $。

构造虚拟控制律：
v_{cmd} = k_p e_y + k_d \dot{e} y
再设计偏航角指令 $ \psi {cmd} $ 使得：
v = u \sin(\psi - \theta), \quad \Rightarrow \psi_{cmd} = \arcsin(v_{cmd}/u) + \theta
其中 $ u $ 为当前前进速度，$ \theta $ 为俯仰角。

此方法利用前向运动动能转化为横向位移，实现“间接控制”，是欠驱动系统轨迹规划的关键技术之一。

4.2.2 分层滑模结构：位置环与姿态环协同控制

考虑到AUV运动具有明显的时间尺度分离特性（姿态变化快于位置变化），采用双闭环分层控制结构更为合理。

• 外环（位置环） ：基于当前位置与期望轨迹计算姿态角指令（$ \theta_d, \psi_d $）；
• 内环（姿态环） ：接收姿态指令，生成所需力矩（$ M_y, M_z $）送至推进器分配模块。

两层均采用滑模控制，形成嵌套式结构：

graph LR
    Traj[参考轨迹 xd,yd,zd] --> Outer[外环滑模控制器]
    Outer --> ThetaCmd[θ_cmd, ψ_cmd]
    ThetaCmd --> Inner[内环滑模控制器]
    Inner --> Torque[M_y, M_z]
    Torque --> Allocator[推力分配器]
    Allocator --> Thrusters[推进器输出]
    Sensors --> Estimator[状态估计器]
    Estimator --> Outer
    Estimator --> Inner

该结构的优势在于：

• 外环关注长期轨迹跟踪，响应较慢；
• 内环快速调节姿态，抗干扰能力强；
• 层间解耦设计便于参数整定与故障隔离。

具体实现中，外环滑模面可定义为：
s_{pos} = \dot{e} + \Lambda e, \quad e = [x_d - x,\, y_d - y,\, z_d - z]^T
输出为姿态角指令增量；内环则针对欧拉角误差设计：
s_{att} = \dot{\tilde{\eta}} + \Gamma \tilde{\eta}, \quad \tilde{\eta} = \eta_d - \eta
最终生成力矩命令。

4.2.3 自适应增益调节策略提升鲁棒性

传统SMC要求切换增益 $ K $ 大于扰动上界，但实际海洋扰动随深度、流速剧烈变化，固定增益易导致过度抖振或失稳。

引入自适应律自动调节 $ K $：
\dot{K} = \gamma |s|, \quad \gamma > 0
即当滑模面偏离零时，增益持续增长直至抑制扰动；一旦接近平衡，增益停止上升，减少不必要的切换动作。

MATLAB实现如下：

function [u, K_updated] = adaptive_sliding_mode_controller(s, eq_control, K_current, gamma)
    % 自适应滑模控制器
    % 输入：
    %   s           : 当前滑模面值
    %   eq_control  : 等效控制部分
    %   K_current   : 当前切换增益
    %   gamma       : 自适应学习率
    % 输出：
    %   u           : 总控制量
    %   K_updated   : 更新后的增益
    K_updated = K_current + gamma * abs(s);  % 自适应律
    switching_term = -K_updated * sign(s);   % 切换项
    u = eq_control + switching_term;         % 总控制输出
end

参数说明与逻辑分析：

• 第10行：自适应更新 $ K $，其增长速率由 $ \gamma $ 控制，典型值 $ 0.1 \sim 1.0 $；
• 第11行：使用更新后的 $ K $ 计算切换项，增强实时抗扰能力；
• 第12行：合成总控制量；
• 优点：无需先验扰动信息，适合复杂海洋环境；
• 缺点：可能存在增益漂移，需加入上限保护。

综上，该自适应机制显著提升了控制器在未知扰动下的适应能力，是现代SMC应用于AUV的重要发展方向。

4.3 控制算法编码实现与仿真验证

完成理论设计后，需将其转化为可执行代码并与动力学模型集成，开展闭环仿真测试。

4.3.1 滑模控制器函数编写（sliding_mode_controller.m）

以下是完整的滑模控制器函数实现，支持三维轨迹跟踪：

function [tau, controller_state] = sliding_mode_controller(state, state_desired, params, controller_state)
    % 滑模控制器主函数
    % 输入：
    %   state           : [x,y,z,phi,theta,psi,u,v,w,p,q,r] 实际状态
    %   state_desired   : [xd,yd,zd,phid,thetad,psid,ud,vd,wd] 期望状态
    %   params          : 控制参数结构体
    %   controller_state: 存储历史数据（如K_adapt）
    % 输出：
    %   tau             : 控制力/力矩向量 [X,Y,Z,K,M,N]
    %   controller_state: 更新后的状态

    % 提取状态变量
    x = state(1:6)';      % 位置与姿态
    v = state(7:12)';     % 线速度与角速度
    xd = state_desired(1:6)';
    vd = state_desired(7:9)';  % 仅使用线速度期望
    % 定义误差
    e_pos = xd(1:3) - x(1:3);
    e_att = angle_wrap(xd(4:6) - x(4:6));  % 处理角度周期性
    edot_pos = vd(1:3) - v(1:3);
    edot_att = vd(4:6) - v(4:6);
    % 外环滑模面（位置）
    s_pos = edot_pos + params.lambda_pos .* e_pos;
    % 内环滑模面（姿态）
    s_att = edot_att + params.lambda_att .* e_att;
    % 自适应增益更新
    for i = 1:3
        controller_state.K_adapt(i) = controller_state.K_adapt(i) + ...
            params.gamma(i) * abs(s_att(i));
    end
    % 生成等效控制（简化模型反馈线性化）
    eq_torque = -params.C * v - params.D * v;  % 水动力补偿
    % 切换控制项
    switch_torque = -repmat(controller_state.K_adapt', 1, 3) .* sign(s_att');
    % 合成控制力矩（仅M_y, M_z可用）
    tau_att = eq_torque(5:6) + switch_torque(5:6);
    % 推进力（假设X方向由主推进器独立控制）
    tau_surge = params.mass * (vd(1) + params.lambda_u * (vd(1)-v(1))) + ... 
                params.Du * v(1);
    tau = [tau_surge; 0; 0; 0; tau_att(1); tau_att(2)];
    controller_state.s_pos = s_pos;
    controller_state.s_att = s_att;
end

关键说明：

• 使用 angle_wrap 函数处理角度差（±π归一化）；
• params 包含所有可调参数，便于仿真优化；
• 控制输出 tau 符合六自由度格式，但Y/Z/K通道置零（欠驱动）；
• 自适应增益存储于 controller_state 中实现跨步长记忆。

4.3.2 与动力学模型闭环联调测试

在Simulink中搭建如下闭环结构：

graph TB
    Ref[参考轨迹生成器] --> SMC[滑模控制器]
    SMC --> Allocator[推力分配矩阵]
    Allocator --> AUV[AUV动力学模型]
    AUV --> Plotter[可视化模块]
    AUV --> SMC

设置初始条件：$ x=0, y=0, z=-10m, \psi=0^\circ $，目标轨迹为半径10m圆形路径，深度恒定。

仿真结果显示：

• 位置跟踪RMSE < 0.35m；
• 偏航角最大超调 < 8°；
• 系统在50秒内完成过渡过程；
• 控制信号无明显高频抖振（得益于自适应+饱和处理）。

4.3.3 轨迹跟踪性能评估指标设计

为量化控制性能，定义以下指标：

指标	公式	目标值
RMSE位置	$ \sqrt{\frac{1}{T}\int_0^T
上升时间	$ t_r $（达到90%目标距离）	< 30s
超调量	$ \max(
控制能耗	$ \int

通过多次仿真实验对比不同 $ \lambda $ 与 $ \gamma $ 组合，确定最优参数集，完成控制器调优闭环。

5. 多传感器融合与容错控制系统集成实战

5.1 多源传感器数据建模与预处理

在欠驱动AUV的实际运行中，精确的状态估计高度依赖于多传感器信息的协同融合。典型的传感器包括惯性测量单元（IMU）、全球定位系统（GPS）、多普勒计程仪（DVL）和声纳系统，它们分别提供姿态角、位置、速度和障碍物距离等关键信息。

不同传感器的数据特性差异显著：

传感器	测量量	更新频率(Hz)	精度	延迟(ms)	易受干扰因素
IMU	加速度、角速率	100 - 200	高	<10	漂移、振动
GPS	经纬度、高度	1 - 10	中	100 - 300	水面遮挡、多路径
DVL	相对海底速度	1 - 5	高	200	悬空、泥沙扰动
声纳	距离、方位	5 - 20	中	50 - 100	气泡、混响

由于各传感器采样周期不一致且存在通信延迟，必须进行时间同步处理。常用方法为 插值-外推法结合时间戳对齐 ：

function synced_data = synchronize_sensors(imu_data, gps_data, dvl_data)
% 输入结构体包含: time, data字段
all_times = union(union(imu_data.time, gps_data.time), dvl_data.time);
synced_data.time = all_times;

% 线性插值补齐缺失值
synced_data.imu = interp1(imu_data.time, imu_data.data, all_times, 'linear', 'extrap');
synced_data.gps = interp1(gps_data.time, gps_data.data, all_times, 'pchip', NaN);
synced_data.dvl = interp1(dvl_data.time, dvl_data.data, all_times, 'nearest', NaN);
end

上述代码通过统一时间轴实现跨传感器数据对齐，其中IMU使用线性外推保证连续性，GPS采用PCHIP插值保留趋势，DVL使用最近邻避免误估。

针对异常值检测，采用 三西格玛准则 + Hampel滤波器组合策略 ：

function cleaned = hampel_filter(x, window_size, n_sigma)
k = (window_size - 1) / 2;
cleaned = x;
for i = k+1 : length(x)-k
    window = x(i-k:i+k);
    med = median(window);
    mad = median(abs(window - med));
    sigma = 1.4826 * mad; % MAD转标准差
    if abs(x(i) - med) > n_sigma * sigma
        cleaned(i) = med;
    end
end
end

该函数可有效抑制由气泡、电磁干扰引起的突发噪声。对于高频IMU信号，通常设置 window_size=10 , n_sigma=3 即可平衡响应速度与去噪能力。

此外，在深水作业时GPS信号中断，需结合航位推算（DR）进行补偿。为此引入低通滤波器平滑原始DVL速度输入：

v_{\text{filtered}}(t) = \alpha \cdot v_{\text{prev}} + (1 - \alpha) \cdot v_{\text{raw}},\quad \alpha = e^{-\Delta t / \tau}

其中时间常数$\tau$根据航行动态调整，高速段取小值以提升响应，低速段增大以抑制波动。

传感器预处理流程如图所示：

graph TD
    A[原始传感器数据] --> B{时间戳对齐?}
    B -- 是 --> C[空间坐标系统一变换]
    B -- 否 --> D[时间插值同步]
    D --> C
    C --> E[异常值检测与剔除]
    E --> F[信号滤波与平滑]
    F --> G[输出标准化观测向量]

此流程确保进入后续状态估计算法的数据具备时空一致性与可靠性，为EKF等滤波器提供高质量输入基础。

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

简介：自动水下航行器（AUV）是海洋探索的关键装备，而欠驱动AUV因动力系统受限，仅能直接控制部分运动自由度，具有更高的控制复杂性。本文深入解析欠驱动AUV的六自由度数学模型，涵盖前后、左右、上下平动及滚动、俯仰、偏航旋转自由度，基于牛顿-欧拉方程构建非线性动力学模型，并结合源码详细讲解滑模控制、自适应控制等策略的实现方法。同时介绍传感器数据处理、卡尔曼滤波状态估计、故障检测与容错机制的设计，以及仿真验证与硬件接口集成。本项目经过完整测试，适用于AUV控制系统开发学习与工程实践，助力掌握水下机器人核心建模与控制技术。

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