0.   引　言

近年来，以无人水下航行器（UUV）为代表的水下无人设备得到了广泛的应用^[1-2]。UUV主要分为2类：遥控水下航行器（ ROV）和自主水下航行器（AUV）。然而，受有限艇载能量影响^[3]，UUV的作业范围和时间受到了极大约束，因此，开展UUV对接回收及协同作业至关重要。

自主对接技术作为多种水下装备协同作业的基础，国内外学者进行了广泛研究^[4]。UUV三维水下对接是指UUV利用声学、光学等设备感知UUV与对接站（以下也称“对接船坞”）的相对位置和位姿，然后按照规划轨迹进入固定或移动对接站。整个对接过程包括搜索段、接近段和对接段。根据对接站是否移动，对接任务大致分为2类：固定对接任务和移动对接任务。在固定对接任务中，UUV可自动进入固定对接站，为电池充电，上传海量采集数据，下载更新任务指令，无需人工操作。与移动对接任务相比，固定对接任务采用的规划和控制方法更简单。但移动对接技术具有更好的灵活性，UUV可与其他无人装备协同完成更复杂的任务^[5]。

一般而言，在UUV与对接站距离较远时，多采用声学引导方式；在距离较近时，为了提升对接精度，可切换为视觉引导方式。本文将针对近距离下的视觉引导控制开展研究。为了实现高效、准确地视觉引导控制，必须解决2个难题：通过视觉解算得到UUV与对接站间的准确位姿关系；克服内外强扰动因素实现鲁棒跟踪控制。

针对视觉解算问题，Palomeras等^[6]设计了一种算法，用于估算AUV和水下对接站之间的相对位姿和相对位置，该方案使用了3维空间中圆形船坞的4个光源及其投影在2维平面中的几何形状和透视关系，从其几何形状变化估算相对距离和相对位姿。Watt等^[7]在对接目标上安装了4个LED灯，1台高清摄像机被放置在潜艇首部，并设计了一种图像处理算法找到LED灯在图像中的位置，从而确定AUV位姿，实时校正ROV与对接站之间的横向运动。Li等^[8]设计了一种可靠的水下导航方法来提供准确的相对位置信息，此方案中对接站周围固定有4个绿色LED灯，AUV的首部安装了2个摄像机进行视觉定位，在位姿解算过程中，根据2个相机中的图像数量在单目和双目组合定位方法之间切换，确保得到准确的相对位置。Trslic等^[9]围绕海底相机位姿估算开发了基于机器视觉的对接系统，该系统中AUV与对接站之间的相对位置采用单个相机和已知的光标记图案估算。通过图像采集、失真消除、曝光估算、高斯滤波器模糊和图像阈值处理等方法提高ROV和对接站之间的位姿估算。

如何在UUV受到内外强扰动影响下设计出良好的鲁棒控制算法，从而满足UUV的控制目标，这是另一个颇具挑战性的问题^[10]。滑模控制（SMC）是一种应用非常广泛的鲁棒控制方法。Gonzalez-García等^[11]设计了一种无模型高阶SMC，确保UUV跟踪误差快速及有限时间收敛到原点，除增强鲁棒性外，误差动力学的有限时间收敛可有助于最大限度地减少UUV推进器的能量消耗。尽管UUV具有非线性和复杂的动力学特性，Wang等^[12]在深度跟踪任务期间设计了一种基于非模型的非奇异终端滑模控制（TSMC）方案来补偿UUV的动态模型。Khalid等^[13]通过输出反馈高阶SMC （HOSMC）解决了基于SMC方案的抖动问题，使用高阶滑动流形设计HOSMC方案，而不是经典的线性滑动面；提出的控制器在六自由度UUV动态模型上进行了验证。在SMC中，除UUV动力学建模误差外，Fischer等^[14]还设计了一种连续鲁棒控制方案，提出了误差符号的稳健积分（RISE）控制，不需要无限带宽的控制来处理时变的不确定性和外部干扰。

基于上述背景，本文将面向UUV水下对接问题，开展视觉引导三维轨迹跟踪控制研究。首先，面向水下对接任务，设计视觉引导对接方案。其次，设计基于YOLOv5的图像处理方法，完成对接目标与UUV之间的相对位置监测和位姿估算。融合径向基函数神经网络（RBFNN）和TSMC的轨迹跟踪算法，设计UUV三维轨迹跟踪控制方法，以克服复杂扰动影响和SMC抖振问题。通过Matlab仿真和水池试验，验证视觉引导对接方案的可行性，以及视觉解算与三维轨迹跟踪控制方法的有效性。

