全驱动式自主水下航行器有限时间编队控制

袁健，张文霞，周忠海

(1.国家海洋监测设备工程技术研究中心，山东 青岛，266001;2.山东省海洋环境监测技术重点实验室，山东 青岛，266001;3.山东省科学院海洋仪器仪表研究所，山东 青岛，266001;4.青岛理工大学 琴岛学院，山东 青岛，266100)

多个自主水下航行器的协同控制对于海洋科学考察、海洋开发以及军事应用等方面都具有重要的理论和现实意义。多自主水下航行器的编队控制是多机器人协调控制中的一个典型科学问题。与空间以及陆地多机器人编队控制技术相比，水下多AUV编队控制非常困难。除了AUV自身运动控制技术比较难实现外，在信号传输方面水声通信方式随着通信距离的增加通信质量显著下降，主要表现在信号的延迟、衰减和失真［1］。当前关于自主水下航行器的编队控制的研究，主要是借鉴了陆地多机器人编队控制策略，多采用分布控制器的设计思路，而忽略多自主水下航行器间的耦合关系，涉及的主要编队控制方法有虚拟结构法［1-6］、递阶控制法［7-8］、跟随领航法［9］、基于行为的智能控制方法［10］等;或者将多AUVs建模为多智能体，而忽略其姿态特性(横滚角、俯仰角和偏航角)。本文考虑到三维空间AUV的位置和姿态协调控制问题，提出一种有限时间一致性算法实现多个全驱动式自主水下航行器的编队控制。利用各AUV的动力学方程和一致性算法得到的速度和位置来求得各AUV的推力和推力矩，并通过仿真验证了控制策略的有效性。

1 AUV三维运动学和动力学模型

1.1 单位的书写规则

定义2个直角坐标系，分别是惯性坐标系O-xyz和AUV本体坐标系E-ξηζ。惯性坐标的原点O定义为海平面内的某点，O-x轴和O-y轴位于水平面内，O-x轴与经线方向平行且指向北，O-y轴与纬线方向平行且指向东，O-z轴垂直于水平面且指向地心。E-ξ、E-η和E-ζ是AUV的惯性主轴，本体坐标系的原点E定义在AUV的重心，E-ξ定义在纵向中剖面内且指向AUV线速度方向，E-η与纵向中剖面垂直且指向右手边，E-ζ在纵向中剖面内且指向下。AUVi在惯性坐标系和本体坐标系下的示意图，如图1所示。

图1 AUVi在2个坐标系下的示意图Fig.1 Schematic of AUViin two coordinate systems

惯性坐标系到本体坐标系的转换矩阵为

式中:

式中:αi为AUVi的横滚角，βi为AUVi的俯仰角，γi为AUVi的偏航角。

在AUVi本体坐标系下，定义

式中:为AUVi的浮心相对于重心的偏移。

AUVi在本体坐标系下的动力学方程为

式中:

式中:ΣFi和 ΣMi、和、和、和、和分别表示AUVi受到的合力和合力矩、重力和重力矩、浮力和浮力矩、推力和推力矩以及水流阻力和阻力矩。

1)重力和重力矩

由于AUVi本体坐标原点定义在重心，不产生重力矩。其在本体坐标系下的表达式为

式中:Gi为AUVi所受的重力。

2)浮力和浮力矩

AUVi产生浮力和浮力矩反映到本体坐标系下，表示为

式中:Bi为AUVi受到的浮力。

3)推力和推力矩

AUVi实现6自由度运动，需要安装3对具有正反推力的推进器。沿着3个坐标轴的方向进行布局，如图2所示。其中推进器T1和T2位于E-ξη平面并且相对于ξ轴对称，其可以实现沿ξ轴的屏东以及绕ζ轴的转动;推进器T3和T4位于E-ηζ平面并且相对于η轴对称，其可以实现沿η轴的平动以及绕ξ轴的转动;推进器T5和T6位于E-ξζ平面并且相对于ζ轴对称，其可以实现沿ζ轴的平动以及绕η轴的转动。

设AUVi第k个推进器的螺旋桨转速为，螺旋桨直径为，第k个推进器的推力系数为，水密度为ρi，则第k个推进器产生的推力为

