0 引　言

AUV在执行任务期间，自身携带能源有限，这是制约其作业时间的关键问题^[1]。科学家们提出了水下对接概念，使 AUV 能够长期独立运行，以满足长期部署的目标^[2]。然而AUV水下对接的成功率受诸多因素的影响，例如海流、天气、设备误差等。

要实现AUV的成功对接，需要AUV能够制导、导航和控制自身进入对接站^[3]。AUV是高度非线性、强耦合的平台^[4]，航行过程中由于水动力参数、海流扰动的不确定性等，难以实现理想的对接效果，对此问题有许多学者展开研究。基于点对点的追踪制导将对接过程视作2个目标点的追逐，利用相对位置数据实时计算AUV自身与对接站之间的角度并生成航向指令引导对接过程，该方法在采用漏斗形对接站的对接系统中被广泛应用^[5]。基于横向偏差收敛的制导将对接过程视作AUV与对接站基准中线之间的横向距离控制，该方法以横向偏差作为反馈输入，航向角指令作为输出，一般与PID控制配合使用^[6]。上述2种方法不能保证AUV进入对接站的航向角，可能出现与对接站的大角度碰撞问题。基于模糊规则的制导，以对接站为中心，根据相对位置划分区域，通过专家经验为每个区域生成航向角指令和速度指令^[7]。该方法不依赖模型和精确的数学运算，但模糊规则的制定需要通过实验反复修正，存在一定局限性。基于余弦曲线调整的制导，通过设计初始航向角和距离，使AUV航向角按余弦曲线平滑调整，一般用于对接末端的姿态调节^[8]。控制方法上，PID控制是目前应用最广泛、最成熟的控制器，其结构简单调节方便，但较难适应复杂模型和变化环境^[9]。滑模控制也是AUV常见的控制方法，通过设计控制律将系统状态控制到期望的滑模面内，对模型参数变化和外界干扰不敏感，但滑模面的切换过程中产生抖振，影响系统稳定^[10]。模型预测控制通过预测模型、滚动优化、反馈校正这3个步骤预测系统未来输出，实现最优控制效果^[11]。该方法适合处理带约束的问题，但计算复杂度高，设计难度较大，依赖精确的数学模型。自抗扰控制相对于其他控制方法而言，无需清楚模型的精确信息，通过估计扰动并实时消除，实现对扰动的抑制，具有良好的抗干扰性和鲁棒性。在水下机器人运动控制领域逐渐得到应用^{[12 − 14]}。

基于上述分析，在洋流干扰和随机扰动下保持对航向和横向偏差的控制能力，是AUV对接的难点问题。针对以上存在问题，本文将追踪制导、横向偏差制导、余弦曲线调整制导等多种制导方法相结合生成三段式对接控制指令，采用自抗扰控制跟踪生成的航向角指令，设计运动学洋流观测器估计洋流并补偿洋流偏角，实现AUV在洋流下的精准对接，通过Matlab/Simulink仿真实验验证了所设计方法的有效性。

1 AUV数学模型 1.1 坐标系与参数定义

本文采用国际水池会议（ITTC）推荐的造船与轮机工程师学会（SNAME）术语公报的体系。坐标系采用2个右手直角坐标系，如图1所示，一个是固定坐标系$E - \xi \eta \zeta $，也叫惯性坐标系，用于表示AUV的空间位置和方向；另一个是运动坐标系$G - xyz$，也叫随体坐标系，固连于AUV上随AUV一起运动，用于表示AUV的水动力和转动惯量，一般选在AUV重心位置^[15]。

相关参数如表1所示。

1.2 AUV运动学模型

AUV的水下运动可视作刚体运动，将其分解为沿着随体坐标系坐标轴方向的平动和绕坐标轴的旋转运动。AUV在惯性坐标系下的速度矢量为$\mathop P\limits^ \cdot = {\left[ {\begin{array}{*{20}{c}} {\mathop {{\xi _E}}\limits^ \cdot }&{\mathop {{\eta _E}}\limits^ \cdot }&{\mathop {{\zeta _E}}\limits^ \cdot } \end{array}} \right]^\text T}$，在随体坐标系下的速度矢量为$V = {\left[ {\begin{array}{*{20}{c}} u&v&w \end{array}} \right]^{\text T}}$，二者之间的转换关系为：