1.   视觉引导对接方案设计

本文基于一款ROV，设计了如图1所示的视觉引导对接方案，通过在线视觉解算和三维轨迹跟踪控制，实现全过程的自主视觉引导对接。该ROV配备了带云台的摄像机，以及4个水平方向推进器和2个垂直方向推进器，具备六自由度运动能力，最大工作深度可达100 m。为完成水下对接任务，设计了使用灯光引导的模拟水下对接站，在长方形框架的4个角布置了4个光源，用于辅助对接框架的目标中心点的位姿解算。

图  1  对接方案示意图

Figure  1.  Schematic diagram of the docking scheme

下载: 全尺寸图片 幻灯片

在视觉引导对接过程中，由ROV上搭载的摄像机实时在线监测对接站上的辅助光源，进而用视觉处理方法推算出对接站中心与ROV间的相对位置及位姿偏差，用于为三维轨迹跟踪控制提供依据。

对接过程分为搜索、视觉引导和对接3个阶段，各阶段的工作内容如下：

1) 搜索阶段：基于先验知识驱动ROV沿一条预规划的轨迹搜索目标物；

2) 视觉引导阶段：一旦目标物进入ROV视场，视觉引导系统将实时解算ROV与对接站中心之间的相对位姿信息，并基于三维轨迹跟踪控制算法，使ROV接近对接站；

3) 对接阶段：在对接站之前，ROV根据视觉系统解算的位姿信息进行位姿的精调，然后穿过对接站。

2.   目标检测及相对位姿估算

ROV视觉处理系统的主要任务包括对接目标检测和相对位姿估算这2个部分。目标检测任务的执行过程如下：首先，使用ROV搭载的相机采集大量水下对接目标图片，形成训练集；接着，对训练集进行标注，并且采用YOLOv5算法进行训练；最后，基于形成的测试集进行测试，确保训练模型具有良好的识别效果。相对位姿估算任务的执行过程如下：首先，对目标定位框进行信息处理；其次，分割对接目标，并且结合对接目标的类别信息选取对接目标模板；最后，通过模板与对接目标之间的特征点进行特征匹配，再基于EPnP算法完成相对位姿的估算。视觉处理总体流程如图2所示。

图  2  视觉处理流程图

Figure  2.  Flow chart of visual processing

下载: 全尺寸图片 幻灯片

2.1   基于YOLOv5的目标检测

YOLOv5基于回归方法学，以整张RGB图像作为输入，在输出层直接回归预测框中物体的类别以及位置。根据网络的深度和特征图的宽度，YOLOv5可以分为4种模型：YOLOv5s，YOLOv5m，YOLOv5l和YOLOv5x。其中YOLOv5s的网络深度和宽度最小。为降低模型的复杂度，避免不必要的算力需求，本研究选择了YOLOv5s训练和修改，其具体结构如图3所示^[15]。图中，CSP为一种深度神经网络结构，CBL为标准卷积函数，Conv为普通卷积层函数，BN 为归一化函数，LeakyReLU为线性激活函数，ResUnit为残差函数，add 为像素相加函数，Concat 为特征融合函数，Focus 为切片函数，slice为切割函数，SPP 为空间金字塔池化函数，MaxPool为最大池化层函数，Upsample为上采样函数。

图  3  YOLOv5方法架构

Figure  3.  YOLOv5 method architecture

下载: 全尺寸图片 幻灯片

所提模型的训练过程在Windows操作系统和PyTorch框架下运行。软件环境为CUDA11.2，CUDNN7.6，Python3.8。用于训练数据集的CPU是Intel(R) Xeon(R) Silver 4110 CPU @2.10 GHz 16 G，GPU是 GeForce RTX 3060 Ti 11 G。

在试验过程中，我们使用ROV自带的视觉系统采集不同时间段、不同距离以及不同拍摄角度的视频数据，接着编程并按照一定的帧间隔提取所需图片；之后，借助LabelImg工具对采集到的2500张图片进行手工标注，制作了用于YOLOv5训练和测试的高精度数据集（见图4）；最后，将制作好的图片导入相应文件夹中。YOLOv5模型的初始学习率设置为0.001，学习率的动量设置为0.9。每次训练批量设置为8次，以提高训练速度。输入图片的分辨率大小设置为640×640像素，迭代次数设置为100。

