CN114035592B - 一种水下滑翔机三维协同路径跟踪控制系统 - Google Patents

Abstract

本发明公开了一种水下滑翔机三维协同路径跟踪控制系统，包括路径跟踪制导模块用于获取水下滑翔机的位置、航向、速度以及给定路径信息，计算给定纵向速度、纵倾角速度、艏向角速度以及纵向跟踪误差；协同模块用于获取纵向跟踪误差和邻居水下滑翔机的路径参数信息，计算协同路径参数信息；预估器模块用于计算总速度误差、纵荡角速度误差与艏摇角速度误差的估计值；动力学控制模块用于计算水下滑翔机在纵向、纵摇以及艏摇方向运动的控制参数；通信网络用于建立水下滑翔机之间进行信息传递的通信结构。本发明适用于多水下滑翔机，采用模块化设计，降低了控制系统结构的复杂性、减少了计算负荷、易于工程实现。

Description

一种水下滑翔机三维协同路径跟踪控制系统

技术领域

本发明涉及水下滑翔机控制领域，尤其涉及一种水下滑翔机三维协同路径跟踪控制系统。

背景技术

自从1974年A.Baz提出水下滑翔机的概念以来，不同性能优异的水下滑翔机已经被研制出来。作为一种新型自主式海洋观测平台，水下滑翔机依靠浮力改变和姿态调节实现滑翔运动，其能量消耗低，续航能力强，噪音极低，适于重复使用和大量投放，已经广泛应用于预警探测、反潜侦察、海洋观测、资源探测、搜救打捞等军事和民用领域，海洋观测快速发展的实际需求也大大促进了AUG集群自主协同理论与技术的蓬勃发展。

但现有的研究成果存在以下问题：

第一，现有水下滑翔机路径跟踪控制方法大多适用于单个海洋航行器，然而随着无人水下机器人技术的发展和日渐成熟，单台水下滑翔机已不能满足发展需求，且其观测或探测功能相对单一，单位时间的作业范围非常有限。

第二，现有水下滑翔机协同路径跟踪问题的研究成果中大多为二维平面或者纵平面的协同路径跟踪，无法满足水下滑翔机集群在水下三维立体空间滑翔作业的实际需求，目前国内外在水下航行器集群三维空间协同路径跟踪控制方面也鲜有研究。

第三，现有水下滑翔机在实际工作时存在模型不确定性、未知环境扰动和输入约束饱和的路径跟踪控制问题，使得控制精度大大降低，无法满足预期要求。

发明内容

本发明提供一种水下滑翔机三维协同路径跟踪控制系统，以克服上述技术问题。

一种水下滑翔机三维协同路径跟踪控制系统，其特征在于，包括路径跟踪制导模块、协同模块、预估器模块、动力学控制模块、通信网络、水下滑翔机，

路径跟踪制导模块用于获取水下滑翔机的位置、航向、速度以及给定路径信息，计算水下滑翔机的给定纵向速度、纵倾角速度、艏向角速度以及纵向跟踪误差；

协同模块用于获取水下滑翔机的纵向跟踪误差和邻居水下滑翔机的路径参数信息，计算水下滑翔机的协同路径参数信息；

预估器模块用于获取给定纵向速度、纵倾角速度、艏向角速度，计算总速度误差、纵荡角速度误差与艏摇角速度误差的估计值；

动力学控制模块用于获取给定纵向速度、纵倾角速度、艏向角速度，总速度误差、纵荡角速度误差与艏摇角速度误差的估计值，计算水下滑翔机在纵向、纵摇以及艏摇方向运动的控制参数；

通信网络用于建立水下滑翔机之间进行信息传递的通信结构；

路径跟踪制导模块包括三个输入端，其中两个输入端分别与水下滑翔机的输出端、协同模块的一个输出端相连，第三个输入端为给定的参考路径信号；路径跟踪制导模块的两个输出端分别与协同模块的输入端、动力学控制模块的一个输入端相连；

