CN112363517B

CN112363517B - 一种水下滑翔机的编队控制方法

Info

Publication number: CN112363517B
Application number: CN202011071831.XA
Authority: CN
Inventors: 罗业腾; 刘世杰; 鞠晓龙; 王瑾; 谭智铎
Original assignee: Shenyang Institute of Automation of CAS
Current assignee: Shenyang Institute of Automation of CAS
Priority date: 2020-10-09
Filing date: 2020-10-09
Publication date: 2021-09-28
Anticipated expiration: 2040-10-09
Also published as: CN112363517A

Abstract

本发明涉及一种水下滑翔机的编队控制方法及系统，包括利用滑翔机自身数据和感知的流场数据进行滑翔机状态估计，用于多水下滑翔机编队控制；结合多机器人协同路径跟踪，用于多水下滑翔机协同速度控制和多水下滑翔机航点生成；调整多水下滑翔机组成的多边形位置关系及状态，用于输出多种编队控制行迹。本发明利用三台水下滑翔机进行三角形编队控制试验，试验测试的编队功能包括编队定向航行、顶点跟踪、中心跟踪、队形缩放、队形旋转、收敛控制和队形随动等，以及两种或多种功能组合，取得了正三角形编队的实际与期望边长误差标准差控制在1KM范围内，队形跟踪误差小于0.5KM的良好效果。

Description

一种水下滑翔机的编队控制方法

技术领域

本发明涉及水下滑翔机控制技术领域，具体地说是一种水下滑翔机的编队控制算法。

背景技术

水下滑翔机是一种通过调节自身浮力和姿态来实现水中滑行的预编程水下潜航器，其编队协同控制不同于其他类型的水下机器人。由于水下滑翔机群仅能在异步出水后进行间接的交互以完成指令更新，是一种信息的异步交互，因此这类交互方式使得具有强时间、空间约束的同步行为采用传统实时协同的方法将难以实现。另一方面，水下滑翔机在水下运动的过程中节点之间不存在信息交互，信息交互的缺失意味着群体中的每个节点都无法获得用于实现行为协同所必备的邻居状态反馈信息，进而无法基于邻居信息实现对自身位姿与速度的调整，因此针对该问题需要结合控制与规划的思想来实现。

目前水下滑翔机多以指定航路点、指定航迹线、和指定艏向三种方式航行，对于多台水下滑翔机之间的相位性、同步性的控制能力不强。同时水下滑翔机集群监控系统中数据管理平台具备一定的开放性和兼容性，可以实时修改滑翔机的一系列参数，以使得基于监控中心的编队控制能力大大提高。

发明内容

针对水下滑翔机群这种特殊的协同运动控制，研究一种双层反馈协同控制机制，即“云端路径协同反馈环”+“水下路径跟踪反馈环”(如图1所示)，进而将协同控制置于可实现交互的云中心执行，研究水下滑翔机集群在海流影响下队形保持的协同运动控制技术，“云端路径协同反馈环”的计算在仿真计算服务器中运行；水下路径跟踪反馈环中主要研究基于海流干扰的自适应抗扰/规避/顺流运动控制技术，驱使每个单体各自跟踪云端路径协同反馈环中规划的信息，进而通过双层反馈环路实现在异步信息交互下的水下滑翔机协同运动控制，“水下路径跟踪反馈环”属于单体的运动控制，将由水下滑翔机集群监控系统传送指令给滑翔机执行。

本发明为实现上述目的所采用的技术方案是：

一种水下滑翔机的编队控制方法，包括：利用滑翔机自身数据和感知的流场与环境要素数据进行滑翔机状态估计，用于多水下滑翔机编队控制；结合多机器人协同路径跟踪，用于多水下滑翔机协同速度控制和多水下滑翔机航点生成；调整多水下滑翔机组成的多边形位置关系及状态，用于输出多种编队控制行迹。

所述该方法包括：

多水下滑翔机实时反馈各自的位置至上位机控制器；