图  4  LabelImg 数据标注

Figure  4.  LabelImg data annotation

下载: 全尺寸图片 幻灯片

在训练完成之后，通过测试集数据来对模型进行评估，通过对比所有保存的中间模型文件的效果，再选取其中测试效果最好的模型，结果如图5所示。

图  5  识别效果

Figure  5.  Recognition effects

下载: 全尺寸图片 幻灯片

从图5的识别效果可以看出，即使对于大逆光以及目标尺寸急速变化等复杂场景，YOLOv5深度学习方法也能有效检测目标，为相对位姿估算提供了强有力的支撑。

2.2   相对位姿估算

2.2.1   相机标定

在机器视觉应用中，通常需要将二维图像中的点与空间某物体表面点的三维几何位置一一对应，因此必须要建立相机的几何模型，掌握相机的内参数和相机畸变参数。但是这些参数大多数情况下需要通过试验和计算才能得到，这个过程就是相机标定，其原理如图6所示。

图  6  相机标定原理图

Figure  6.  Schematic diagram of camera calibration

下载: 全尺寸图片 幻灯片

本文采用张氏标定法进行标定^[16]，此方法利用如图7所示的棋盘格作为标定板，使用ROV自带的相机采集大量标定板水下图像，然后基于Matlab图像标定工具箱完成相机参数求解。

图  7  水下相机标定棋盘格

Figure  7.  Underwater camera calibration checkerboard

下载: 全尺寸图片 幻灯片

图8所示为基于Matlab标定之后每张图像的重投影误差，以及通过相机外参数绘制的标定板和相机位置结果。

图  8  相机标定结果

Figure  8.  Camera calibration results

下载: 全尺寸图片 幻灯片

由图8标定结果可见，重投影平均像素误差0.188像素，说明准确度较高。表1为通过Matlab标定后的相机参数，可为相对位姿估算提供依据。

表  1  标定结果

Table  1.  Calibration results

参数	数值		参数	数值
$c_x$	136.082		${k_2}$	1.863
$c_y$	176.148		${k_3}$	0
${f_x}$	659.766		${m_1}$	0
${f_y}$	662.659		${m_2}$	0
${k_1}$	−0.567

下载: 导出CSV 

| 显示表格

2.2.2   基于EPnP算法的相对位姿估算

在目标检测环节，采用YOLOv5得到了对接目标上的光源像素坐标值，即通常说的二维坐标，而通过事先设计好的对接目标尺寸以及目标光源位置，可以得到每个光源的世界坐标，即所谓的三维坐标。单目视觉中通过二维−三维坐标点求解摄像机位姿的问题被称为n点透视问题，常用求解方法可以分为迭代算法和解析算法2大类。在本文中，基于实时性考虑选择解析算法，即EPnP算法求解^[17]。

EPnP算法位姿估算的核心思想是利用４个虚拟控制点通过线性加权来表示真实物体上的各点在相机坐标系下的坐标^[18]，其原理如图9所示。

图  9  EPnP问题示意图

Figure  9.  Schematic diagram of the EPnP problem

下载: 全尺寸图片 幻灯片

假设参考点在世界坐标系中的坐标为$\boldsymbol{P}_i^{\text{w}}$，$i = 1,2,\ldots ,n$，在相机坐标系C中的坐标为${\boldsymbol{P}}_i^{\text{c}}$。4个控制点在世界坐标系W中的坐标为${\boldsymbol{C}}_j^{\text{w}}$，$j = 1,2, \ldots ,4$，在相机坐标系中的坐标为${\boldsymbol{C}}_j^{\text{c}}$。可将参考点的坐标表示为控制点坐标的加权之和：

式中，${\alpha _{ij}}$为每个点对应的齐次重心坐标。

在相机坐标系中，存在同样的加权和关系：

假设相机的外参数为$\left[ {\boldsymbol{R}}\;\;\;{\boldsymbol{t}}\right]$，虚拟控制点${\boldsymbol{C}}_j^{\text{w}}$和${\boldsymbol{C}}_j^{\text{c}}$之间的关系可以表示为

参考点坐标表示为控制点坐标的加权之和：