协同模块的两个输入端分别与路径跟踪制导模块的一个输出端和通信网络相连；

预估器模块的两个输入端分别与路径跟踪制导模块的一个输出端、水下滑翔机的输出端相连；预估器模块的输出端与动力学控制模块的一个输入端相连；

动力学控制模块的输出端与水下滑翔机的输入端相连。

优选地，所述计算水下滑翔机的给定纵向速度、纵倾角速度、艏向角速度以及纵向跟踪误差是通过公式(1)、(2)进行计算，

其中，U_iU、q_iq与r_ir分别为给定的纵向速度信号、纵倾角速度信号和艏向角速度信号；x_ie、Θ_ie、Ψ_ie分别表示水下滑翔机的纵向跟踪误差、航迹角跟踪误差和方位角跟踪误差；

分别为总速度误差U_ie、纵荡角速度误差q_ie与艏摇角速度误差的估计值；Θ_ie＝θ_i-α_i-Θ_iθ，Θ_iθ为期望航迹角；

为路径参数更新速度；

分别为经矩阵旋转后的航迹角和方位角，其中Δ_i为前视距离，z_ie为水下滑翔机在垂向方向的位置误差；v_s是路径参数χ_i的路径更新速度，ω_i为协同路径参数信息；

Ψ_iψ为期望方位角；k_i1，k_i2，k_i3，Δ_ix，Δ_iθ和Δ_iψ为正常数；

y_ie为水下滑翔机在横向方向的位置误差；θ_id和ψ_id为预定义路径的切线角。

优选地，所述计算水下滑翔机的协同路径参数信息是通过公式(3)计算，

其中，k_i3为控制增益，e_i＝h_i(χ_i,χ_j,δ_ij)为一般形式的协同误差，其是关于本船路径参数χ_i、邻居水下滑翔机路径参数χ_j以及路径参数相对距离δ_ij的函数；v_s是路径参数χ_i的路径更新速度，ω_i为协同路径参数，

是路径参数更新速度，x_ie是水下滑翔机的纵向跟踪误差。

优选地，所述计算总速度误差、纵荡角速度误差与艏摇角速度误差的估计值是通过公式(4)进行计算，

其中β_iU(ζ_iU)、β_iq(ζ_iq)和β_ir(ζ_ir)是已知连续隶属函数，其满足

这里

都是正实数；

是正常数；

Ξ_iU，Ξ_iq，Ξ_ir为有界权值，用于模糊系统对系统动态的估计；

和

分别是Ξ_iU，Ξ_iq和Ξ_ir的估计值。

优选地，所述

和

的计算过程为，

定义：

其中

为水下滑翔机在总速度、纵荡方向、艏摇方向上的总扰动；ε_iU,ε_iq,ε_ir为水下滑翔机在总速度、纵荡方向、艏摇方向上的估计误差，且满足

和

都是正实数，

输出信号

和

的设计如下：

其中

σ_iU,σ_iq,

为正常数；Proj[]表示投影算子；由投影算子可知，

其中

是正常数；

是用来估计

的低通滤波器权值。

优选地，所述计算水下滑翔机在纵向、纵摇以及艏摇方向运动的控制参数是通过公式(7)进行计算，

其中，

Δ_iU,Δ_iq,

是正常数；

sign()为符号函数，当b_iu≥0时，sign(b_iu)＝1，当b_iu＜0时，sign(b_iu)＝-1；

当b_iq≥0时，sign(b_iq)＝1，当b_iq＜0时，sign(b_iq)＝-1；

当b_ir≥0时，sign(b_ir)＝1，当b_ir＜0时，sign(b_ir)＝-1；ε_iu,ε_iq,

都是正常数，分别用来处理当b_iu＝0，b_iq＝0或b_ir＝0时的奇异性，τ_iU、τ_iq和τ_ir分别为水下滑翔机在纵向、纵摇方向和艏摇方向上的控制输入参数。