根据编队队形需求、流场与环境要素数据以及实时监测的多水下滑翔机位置，按照协同编队控制算法计算协同指令；

根据协同指令计算多水下滑翔机各航点执行指令，用于控制各个滑翔机的水下航点轨迹；

调整误差、重复迭代上述指令计算的步骤，调整多水下滑翔机组成的多边形位置关系及状态，输出多种编队控制行迹。

所述编队队形需求包括：形状、大小、群体运动方向；

所述协同指令包括：队形的路点位置序列、队形的速度、行进方向；

所述各航点执行指令包括：各个滑翔机的俯仰角、航向角、驱动浮力；

所述协同编队控制算法，包括：

对于滑翔机单个质点：

编队队形需求包括：形状与大小、群体运动方向；即，

为队形需求；其中x_di(θ_i)与y_di(θ_i)代表每个滑翔机期望的水平位置，此位置信息即定义了期望队形的形状与大小，ψ_di(θ_i)代表第i个滑翔机期望的运动航向，即群体运动方向；θ为虚拟动点的路径参数；

A.设计虚拟动点的协同更新律，计算θ，

虚拟动点的协同更新律为：

其中，1_n为n个1组成的列向量，L为拉普拉斯矩阵，控制参数

控制参数

系统过程状态

μ的向量表达为

具体形式

B.把θ代入

计算当前队形中第i个质点的队形需求η_di(θ_i)，

C.把η_di(θ_i)代入质点执行控制器公式，计算控制输入量τ＝[τ_u，τ_v，τ_r]^T以驱动质点的运动，τ_u，τ_v为质点的驱动力，τ_r为质点驱动力矩；

质点执行控制器公式为：

τ_i＝z_i1-kz_i2+r_iJ_iSυ_ic

其中，z_i1为路径跟踪误差，z_i1＝J_i ^T(η_i-η_di)，z_i2为质点速度误差变量，z_i2＝v_i-v_id，η_i为实时监测的多水下滑翔机位置，η_di为队形需求η_di(θ_i)，v_i为为体坐标系下的实际速度，v_id为替代虚拟控制率的一阶滤波器状态(即动态面控制)；

r_i为当前第i个质点的艏向角速度；J_i为质点体坐标系与平面坐标系之间的转移矩阵

为自变换矩阵，满足S＝-S^T；

υ_ir＝υ_i-υ_if，υ_ir为海流干扰下的合速度，υ_i为体坐标系下的实际速度，υ_if为体坐标系下的海流流速，υ_ic＝J_iυ_if为平面坐标系下的海流速度，即流场与环境要素数据；

D.将τ_i代入考虑海流后的质点模型，计算当前队形中第i个质点的路点(η_i(t))；

考虑海流后的质点模型为

依次计算，得到队形的中各个质点的路点位置序列；

E.单个滑翔机质点的速度、行进方向包括：

其中，单个滑翔机质点的速度为：一段时间内的先后路点位置的距离与时间的比值；

单个滑翔机质点的行进方向为：一段时间内的先后路点位置的距离与实测GPS位置的差值的比值；

F.组成队形的速度、行进方向包括：

其中，组成队形的速度为：队形中所有单个滑翔机质点一段时间内先后路点位置的距离与时间的比值的平均值；

组成队形的行进方向为：队形中所有单个滑翔机质点一段时间内的先后路点位置的距离与实测GPS位置差值的比值的平均值。

所述多水下滑翔机实时反馈各自的位置，包括：当水下滑翔机依次出水时，定位第一出水的GPS位置，对后续出水滑翔机进行异步出水位置估计。

一种水下滑翔机的编队控制系统，包括：陆基站控制器、云端的监控与数据服务器、单体水下滑翔机控制器，各个控制器中包括处理单元和存储单元以及通讯单元，存储单元存储程序步骤，处理单元加载程序并执行如权利要求1-5任意一项所述的一种水下滑翔机的编队控制方法的步骤。