设K为相机机的内参矩阵，可以通过标定获得。$\boldsymbol{U}_i$为参考点$\boldsymbol{P}_i^{\text{w}}$的二维投影，则根据相机投影模型可得到：

式中：ω_i为投影模型中的尺度因子，是一个标量。

式（5）改写成矩阵形式，可得：

式中：（${u_i}$，${v_i}$）为参考点在对应二维像素点的坐标；${f_x}$，${f_y}$，${c_x}$$，c_y$为相机的内参；（$x_j^{\text{c}}$，$y_j^{\text{c}}$，${\textit{z}}_j^{\text{c}}$）为参考点在相机坐标系下的坐标值。

将式（7）展开整理，可得如下方程：

式（8）中，相机内参数和二维像素点坐标都是已知量，未知量是4个控制点在相机坐标系下的坐标。共12个位置参数，一个像素点可以列2个方程，n 个像点可以列出2n 个方程。进一步可以得到如下线性方程组：

其中，$\boldsymbol{X}=\left[{\boldsymbol{C}}_1^{\mathrm{cT}}\; \; {\boldsymbol{C}}_2^{\mathrm{cT}}\; \; {\boldsymbol{C}}_3^{\mathrm{cT}}\; \; {\boldsymbol{C}}\mathrm{_4^{cT}}\right]^{\text{T}}$。

式（9）中，4个控制点总共12个未知变量，M为2n×12的矩阵。因此，${{\boldsymbol{V}}_k}$为矩阵M的第k 个零特征值对应的第k 个右奇异向量，维度为12×1，其解x在M的右零空间中，可以通过奇异 值分解（SVD）得到。

由式（10）即可求解目标与ROV相机间的相对位姿关系。

3.   三维轨迹跟踪控制器设计

3.1   ROV数学建模

忽略横摇问题，可以建立ROV的五自由度运动学和动力学模型^[19]：

式中：$x,y,{\textit{z}}$分别为纵向、横向、垂向位移；$\theta$为俯仰角；$\psi$为偏航角；$u,v,w$分别为纵向、横向、垂向速度；q为俯仰角速度；r为偏航角速度；$\tau_{\mathrm{T}1}, \tau_{\mathrm{T}2}, \tau_{\mathrm{T}3}$分别为纵向、横向、垂向控制力；$\tau_{\mathrm{T}4}$为俯仰角控制力矩;$\tau_{\mathrm{T5}}$为偏航角控制力矩；m为ROV的质量；${I_y},{I_{\textit{z}}}$为转动惯量；${X_{\dot u}}$,${Y_{\dot v}}$,${Z_{\dot w}},{M_{\dot q}},{N_{\dot r}}$分别为横向、纵向、垂向、俯仰角和航向角5个自由度的水动力导数；${X_u},{Y_v},{Z_w},{M_q},{N_r}$分别为横向、纵向、垂向、俯仰角和航向角5个自由度的一阶阻尼系数；${X_{u\left| u \right|}},{Y_{v\left| v \right|}},{Z_{w\left| w \right|}},{M_{q\left| q \right|}},{N_{r\left| r \right|}}$分别为横向、纵向、垂向、俯仰角和航向角5个自由度的二阶阻尼系数；$\tau_{\mathrm{d}1}^*\left(t\right)$，$\tau_{\mathrm{d}2}^*\left(t\right)$，$\tau_{\mathrm{d}3}^*\left(t\right)$，$\tau_{\mathrm{d}4}^*\left(t\right)$，$\tau_{\mathrm{d}5}^*\left(t\right)$分别为5个自由度的复杂外界环境扰动力。

为便于控制器设计，将式（11）转化为标准模式：

其中：

假设1：在此模型中，${d_u},{d_v},{d_w},{d_r},{d_r}$为环境扰动干扰，并且有界。

3.2   跟踪误差模型

ROV三维轨迹跟踪原理如图10所示。其中：$E，B$和$F$分别为惯性坐标系、载体坐标系和Serret-Frene坐标系；$F_{\mathrm{L}}$表示三维空间LOS导引算法的期望点坐标；载体坐标系$B$的原点与ROV的质心重合，其满足右手坐标系；$F$坐标系的原点为视觉检测出的对接站中心位置；${\psi }_{\text{LOS}}和{\theta }_{\text{LOS}}$为航向引导角和俯仰引导角；${\psi _{\text{e}}}和{\theta }_{\text{e}}$为偏航角和俯仰角的误差；${\varDelta _y}$和${\varDelta _{\textit{z}}}$为前视距离。