本发明提供一种水下滑翔机三维协同路径跟踪控制系统，第一，与现有针对单水下滑翔机的控制方法相比，本发明通过采用路径跟踪以及路径参数协同方法，使得所提出的协同下潜控制方法适用于多水下滑翔机，拓展了水下滑翔机控制的应用范围，使得该协同控制系统更有利于实际工作。

第二，与现有水下滑翔机协同路径跟踪问题的研究成果中大多为二维平面或者纵平面的协同路径跟踪相比，本发明为水下滑翔机集群构造了三维协同路径跟踪控制系统，同时考虑了内部模型不确定性、外部环境扰动和执行机构输入受限。

第三，与现有路径跟踪控制方法大多要求航行器的状态信息事精确可测的相比，本发明通过引入模糊预估器滤除系统不确定性和外扰中存在的高频分量。

第四，综上所述，本发明可应用于水下滑翔机并能实现多水下滑翔机协同下潜队形，而且控制系统采用模块化设计，显著降低了控制系统结构的复杂性、减少了计算负荷、易于工程实现。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明水下滑翔机三维协同控制系统结构示意图。

图2是本发明三艘水下滑翔机通信网络结构示意图；

图3是本发明三艘水下滑翔机三维协同运动轨迹示意图；

图4是本发明三艘水下滑翔机协同路径参数曲线图；

图5是本发明三艘水下滑翔机跟踪误差曲线图；

图6是本发明三艘水下滑翔机扰动估计性能图；

图7是本发明一号水下滑翔机的三个控制输入。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为解决现有技术存在的不足，本发明提出一种水下滑翔机三维协同路径跟踪控制系统，不仅能够应用于水下滑翔机，实现多种协同下潜控制，而且协同路径跟踪控制系统采用模块化设计，显著降低控制系统结构的复杂性、减少计算负荷、易于工程实现，从而极大地提高控制系统的整体性能。

图1为本发明水下滑翔机三维协同控制系统结构示意图，如图1所示，本实施例的方法可以包括：

为了实现上述目的，本发明的技术方案如下：一种水下滑翔机三维协同路径跟踪控制系统，包括通信网络、路径跟踪制导模块、协同模块、预估器模块、动力学控制模块；

所述的路径跟踪制导模块共三个输入端，其中两个输入端分别与水下滑翔机的输出端、协同模块的一个输出端相连，第三个输入端为给定的参考路径信号；路径跟踪制导模块的两个输出端分别与协同模块的输入端、动力学控制模块的一个输入端相连。

协同模块的两个输入端分别与路径跟踪制导模块的一个输出端和通信网络相连。

预估器模块的两个输入端分别与路径跟踪制导模块的一个输出端、水下滑翔机的输出端相连；预估器模块的输出端与动力学控制模块的一个输入端相连。

动力学控制模块的输出端与水下滑翔机的输入端相连。

通信网络用于建立水下滑翔机之间进行信息传递的通信结构。

水下滑翔机在三维空间运动时的运动学模型为：

动力学模型为：

式中下标i表示第i艘水下滑翔机；

为水下滑翔机的总速度，α_i＝arctan(w_i/u_i)和β_i＝arcsin(v_i/U_i)分别为攻角和侧滑角，其中u_i＞0；

u_i、v_i、w_i、q_i、r_i分别为水下滑翔机的纵向速度、横向速度、垂荡速度、纵倾角速度和艏向角速度；

x_i、y_i、z_i、θ_i、ψ_i分别为水下滑翔机在纵向、横向、垂向、纵摇方向和艏摇方向的位置信息；

Θ_i＝θ_i-α_i、Ψ_i＝ψ_i+β_i分别为航迹角和方位角；

b_iu、b_iq分别为水下滑翔机在纵向、纵摇方向上的未知控制增益，b_irx、b_iry分别为水下滑翔机艏摇方向上的未知控制增益在坐标轴上的分量，其与水下滑翔机质量相关；

