0.   引言

自主式水下机器人（Autonomous Underwater Vehicle，AUV）广泛应用于军民用领域，是海洋开发中使用的重要工具之一。其具有机动性好和巡航范围大等优点，在水下观测、制图、定位和深海探测中扮演着重要角色^[1]。而三维路径跟踪作为AUV的重要功能，对于其能否精准遂行规定的使命任务、顺利完成回收和布放等都有着重要意义^[2]。

AUV是一种无缆自主式水下机器人，配备有电池及各种传感器，可水下自主作业。迄今，研制的AUV多以欠驱动型为主。例如，美国的Hydroid公司的Remus 6000^[3]配备有测深仪、侧扫声呐、水下摄像机等多种传感器，可用于海底测绘等任务；美国金枪鱼机器人公司的Bluefin 21^[4]可携带多种传感器及有效载荷，配备有大容量电池，主要用于近海勘探、搜救及反水雷等军事用途。虽然欠驱动型AUV制造成本较低、航速快、推进效率高，但定位精度和对危险环境的应对能力不强^[5]。而相比于欠驱动型AUV，全驱动型AUV操控性能则较好，运动控制灵活。其中，三维路径跟踪控制作为衡量AUV控制性能的主要指标，是AUV自主作业的关键技术。

三维路径跟踪控制主要是将其解耦为水平面（横向）和垂直面（垂向）控制，并分别设计转艏控制器、深度控制器等，以实现三维路径跟踪控制。由于AUV本身存在不确定性和非线性的特点，加上海流等干扰，给控制器的设计带来了很大难度。目前，国内研究人员多围绕上述问题针对AUV的三维路径跟踪开展研究。例如，Qi^[6]采用神经网络进行非线性重建，引入自适应方法使控制器具备了较好的鲁棒性和抗干扰性，但是研究未考虑到AUV是离散系统；王晓伟等^[7]采用反步法设计了水平面跟踪控制器，利用微分器对未知状态和不确定项进行估计，但是这种方法的计算量较大，对于实际的AUV系统无法实时处理；刘昌鑫等^[8]采用非线性模型预测控制方法设计了AUV的约束路径跟踪控制律，以解决有约束路径跟踪控制问题，但仍存在计算量大的问题。由此可见，简化控制器的设计，使其具有较好的鲁棒性和抗干扰性，是目前AUV三维路径跟踪控制器的设计目标。

鉴此，本文将以国内自主研发的“探海I型”全驱动型AUV为对象，介绍系统的组成，研究并建立该AUV的推进器、水平面和垂直面的数学模型；设计三维路径跟踪控制器，采用S面控制三维路径跟踪控制器设计部分，契合AUV自身的非线性特性，并引入积分项，以增强控制器的抗干扰性能，旨在较好地解决简化控制器设计问题。

1.   “探海I型”AUV数学模型

本文所述的数学模型主要来源于Remus模型。为更直观地描述水下机器人的水动力模型，一般建立两种坐标系：惯性（固定）坐标系E-ξηζ和载体（运动）坐标系O（G）-xyz，如图 1所示。

图  1  AUV坐标系示意图

Figure  1.  Definition of AUV coordinate system

下载: 全尺寸图片 幻灯片

AUV的运动学方程将浮心作为载体坐标系原点，通过长期的理论分析和工程实践得到如下方程：

式中：m为AUV本体质量；x_G，y_G，z_G为AUV重心坐标；I_x，I_y，I_z为AUV在3个轴方向的转动惯量；u，v，w为AUV线速度在载体坐标系3个轴方向的分量；p，q，r为AUV角速度在载体坐标系3个轴方向的分量^[9]；X，Y，Z分别为3个轴方向的力；K，M，N分别为3个轴方向的力矩。

1.1   推进器模型

本文“探海Ⅰ型”AUV属于全驱动型AUV，采用鱼雷外型，设计有5个推进器，如图 2所示。其中：2，3号电机为侧向推进器；4，5号电机为垂向推进器；1号电机为艉部主推进器。6自由度（x轴向进退、y轴向平移、z轴向浮潜、x轴向横倾、y轴向纵倾、z轴向转艏）状态变量为[ξ, η, ζ, φ, θ, ψ]^T，其中ξ，η，ζ为AUV质心在惯性坐标系中的位置；φ，θ，ψ为AUV相对于惯性坐标系的姿态角，即横倾角、纵倾角、转艏角。横倾角φ不予考虑。

图  2  全驱动型AUV推进器布置

Figure  2.  Propeller configuration of fully-actuated AUV

下载: 全尺寸图片 幻灯片

假设推进器为线性模型，则推进器推力X_T表示为

