全驱动型AUV 三维路径跟踪控制系统设计及分析

姚金艺，曾庆军，赵强，朱志宇，戴文文

江苏科技大学电子信息学院，江苏镇江212003

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 坐标系示意图Fig.1 Definition of AUV coordinate system

AUV 的运动学方程将浮心作为载体坐标系原点，通过长期的理论分析和工程实践得到如下方程：式中：m为AUV 本体质量；xG，yG，zG为AUV 重心坐标；Ix，Iy，Iz为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 推进器布置Fig.2 Propeller configuration of fully-actuated AUV

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

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

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

进退推力：

平移推力：

浮潜推力：

水平力矩：

垂向力矩：

式中，d1，d2分别为T1，T2的推力距。将式（3）～式（7）整合，得到

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

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

1.2 垂直面控制模型

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

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

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

y 轴向纵倾运动学方程（恒速，u̇=0）：

式中：Fi为高斯白噪声；Zw||w，Zq||q，Zẇ，Zq̇，Zuq，Zuw，Mw||w，Mq||q，Mẇ，Mq̇，Muq，Muw均为水动力参数，其中Zẇ，Zq̇为多元矩阵函数G中相应分量的一阶偏导数，Zw||w，Zq||q为多元矩阵函数G中的二阶混合偏导数；Zg，Mg为重力；Zprop，Mprop分别为z 轴的推力和y 轴推力矩；xg，zg为重心坐标。

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

z轴向浮潜运动学方程：

y 轴向纵倾运动学方程：

1.3 水平面模型

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

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

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

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

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

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

式中：Xu||u，Xu̇，Xvr，Xrr，Yv||v，Yv̇，Yvr，Yuv，Nv||v，Nr||r，Nṙ，Nur，Nuv均 为 水 动 力 参 数；Xprop，Yprop，Nprop为各自方向的力和力矩。

然后，将“探海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 里考虑为深度和转艏角误差变化率；k1和k2为对应偏差和偏差率的控制系数，可以将其类比为PID 控制器中的PD 系数。

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

式中，k3为控制系数。

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

2.2 稳性分析

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

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

转艏力矩Nprop可以写为

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

取Lyapunov 函数（V）：

求导得到

因C2＞0 ，z 轴 向 速 度u＞0 ，水 动 力 参 数Nr||r＜0，Nr＜0，故可知转艏角控制器是稳定的［13］。同理，可证明纵、垂向面的深度控制器也是稳定的。

3 全驱动控制系统概述

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

图3 全驱动AUV 实体样机Fig.3 The full-scale prototype of fully-actuated AUV

表1 全驱动型AUV 技术参数Table 1 Specifications of fully-actuated AUV

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

图4 AUV 系统组成示意图Fig.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 面三维路径跟踪图Fig.5 3D path-following map by intelligent integral S-plane controller

图6 PID 三维路径跟踪图Fig.6 3D path-following map by PID controller

图7 智能积分S 面水平面航线图Fig.7 Horizontal route map by intelligent integral S-plane controller

图8 PID 水平面航线图Fig.8 Horizontal route map by PID controller

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

图9 深度曲线对比图Fig.9 Contrast map of depth curves

图10 位置误差对比图Fig.10 Contrast map of position errors

表2 AUV 六边形跟踪平均误差Table 2 The average errors of AUV hexagon tracking

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 现场实验Fig.11 Field experiment

表3 自主巡航试验部分数据Table 3 The partial experimental data acquired from autonomous cruise

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

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

图12 水下六边形自主巡航经纬度轨迹三维图Fig.12 3D diagram of longitude and latitude trajectory of underwater hexagonal autonomous cruise

图13 水下六边形自主巡航经纬度轨迹平面图Fig.13 Plane diagram of longitude and latitude trajectory of underwater hexagonal autonomous cruising

图14 航向角曲线Fig.14 Curves of azimuth angle

图15 深度曲线图Fig.15 Depth curves

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

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

5 结 语

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