$ \mathop P\limits^ \cdot = {\boldsymbol{T}_1}V，$

(1)

$ {\boldsymbol{T}_1} = \left[ {\begin{array}{*{20}{c}} {\cos \psi \cos \theta }&{\cos \psi \sin \theta \sin \phi - \sin \psi \cos \phi }&{\cos \psi \sin \theta \cos \phi + \sin \psi \sin \phi } \\ {\sin \psi \cos \theta }&{\sin \psi \sin \theta \sin \phi + \cos \psi \cos \phi }&{\sin \psi \sin \theta \cos \phi - \cos \psi \sin \phi } \\ { - \sin \theta }&{\cos \theta \sin \phi }&{\cos \theta \cos \phi } \end{array}} \right]。$

(2)

AUV在惯性坐标系下任一位置角速度矢量为$ \mathop \varTheta \limits^ \cdot = {\left[ {\begin{array}{*{20}{c}} {\mathop \phi \limits^ \cdot }&{\mathop \theta \limits^ \cdot }&{\mathop \psi \limits^ \cdot } \end{array}} \right]^{\rm T}} $，在随体坐标系下的角速度矢量为$\varOmega = {\left[ {\begin{array}{*{20}{c}} p&q&r \end{array}} \right]^{\text T}}$，则二者之间的转换关系为：

$ \mathop \varTheta \limits^ \cdot = {T_2}\varOmega，$

(3)

$ {\boldsymbol{T}_2} = \left[ {\begin{array}{*{20}{c}} 1&{\sin \phi \tan \theta }&{\cos \phi \tan \theta } \\ 0&{\cos \phi }&{ - \sin \phi } \\ 0&{\sin \phi /\cos \theta }&{\cos \phi /\cos \theta } \end{array}} \right]。$

(4)

式中：${\boldsymbol{T}_1}$、${\boldsymbol{T}_2}$为随体坐标系到惯性坐标系的矢量转换矩阵。

1.3 AUV动力学模型

根据刚体动力学理论对AUV在水中的运动进行分析，其主要受推力器推力、重力、浮力、水动力及相关力矩的作用，综合形成AUV六自由度的空间运动。利用牛顿—欧拉方程搭建 AUV的空间动力学模型如下^[16]：

$ \boldsymbol{M_{\boldsymbol{RB}}}\mathop v\limits^ \cdot + \boldsymbol{C_{\boldsymbol{RB}}}\left( v \right)v = {\tau _{RB}}，$

(5)

$ {\tau _{RB}} = {\tau _{hyd}} + {\tau _{hs}} + {\tau _{\rm wind}} + {\tau _{\rm wave}} + \tau，$

(6)

$ {\tau _{hyd}} = - {M_A}\mathop v\limits^ \cdot - {C_A}\left( v \right)v - D\left( v \right)v ，$

(7)

$ {\tau _{hs}} = g\left( \eta \right) + {g_o}。$

(8)

式中：$v = {\left[ {\begin{array}{*{20}{c}} V &\varOmega \end{array}} \right]^{\mathrm{T}}}$；$\boldsymbol{M_{\boldsymbol{RB}}}$为刚体惯性矩阵；$\boldsymbol{C_{\boldsymbol{RB}}}$为刚体科里奥利和向心力矩阵；${\tau _{RB}}$为刚体所受的所有外力和外力矩；${\tau _{hyd}}$为刚体所受的水动力项；${\tau _{hs}}$为由重力浮力等引起的水静力项；${\tau _{\text{wind}}}$和${\tau _{\text{wave}}}$为风和波浪引起的干扰力和力矩；$\tau = [\begin{array}{*{20}{c}} X\;\;Y\;\;Z\;\;K\;\;M\;\;N \end{array}]$为AUV本身的推进力和力矩。在本文所用的AUV模型中由螺旋桨和十字舵提供。

1.4 洋流模型

现实对接环境中，洋流因素不可忽略，因此研究AUV运动时，需建立海流简化模型加以考虑。