该系统包括：

所述单体水下滑翔机控制器，用于实时反馈各自的位置，经过云端的监控与数据服务器转发给陆基站的规划与控制服务器；

所述陆基站规划与控制服务器，用于根据编队队形需求、流场与环境要素数据以及实时监测的多水下滑翔机位置，按照协同编队控制算法计算协同指令；

所述云端的监控与数据服务器，用于根据协同指令计算多水下滑翔机各航点执行指令，控制各个滑翔机的水下航点；

上述各控制器及服务器协同工作，调整误差、重复迭代计算，实现对多水下滑翔机组成的多边形位置关系及状态的控制，输出多种编队控制行迹。

本发明具有以下有益效果及优点：

1.本发明在多水下滑翔机规划和控制系统的基础上，研究了利用滑翔机自身数据和感知的流场数据进行滑翔机状态估计，支撑多水下滑翔机编队控制。结合多机器人协同路径跟踪算法，研究了多水下滑翔机协同速度控制方法和多水下滑翔机航点生成方法，通过调整水下滑翔机多边形和基准多边形的相对关系以及基准多边形的状态，可以扩展编队的功能。

2.本发明利用三台水下滑翔机进行三角形编队控制试验，试验测试的编队功能包括编队定向航行、顶点跟踪、中心跟踪、队形缩放、队形旋转、收敛控制和队形随动等，以及两种或多种功能组合，取得了正三角形编队的实际与期望边长误差标准差控制在1KM范围内，队形跟踪误差小于0.5KM的良好效果。

附图说明

图1为本发明方法的编队控制设计框图；

图2为本发明方法的信息流程；

图3为本发明方法的规划服务器和监控服务器之间的信息交互；

图4为本发明方法的规划与控制系统内部结构图和数据流；

图5为本发明方法的水下滑翔机三角形编队航行效果。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方法做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。

除非另有定义，本文所使用的所有技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

本项研究在多水下滑翔机规划和控制系统的基础上，研究了利用滑翔机自身数据和感知的流场数据进行滑翔机状态估计，支撑多水下滑翔机编队控制。结合多机器人协同路径跟踪算法，研究了多水下滑翔机协同速度控制方法和多水下滑翔机航点生成方法，通过调整水下滑翔机多边形和基准多边形的相对关系以及基准多边形的状态，可以扩展编队的功能。

步骤1：根据编队队形需求、流场与环境要素数据以及实时监测的多水下滑翔机位置，按照协同编队控制算法计算协同指令；

算法说明：

所述编队队形需求包括：形状与大小、群体运动方向；

三角队形编队控制算法中，

为当前队形中第i个质点的队形需求，其中x_di(θ_i)与y_di(θ_i)代表每个滑翔机期望的水平位置，此位置信息即定义了期望队形的形状与大小，ψ_di(θ_i)代表第i个滑翔机期望的运动航向，即群体运动方向。

三角队形编队控制算法：

采用协同路径跟踪(Cooperative Path Following)控制方法，以下算法将给出被控对象描述、控制目标定义、控制器结构以及算法说明。通过对质点的编队控制为水下滑翔机的协同行为提供协同指令，协同指令内容包括：当前队形的速度、行进航向、路点序列。例如要想知道3个滑翔机质点a、b、c的当前队形的路点序列(η_a(t)，η_b(t)，η_c(t))，必须分别计算每个质点的路点(η_i(t))。

被控对象描述：

针对二维平面质点运动模型

考虑海流后的质点模型为

其中，η_i＝[x_i，y_i，ψ_i]^T为待计算的平面坐标系下的位置向量，即，当前队形中第i个质点的路点(η_i(t))；υ_i＝[u_i，v_i，r_i]^T为质点体坐标系下的速度向量，τ_i＝[τ_iu，τ_iv，τ_ir]^T为控制输入向量，r_i为当前第i个质点的艏向角速度，矩阵