式中：D为螺旋桨直径；n为螺旋桨转速；ρ为流体密度；t_r为推力减额系数（很小，可忽略）；K_T为无因次推力系数。

假设已知各推进器的推力/力矩向量为T₁，T₂，T₃，T₄，T₅，同一个平面的电机旋向相反，因未达到电机的最大功率，推进器反向效率不予考虑，则各推力和力矩由下式表示。

进退推力：

平移推力：

浮潜推力：

水平力矩：

垂向力矩：

式中，d₁，d₂分别为T₁，T₂的推力距。将式（3）~式（7）整合，得到

式（8）中，若主推进器转速恒定，则可将其视为轴向阻尼二次项系数X_u|u|与速度的二次项关系，即

式中：T_x为x轴向推力；u_obj与|u_obj|分别为目标速度及其绝对值。

1.2   垂直面控制模型

基于AUV的6自由度模型，可得到AUV垂直面控制模型。

首先，得到简化的AUV垂直面运动学方程如下。

z轴向浮潜运动学方程（一般情况下，y_g=0）：

y轴向纵倾运动学方程（恒速，$\dot{u}=0$）：

式中：F_i为高斯白噪声；Z_w|w|，Z_q|q|，$Z_{\dot{w}}$，$Z_{\dot{q}}$，Z_uq，Z_uw，M_w|w|，M_q|q|，$M_{\dot{w}}$，$M_{\dot{q}}$，M_uq，M_uw均为水动力参数，其中Z_ẇ，$Z_{\dot{q}}$为多元矩阵函数G中相应分量的一阶偏导数，Z_w|w|，Z_q|q|为多元矩阵函数G中的二阶混合偏导数；Z_g，M_g为重力；Z_prop，M_prop分别为z轴的推力和y轴推力矩；x_g，z_g为重心坐标。

然后，将“探海I型”模型参数代入垂直面控制模型，得到如下AUV垂直面运动学方程。

z轴向浮潜运动学方程：

y轴向纵倾运动学方程：

1.3   水平面模型

若假设AUV的航行深度未发生改变，仅航向、航迹发生变化，则认为AUV的重心保持在水平面上。水平面内，AUV在惯性坐标系的坐标变换关系即可表示为

其中，惯性坐标系以水平面一点E为原点，ξ轴指向地理北向，η轴指向地理东向，ζ轴指向地心。

当w=0，p=0，q=0时，首先简化得到以下AUV的水平面运动学方程。

x轴向进退运动学方程（一般情况下，y_g=0）：

y轴向平移运动学方程（一般情况下，y_g=0）：

z轴向转艏运动学方程（一般情况下，y_g=0）：

式中：X_u|u|，${X_{\dot u}}$，X_vr，X_rr，Y_v|v|，$Y_{\dot{v}}$，Y_vr，Y_uv，N_v|v|，N_r|r|，N_ṙ，N_ur，N_uv均为水动力参数；X_prop，Y_prop，N_prop为各自方向的力和力矩。

然后，将“探海I型”模型参数代入水平面控制模型，得到如下AUV水平面运动学方程。

x轴向进退运动学方程：

y轴向平移运动学方程：

z轴向转艏运动学方程：

2.   AUV三维路径跟踪控制器设计

2.1   引入智能积分的S面控制器设计

在工程实际中，模糊控制器和PID控制器应用得最为广泛^[10]。但是，AUV模糊控制器非常依赖于设计人员的实际经验，且AUV是非线性系统，而基于线性模型的PID控制器控制效果往往不令人满意。因此，结合上述两种控制器的优缺点，刘学敏等^[11]提出了改进的S面控制方法，该方法被广泛应用于水下机器人的控制领域，取得了很好的控制效果。S面控制器的控制模型为

式中：u为控制输出，在AUV里考虑其为对应推进器的推力和转矩；Δu为调整项，可以将其考虑为一段时间内的固定干扰力或是其他调整因素；e和ė为控制输入信息，其中e在AUV里考虑为深度和转艏角误差信息，ė在AUV里考虑为深度和转艏角误差变化率；k₁和k₂为对应偏差和偏差率的控制系数，可以将其类比为PID控制器中的PD系数。

S面控制器本质上是一种特殊的非线性PID控制器，这里并没有引入积分项，所以对洋流的抗干扰能力较弱。结合PID的特点，对S面控制器的Δu调整项设计成积分项，控制模型即变为

式中，k₃为控制系数。

对于式（22），当[e(t)*ė(t)>0]或[ė(t)=0]时，对S面控制进行积分；当[e(t)*ė(t) < 0]或[e(t)=0]时，不对S面控制进行积分。由此，通过引入积分项可以减少跟踪时的稳态误差^[12]。