图  10  三维轨迹跟踪

Figure  10.  Three-dimensional tracking

下载: 全尺寸图片 幻灯片

在$E$惯性系中，ROV的位置矢量表示为$\boldsymbol{B}\mathrm{^E}= \left(x_{\text{B}},y_{\text{B}},\text{z}_{\text{B}}\right)^{\text{T}}$，速度矢量为$\dot{\boldsymbol{B}}^{\mathrm{E}}=\left(\dot{x}_{\text{B}},\dot{y}_{\text{B}},\dot{\text{z}}_{\text{B}}\right)^{\text{T}}$，因此，其合速度为${\boldsymbol{U}}=\sqrt{\left(\dot{\boldsymbol{B}}^{\mathrm{E}}\right)^{\text{T}}\dot{\boldsymbol{B}}^{\mathrm{E}}}=\sqrt{u^2+v^2+w^2}$，${\psi _{\text{B}}}$和${\theta _{\text{B}}}$分别为ROV的偏航角和俯仰角^[20]。

定义$\psi_{\mathrm{F}}$和$\theta_{\mathrm{F}}$为$F$坐标系相对于$E$坐标系的旋转角。${c_1}\left( s \right)$和${c_2}\left( s \right)$分别为路径曲率和挠率，且有

对式（16）求导，可得：

定义跟踪误差为${\boldsymbol{\varepsilon}} = \left[ {{x_{\text{e}}},{y_{\text{e}}},{{\textit{z}}_{\text{e}}}} \right]$，则基于坐标变换关系，可得：

${\psi _{\text{e}}}$和和俯仰角误差${\theta }_{\text{e}}$分别定义为

式中：${\psi }_{\text{d}}$和${\theta }_{\text{d}}$分别为期望的偏航角和俯仰角。

位置和位姿跟踪误差可基于视觉处理获取。

3.3   运动学控制器设计

视线（line-of-sight, LOS）制导法是应用于ROV的流行制导方法，其主要思想是模仿舵手的动作，将ROV引导到沿轨迹的投影点前方的前视距离。航向引导角${\psi _{{\text{LOS}}}}$和俯仰引导角${\theta _{{\text{LOS}}}}$可由三维LOS制导律得到^[21]：

其中，前视距离${\varDelta _y}$和${\varDelta _{\textit{z}}}$均大于0。

为提高全局跟踪过程中的引导性能，引入模糊逻辑优化${\varDelta _{\text{*}}}$。改进的前视距离${\varDelta _{\text{*}}}$为

式中：${\varDelta }_{*}{}_{\text{max}}和{\varDelta }_{*}{}_{\text{min}}$为预定义的最大及最小前瞻距离；${N_ * }$为模糊逻辑计算的调整参数。

${\varDelta _y}$的模糊控制器选择为

${\varDelta _{\textit{z}}}$的模糊控制器选择为

理论航迹角$\chi$和潜浮角$\gamma$分别为

在运动过程中会产生漂角和攻角，分别为

期望的偏航角和俯仰角分别为

对式（18）求导，得

对式（19）求导，可得出航向角和潜浮角误差动力学模型为

其中，

由于运动学控制的主要目标是使跟踪误差矢量为0，考虑如下候选李雅普诺夫函数^[22]：

为使${\dot V_1} \lt 0$，设计虚拟向导速度控制律为

其中，${k_1} > 0$，${k_2} > 0$，${k_3} \gt 0$。

进一步可得

定义$\tilde{\psi}_{\text{e}}=\psi_{\text{e}}-\psi_{\text{los}}，\tilde{\theta}_{\text{e}}=\theta_{\text{e}}-\theta_{\text{LOS}}$，即${\psi _{\text{e}}} \to {\psi _{{\text{LOS}}}}, {\theta _{\text{e}}} \to {\theta _{_{{\text{LOS}}}}}$时，有

可见，对于所有有界初始条件，${\dot V_1}$是一致最终有界的，系统趋近稳定，即ROV能跟踪上期望路径。

3.4   动力学控制器设计

跟踪控制的目标是推导有效的反馈控制律以产生控制输入${{\boldsymbol{\tau}} _{\text{*}}}$，使得对应方向的速度误差在t时刻趋于∞时可以一致且渐近地收敛到0^23]。动力学控制器可分为前向控制器、俯仰控制器以及偏航控制器3个部分。