为质点体坐标系与平面坐标系之间的转移矩阵。υ_ir＝υ_i-υ_if，υ_ir为海流干扰下的合速度，υ_i为体坐标系下的实际速度，υ_if为体坐标系下的海流流速，υ_ic＝J_iυ_if为平面坐标系下的海流速度，即流场与环境要素数据。

为自变换矩阵，满足S＝τS^T。

控制目标定义：

对于由N个质点组成的系统，索引为i＝1，…，N。定义

为期望的参数化路径，即第i个质点的在当前队形中的位置需求，其中，

为虚拟动点的路径参数，记期望的虚拟动点参考速度为v_di；

则多质点协同跟踪路径η_di的控制目标包括三点：1)质点位于路径上；2)虚拟动点的速度趋近于参考速度；3)每个质点的路径在几何上保持一致。数学描述为：

(1)

(2)

(3)

将计算上述三个数学描述转换为如下求解过程：

首先定义单个质点的速度跟踪误差

其控制目标为

控制器结构：

1)定义分散式协同控制率：

其中：

为引入的一个辅助状态，与路径跟踪误差z_1i相关的函数μ的向量表示为

具体表达式为

为正常数。结合控制目标，ω_si为第i个质点的分散式协同控制率，并记

2)将分散式协同控制率改写成状态空间形式，则虚拟动点的协同更新律：

其中，以上2个公式均为向量表达，因此不带下标i，1_n为n个1组成的列向量，L为拉普拉斯矩阵，控制参数

控制参数

3)则质点执行控制器设计为：

τ_i＝z_i1-kz_i2+r_iJ_iSυ_ic

其中，z_i1为路径跟踪误差，z_i1＝J_i ^T(η_ηi-η_di)，z_i2为质点速度误差变量，z_i2＝v_i-v_id，v_i为体坐标系下的实际速度，

为用于替代虚拟控制律的一阶滤波器状态(动态面控制技术)；η_i为实时监测的多水下滑翔机位置，η_di为队形需求η_di(θ_i)；

整个三角队形编队控制算法的输入包括①η_di(θ_i)，②流场与环境要素数据υ_c，以及③实时监测的多水下滑翔机当前位置η_i＝[x_i，y_i，ψ_i]，i＝a，b，c，对应于η_i的初值η_i(0)，即(η_a(0)，η_b(0)，η_c(0))；输出为协同指令，即滑翔机下一个周期应执行的④路点序列(η_a(t)，η_b(t)，η_c(t))、⑤队形的速度、⑥行进方向。

其中，单个滑翔机质点的速度为：一段时间内的先后路点位置的距离与时间的比值；单个滑翔机质点的行进方向为：一段时间内的先后路点位置的距离与实测GPS位置的差值的比值；

其中，⑤组成队形的速度为：队形中所有单个滑翔机质点一段时间内先后路点位置的距离与时间的比值的平均值；⑥行进方向为：队形中所有单个滑翔机质点一段时间内的先后路点位置的距离与实测GPS位置差值的比值的平均值；

在上述动点协同更新律与单体执行控制器的作用下可使得队形实现稳定控制。

步骤2：根据协同指令计算多水下滑翔机各航点执行指令，用于控制各个滑翔机的水下航点轨迹；包括:

所述单个滑翔机质点的航点执行指令包括：俯仰角、航向角、驱动浮力；

⑦俯仰角、⑧航向角、⑨驱动浮力也是基于路点序列(η_a(t),η_b(t),η_c(t))、队形的速度、行进方向来计算的。

单个滑翔机质点的⑦俯仰角和⑨驱动浮力，可根据一段时间内的单个滑翔机质点的先后路点位置的距离和行进方向进行查表计算。

⑧单个滑翔机质点的航向角即为⑥单个滑翔机质点的行进方向。