为方便研究，将AUV航行过程中的洋流视作水平无旋洋流，在惯性坐标系下定义为${V_C} = [{V_c} \ {\beta _c}]$，其中${V_c}$为洋流速度大小；${\beta _c}$为洋流方向；则洋流在惯性坐标系下的速度矢量为$V_c^E = \left[ {\begin{array}{*{20}{c}} {{V_c}\cos \left( {{\beta _c}} \right)}&{{V_c}\sin \left( {{\beta _c}} \right)}&0 \end{array}} \right]$，转换到随体坐标系下的洋流速度矢量为$V_c^b = T_1^{ - 1}V_c^E$ ^[16]。

AUV相对于洋流的速度矢量可表示为${V_r} = V - V_c^b$，替换掉动力学方程中的矢量$V$，即可得AUV在洋流下的运动模型。

2 带洋流补偿的三段式对接航向角制导

AUV的对接运动控制，是AUV能完成对接等作业任务的基础。AUV根据任务使命的要求，规划出完成任务所需的期望目标输出，该过程称为制导。根据该期望输出计算出AUV所需的推进器输入量和舵角输入量，来控制AUV按照预定的姿态或轨迹航行，进而完成任务，该过程称之为控制。

针对本文中的AUV对接过程，为简化控制方案设计，设置螺旋桨以恒定转速运行，AUV对接航速保持不变。根据制导环节规划出期望的航向角，设计控制器以跟踪期望航向角，最终实现成功对接。

对接航向角制导，采用三段式制导法。在AUV距离对接站较远的情况下，采用基于点对点的追踪制导，追踪对接站前方的某一个点，即第一段制导；当AUV到达该点附近时，采用基于横向偏差收敛的制导法，旨在让AUV的运行轨迹保持在对接站的基准中线上，即第二段制导；当AUV到达对接站前方很近位置时，采用基于余弦曲线调整的制导，将AUV的航向角调节至0，最终进入对接站，即第三段制导。

2.1 基于点对点的追踪制导

定义AUV航行中的实时位置为$\left( {x,y} \right)$；追踪点的位置为$({{x_p},{y_p}})$。追踪点的$y$方向位置定义设置为与对接站中线$y$方向位置相同，则期望的航向角计算式为：

$ {\psi _d} = a\tan 2\left( {\frac{{{y_p} - y}}{{{x_p} - x}}} \right)\frac{{180}}{\text{π} } 。$

(9)

2.2 基于横向偏差收敛的制导

当AUV到达追踪点附近时，判断AUV的$x$位置实施制导律切换。该阶段主要的控制对象是AUV的横向位移$y$和对接站${y_d}$之间的差值，$e = {y_d} - y$，当差值逐渐收敛为0并稳定时，即可满足要求，此时AUV以不为0的航向角沿着对接站的基准中线向对接站接近。PID是基于误差的控制，且不依赖模型，因此该阶段采用PID控制来生成期望的航向角指令，如图2所示。

2.3 基于余弦曲线调整的制导

当AUV航行到对接站前方很近一段距离时，制导律切换。该阶段目的是调整航向角，使其逐渐减小为0。不存在洋流时，该阶段制导可不必发挥作用，但存在洋流时，AUV在对接第二阶段结束时的航向角不为0，若以此航向角对接，则可能超过漏斗形对接站允许的对接角度范围，或者与对接站产生大角度碰撞，造成对接失败。

该阶段选取如下余弦曲线作为参考航向曲线：

$ {\psi _r} = \frac{{{\psi _{crab}}}}{2}\left( {1 - \cos \left( {\frac{d}{{{d_f}}}{\text π} } \right)} \right)。$

(10)

式中：${\psi _r}$为对接参考航向角；${\psi _{crab}}$为对接第二阶段末的AUV航向角。该航向角与洋流有关，$d$为AUV与对接站之间的距离值；${d_f}$为第二阶段末AUV和对接站的距离值。

定义对接站的位置为$\left( {{x_d},{y_d}} \right)$，则$d$可表示为：

$ d = \sqrt {{{\left( {{x_d} - x} \right)}^2} + {{\left( {{y_d} - y} \right)}^2}}。$

(11)

${d_f}$的取值是对接第3阶段的关键，取值过大则末端航向角调整时间过长，AUV横向偏移过大，超过对接站导向罩的端面半径值时对接失败；若取值过小，则AUV的航向角来不及调整到0附近，二者皆影响对接成功率。考虑AUV的对接速度、最小转弯半径、最大偏航速率，经计算得${d_f}$的取值如下：