d_iu、d_iv、d_iw、d_iq、d_ir分别为水下滑翔机在纵向、横向、垂向、纵摇方向和艏摇方向上的外部环境扰动；

τ_iu、τ_iq和τ_ir分别为水下滑翔机在纵向、纵摇方向和艏摇方向上的控制输入；

f_iu(t,u_i,v_i,w_i,θ_i,ψ_i,q_i,r_i)、f_iv(t,u_i,v_i,w_i,θ_i,ψ_i,q_i,r_i)和f_iw(t,u_i,v_i,w_i,θ_i,ψ_i,q_i,r_i)、f_iq(t,u_i,v_i,w_i,θ_i,ψ_i,q_i,r_i)、f_ir(t,u_i,v_i,w_i,θ_i,ψ_i,q_i,r_i)分别代表在纵向、横向、垂向、纵摇方向和艏摇方向上与水动力阻尼效应有关的非线性函数。

m_iu、m_iv、m_iw、m_iq、m_ir分别表示水下滑翔机在纵向、横向、垂向、纵摇方向和艏摇方向上的惯性矩阵。

所述的通信网络的结构如下：

考虑多水下滑翔机协同潜水时，各水下滑翔机间的通讯结构由

表示。其中

为节点集合，每艘水下滑翔机对应一个节点。

为任意两个节点的无序对集合，表示第i艘水下滑翔机和第j艘水下滑翔机之间存在信息传递，称为边集，边(n_i,n_j)表示节点间的通讯关系。

图的邻接矩阵记作

并满足

一种水下滑翔机三维协同潜水控制系统，包括：

A、路径跟踪制导模块，路径跟踪制导模块用于获取水下滑翔机的位置、航向、速度以及给定路径信息，计算水下滑翔机的给定纵向速度、纵倾角速度、艏向角速度以及纵向跟踪误差。

路径跟踪制导模块的输入信号包括水下滑翔机的位置信号x_i、y_i、z_i，航向信号Θ_i、Ψ_i，速度信号U_i、q_i、r_i以及给定路径信号x_id、y_id、z_id；经计算，所述的路径跟踪制导模块的输出信号U_iU、q_iq、r_ir表示为：

所述的路径跟踪制导模块的输出信号x_ie的导数设计如下：

其中，U_iU、q_iq与r_ir分别为给定的纵向速度信号、纵倾角速度信号和艏向角速度信号；

x_ie、Θ_ie、Ψ_ie分别表示水下滑翔机的纵向跟踪误差、航迹角跟踪误差和方位角跟踪误差；

分别为总速度误差U_ie、纵荡角速度误差q_ie与艏摇角速度误差的估计值；Θ_ie＝θ_i-α_i-Θ_iθ，Θ_iθ为期望航迹角；

为路径参数更新速度；

分别为经矩阵旋转后的航迹角和方位角，其中Δ_i为前视距离，z_ie为水下滑翔机在垂向方向的位置误差；v_s是路径参数χ_i的路径更新速度，ω_i为协同路径参数信息；

Ψ_iψ为期望方位角；k_i1，k_i2，k_i3，Δ_ix，Δ_iθ和Δ_iψ为正常数；

y_ie为水下滑翔机在横向方向的位置误差；θ_id和ψ_id为预定义路径的切线角。

B、协同模块，协同模块用于获取水下滑翔机的纵向跟踪误差和邻居水下滑翔机的路径参数信息，计算水下滑翔机的协同路径参数信息。

协同模块的输入信号为第i艘水下滑翔机的邻居船舶的路径参数信息χ_j和路径跟踪制导模块输出的第i艘水下滑翔机的纵向位置误差x_ie，所述的协同模块的输出信号ω_i设计为：

其中，k_i3为控制增益，e_i＝h_i(χ_i,χ_j,δ_ij)为一般形式的协同误差，其是关于本船路径参数χ_i、邻居水下滑翔机路径参数χ_j以及路径参数相对距离δ_ij的函数；通过选取不同形式的e_i，实现不同的欠驱动水下滑翔机协同下潜控制。