此时滑翔机真正执行的指令为将协同指令基于滑翔机模型计算得到的每台滑翔机的⑦俯仰角、⑧航向角及⑨驱动浮力，随后滑翔机执行这些指令。当有滑翔机出水时(假设为A)，则将A的实际GPS位置信息与尚未出水的B、C位于水下的位置估计信息一同反馈至协同控制算法的输入端，这些信息作为初值再经过质点协同控制算法输出下一周期的协同指令，以此循环往复逐渐调整误差最终形成期望的队形。信息流程见图2。

步骤3：三角队形编队控制算法主要功能：调整误差、重复迭代上述指令计算的步骤，调整多水下滑翔机组成的多边形位置关系及状态，输出多种编队控制行迹。

仿真结果与滑翔机编队交互

依托于水下滑翔机监控与数据服务器(以下简称监控服务器)和水下滑翔机规划与控制服务器(以下简称规划服务器)。监控服务器通过铱星与水下滑翔机建立通信连接，回收观测数据和载体状态数据，下达对水下滑翔机的控制指令。规划服务器通过监控服务器接收水下滑翔机的状态和观测数据，执行相应的规划算法，并将规划结果通过监控服务器下达给水下滑翔机执行，见图3。

规划服务器和监控服务器通过sftp协议建立通信，监控服务器每小时45分向规划服务器发送水下滑翔机上一周期出、入水的经度和纬度，以及推算的上一周期深平均流。规划服务器每小时50分检测是否有本任务中的水下滑翔机出水，如果有，则读取状态信息并执行规划算法，规划结果在结果生成时刻以及每小时00和30分发送给监控服务器，监控服务器在下一次水下滑翔机出水时将航点发送给滑翔机执行。

水下滑翔机规划与控制系统设计面向任务的分层结构，上一层任务通过任务分解成多个子任务，并对子任务的状态进行评估，根据评估结果进行任务内资源的调动和分配。不能再分的子任务节点作为一个执行单元，读取本任务内相关滑翔机的状态并执行相应程序。

图4给出了规划与控制系统的基本控制流程。系统的数据输入主要可分为两种：一种是水下滑翔机的状态数据，来自监控服务器；另一种是环境要素数据，包括外来数据，如来自模式的温、盐、流场数据和地形数据，和自身感知数据，如来自监控服务器的推算深平均流数据。系统中包含水下滑翔机运动模型，用于计算水下滑翔机的状态数据，在真实试验中提供水下滑翔机的估计位置，在仿真试验中还提供水下滑翔机模拟出水和流场推测等数据。

本年度利用三台水下滑翔机进行三角形编队控制试验，试验从2019年7月20号开始，到8月1号结束，历时12天。试验测试的编队功能包括编队定向航行、顶点跟踪、中心跟踪、队形缩放、队形旋转、收敛控制和队形随动等，以及两种或多种功能组合，取得了正三角形编队的实际与期望边长误差标准差控制在1KM范围内，队形跟踪误差小于0.5KM的良好效果，见图5。

其中，“编队定向航行、顶点跟踪、中心跟踪、队形缩放、队形旋转、收敛控制和队形随动”的定义；

定向航行：设定期望的航行角度φ，即编队稳定后整体及各水下滑翔机的运动方向；

顶点跟踪：以三台水下滑翔机组成的等边三角形中的任一点对期望的路径η_d进行跟踪；

中心跟踪：以三台水下滑翔机组成的等边三角形中的几何中心对期望的路径η_d进行跟踪；

队形缩放：调整等边三角形期望边长κ，使得水下滑翔机编队的边长变大或变小；

队形旋转：调整期望三角形的旋转角度ψ使得水下滑翔机编队相对地球坐标系旋转角度ψ；

收敛控制：设定控制器的积分时间t，使得多水下滑翔机快速或慢速形成期望队形；

队形随动：设定跟踪路径参数θ(t)，使得水下滑翔机编队能够以中心跟踪或顶点跟踪方式跟踪时变曲线η_d(θ)。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰应视为本发明的保护范围。