定义前向、俯仰和偏航跟踪误差为

基于跟踪误差，构建如下滑模面：

式中：${c_u} \gt {\text{0}}$，${l_u} \gt 0$，${\text{0}} \lt m \lt {\text{1}}$，${c_q} \gt {\text{0}}$，${l_q} \gt {\text{0}}$，${\text{0}} \lt n \lt {\text{1}}$，${c_r} \gt {\text{0}}$，${l_r} \gt {\text{0}}$，${\text{0}} \lt q \lt {\text{1}}$,${s_u}$,${s_q}$,${s_r}$分别为前向、俯仰和偏航滑模面。

将式（12）和（34）代入式（35），可得

进而可得所需的控制力/力矩为

式中：$k,l,n \gt 0,0 \lt \beta ,\gamma ,\alpha \lt 1$；${\tau _u}$，${\tau _q}$，${\tau _r}$分别为前向、俯仰和偏航方向的控制输入。

为了提升跟踪控制器的鲁棒性，本文通过RBF神经网络对外界环境扰动${d_u},{d_q},{d_r}$进行估算，由RBF神经网络的非线性逼近能力可知，RBF神经网络存在最优网络权值$\boldsymbol{W}^{\text{*}}$，使得

式中：$\boldsymbol{H}\left(\dot{x}\right)$为RBF神经网络隐含层输出，$\boldsymbol{W}^{\text{*}}$为神经网络的最优权值向量；$\boldsymbol{d}_{\text{*}}$为外界环境扰动向量；$\boldsymbol{\varepsilon}$为网络逼近误差，且存在正常量${\varepsilon _0}$，使得$\left\| \varepsilon \right\| \leqslant {\varepsilon _0}$。

在跟踪控制律中，RBF神经网络输出可表示为

式中：$\hat{\boldsymbol{W}}\mathrm{^T}$为RBF神经网络权值的估算值；$\hat{\boldsymbol{d}_*}$为外界环境扰动估算值。

因此，所需驱动力/力矩为

为了证明动力学控制器的稳定性，预选李雅普诺夫函数为

取${\dot{\hat{W}}}_{1}={s}_{r}，{\dot{\hat{W}}}_{2}={s}_{u}，{\dot{\hat{W}}}_{3}={s}_{q}$，则式（40）可以进一步推导得到

可知，当${\dot V_1} \leqslant 0$时，通过选取合适的控制器参数，可使$\dot V$恒小于等于0，闭环控制器渐近稳定，使得ROV的控制速度和角度能跟上期望轨迹。

4.   数值仿真及水池试验

4.1   数值仿真

为完成水下对接任务，首先通过数值仿真，验证本文设计的神经网络改进滑模三维轨迹跟踪控制器的有效性。在数值仿真中，将本文方法与常用的积分滑模和改进滑模跟踪控制方法进行对比。

仿真中使用的3种控制算法如下：1）本文提出的神经网络改进滑模方法，其中，运动学控制采用模糊LOS制导律，动力学控制采用改进滑模，同时加入RBF神经网络进行扰动估算；2）改进滑模方法，其中，运动学控制采用模糊LOS制导律，动力学控制采用改进滑模； 3）积分滑模方法，其中，运动学采用模糊LOS制导律，动力学采用常规积分滑模。

为了充分验证算法的有效性，设计了3种仿真工况。工况1：螺旋线轨迹跟踪；工况2：直线轨迹跟踪；工况3：折线轨迹跟踪。为了验证所提方法的鲁棒性，仿真过程中在ROV运动学模型中增加了较为复杂的扰动类型。此外，考虑到ROV在实际工作场景中面临的环境相互作用扰动，数值模拟中设置的复合扰动既具有时变性，又具有状态依赖性。

4.1.1   案例1：螺旋线轨迹跟踪

选择的期望路径为${{\boldsymbol{P}}_{\text{d}}}\left( \mu \right) = [ \mu , 20\cos \left( {0.05\mu } \right), 20\sin \left( {0.05\mu } \right) ]^{\text{T}}$；选择期望速度为$u_{\text{d}} = 1{\text{ m/s}}$；初始状态为${{\boldsymbol{\eta}} _0} = [ 0,10, 0,0,0,0 ]^{\text{T}}$；初始速度为${{\boldsymbol{v}}_0} = [ 0.1,0, 0, 0.1,0.1 ]^{\text{T}}$。为验证所提出的控制器的抗干扰能力，在模型中应用由幅值恒定的时变正弦和余弦函数及状态相关的二次项和常数项组成的复杂扰动：