C、预估器模块，预估器模块用于获取给定纵向速度、纵倾角速度、艏向角速度，计算总速度误差、纵荡角速度误差与艏摇角速度误差的估计值。

预估器模块的输入信号为路径跟踪控制模块的输出信号U_iU、q_iq、r_ir，所述的预估器模块设计为：

其中β_iU(ζ_iU)、β_iq(ζ_iq)和β_ir(ζ_ir)是已知连续隶属函数，其满足

这里

都是正实数；

是正常数；

Ξ_iU，Ξ_iq，Ξ_ir为有界权值，用来实现模糊系统对系统动态的估计；

和

分别是Ξ_iU，Ξ_iq和Ξ_ir的估计值。

本发明用模糊系统对其进行逼近，给定

和

则存在有界权值Ξ_iU、Ξ_iq、Ξ_ir，定义：

其中

为水下滑翔机在总速度、纵荡方向、艏摇方向上的总扰动；ε_iU,ε_iq,ε_ir为水下滑翔机在总速度、纵荡方向、艏摇方向上的估计误差，且满足

和

都是正实数。

输出信号

和

的设计如下：

其中

σ_iU,σ_iq,

为正常数；Proj[]表示投影算子；由投影算子可知，

其中

是正常数；

是用来估计

的低通滤波器权值。

D、动力学模块，动力学控制模块用于获取给定纵向速度、纵倾角速度、艏向角速度，总速度误差、纵荡角速度误差与艏摇角速度误差的估计值，计算水下滑翔机在纵向、纵摇以及艏摇方向运动的控制参数。

动力学模块的输入信号为预估器的输出信号以及路径跟踪制导模块的三个输出信号，动力学模块的输出信号τ_iU、τ_iq和τ_ir设计为

其中，

Δ_iU,Δ_iq,

是正常数；

sign(·)为符号函数，当b_iu≥0时，sign(b_iu)＝1，当b_iu＜0时，sign(b_iu)＝-1；

当b_iq≥0时，sign(b_iq)＝1，当b_iq＜0时，sign(b_iq)＝-1；

当b_ir≥0时，sign(b_ir)＝1，当b_ir＜0时，sign(b_ir)＝-1。ε_iu,ε_iq,

都是很小的正常数，分别用来处理当b_iu＝0，b_iq＝0或b_ir＝0时的奇异性。

下面结合附图以一个具体的三艘水下滑翔机三维协同控制为例对本发明进行进一步说明。图1所示为本发明的结构示意图，水下滑翔机协同控制中的每艘水下滑翔机都满足式(1)、(2)所示的运动学和动力学模型，三艘水下滑翔机三维协同潜水的通讯网络结构如图2所示，1号水下滑翔机将路径参数信息发送给2号水下滑翔机；2号水下滑翔机与3号水下滑翔机进行信息的来回传递；最后3号水下滑翔机将路径参数信息发送给1号水下滑翔机。

此例的控制目标是三艘水下滑翔机在三维空间内能够跟踪给定的参数化路径(x_id(χ_i),z_id(χ_i))，并保证协同潜水的任务。

本发明设计的模型的具体参数如下：

三个水下滑翔机的初始状态为：

(x₁,y₁,z₁,Θ₁,Ψ₁)＝(-5,5,0,0,0)

(x₂,y₂,z₂,Θ₂,Ψ₂)＝(-8,-25,2,0,0)

(x₃,y₃,z₃,Θ₃,Ψ₃)＝(-10,25,5,0,0)

水下滑翔机分别跟踪的三条参数化路径设计如下：

控制参数选择如下：

k_i1＝0.5,k_i2＝0.5,k_i3＝0.5,k_i4＝3,k_i5＝3,k_i6＝2,