2.2   稳性分析

首先，若只考虑水平面纵向和转艏运动，则AUV水平面运动学方程为

式中，N_r为多元矩阵函数G的一阶偏导数。

转艏力矩N_prop可以写为

式中，N_max为z轴转矩N的最大值。为简化考虑，${C_1} = \frac{{{\mathit{\boldsymbol{N}}_{\max }}{k_1}}}{{2{{\dot e}_{\max }}}}$, ${C_2} = \frac{{{\mathit{\boldsymbol{N}}_{\max }}{k_2}}}{{2{{\dot e}_{\max }}}}$。根据$\left\{\begin{array}{l}{\dot{e}=-r} \\ {\ddot{e}=-\dot{r}}\end{array}\right.$，将其代入式（23）和式（24），化简可得

式中，ë为艏向角误差变化率的一阶导数。

取Lyapunov函数（V）：

求导得到

因C₂>0，z轴向速度u>0，水动力参数N_r|r| < 0, N_r < ]，故可知转艏角控制器是稳定的^[13]。同理，可证明纵、垂向面的深度控制器也是稳定的。

3.   全驱动控制系统概述

鉴于欠驱动型AUV在操纵性方面存在缺陷，自主研发了全驱动型AUV样机。该型AUV具有位置跟踪精度高、控制灵活等特点。图 3所示为该型AUV实体样机，具体参数见表 1。

图  3  全驱动AUV实体样机

Figure  3.  The full-scale prototype of fully-actuated AUV

下载: 全尺寸图片 幻灯片

表  1  全驱动型AUV技术参数

Table  1.  Specifications of fully-actuated AUV

参数	数值
长度/mm	2 100
直径/mm	220
空气中净重/kg	65
最高航速/kn	2.5
续航力/h	6
最大工作深度/m	60
通信距离/km	水面≥3，水下0.5

下载: 导出CSV 

| 显示表格

图 4所示为该AUV主要设备及各段的组成。其中，水面设备主要由光端机、无线数传电台、光滑环、光纤绞盘和岸基控制单元等组成。舱段包括艏段、艏部推进段、电子舱段、艉部推进段和主推进段。AUV控制设备主要包括光端机、网络交换机、自动驾驶仪、PC104摄像机、图像采集卡、电机驱动器、照明灯控制板和电池节点控制器，以及多普勒计程仪、光纤惯导、深度传感器、漏水传感器和入水传感器等^[14]。

图  4  AUV系统组成示意图

Figure  4.  Schematic diagram of AUV system composition

下载: 全尺寸图片 幻灯片

该型AUV软件系统基于自动驾驶仪PC104上的Linux架构平台，使用美国麻省理工学院的水下机器人开源库（Mission Orientated Operating Suite，MOOS）进行开发，通过核心控制单元MOOSDB进行信息交互^[15]。MOOS开源库采用星型拓扑结构，具有模块化、可维护性强等特点，整个软件结构包括中央控制单元MOOSDB、传感器数据采集单元、运动控制单元、pHelm行为控制单元和岸基控制单元等。

4.   仿真及湖试结果分析

4.1   仿真分析

本文分别设计了PID控制器和智能积分S面控制器，并对AUV进行了三维路径跟踪仿真。仿真采用的是“探海I型”AUV水动力等模型参数。主推进器给定恒定推力，目标深度为3 m，三维跟踪初始位置（单位：m）为（10，150，0），六边形的6个定点坐标分别为（130，50，3）、（160，90，3）、（210，95，3）、（240，55，3）、（210，15，3）、（160，10，3），仿真结果如图 5~图 8所示。仿真中，在0~3 200 s范围均采用智能积分S面控制器，在3 200 s时AUV到达六边形跟踪初始位置（160，90，3），施加x轴正向0.1 m/s洋流干扰，并切换控制器进行对比。

图  5  智能积分S面三维路径跟踪图

Figure  5.  3D path-following map by intelligent integral S-plane controller

下载: 全尺寸图片 幻灯片

图  6  PID三维路径跟踪图

Figure  6.  3D path-following map by PID controller

下载: 全尺寸图片 幻灯片

图  7  智能积分S面水平面航线图

Figure  7.  Horizontal route map by intelligent integral S-plane controller

下载: 全尺寸图片 幻灯片

图  8  PID水平面航线图

Figure  8.  Horizontal route map by PID controller

下载: 全尺寸图片 幻灯片