案例1的跟踪结果如图11所示。可见，传统的积分滑模和改进滑模均可以将ROV调节到期望的轨迹（期望速度为1 m/s），但传统的积分滑模在$x=-19.93 \;{\mathrm{m}},\;y=3.14 \;{\mathrm{m}}, z=-32.81 \;{\mathrm{m}}$的位置跟踪到期望轨迹，本文所提改进滑模在$x=-9.43\;{\mathrm{m}}, y=-17.64\;{\mathrm{m}}, z=-9.82\;{\mathrm{m}}$的位置上跟踪到期望轨迹，加入RBF神经网络后，可让ROV在$x=-7.92\;{\mathrm{m}}, \ y=-18.36\;{\mathrm{m}}, z=-8.15\;{\mathrm{m}}$的位置上跟踪到期望轨迹。此外，为了更好地分析跟踪性能，对图12所示3个方向的跟踪误差进行对比。可见，改进滑模在x方向的跟踪误差明显小于传统的积分滑模，而且加入神经网络扰动估算之后，超调也更小。通过对比在x，y和z方向的跟踪误差可以发现，改进滑模具有更快的响应时间。

图  11  三维螺旋线轨迹跟踪

Figure  11.  Three dimensional helical trajectory tracking

下载: 全尺寸图片 幻灯片

图  12  三维螺旋线轨迹不同方向的跟踪误差

Figure  12.  The three-dimensional helical trajectory tracking errors in different directions

下载: 全尺寸图片 幻灯片

4.1.2   案例2：直线轨迹跟踪

选择期望路径为${{\boldsymbol{P}}_{\text{d}}}\left( \mu \right) = {\left[ {\mu ,\mu ,\mu } \right]^{\text{T}}}$；选择期望速度为${u_{\text{d}}} = 1{\text{ m/s}}$；初始状态为${{\boldsymbol{\eta }}_0} = {\left[ {0,10,0,0,0,0} \right]^{\text{T}}}$；初始速度为${{\boldsymbol{v}}_0} = {\left[ {0.1,0,0,0.1,0.1} \right]^{\text{T}}}$；在扰动方面，加入和案例1相同的扰动类型。

案例2的跟踪结果如图13所示。在直线轨迹跟踪过程中可见，传统的积分滑模、改进滑模和加入神经网络后三者的跟踪效果基本相同，都可在一定时间内跟踪上期望轨迹。为更加直观地分析，观察图14的跟踪误差结果，可很容易发现，虽然采用传统积分滑模、改进滑模以及加入神经网络后，最终跟踪误差没有很大的差别，但是，从x和z方向的跟踪误差效果图可清晰地看出，改进滑模具有更快的响应时间和更小的超调，确保了ROV能够快速跟踪期望轨迹。

图  13  直线轨迹跟踪

Figure  13.  Straight-line trajectory tracking

下载: 全尺寸图片 幻灯片

图  14  直线轨迹不同方向跟踪误差

Figure  14.  The straight-line trajectory tracking errors in different directions

下载: 全尺寸图片 幻灯片

4.1.3   案例3：折线轨迹跟踪

选择期望路径为

选择期望速度为${u_{\text{d}}} = 1{\text{ m/s}}$；初始状态为：${{\boldsymbol{\eta}} _0} = {\left[ {0,10,0,0,0,0} \right]^{\text{T}}}$；初始速度为${{\boldsymbol{v}}_0} = [ 0.1,0,0, 0.1, 0.1 ]^{\text{T}}$；在扰动方面，应用了如下复杂扰动类型：

案例3的跟踪结果如图15所示，可见，不管是传统积分滑模算法还是改进滑模算法，在一定时间内都可让ROV到达所需轨迹。但是，观察折线轨迹跟踪效果，同时分析3个平面的跟踪状况，可以看出，改进滑模具有更小的转弯半径，确保了在轨迹转折点可以更加圆滑地到达期望轨迹，尤其是加入神经网络之后，跟踪曲线基本和期望曲线重合，具有更好的跟踪效果。进一步分析图16所示的跟踪误差效果图，可以看出，在3个方向的跟踪过程中，改进滑模在转弯时具有更加平滑的跟踪效果和更小的跟踪误差，同时，转弯时超调相比积分滑模也明显减少。此外，观察在x和y方向上的跟踪误差情况，改进滑模的响应速度比传统的积分滑模更快。