σ_iU＝0.00001,σ_iq＝0.0001,σ_ir＝0.0001,κ_iU＝20000,κ_iq＝10000,κ_ir＝10000,Δ_ix＝3,Δ_iθ＝3,Δ_iψ＝3,Δ_iU＝3,Δ_iq＝3,Δ_ir＝3,Δ_i＝3,λ_i＝1,μ_i＝0.05,

和v_s＝0.1，所用的控制系统满足式(3)-(9)所描述的控制系统结构，仿真结果如图3-7所示。图3是三艘水下滑翔机在三维空间内基于模糊预估器模块的协同运动轨迹，图中的实线为给定的参数化路径，虚线为三艘水下滑翔机的实际运动轨迹，从图中可以看出三艘水下滑翔机即使受到模型的不确定和海洋扰动的影响可以跟踪预先设计的三维路径。图4为三艘水下滑翔机的路径参数变化，从图中可以看出经过一段暂态调整过程，三艘水下滑翔机的路径参数趋于一致，达到了路径参数同步的控制效果。图5显示的是三艘水下滑翔机的跟踪误差，可以看出经过一段时间后跟踪误差均可收敛到一个小的零值附近。图6给出了纵向、纵荡方向、艏摇方向的不确定性估计性能，仿真结果表明，所设计的预估器模块可以有效地估计水下滑翔机的模型不确定性和外部环境扰动。图7为控制输入，其中m_1b、x_1p1、y_1p2分别对应的是第一艘水下滑翔机的τ_u、τ_q、τ_r，可以看到水下滑翔机的三个控制输入经过一段时间后都可达到稳定值。

整体有的有益效果：

第一，与现有针对单水下滑翔机的控制方法相比，本发明通过采用路径跟踪以及路径参数协同方法，使得所提出的协同下潜控制方法适用于多水下滑翔机，拓展了水下滑翔机控制的应用范围，使得该协同控制系统更有利于实际工作。

第二，与现有水下滑翔机协同路径跟踪问题的研究成果中大多为二维平面或者纵平面的协同路径跟踪相比，本发明为水下滑翔机集群构造了三维协同路径跟踪控制系统，同时考虑了内部模型不确定性、外部环境扰动和执行机构输入受限。

第三，与现有路径跟踪控制方法大多要求航行器的状态信息事精确可测的相比，本发明通过引入模糊预估器滤除系统不确定性和外扰中存在的高频分量。

第四，综上所述，本发明可应用于水下滑翔机并能实现多水下滑翔机协同下潜队形，而且控制系统采用模块化设计，显著降低了控制系统结构的复杂性、减少了计算负荷、易于工程实现。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (4)

1.一种水下滑翔机三维协同路径跟踪控制系统，其特征在于，包括路径跟踪制导模块、协同模块、预估器模块、动力学控制模块、通信网络、水下滑翔机，

路径跟踪制导模块用于获取水下滑翔机的位置、航向、速度以及给定路径信息，计算水下滑翔机的给定纵向速度、纵倾角速度、艏向角速度以及纵向跟踪误差；

协同模块用于获取水下滑翔机的纵向跟踪误差和邻居水下滑翔机的路径参数信息，计算水下滑翔机的协同路径参数信息；

预估器模块用于获取给定纵向速度、纵倾角速度、艏向角速度，计算总速度误差、纵荡角速度误差与艏摇角速度误差的估计值，所述计算总速度误差、纵荡角速度误差与艏摇角速度误差的估计值是通过公式(1)进行计算，

其中β_iU(ζ_iU)、β_iq(ζ_iq)和β_ir(ζ_ir)是已知连续隶属函数，其满足

这里

都是正实数；

是正常数；

Ξ_iU，Ξ_iq，Ξ_ir为有界权值，用于模糊系统对系统动态的估计；

和

分别是Ξ_iU，Ξ_iq和Ξ_ir的估计值，

所述

和

的计算过程为，

定义：

其中

为水下滑翔机在总速度、纵荡方向、艏摇方向上的总扰动；ε_iU,ε_iq,ε_ir为水下滑翔机在总速度、纵荡方向、艏摇方向上的估计误差，且满足

