0 引　言

自主水下潜航器（AUV）具有智能、灵活和隐蔽等特性，在海洋科考、水下救援和军事侦察等领域得到了广泛应用^{[1 − 3]}。然而AUV受自身携带能源和水下恶劣通信环境的限制^[4]，长时间作业后需要及时回收进行能源补充和数据回传等操作。近年来，水下对接已成为一种较为自主的回收方式^{[5 − 7]}，但水下环境复杂，近距离回收阶段对姿态的控制要求较为苛刻，AUV需要对目标姿态进行快速响应及跟踪才能实现可靠回收。

目前，实现AUV控制的主要方法包括PID控制、模糊控制、滑模控制和自抗扰控制等^[8,9]。PID控制算法简单且比较容易实现，在水下机器人控制领域得到广泛应用^[10]。但对于模型复杂、非线性的AUV控制对象，PID算法难以实现精确的运动控制和干扰补偿，实际应用中通常要与其他的控制方法相结合。模糊控制通过利用模糊规则和论域分割来简化复杂系统，能够有效地处理系统的非线性和不确定^[11]。但由于无法获取较好的AUV水下操纵经验，模糊规则及隶属函数的设定存在困难。滑模控制能够根据状态变量信息不断优化系统结构，具有良好的响应特性和鲁棒性^[12]。但滑模面的设计和参数调整相对复杂，且变结构控制律的不连续导致系统存在抖振缺陷。自抗扰控制是在PID基础上进行改进的控制方法^[13,14]，通过引入抗扰动观测器来实现对系统扰动的实时估计和补偿，不需要被控对象具有精确的数学模型，并且具有强抗扰性、高精度和自适应性。

本文采用自抗扰控制技术来实现AUV的姿态跟踪控制，降低控制系统对复杂运动模型的依赖。在构建AUV运动学和动力学模型的前提下，设计一种含平滑低通滤波的改进型自抗扰控制器，并采用无扰动和含阶跃、正弦波的扰动波形作为系统输入进行仿真。通过与PID的结果进行比对，验证了该改进型控制器的有效性。

1 水下航行器运动模型 1.1 坐标系构建

为了分析AUV在整个海洋空间中的运动，按照ITTC推荐及SNAME术语体系的要求^[15]，建立了固定坐标系$E - \xi \eta \zeta $和运动坐标系$O - xyz$，如图1所示。

AUV的浮心为运动坐标系的原点，空间位置可以用x、y、z来表示。上述两坐标系中的$ \phi 、\theta 、\psi $分别为横滚角、俯仰角和航向角；u、v、w分别为横向、纵向、垂向线速度；p、q、r分别为横倾、纵倾、摇艏角速度；此外AUV在3个方向上所受的力分别用X、Y、Z表示，对应的力矩分别为L、M、N。上述变量组成的位姿向量、速度向量、力/力矩向量表达式分别如下：