图  15  折线轨迹跟踪

Figure  15.  Broken line trajectory tracking

下载: 全尺寸图片 幻灯片

图  16  折线轨迹不同方向跟踪误差

Figure  16.  The broken line trajectory tracking errors in different directions

下载: 全尺寸图片 幻灯片

通过对螺旋线、直线以及折线等不同工况下的轨迹跟踪仿真结果的对比分析，可以发现，本文所提出的控制器具有最佳的跟踪精度和稳定性，尤其是在加入RBFNN对扰动进行估算后，验证了所提方法既具有良好的跟踪精度，又具有良好的跟踪稳定性。同时，本文提出的SMC提升了算法在轨迹跟踪方面的响应时间，在引入RBFNN进行扰动估算后，可以实现面向复杂海流干扰的三维时变LOS导引及鲁棒运动控制。

4.2   水池试验

为检验所提算法和方案的实际可行性和可拓展性，将设计方案和控制器在BlueROV2上进行集成，并在华中科技大学拖曳水池中开展视觉引导对接试验。该水池长175 m，宽6 m，水深4 m。首先，将ROV置于距离对接目标10～15 m的水平面上，水下对接目标静止不动。然后，根据预先规划好的搜索路径，采用设计的跟踪算法，完成目标搜索，并依据视觉引导信息来控制ROV接近对接站（在对接过程中，YOLOv5目标检测算法对接光源识别效果如图17所示）。最后，完成基于视觉的位姿精调，控制ROV完成对接任务，采用手机拍照、录视频和ROV自带相机录视频的方式记录水下对接过程的试验情况，如图18所示。

图  17  不同距离下YOLOv5目标检测算法对灯光的识别效果

Figure  17.  Recognition effect of YOLOv5 object detection algorithm under different lighting conditions at different distances

下载: 全尺寸图片 幻灯片

图  18  水下对接过程图

Figure  18.  Diagram of underwater docking process

下载: 全尺寸图片 幻灯片

图18(a)～图18(d)所示为岸上相机的视角所拍摄的ROV与对接站状态，图中的窗户等为水面倒影；图18(e)～图18(f)所示为ROV上相机视角所拍摄的对接站状态。

在水池试验中，将ROV置于不同位置和位姿，最后均能通过视觉引导控制，实现准确对接作业。上述结果表明，本文设计的视觉处理算法能有效检测目标并估算相对位姿，三维轨迹跟踪控制器能基于视觉解算结果完成高效跟踪控制。

如图19所示，在初始情况下，UUV通过改进控制算法的控制很快跟踪到期望轨迹，但是在转弯时刻，跟踪出现了较大的偏差。之后，通过视觉进行位姿解算，得到相应的偏差量，再通过改进滑模算法控制ROV进行位姿调整后，继续跟踪到期望轨迹，实现了对接过程。综上，本文提出的视觉引导轨迹跟踪控制方案合理可行，能有效解决水下对接问题。

图  19  对接过程中ROV运动曲线

Figure  19.  Motion trajectory curve of ROV for docking process

下载: 全尺寸图片 幻灯片

5.   结　语

本文针对UUV的自主对接问题，提出了一种基于视觉引导的轨迹跟踪控制方案。面对位置未知的对接目标，该方案采用视觉处理方法，估算出UUV本体与对接目标间的相对位姿关系，从而为三维轨迹跟踪控制提供依据。采用此方案，可避免传统惯性导航系统的测量误差影响。文中针对视觉处理和三维轨迹跟踪控制开展了研究工作。在视觉处理过程中，设计YOLOv5神经网络实现了对接目标的训练和监测，在此基础上，利用EPnP算法实现了UUV与对接目标间的相对位姿估算。基于视觉解算结果，设计三维LOS导引策略，并融合RBFNN和改进TSMC算法，设计了高效的三维鲁棒轨迹跟踪控制器，克服了复杂扰动影响和SMC抖振问题。最后，通过数值仿真和水池试验，验证了本文提出的视觉引导对接控制算法的有效性。

未来，将探索更复杂环境下的移动对接策略，为解决UUV水下对接奠定基础。