和

都是正实数，

输出信号

和

的设计如下：

其中

为正常数；Proj[]表示投影算子；由投影算子可知，

其中

是正常数；

是用来估计

的低通滤波器权值，

动力学控制模块用于获取给定纵向速度、纵倾角速度、艏向角速度，总速度误差、纵荡角速度误差与艏摇角速度误差的估计值，计算水下滑翔机在纵向、纵摇以及艏摇方向运动的控制参数；

通信网络用于建立水下滑翔机之间进行信息传递的通信结构；

路径跟踪制导模块包括三个输入端，其中两个输入端分别与水下滑翔机的输出端、协同模块的一个输出端相连，第三个输入端为给定的参考路径信号；路径跟踪制导模块的两个输出端分别与协同模块的输入端、动力学控制模块的一个输入端相连；

协同模块的两个输入端分别与路径跟踪制导模块的一个输出端和通信网络相连；

预估器模块的两个输入端分别与路径跟踪制导模块的一个输出端、水下滑翔机的输出端相连；预估器模块的输出端与动力学控制模块的一个输入端相连；

动力学控制模块的输出端与水下滑翔机的输入端相连。

2.根据权利要求1所述的一种水下滑翔机三维协同路径跟踪控制系统，其特征在于，所述计算水下滑翔机的给定纵向速度、纵倾角速度、艏向角速度以及纵向跟踪误差是通过公式(4)、(5)进行计算，

其中，U_iU、q_iq与r_ir分别为给定的纵向速度信号、纵倾角速度信号和艏向角速度信号；x_ie、Θ_ie、Ψ_ie分别表示水下滑翔机的纵向跟踪误差、航迹角跟踪误差和方位角跟踪误差；

分别为总速度误差U_ie、纵荡角速度误差q_ie与艏摇角速度误差的估计值；Θ_ie＝θ_i-α_i-Θ_iθ，Θ_iθ为期望航迹角；

为路径参数更新速度；

分别为经矩阵旋转后的航迹角和方位角，其中Δ_i为前视距离，z_ie为水下滑翔机在垂向方向的位置误差；v_s是路径参数χ_i的路径更新速度，ω_i为协同路径参数信息；

Ψ_iψ为期望方位角；k_i1，k_i2，k_i3，Δ_ix，Δ_iθ和Δ_iψ为正常数；

y_ie为水下滑翔机在横向方向的位置误差；θ_id和ψ_id为预定义路径的切线角。

3.根据权利要求1所述的一种水下滑翔机三维协同路径跟踪控制系统，其特征在于，所述计算水下滑翔机的协同路径参数信息是通过公式(6)计算，

其中，k_i3为控制增益，e_i＝h_i(χ_i,χ_j,δ_ij)为一般形式的协同误差，其是关于本船路径参数χ_i、邻居水下滑翔机路径参数χ_j以及路径参数相对距离δ_ij的函数；v_s是路径参数χ_i的路径更新速度，ω_i为协同路径参数，

是路径参数更新速度，x_ie是水下滑翔机的纵向跟踪误差。

4.根据权利要求1所述的一种水下滑翔机三维协同路径跟踪控制系统，其特征在于，所述计算水下滑翔机在纵向、纵摇以及艏摇方向运动的控制参数是通过公式(7)进行计算，

其中，

是正常数；

sign(·)为符号函数，当b_iu≥0时，sign(b_iu)＝1，当b_iu＜0时，sign(b_iu)＝-1；

当b_iq≥0时，sign(b_iq)＝1，当b_iq＜0时，sign(b_iq)＝-1；

当b_ir≥0时，sign(b_ir)＝-1，当b_ir＜0时，sign(b_ir)＝-1；

都是正常数，分别用来处理当b_iu＝0，b_iq＝0或b_ir＝0时的奇异性，τ_iU、τ_iq和τ_ir分别为水下滑翔机在纵向、纵摇方向和艏摇方向上的控制输入参数。

Images