$ \left\{ {\begin{array}{*{20}{l}} {\eta = {{\left[ {\begin{array}{*{20}{c}} x&y&z&\phi &\theta &\psi \end{array}} \right]}^{\rm T}}}，\\ {v = {{\left[ {\begin{array}{*{20}{c}} u&v&w&p&q&r \end{array}} \right]}^{\rm T}}} ，\\ {\tau = {{\left[ {\begin{array}{*{20}{c}} X&Y&Z&L&M&N \end{array}} \right]}^{\rm T}}} 。\end{array}} \right. $

(1)

1.2 空间运动模型 1.2.1 运动学模型

AUV的刚体运动涉及6个自由度，分为沿x、y和$z$轴的平动和旋转运动。基于上述两坐标系间的转换关系，可以建立AUV的空间运动学方程式如下：

$ \dot \eta = \left( {\begin{array}{*{20}{c}} {{{\boldsymbol J}_1}\left( \eta \right)}&{{0_{3 \times 3}}} \\ {{0_{3 \times 3}}}&{{{\boldsymbol J}_2}\left( \eta \right)} \end{array}} \right)\nu。$

(2)

${{\boldsymbol J}_1}\left( \eta \right)$和${{\boldsymbol J}_2}\left( \eta \right)$分别为线速度和角速度转换矩阵，具体表达式如下：

$\begin{split} {{\boldsymbol J}_1}(\eta ) =& \left[ \begin{array}{*{20}{c}} \begin{gathered} \cos \psi \cos \theta \\ \sin \psi \cos \theta \\ - \sin \theta \\ \end{gathered} &\begin{gathered} \cos \psi \sin \theta \sin \varphi - \sin \psi \cos \varphi \\ \sin \psi \sin \theta \sin \varphi + \cos \psi \cos \varphi \\ \cos \theta \sin \varphi \\ \end{gathered} \end{array}\right. \\ &\left.\begin{gathered} \cos \psi \sin \theta \cos \varphi - \sin \psi \sin \varphi \\ \sin \psi \sin \theta \cos \varphi - \cos \psi \sin \varphi \\ \cos \theta \cos \varphi \\ \end{gathered} \right]，\end{split}$

(3)

$ {{\boldsymbol J}_2}(\eta ) = \left[ {\begin{array}{*{20}{c}} \begin{gathered} 1 \\ 0 \\ 0 \\ \end{gathered} &\begin{gathered} \tan \theta \sin \varphi \\ \cos \varphi \\ \sin \varphi \sec \theta \\ \end{gathered} &\begin{gathered} \tan \theta \cos \varphi \\ - \sin \varphi \\ \cos \varphi \sec \theta \\ \end{gathered} \end{array}} \right]。$

(4)

实际运动中，AUV姿态角的取值范围为：

$ \left\{ {\begin{array}{*{20}{l}} { - {\text π} < \phi \leqslant {\text π} }，\\ { - \dfrac{{\text π} }{2} < \theta \leqslant \dfrac{{\text π} }{2}}，\\ { - {\text π} < \psi \leqslant {\text π} } 。\end{array}} \right. $

(5)

1.2.2 动力学模型

AUV水下运动时受到惯性力、静水力、流体力、螺旋桨推进力的共同作用，动力学呈现非线性、不确定性等复杂特性。根据AUV所受外力和力矩关系，并且依据牛顿定律和动量矩定理可推导出AUV的动力学方程式为：

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

(6)

式中：M为惯性矩阵，包括刚体惯性力矩阵M_RB和附加质量系数力矩阵M_A；C为科里奥利向心力矩阵，包括刚体科氏向心力矩阵C_RB(v)和科氏向心力系数矩阵C_A(v)；D(v)为阻尼矩阵；g(η)为恢复力矩；τ为执行机构产生的控制力矩阵。

1.3 模型简化

AUV的六自由度模型较为复杂，具有非线性和强耦合性。为了便于控制器系统的设计，针对本文AUV的特性及运动规律，可对模型进行合理简化。AUV空间运动可解耦成沿3个平面的运动，并且由于AUV四桨矢量推动器呈轴对称“X”型结构，自身动力抗横滚，因此将AUV模型简化成水平面与垂直面的运动模型。

1.3.1 水平面运动模型

AUV在水平面的运动为沿$x$轴的进退运动和绕$z$轴的航向运动，忽略AUV的升沉运动和横滚运动，可得AUV水平面的运动学方程为：

$ \left\{ \begin{gathered} \dot x = u\cos \psi - v\sin \psi，\\ \dot y = u\sin \psi + v\cos \psi，\\ \dot \psi = r。\\ \end{gathered} \right. $

(7)

AUV水平面的动力学方程为：

$ \left\{ \begin{gathered} \left( {m - {X_{\dot u}}} \right)\dot u = \left( {m - {Y_{\dot v}}} \right)vr + \left( {{X_u} + {X_{u|u|}}|u|} \right)u + X ，\\ \left( {m - {Y_{\dot v}}} \right)\dot v = - \left( {m - {X_{\dot u}}} \right)ur + \left( {{Y_v} + {Y_{v|v|}}|v|} \right)v ，\\ \left( {{I_z} - {N_{\dot r}}} \right)\dot r = - \left( {{X_{\dot u}} - {Y_{\dot v}}} \right)uv + \left( {{N_r} + {N_{r|r|}}|r|} \right)r + N。\\ \end{gathered} \right. $

(8)

1.3.2 垂直面运动模型

AUV在垂直面的运动为沿$z$轴的升沉运动和绕$y$轴的俯仰运动，忽略AUV的侧向运动和横滚运动，可得AUV垂直面的运动学方程为：

$ \left\{ \begin{gathered} \dot x = u\cos \theta - w\sin \theta，\\ \dot z = - u\sin \theta + w\cos \theta，\\ \dot \theta = q。\\ \end{gathered} \right. $

(9)

AUV垂直面的动力学方程为：

$ \left\{ \begin{gathered} \left( {m - {X_{\dot u}}} \right)\dot u = - \left( {m - {Z_{\dot w}}} \right)uw + \left( {{X_u} + {X_{u|u|}}|u|} \right)u - \\ \qquad\qquad\quad (W - B)\sin \theta + X，\\ \left( {m - {Z_{\dot w}}\dot w} \right)\dot w = {Z_{\dot q}}\dot q + \left( {m - {X_{\dot u}}} \right)uw + (W - B)\cos \theta + \\ \qquad\qquad\qquad \left( {{Z_w} + {Z_{w|w|}}|w|} \right)w，\\ \left( {{I_y} - {M_{\dot q}}} \right)\dot q = {M_{\dot w}}\dot w + \left( {{X_{\dot u}} - {Z_{\dot w}}} \right)uw + + {Z_B}B\sin \theta + \\ \qquad\qquad\quad \left( {{M_q} + {M_{q|q|}}|q|} \right)q + M。\\ \end{gathered} \right. $

(10)

2 改进型自抗扰控制器

自抗扰控制的核心在于将非线性、不确定性与外部扰动视为系统的总扰动，并利用对象的输入输出信息进行实时估计。通过将控制系统补偿为标准的积分串联型，结合比例-微分控制部分实现精准而稳定的姿态跟踪控制。自抗扰控制器由跟踪微分器、扩张状态观测器和非线性状态误差反馈控制律3部分组成，本文根据其原理对系统各部件进行设计，并采用滑动平均低通滤波对干扰通道进行补偿优化。

2.1 跟踪微分器

跟踪微分器是系统的重要的组成部分，负责安排过渡过程，防止输入信号的跳变。它通过建立无超调的快速跟踪，可有效解决响应速度与超调性之间的矛盾。跟踪微分器的公式为：

$ \left\{ \begin{gathered} {\text{fh}} = {\text{fhan}}\left( {{\theta _1} - {\theta _0},{\theta _2},r,{h_0}} \right)，\\ {\theta _1} = {\theta _1} + h{\theta _2}，\\ {\theta _2} = {\theta _2} + h{\text{fh}}，\\ \end{gathered} \right. $

(11)

式中：${\theta _0}$为给定输入信号，即AUV与对接站之间的相对航向或俯仰偏差角度；${\theta _1}$为对${\theta _0}$的平滑过渡信号；${\theta _2}$是对${\theta _0}$的微分信号；h为积分步长，与系统的采样频率有关；fh为快速最优控制综合函数，其中r为跟踪速度因子，${h_0}$为滤波因子。fh的具体实现如下式所示:

$ \left\{ {\begin{array}{*{20}{l}} {d = {h_0}r_0^2,{a_0} = {h_0}{\theta _2},y = {\theta _1} - {\theta _0} + {a_0}}，\\ {{a_1} = \sqrt {d(d + 8|y|)} }，\\ {{a_2} = {a_0} + {\rm sign} (y)\dfrac{{\left( {{a_1} - d} \right)}}{2}}，\\ {{s_y} = \dfrac{{({\rm sign} (y + d) - {\rm sign} (y - d))}}{2}}，\\ {a = \left( {{a_0} + y - {a_2}} \right){s_y} + {a_2}} ，\\ {{s_a} = \dfrac{{({\rm sign} (a + d) - {\rm sign} (a - d))}}{2}} ，\\ {{\text{ fhan }} = - {r_0}\left( {\dfrac{a}{d} - {\rm sign} (a)} \right){s_a} - {r_0}{\rm sign} (a)} 。\end{array}} \right. $

(12)

2.2 非线性状态误差反馈控制律

非线性状态误差反馈控制律是系统的另一关键组成部分，用于制定被控对象的控制策略，它能够根据系统的状态误差来调整控制器的输出。非线性状态误差反馈控制律结合了跟踪微分器和扩张观测器的输出，通过非线性方式将信号误差和信号误差的微分进行组合，从而得到输出观测估计值误差。状态误差反馈控制律如下：

$ \left\{ \begin{gathered} {e_{\text{1}}} = {\theta _1} - {z_1} ，\\ {e_2} = {\theta _2} - {z_2} ，\\ {u_0} = {\beta _0}{\text{fal}}({e_1},{\alpha _0},\delta ) + {\beta _1}{\text{fal}}({e_2},{\alpha _1},\delta )。\\ \end{gathered} \right. $

(13)

式中：${e_{\text{1}}}$为信号误差；${e_2}$为信号误差的微分；u₀为误差反馈的控制量；β为控制器的增益系数；fal为非线性函数，其公式如下：

$ {\text{fal}}(e,\alpha ,\delta ) = \left\{ \begin{gathered} \dfrac{e}{{{\delta ^{1 - \alpha }}}},\left| e \right| \leqslant \delta，\\ {\left| e \right|^\alpha }{\rm sign}(e),\left| e \right| \geqslant \delta 。\\ \end{gathered} \right. $

(14)

2.3 改进型扩张状态观测器

扩张状态观测器是系统的核心部分，一方面它能够对系统输入信号的状态变量进行跟踪，实时了解系统的状态；另一方面它还能够识别和估计系统内部扰动和外部高频噪声干扰，将其扩张成额外的状态变量，并以反馈的形式对其加以及时补偿。扩张状态观测器通过对系统状态和扰动的估计和补偿，能够为系统提供准确的控制信息，从而实现对系统的主动干预和自适应调整。扩张状态观测器的公式为：

$ \left\{ {\begin{array}{*{20}{l}} {e = {z_1} - \theta }, \\ {fe = {\rm fal}\left( {e,{\alpha _2},{\delta _1}} \right)}, \\ {f{e_1} = {\rm fal}\left( {e,{\alpha _3},{\delta _1}} \right)} ,\\ {{{\dot z}_1} = {z_2} - {\beta _{01}}e} ,\\ {{{\dot z}_2} = {z_3} + {b_0}u - {\beta _{02}}fe}, \\ {{{\dot z}_3} = - {\beta _{03}}f{e_1}} 。\end{array}} \right. $

(15)

式中：e为跟踪误差；$ {z_1} $和$ {z_2} $分别为输出信号的跟踪和微分信号；$ {z_3} $为扰动观测值。

由于$ {z_3} $具有高频特性，将其直接用于干扰信号的补偿可能会导致控制系统发生抖振，并且难以通过调整系统参数来消除该现象。本文采用滑动平均低通滤波的方式对$ {z_3} $进行处理，以优化系统的结构并提高系统的稳定性。

滑动平均低通滤波公式如下:

$ {{{u}}_{\text{1}}}(k) = \frac{1}{N} \sum_{i = 0}^{N - 1} {z_3}(k - i)。$

(16)

式中：N为采样深度；i为求和计算工程中起索引作用的变量。

最终控制器通过补偿获得的输出为:

$ u = {u_0} - \frac{{{u_1}}}{{{b_0}}}。$

(17)

自抗扰与滑动平均低通滤波控制器的系统框图如图2所示。

3 仿真实验分析

为验证上述改进型自抗扰控制器的有效性，在Matla/Simulink中搭建了控制器仿真模型。在仿真实验中，采用自定义的预设姿态角作为AUV回收时的航向和俯仰偏差姿态数据，并将改进的自抗扰控制器与传统PID姿态跟踪效果进行对比验证。

3.1 无扰动仿真实验

无外界扰动环境下，将预设姿态角设定为固定值，此时预设姿态曲线为一条$\psi = 0.52$的直线。改进型自抗扰控制器与PID控制器的姿态跟踪仿真结果如图3所示。

从仿真对比实验结果可知，改进型自抗扰控制器的进入稳态的时间比PID提前约3 s，并且有着较高的姿态的跟踪精度，可以实现更快更精准跟踪；此外，其姿态控制调节时间比PID超前，可以实现更快的响应控制；并且姿态调节波动的峰值较低，姿态跟踪过程较为平滑，超调量由17.2%降低到9.4%。仿真结果对比说明，改进型自抗扰控制器比PID控制器的动态响应效果更好，可以实现对设定简单姿态的稳定跟踪。

3.2 有扰动仿真实验

为有效验证在有干扰环境下的姿态跟踪效果，在设定姿态期望值为0的基础上，分别引入经典的阶跃扰动波形和高频正弦扰动波形进行实验，仿真实验结果如图4和图5所示。

可以看出，改进型自抗扰控制器稳定回归到预设曲线的调整时间比PID要减少2.5 s左右，可以快速恢复到稳态，并且其超调波动控制在很小的范围内，姿态跟随效果更好。仿真结果对比说明，改进型自抗扰控制器比PID控制器具有更强的抗干扰能力和自适应力。

从仿真对比实验结果可知，在受到高频正弦波的干扰下，PID已发生明显震荡，导致姿态跟踪失去了稳定性。而改进型自抗扰控制器在2 s左右实现了平稳收敛，跟踪曲线与预设曲线贴合紧密，观测误差不存在振荡。说明采用改进型自抗扰控制器可以实现对设定较复杂姿态变化的稳定跟踪，具有较强的鲁棒性和稳定性。

4 结　语

本文在建立AUV水平面和垂直面数学模型的基础上，构造了一款改进型自抗扰控制器进行对接姿态跟踪仿真研究。得到的实验数据表明，在无扰动情况下姿态控制可以进行快速响应并精确跟踪，在有扰动情况下姿态可以进行快速恢复并保持稳态。该改进型自抗扰控制器明显改善了传统PID的动静态响应特性和鲁棒性，具有较强的姿态控制优势，可以为水下机器人回收时的姿态控制提供技术支撑，具有实际的应用价值。