设沿着E-ξ、E-η和E-ζ轴方向上的合力分别为、和，作用于E-ξ、E-η 和E-ζ轴上的合力矩分别为、和，分别表示为

式中:为AUVi第k个推进器和与之平行的坐标轴之间的垂直距离。

图2 AUVi推力和推力矩示意图Fig.2 Thrusts and thrust torques

4)水流阻力和阻力矩

AUVi受到水动力作用，由此产生的力和力矩要反映到局部坐标系中。设水流在惯性坐标系下的速度为，则水流速度在 AUVi本体坐标系下的速度可以表示为:，则水流相对于AUVi的速度可以表示为

水流阻力与AUVi相对水流速度的平方成正比。沿E-ξ、E-η和E-ζ轴方向的水流阻力分别为、和，可以表示为

七岁时，波尔进了当地颇有名气的伽莫霍姆小学，在学校里，他聪明好学，各门功课都是优秀。课余时间，爸爸常常带着尼尔斯·波尔和他弟弟海拉德·波尔一块去远足、郊游，边走边由爸爸给他们哥俩介绍一些各方面的知识。

式中:ρi为水密度，Cdi为轴向阻力系数，Sxi、Syi、Szi为AUVi分别垂直于E-ξ、E-η 和E-ζ轴的横截面积。

水流产生的阻力矩与AUVi角速度平方成正比。阻力矩系数Kxi、Kyi、Kzi由试验测得，可以求出流体阻力矩在E-ξ、E-η和E-ζ轴方向的分量分别为

2 有限时间二阶一致性的编队控制律

提出一种基于虚拟领航者位置信息的有限时间一致性算法实现多自主水下航行器的编队控制。该编队控制方法要求AUV利用自身的位置和其到虚拟领航者的距离hi之差进行一致性协同，就是使每个AUV所拥有的虚拟领航者的位置矢量信息(这里(1:3)－hi，其中xi(1:3)表示AUVi的位置信息分量)在有限时间内达到一致:→，i≠j，如图 3 所示。

图3 基于虚拟领航者位置信息的7个AUV编队控制矢量关系示意图Fig.3 Vector relationship between seven AUVs for formation control based on positions of the virtual leader

该方法要求所有AUV在惯性坐标系下的线速度、角速度和角位移矢量在有限时间内达到一致，并使每个AUV所有用的虚拟领航者的位置信息在有限时间内达到一致，这就要求存在一个时刻ts，使得t≥ts时

式中:Nj为与AUVi有通信联系的其他AUV集合，表示矢量的范数，hi为AUVi到虚拟领航者的距离，表示AUVi的虚拟领航者的平移位置矢量，xi(4:6)表示AUVi的角位移信息分量，vi表示AUVi的速度信息矢量。

为实现多个自主水下航行器在有限时间内达到预定的编队形状，需要对系统的运动学方程进行有限时间一致性设计，采用二阶有限时间一致性协议，可实现多个AUV的状态在有限时间内达到预定的编队形状。

式中:0＜a，b≤1为与收敛性速度相关的系数，表示矢量每个元素的绝对值，sgn(·)为矢量符号函数，其表达式为

令

所以式(11)可以表示为

下面证明所提一致性控制协议作用下闭环系统的稳定性。

证明:

定义矢量李亚普诺夫函数:

沿着闭环系统(1)的轨线求v的时间导数，并考虑到为奇函数，得

由

所以

证毕。

利用控制律(4)带入式(1)求出和，再带入式(2)，可以求出合力和合力矩ΣFi和ΣMi，由于和、和、和均为已知或可测定，所以利用式(3)求出编队情形下各个AUV的推进器的驱动力和，其表达式为

再利用式(3)的6个方程，从而可以求出沿着E-ξ、E-η和E-ζ轴方向上的合力、和和合力矩、和。

3 数值仿真

以7个AUV编队为例。设AUVi到虚拟领航者的距离其表达式为