$ |{d_f}| \geqslant k|\frac{{{\psi _{crab}}}}{2}\frac{{\text π} }{{{r_{\max }}}}U|。$

(12)

式中：${r_{\max }}$为AUV的最大偏航速率；$U$为设置的对接速度；$k$为增益系数。

2.4 基于运动学洋流速度观测器的洋流角度补偿

AUV航行过程中，洋流横向分量会造成航向角偏移，因此需设计洋流观测器估计洋流并实现补偿。根据运动学模型可知：

$ \mathop P\limits^ \cdot = {T_1}{V_r} + V_c^E。$

(13)

设计洋流观测器为^[17]：

$ \left\{ {\begin{array}{*{20}{l}} {\mathop {\mathop {V_c^E}\limits^ \wedge }\limits^ \cdot = \boldsymbol{A}\left( {P - \mathop P\limits^ \wedge } \right)}，\\ {\mathop {\mathop P\limits^ \wedge }\limits^ \cdot = {T_1}{V_r} + \mathop {V_c^E}\limits^ \wedge + \boldsymbol{B}\left( {P - \mathop P\limits^ \wedge } \right)} 。\end{array}} \right. $

(14)

式中：$P$均为AUV在惯性坐标系下的位置向量；$\boldsymbol A$、$\boldsymbol B$均为观测器参数矩阵。选择合适的矩阵参数，使上述方程的特征根为正实数，则观测器估计误差可渐进指数稳定。

上述观测器观测的是惯性坐标系下洋流分量，经坐标转换可求得在随体坐标系下的估计值$\mathop {V_c^b}\limits^ \wedge = \left[ {\begin{array}{l} {\mathop {{u_c}}\limits^ \wedge }\;\;{\mathop {{v_c}}\limits^ \wedge }\;\;0 \end{array}} \right]$，洋流纵向分量$\mathop {{u_c}}\limits^ \wedge $只影响AUV运动速度，不影响航向。因此只需补偿$\mathop {{v_c}}\limits^ \wedge $造成的航向角偏移，补偿角计算式为：

$ {\psi _{crab}} = a\tan 2\left( {\frac{{\mathop {{v_c}}\limits^ \wedge }}{U}} \right)。$

(15)

三段式制导生成期望的航向角指令，减去洋流补偿角，即最终的航向角指令。

3 基于自抗扰的对接航向角控制

AUV模型具有非线性、强耦合、内部参数时变等特点，且对接过程中洋流可变，使其航向角控制变得复杂，自抗扰控制可很好地解决上述问题。

3.1 自抗扰控制理论

自抗扰控制（ADRC）由韩京清研究员提出，主要由跟踪-微分器（TD）、扩张状态观测器（ESO）及非线性误差反馈控制律（NLSEF）这3个环节组成，跟踪-微分器用于跟踪输入指令及其微分信号，扩张状态观测器用于解决总扰动的观测问题，从而实现补偿，状态误差反馈控制律的作用是用于生成控制量^[18]。

以二阶对象为例：

$ \mathop y\limits^{ \cdot \cdot } = f\left( {y,\mathop y\limits^ \cdot ,w\left( t \right),t} \right) + {b_0}u。$

(16)

式中：$w\left( t \right)$为外扰作用；$ f\left( {y,\mathop y\limits^ \cdot ,w\left( t \right),t} \right) $为综合了外扰与内扰的总扰动。选取状态变量：$x_1 = y$，$x_2 = \mathop y\limits^ \cdot $，则可将上式转化为状态方程：

$ \begin{gathered} \mathop {{x_1}}\limits^ \cdot = {x_2}，\\ \mathop {{x_2}}\limits^ \cdot = f\left( {y,\mathop y\limits^ \cdot ,w\left( t \right),t} \right) + {b_0}u ，\\ y = {x_1}。\\ \end{gathered} $

(17)

ADRC的核心在于实时估计$ f\left( {y,\mathop y\limits^ \cdot ,w\left( t \right),t} \right) $并加以消除，将二阶对象变成如下的线性积分器串联标准型，从而简化控制过程为：

$ \mathop y\limits^{ \cdot \cdot } = {u_0}。$

(18)

3.2 AUV航向角模型解耦

设计航向角自抗扰控制器，需将航向角控制模型改写为自抗扰控制二阶对象的标准型。考虑到AUV本体模型的对称性及控制航向角时，其他姿态保持不变，对AUV运动模型进行化简并将加速度项移到左侧，得到AUV水平面运动方程：

$ m\mathop u\limits^ \cdot - {X_{\mathop u\limits^ \cdot }}\mathop u\limits^ \cdot = {X_{u|u|}}u|u| + \left( {{X_{vr}} + m} \right)vr + {X_{rr}}{r^2} + {X_{prop}}，$

(19)

$\begin{split} m\mathop v\limits^ \cdot -& {Y_{\mathop v\limits^ \cdot }}\mathop v\limits^ \cdot - {Y_{\mathop r\limits^ \cdot }}\mathop r\limits^ \cdot = {Y_{v|v|}}v|v| + {Y_{r|r|}}r|r| + \\ &\left( {{Y_{ur}} - m} \right)ur + {Y_{uv}}uv + {Y_{uu{\delta _r}}}{u^2}{\delta _r} ，\end{split}$

(20)

$\begin{aligned} - {N_{\mathop v\limits^ \cdot }}\mathop v\limits^ \cdot +& {I_{zz}}\mathop r\limits^ \cdot - {N_{\mathop r\limits^ \cdot }}\mathop r\limits^ \cdot = {N_{v|v|}}v|v| + {N_{r|r|}}r|r| +\\ &{N_{ur}}ur + {N_{uv}}uv + {N_{uu{\delta _r}}}{u^2}{\delta _r}。\end{aligned}$

(21)

写成矩阵形式并求逆，左侧只保留加速度项，可得：

$ \left[ {\begin{array}{*{20}{c}} {\mathop u\limits^ \cdot } \\ {\mathop v\limits^ \cdot } \\ {\mathop r\limits^ \cdot } \end{array}} \right] = {\left[ {\begin{array}{*{20}{c}} {m - {X_{\mathop u\limits^ \cdot }}}&0&0 \\ 0&{m - {Y_{\mathop v\limits^ \cdot }}}&{ - {Y_{\mathop r\limits^ \cdot }}} \\ 0&{ - {N_{\mathop v\limits^ \cdot }}}&{{I_{zz}} - {N_{\mathop r\limits^ \cdot }}} \end{array}} \right]^{ - 1}}\left[ {\begin{array}{*{20}{c}} {\sum X } \\ {\sum Y } \\ {\sum N } \end{array}} \right] 。$

(22)

定义矩阵$ \boldsymbol{Minv} $为：

$ \boldsymbol{Minv} = {\left[ {\begin{array}{*{20}{c}} {m - {X_{\mathop u\limits^ \cdot }}}&0&0 \\ 0&{m - {Y_{\mathop v\limits^ \cdot }}}&{ - {Y_{\mathop r\limits^ \cdot }}} \\ 0&{ - {N_{\mathop v\limits^ \cdot }}}&{{I_{zz}} - {N_{\mathop r\limits^ \cdot }}} \end{array}} \right]^{ - 1}}。$

(23)

化简后的运动学方程为：

$ \mathop \psi \limits^ \cdot = r 。$

(24)

与上式相结合，考虑洋流对建模的影响，式中的$u$、$v$均为相对速度，可得：

$ \begin{gathered} \mathop \psi \limits^{ \cdot \cdot } = \mathop r\limits^ \cdot = \boldsymbol{Minv}(3,2)\sum Y + \boldsymbol{Minv}(3,3)\sum Z = \\ \boldsymbol{Minv}(3,2){Y_{uu{\delta _r}}}{u^2}{\delta _r} + \boldsymbol{Minv}(3,3){N_{uu{\delta _r}}}{u^2}{\delta _r} +\\ f\left( {\mathop \psi \limits^ \cdot ,{\psi _w},{V_c},t} \right) = b{\delta _r} + f 。\\ \end{gathered} $

(25)

式中：$ b $为舵角输入的增益参数；$ f\left( {\mathop \psi \limits^ \cdot ,{\psi _w},{V_c},t} \right) $为包含了水动力参数项、洋流项、内部扰动项、外部扰动项的总扰动项，简写为$f$。

本研究中所使用的AUV模型为螺旋桨+舵形式的欠驱动AUV，舵角偏转存在限制，即$|{\delta _r}| \leqslant \displaystyle\frac{{30}}{{180}}{\text π}$。

3.3 航向角自抗扰控制器

结合自抗扰控制理论和航向角解耦后的动力学模型，可设计航向角控制器（见图3）。

3.3.1 跟踪微分器

跟踪微分器的输入为航向角指令${\psi _0}$；输出为航向角指令的过渡信号${\psi _1}$和微分信号${\psi _2}$。

建立如下的离散最速反馈系统：

$ \left\{ {\begin{array}{l} {\mathop {{\psi _1}}\limits^ \cdot = {\psi _2} }，\\ {\mathop {{\psi _2}}\limits^ \cdot = fhan\left( {{\psi _1} - {\psi _0},{\psi _2},{r_0},{h_0}} \right)} 。\end{array}} \right. $

(26)

$ fhan\left( {{\psi _1} - {\psi _0},{\psi _2},{r_0},{h_0}} \right) $为最速综合函数，表达式为：

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

(27)

式中：${r_0}$和${h_0}$为跟踪微分器的控制量，${r_0}$的大小反应跟踪输入信号的速率，${h_0}$的大小表征滤波效果。

3.3.2 扩张状态观测器

扩张状态观测器将总扰动扩张成系统的一个新状态变量，利用系统输入、输出重构出包含系统原有状态变量与扰动的所有状态。该环节建立的状态观测器方程如下：

$ \left\{ {\begin{array}{*{20}{l}} {\mathop z\limits^ \cdot = \boldsymbol{Az} + \boldsymbol{B}{\delta _r} + \boldsymbol{L}\left( {\psi - \boldsymbol{Cz}} \right)}，\\ {\mathop \psi \limits^ \wedge = Cz} 。\end{array}} \right. $

(28)

式中：$ {\boldsymbol{z}} = {\left[ {\begin{array}{*{20}{c}} {{z_1}}&{{z_2}}&{{z_3}} \end{array}} \right]^{\text{T}}} $为观测器状态向量；$\boldsymbol L$为观测器误差反馈增益矩阵；$\boldsymbol A$、$\boldsymbol B$、$\boldsymbol C$均为状态方程系数矩阵。改写观测器方程为：

$ \left\{ {\begin{array}{*{20}{l}} {\mathop z\limits^ \cdot = \left[ {\boldsymbol{A} - \boldsymbol{LC}} \right]z + \left[ {\boldsymbol{B},\boldsymbol{L}} \right]{u_c}}，\\ {{\psi _c} = z} 。\end{array}} \right. $

(29)

式中：${u_c} = \left[ {\begin{array}{*{20}{c}} {{\delta _r}}&\psi \end{array}} \right]$为组合输入；${\psi _c}$为输出。经参数化设计，把特征方程的极点放在同一个位置，取观测器增益矩阵为：

$ \boldsymbol{L} = {\left[ {\begin{array}{*{20}{c}} {3{w_0}}&{3w_0^2}&{w_0^3} \end{array}} \right]^{\text{T}}}。$

(30)

${w_0}$为观测器带宽，则二阶系统的特征方程为：

$ \lambda \left( s \right) = |sI - \left( {\boldsymbol{A} - \boldsymbol{LC}} \right)| = {\left( {s + {w_0}} \right)^3}。$

(31)

此种设计简化了扩张状态观测器的设计参数，便于调节。

3.3.3 状态误差反馈控制律

由于扩张状态观测器能够实时估计并补偿外部与内部扰动，状态误差反馈控制律进一步简化为PD组合的设计，其控制表达式如下：

$ {\delta _0} = {k_p}\left( {{\psi _1} - {z_1}} \right) + {k_d}\left( {{\psi _2} - {z_2}} \right)。$

(32)

式中：${\delta _0}$为包含扰动的控制器输出；${z_1}$、${z_2}$为扩张状态观测器的观测值。上述误差反馈控制律的设计，使得系统闭环传递函数成为一个没有零点的二阶系统：

$ G = \frac{{{k_p}}}{{{s^2} + {k_d}s + {k_p}}}。$

(33)

${k_p}$、${k_d}$均为需要设计的控制器增益矩阵$\boldsymbol{K} = {\left[ {{k_p}}\ {{k_d}} \right]^{\text T}}$的参数，经过参数化选择：

$ {k_p} = {w_c}^2,{k_d} = 2{w_c}。$

(34)

${w_c}$称之为控制器带宽，该环节只需调节${w_c}$一个参数。

3.3.4 控制量生成

将扩张状态观测器观测到的扰动${z_3}$补偿到状态误差反馈控制律生成的控制信号${\delta _0}$中，即可得控制量为：

$ {\delta _r} = \frac{{{\delta _0} - {z_3}}}{b}。$

(35)

航向角控制模型简化为线性积分串联标准型。

4 仿真实验

为验证本文设计的航向角制导和控制环节的正确性，针对欠驱动型REMUS 100 AUV的水下对接进行仿真和分析。在Matlab/Simulink平台搭建仿真模型，包括AUV本体数学模型、三段式航向角制导模型、航向角自抗扰控制模型。输入不同大小和方向的洋流信号，观测AUV运动轨迹、航向角变化并验证对接效果；模拟外界的不确定性干扰，设置添加噪声信号，验证设计算法的抗干扰能力及对接效果；与采用常规PID航向角控制的对接系统比较，验证设计算法的高效性。通过上述仿真，验证设计算法的工程实用性。

对接起始条件：设置AUV的起点为（0,0）；对接站位置为（560,200）；第一阶段转折点为（400,200）；第二阶段转折点为（550,200）。第一阶段和第二阶段转折点之间间隔150 m，第二阶段转折点和对接站之间的间隔为10 m，以上位置坐标的单位为m。

对接要求：根据伍兹霍尔海洋研究所设计的REMUS对接站的尺寸要求，进入对接站的横向距离满足小于0.38 m^[19]，航向角要求小于45°^[2]。

4.1 不同洋流下的仿真结果

考虑到洋流横向分量为AUV对接的主要影响因素之一，因此洋流角度设置为±90°，洋流大小设置0、0.1、0.3、0.4、0.5 m/s这6种恒值情况，仿真结果如图4和图5所示。

上述仿真结果表明，在洋流不断变大的情况下，AUV最终对接的横向偏差始终在±0.25 m以内，进入对接站的航向角在±4°以内，满足对接要求，验证了该方法的有效性。

4.2 外加扰动下的仿真结果

对控制端加入幅值为200°的随机噪声信号，在0、0.25 m/s ±90°洋流下仿真，结果如图6所示。

可知，控制段加入随机噪声误差，AUV对接过程中的航向角有±0.5°的波动，但AUV对接横向偏差在±0.25 m以内，进入对接站的航向角在±4°以内，仍能满足对接要求，与没有外界扰动的情况基本一致，验证了该方法的抗扰动性能，能够抵抗外来突发情况和不确定性。

4.3 与PID控制的效果对比

通过等效法计算自抗扰控制器对应的PID参数，设计航向角自抗扰控制器，调节其参数使其与自抗扰控制的对接效果基本一致，再次添加扰动并在0.25 m/s +90°洋流下仿真，结果如图7所示。

可知，2种控制方法的对接轨迹曲线基本一致，其最终横向偏差皆在±0.25 m以内，进入对接站的航向角皆在±4°以内，但对接过程中的航向角变化曲线，采用PID控制器的仿真效果有±1°左右的波动，而自抗扰控制器的仿真效果波动在±0.5°以内，采用自抗扰控制器的航向角波动更小，验证了该方法的实用性。

4.4 洋流观测器效果与输出

以0.25 m/s、−90°洋流为例仿真，验证洋流观测器的观测效果，仿真结果如图8所示。

可知，洋流观测器能快速准确的估计洋流大小和方向，估计误差在10 s内迅速收敛为0，设计有效。

5 结　语

针对欠驱动AUV在洋流下对接同时保证航向角和横向距离的需求，本文提出一种基于自抗扰控制的三段式对接算法。该算法将AUV的对接分成3个阶段，首段采用追踪制导，中段采用基于横向偏差的制导，末端采用基于余弦曲线调整的制导，三段制导生成连续的航向角指令，自抗扰控制跟踪航向角指令，洋流观测器估计并补偿洋流，完成对接过程。搭建Simulink仿真平台，仿真结果表明，本文设计的对接算法能够满足AUV在不同洋流下的对接要求，有效保证了航向角和横向距离皆在允许范围内，面对干扰有一定抵抗能力。