仿真结果表明：智能积分S面控制器和PID控制器虽然都能完成三维路径跟踪任务，但是在深度控制方面，PID深度控制误差大于智能积分S面控制（图 9）；在水平面跟踪方面，智能积分S面控制器收敛速度快于PID控制器（图 10）；AUV切换目标航线时会产生一定的超调，而出现超调会使AUV的跟踪路线产生误差，从而导致这2种控制器的跟踪时间不一致；智能积分S面控制器在海流干扰下的三维路径跟踪性能优于PID控制器。表 2给出了智能积分S面和PID跟踪六边形时每条边跟踪的平均误差。

图  9  深度曲线对比图

Figure  9.  Contrast map of depth curves

下载: 全尺寸图片 幻灯片

图  10  位置误差对比图

Figure  10.  Contrast map of position errors

下载: 全尺寸图片 幻灯片

表  2  AUV六边形跟踪平均误差

Table  2.  The average errors of AUV hexagon tracking

边号	智能积分S面误差/m	PID误差/m
1	0.11	0.23
2	0.31	0.71
3	0.36	1.12
4	0.34	1.55
5	0.27	0.92
6	0.24	1.21

下载: 导出CSV 

| 显示表格

4.2   湖试结果分析

图 11所示为湖试中的AUV，试验湖区符合试验条件要求。其中，水深为5~40 m，水流流速不超过2 kn。在湖试中，给定巡航中心点和运动图形，水下采用航位推算系统自主巡航。使命规划如下：航速0.8 kn，潜深5m，初始位置GPS经纬度（120.317 245°，31.109 611°），起始航迹点GPS经纬度（120.318 199°，31.109 256°），回收航迹点GPS经纬度（120.317 280°，31.109 557°），巡航图形为六边形，巡航中心位置GPS经纬度（120.317 247°，31.109 610°），自主导航时间7 800 s。表 3给出了水下六边形自主巡航试验的部分数据。

图  11  现场实验

Figure  11.  Field experiment

下载: 全尺寸图片 幻灯片

表  3  自主巡航试验部分数据

Table  3.  The partial experimental data acquired from autonomous cruise

时间	经度/（°）	纬度/（°）	实际航向角/（°）
14:14:27	120.318 038	31.109 033	135.044
14:28:47	120.318 420	31.109 067	27.849
15:28:03	120.318 435	31.109 098	22.086

下载: 导出CSV 

| 显示表格

根据航位推算系统得到经纬度，绘制出AUV巡航的轨迹图，如图 12和图 13所示。图 14和图 15给出了航向角和深度曲线。

图  12  水下六边形自主巡航经纬度轨迹三维图

Figure  12.  3D diagram of longitude and latitude trajectory of underwater hexagonal autonomous cruise

下载: 全尺寸图片 幻灯片

图  13  水下六边形自主巡航经纬度轨迹平面图

Figure  13.  Plane diagram of longitude and latitude trajectory of underwater hexagonal autonomous cruising

下载: 全尺寸图片 幻灯片

图  14  航向角曲线

Figure  14.  Curves of azimuth angle

下载: 全尺寸图片 幻灯片

图  15  深度曲线图

Figure  15.  Depth curves

下载: 全尺寸图片 幻灯片

由图 12和图 13可以看出，AUV自主巡航的稳定性很高，在巡航结束后浮出水面返回回收点的过程中，湖面起风，水流波动较大，导致了AUV部分轨迹杂乱，但总体上使用水下航位推算系统得到的AUV自主导航航迹辨识度较高。

由图 14和图 15分别可以看出，AUV在路径跟踪时的航向角有一定的超调，这是因为切换目标航线时引起了较大的转艏角和一定的水流干扰所致；AUV深度曲线存在一定的波动，这是AUV路径跟踪时因为稳心差而引起晃动，但总体上还是能够较好地跟踪目标深度。

综上所述，AUV在执行路径跟踪使命时，通过GPS、光纤惯导、深度计和多普勒计程仪等多种传感器进行航位推算，以及由自主决策单元计算出期望深度、期望航向角等数据，能够很好地解决传感器自身误差及安装误差造成AUV自主导航精度达不到实际工程应用要求的问题。

5.   结语

本文对国内自主研发的“探海I型”全驱动型AUV的推进器、垂直面和水平面建立了数学模型，设计的智能积分S面控制器契合了AUV自身的非线性特性，并引入积分项，增强了控制器的抗干扰性能，较好地解决了简化控制器的设计问题。仿真分析和湖试结果表明，该AUV运行稳定，所设计的控制器有效，能够较好地完成三维路径跟踪任务，满足了全驱动型AUV水下应用的实际需求。采用的三维路径跟踪控制器设计方法可为全驱动型AUV三维路径跟踪研究奠定基础。