式中:r为AUV到虚拟领航者的选定的单位距离，这里r=12 m。AUVi质量mi=1 000 kg，转动惯量=100 kg·m2，惯性积100 kg·m2。浮心在本体坐标系下的坐标［1 1 1］T。重力加速度g=9.8 N/kg。水流相对于AUVi的速度［2 3 4］T，水密度 ρi=1 000 kg/m3，轴向阻力系数Cdi=1，截面积Sxi=2 m2，Syi=10 m2，Szi=10 m2;阻力矩系数Kxi=1、Kyi=1、Kzi=1。由于AUV通信范围的有限性，仅与在其通信范围内(记为SetDis)的其他AUV进行通信以进行信息交换，这里 SetDis=30 m，各个AUV的最大速度和角度具有约束，最大速度约束为，最大角度度约束为30 rad/s。所以在编队成形过程中，其通信拓扑为时变的，即连接矩阵A(t)为时变的，对于每个AUV存储的连接矩阵，需要实时调整A(t)的元素取值:在通信范围之内的对应A(t)的元素置为1，在通信范围之外的对应A(t)的元素置为0。选取a=1，b=1，χ=0.3。以正方体表示具有速度和姿态的全驱动式自主水下航行器。仿真结果如图4、5所示。

从图4中可以看出尽管在初始时刻各个AUV的初始位置、姿态和速度状态不同，但采用提出的有限时间一致性控制后，多AUV系统在有限时间内实现了所要求的编队形状，并且从图4中可以看出所有AUV的姿态和速度也在有限时间内达到了一致，从而实现了在有限时间内的编队形状。由于通信距离的约束和最大速率约束，导致在编队控制过程中的编队轨迹发生了改变，这也是多个AUV之间相互通信协调控制的结果。图5为编队控制过程中AUV1的驱动控制力和控制力矩。

图4 7个AUVs的编队控制Fig.4 Formation control of seven AUVs

图5 AUV1推力和推力矩Fig.5 Thrusts and thrust torques of AUV1

4 结束语

本文研究了全驱动式水下航行器的有限时间编队控制问题。仿真结果表明，尽管在初始时刻各个AUV的初始状态不同，但采用有限时间一致性控制后，所有AUV的位置和姿态都在有限时间内达到了一致，验证了提出的有限时间一致性控制策略的有效性。通常情况，由于AUV驱动控制力受到驱动电机的约束，速度不可能任意调节，而是具有一定约束的控制输入，必定会加大系统同步所需的时间或者由于控制输入约束而导致甚至无法实现期望的编队队形和姿态，研究充分考虑驱动电机驱动能力特性的具有控制约束情形下的自主水下航行器的编队控制问题是下一步研究的重点。

［1］LEWIS M A，TAN K H.High precision formation control of mobile robots using virtual structure approach［J］.Autonomous Robots，1997，4:387-403.

［2］LEONARD N E，FIORELLI E.Virtual leader，artificial potentials and coordinated control of groups［C］//Proceedings of IEEE Conference on Decision and Control.Orlando，USA，2001:2968-2973.

［3］LAWTON A R，YOUNG B J，BEARD R W.A decentralized approach to elementary formation maneuvers［C］//IEEE International Conference on Robotics and Automation.San Francisco，USA，2000:2728-2733.

［4］LAWTON J R，BEARD R W，YOUNG B J.A decentralized approach to formation maneuvers［J］.IEEE Transactions on Robotics and Automation，2003，19(6):933-941.

［5］REN W，SORENSEN N.Distributed coordination architecture for multi-robot formation control［J］.Robotics and Autonomous Systems，2008，56(4):324-333.

［6］REN W，BEARD R W.Decentralized scheme for spacecraft formation flying via the virtual structure approach［J］.Journal of Guidance，Control and Dynamics，2004，27(1):73-82.

［7］XIANG X B，GUAN Z H.Coordinated control for multi-AUV systems based on hybrid automata［C］//Proceedings of the 2007 IEEE International Conference on Robotics and Biomimetics.Sanya，China，2007:2121-2126.

［8］TANGIRALA S，KUMAR R，BHATTACHARYYA S，et al.Hybrid-model based hierarchical mission control architecture for autonomous underwater vehicles［C］//American Control Conference.Portland，USA，2005:668-673.

［9］YANG E F，GU D B.Nonlinear formation-keeping and mooring control of multiple autonomous underwater vehicles［J］.IEEE/ASME Transactions on Mechatronics，2007，2(2):164-178.

［10］KALANTAR S，ZIMMER U R.Distributed shape control of homogeneous swarms of autonomous underwater vehicles［J］.Autonomous Robots，2007，22(1):37-53.