CN109739249B - 一种速度状态缺失条件下的多uuv编队协调控制方法 - Google Patents

Abstract

本发明公开了一种速度状态缺失条件下的多UUV编队协调控制方法，包括：进行编队初始化；进行反馈线性化转换，得到反馈线性化坐标系下的线性运动状态向量；在反馈线性化坐标系下设计编队协调控制器；基于反馈线性化逆运算，将控制输入转换为大地坐标系下的协调控制输入；对协调控制输入进行解算，得到UUV执行机构的控制指令；确定当前时刻的完整运动状态信息；根据编队协调关系及通信拓扑关系进行数据传输和共享；判断UUV编队是否完成编队任务。本发明有效提高了在速度状态缺失和外部扰动条件下UUV编队的自主性和稳定性，优化了编队的控制算法，扩大了UUV编队控制方法的适用范围，提高了编队在任务过程中的抗扰动性。

Description

一种速度状态缺失条件下的多UUV编队协调控制方法

技术领域

本发明属于多UUV编队协调控制技术领域，尤其涉及一种速度状态缺失条件下的多UUV编队协调控制方法。

背景技术

在复杂海洋环境中，单个UUV限于自身能源和传感器的限制很难完成复杂任务，而UUV编队依靠更大的平台负载能力，更好的执行效率，更好的冗余性等对海洋勘测任务起到越来越重要的作用。而多UUV编队协同执行水下任务的重要条件是编队各UUV成员可以通过自身及编队其他成员的运动状态信息自主实现编队的形成以及任务过程中的固定队形保持。所以稳定的协调控制方法是编队协同的基础。但由于海洋的扰动，传感器故障，通信带宽受限等多种因素的干扰可能存在速度状态缺失的可能性。如何在不良的海洋状况下实现多UUV编队的协调稳定控制是提高和扩大UUV编队综合能力的重要指标。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种速度状态缺失条件下的多UUV编队协调控制方法，有效提高了在速度状态缺失和外部扰动条件下UUV编队的自主性和稳定性，优化了编队的控制算法，扩大了UUV编队控制方法的适用范围，提高了编队在任务过程中的抗扰动性。

为了解决上述技术问题，本发明公开了一种速度状态缺失条件下的多UUV编队协调控制方法，包括：

进行编队初始化，完成UUV编队各成员的随机布防和预设参数设置；

对大地坐标系{E}下的位姿状态信息η_i(t)与ESO观测器获取的速度状态观测信息

进行反馈线性化转换，得到反馈线性化坐标系{F}下的线性运动状态向量x_i和v_i；

在反馈线性化坐标系{F}下设计编队协调控制器；其中，所述编队协调控制器的控制输入u_i(t)包括：编队协调控制函数、速度观测函数和速度补偿函数；

基于反馈线性化逆运算，将控制输入u_i(t)转换为UUV运动控制所需要的大地坐标系{E}下的协调控制输入

对协调控制输入

进行解算，得到UUV执行机构的控制指令；其中，所述UUV执行机构的控制指令，包括：水平舵舵角控制指令、垂直舵舵角控制指令和推进器转速控制指令；

通过UUV自身携带的导航设备及姿态传感器，获取当前时刻自身的位姿状态信息η_i，并通过ESO观测器获取在环境扰动作用下的UUV编队当前时刻的速度状态观测值

将当前时刻的位姿状态信息η_i与当前时刻的速度状态观测值

作为当前时刻完整运动状态信息；

根据编队协调关系及通信拓扑关系，对当前时刻完整运动状态信息进行数据传输和共享，以使各UUV编队各成员确认自己的编队位置；

根据编队位置确认结果，判断UUV编队是否完成编队任务；其中，若确认完成编队任务，则UUV任务结束，执行编队返航指令。

在上述速度状态缺失条件下的多UUV编队协调控制方法中，反馈线性化转换，包括：

针对任一第i个UUV，选择转换坐标系：

x_i＝[h₁(η_i),h₂(η_i),h₃(η_i),h₄(η_i),h₅(η_i)]^T＝η_i

v_i＝[L_fh₁(η_i),L_fh₂(η_i),L_fh₃(η_i),L_fh₄(η_i),L_fh₅(η_i)]^T

其中，η_i表示第i个UUV的位置姿态信息向量，η_i＝[x,y,z,θ,ψ]，x,y,z,θ,ψ分别表示大地坐标系{E}下x,y,z轴的位置坐标、俯仰角和艏向角；

分别表示通过ESO观测器获取的速度状态观测值；

L_fh_j(η_i)表示一阶Lie代数：

在上述速度状态缺失条件下的多UUV编队协调控制方法中，控制输入u_i(t)、编队协调控制函数g(x_i)、速度观测函数

和速度补偿函数

的表示如下：

其中，k₁,k₂,k₃表示协调控制器的控制增益；a_ij(t)表示第i个UUV与第j个UUV之间的通信拓扑关系；c_li(t)表示第i个UUV与领航者之间的通信拓扑关系；

表示ESO观测器获取第i个和第j个UUV的速度状态观测值的加速度；

表示外部海洋环境扰动。

在上述速度状态缺失条件下的多UUV编队协调控制方法中，大地坐标系{E}下的协调控制输入

的表示如下：

其中，Γ^-1(η_i)为Γ(η_i)的逆矩阵，B(η_i)为一个二阶Lie代数的列向量；

其中：

m^*g^*＝m₃₃g′₃₄+m₃₄g′₄₄，m′g′＝m₂₂g′₂₄+m₂₅g′₅₅

其中，c·和s·分别代表cos·和sin·函数；

其中，m_ij,{i＝1,2,...,5；j＝1,2,...,5}是UUV水动力模型中的惯性矩阵的逆矩阵-M^-1中对应的单元，通过对模型惯性矩阵及附加惯性质量矩阵之和求逆矩阵获得；f_i(x),i＝{1,2,...,10}是由UUV的数学模型通过转化为标准非线性模型而生成的列向量，J(η_i),

表示UUV模型中的运动学模型转换矩阵、克罗拉克向心力矩阵和阻尼矩阵。

在上述速度状态缺失条件下的多UUV编队协调控制方法中，还包括：

利用第i个UUV的位置观测误差向量

设计针对该UUV的ESO观测器为：

其中，β_x,β_v和β_χ为观测器的增益恒量，g(·)为连续时间函数，g(s)＝s。

本发明具有以下优点：

(1)本发明通过采用ESO观测器以及反馈线性化方法，在基于一致性理论的基础上提出针对非线性耦合模型的UUV编队协调控制方法，有效解决了真实海洋环境下多UUV编队在速度传感器失效等速度状态缺失条件下的多UUV编队协调控制问题。

(2)本发明提出的扩展状态观测器(ESO)对非线性模型具有很好的适应性，无论系统不确定性以及外部扰动是否是连续和已知的，只需要通过UUV的位姿状态信息及控制输入增加一个纬度的状态变量，就可以观测出系统的状态变量及外部扰动的观测值，提高了编队协调控制方法对非线性模型的适用性，增加了UUV编队在外部未知扰动作用下非线性耦合UUV系统的抗扰动性。

(3)本发明采用了反馈线性化及反馈线性化逆运算方法，通过选择适当的坐标系转化，实现非线性耦合的UUV模型在反馈线性化转化坐标系下与编队线性协调控制方法有效结合，提高了编队之间协调控制的优化效果和适用范围，增加了编队成员的自主性，同时又保留了编队原有模型的不破坏性。

(4)本发明只进行成员之间的位姿状态信息的交互，在带宽受限的水声通信系统中，可以有效减少通信数据包的单包数据量，充分的解决了海洋通信带宽受限的问题，大大减少了通信失效和丢包，减轻了水声通信的负担。

附图说明

图1是本发明实施例中一种速度状态缺失条件下的多UUV编队协调控制方法的步骤流程图；

图2是本发明实施例中一种基于ESO观测器的多UUV编队协调控制系统结构图；

图3是本发明实施例中一种UUV的系统示意图；

图4是本发明实施例中一种多UUV编队队形的结构示意图；

图5是本发明实施例中一种多UUV编队各成员位姿状态变化示意图；

图6是本发明实施例中一种多UUV编队各成员速度观测值变化示意图；

图7是本发明实施例中一种多UUV编队各成员三维运动轨迹示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。

参照图1，示出了本发明实施例中一种速度状态缺失条件下的多UUV编队协调控制方法的步骤流程图。在本实施例中，所述速度状态缺失条件下的多UUV编队协调控制方法，包括：

步骤101，进行编队初始化，完成UUV编队各成员的随机布防和预设参数设置。

步骤102，对大地坐标系{E}下的位姿状态信息η_i(t)与ESO观测器获取的速度状态观测信息

进行反馈线性化转换，得到反馈线性化坐标系{F}下的线性运动状态向量x_i和v_i。

在本实施例中，反馈线性化转换具体可以包括：

针对任一第i个UUV，选择转换坐标系：

x_i＝[h₁(η_i),h₂(η_i),h₃(η_i),h₄(η_i),h₅(η_i)]^T＝η_i

v_i＝[L_fh₁(η_i),L_fh₂(η_i),L_fh₃(η_i),L_fh₄(η_i),L_fh₅(η_i)]^T

其中，η_i表示第i个UUV的位置姿态信息向量，η_i＝[x,y,z,θ,ψ]，x,y,z,θ,ψ分别表示大地坐标系{E}下x,y,z轴的位置坐标、俯仰角和艏向角；

分别表示通过ESO观测器获取的速度状态观测值。

L_fh_j(η_i)表示一阶Lie代数：

步骤103，在反馈线性化坐标系{F}下设计编队协调控制器。

在本实施例中，所述编队协调控制器的控制输入u_i(t)包括：编队协调控制函数、速度观测函数和速度补偿函数。

优选的，控制输入u_i(t)、编队协调控制函数g(x_i)、速度观测函数

和速度补偿函数

的表示可以如下：

其中，k₁,k₂,k₃表示协调控制器的控制增益；a_ij(t)表示第i个UUV与第j个UUV之间的通信拓扑关系；c_li(t)表示第i个UUV与领航者之间的通信拓扑关系；

表示ESO观测器获取第i个和第j个UUV的速度状态观测值的加速度；

表示外部海洋环境扰动。

步骤104，基于反馈线性化逆运算，将控制输入u_i(t)转换为UUV运动控制所需要的大地坐标系{E}下的协调控制输入

在本实施例中，大地坐标系{E}下的协调控制输入

的可以表示如下：

其中，Γ^-1(η_i)为Γ(η_i)的逆矩阵，B(η_i)为一个二阶Lie代数的列向量。

其中：

m^*g^*＝m₃₃g′₃₄+m₃₄g′₄₄，m′g′＝m₂₂g′₂₄+m₂₅g′₅₅

其中，c·和s·分别代表cos·和sin·函数。

其中，m_ij,{i＝1,2,...,5；j＝1,2,...,5}是UUV水动力模型中的惯性矩阵的逆矩阵-M^-1中对应的单元，通过对模型惯性矩阵及附加惯性质量矩阵之和求逆矩阵获得；f_i(x),i＝{1,2,...,10}是由UUV的数学模型通过转化为标准非线性模型而生成的列向量，J(η_i),

表示UUV模型中的运动学模型转换矩阵、克罗拉克向心力矩阵和阻尼矩阵。

步骤105，对协调控制输入

进行解算，得到UUV执行机构的控制指令。

在本实施例中，所述UUV执行机构的控制指令包括但不仅限于：水平舵舵角控制指令、垂直舵舵角控制指令和推进器转速控制指令。

步骤106，通过UUV自身携带的导航设备及姿态传感器，获取当前时刻自身的位姿状态信息η_i，并通过ESO观测器获取在环境扰动作用下的UUV编队当前时刻的速度状态观测值

将当前时刻的位姿状态信息η_i与当前时刻的速度状态观测值

作为当前时刻完整运动状态信息。

步骤107，根据编队协调关系及通信拓扑关系，对当前时刻完整运动状态信息进行数据传输和共享，以使各UUV编队各成员确认自己的编队位置。

步骤108，根据编队位置确认结果，判断UUV编队是否完成编队任务。

在本实施例中，若确认完成编队任务，则UUV任务结束，执行编队返航指令。若确认未完成编队任务，可返回步骤102重复执行，直至确认完成编队任务。

在本发明的一优选实施例在，该速度状态缺失条件下的多UUV编队协调控制方法，还可以包括：

利用第i个UUV的位置观测误差向量

设计针对该UUV的ESO观测器为：

其中，β_x,β_v和β_χ为观测器的增益恒量，g(·)为连续时间函数，g(s)＝s。

在上述实施例的基础上，下面结合图2～7对本发明所述的速度状态缺失条件下的多UUV编队协调控制方法进行详细说明。

在实际应用中，所述速度状态缺失条件下的多UUV编队协调控制方法的具体执行步骤可以如下：

(1)UUV编队各成员初始化

将多UUV编队随机布放并且对UUV编队中全部成员进行初始化设置，包括：UUV当前随机布放位置、姿态、初始速度、编队预设队形结构、预设编队任务文本预置参数设置等。

(2)反馈线性化转换

将大地坐标系{E}下的位姿状态信息η_i(t)与ESO观测器获取的速度状态观测信息

进行坐标转化，转换为反馈线性化坐标系{F}下的线性运动状态向量x_i和v_i：

x_i(t)＝[h₁(η_i(t)),h₂(η_i(t)),h₃(η_i(t)),h₄(η_i(t)),h₅(η_i(t))]^T＝η_i

v_i(t)＝[L_fh₁(η_i(t)),L_fh₂(η_i(t)),L_fh₃(η_i(t)),L_fh₄(η_i(t)),L_fh₅(η_i(t))]^T

由于在实际UUV系统中，自身浮心和重心的设置关系，UUV横摇转动的控制可以通过自身的自稳定实现，为了便于实现控制，减少耦合关系，本发明在此对其不做相应的控制，故而位姿状态和速度状态都只有5个变量。

(3)在反馈线性化坐标系{F}下设计编队协调控制器

如前所述，控制输入u_i(t)、编队协调控制函数g(x_i)、速度观测函数

和速度补偿函数

的表示如下：

(4)坐标系转换

基于反馈线性化逆运算，将控制输入u_i(t)转换为UUV运动控制所需要的大地坐标系{E}下的协调控制输入

(5)控制指令解算

利用UUV运动控制计算机的运动控制系统将协调控制输入

解算为UUV执行机构的控制指令。

(6)利用UUV自身携带的传感器系统及导航系统，获取自身的位姿状态信息，根据位姿状态系信息，利用ESO观测器获取在环境扰动作用下的UUV编队速度状态观测值

并将位姿状态信息η_i与该ESO观测器所获得速度状态观测值

作为协调控制系统下一时刻的输入量。

(7)UUV使命任务计算机根据编队协调关系及通信拓扑关系，通过编队成员之间的水声通信系统实现预设通信拓扑条件下各成员之间的位姿状态信息η_i与ESO观测器所获得的速度状态观测值

传输。

(8)根据UUV编队各成员当前的运动状态以及预设的编队任务文本判断编队的任务完成情况，如果完成预设编队任务，则执行步骤(9)，如果没有完成预设编队任务，则执行步骤(2)。

(9)UUV任务结束，执行编队返航指令。

例如：

由1个领航者和4个跟随者组成领航者跟随队形结构的多UUV编队，在大地坐标系{E}下，定常海流为u_cx＝0.2m/s,u_cy＝0.2m/s，UUV的初始位置为随机布放，初始速度状态为0，预设队形结构(通信拓扑关系)如图4所示，预设的编队领航者的航行路径为：

在固定队形结构中，各跟随者UUV的编队参考点相对于领航者的空间距离是相对固定的，那么如果跟随者可以稳定收敛于领航者，那么跟随者UUV也一定可以稳定收敛于各自需要跟随的编队参考点。

仿真结果分析：图5为多UUV编队各成员位姿状态变化曲线，图6为多UUV编队各成员的速度状态观测值的变化曲线，图7为多UUV编队的3维空间运动轨迹曲线。由仿真结果可以看出，多UUV编队在领航者跟随的队形结构下，利用ESO观测器对UUV在海流扰动作用下的运动状态进行观测，实现了UUV编队的队形形成以及队形保持，体现出在本发明提出的方法作用下UUV编队整体具有很好的收敛性、稳定性和抗扰动性。

本说明中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。

以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (3)

1.一种速度状态缺失条件下的多UUV编队协调控制方法，其特征在于，包括：

进行编队初始化，完成UUV编队各成员的随机布防和预设参数设置；

对大地坐标系{E}下的位姿状态信息η_i(t)与ESO观测器获取的速度状态观测信息

进行反馈线性化转换，得到反馈线性化坐标系{F}下的线性运动状态向量x_i和v_i；

在反馈线性化坐标系{F}下设计编队协调控制器；其中，所述编队协调控制器的控制输入u_i(t)包括：编队协调控制函数、速度观测函数和速度补偿函数；

基于反馈线性化逆运算，将控制输入u_i(t)转换为UUV运动控制所需要的大地坐标系{E}下的协调控制输入

对协调控制输入

进行解算，得到UUV执行机构的控制指令；其中，所述UUV执行机构的控制指令，包括：水平舵舵角控制指令、垂直舵舵角控制指令和推进器转速控制指令；

通过UUV自身携带的导航设备及姿态传感器，获取当前时刻自身的位姿状态信息η_i，并通过ESO观测器获取在环境扰动作用下的UUV编队当前时刻的速度状态观测值

将当前时刻的位姿状态信息η_i与当前时刻的速度状态观测值

作为当前时刻完整运动状态信息；

根据编队协调关系及通信拓扑关系，对当前时刻完整运动状态信息进行数据传输和共享，以使各UUV编队各成员确认自己的编队位置；

根据编队位置确认结果，判断UUV编队是否完成编队任务；其中，若确认完成编队任务，则UUV任务结束，执行编队返航指令；

其中：

控制输入u_i(t)、编队协调控制函数g(x_i)、速度观测函数

和速度补偿函数

的表示如下：

其中，k₁,k₂,k₃表示协调控制器的控制增益；a_ij(t)表示第i个UUV与第j个UUV之间的通信拓扑关系；c_li(t)表示第i个UUV与领航者之间的通信拓扑关系；

表示ESO观测器获取第i个和第j个UUV的速度状态观测值的加速度；

表示外部海洋环境扰动；

大地坐标系{E}下的协调控制输入

的表示如下：

其中，Γ^-1(η_i)为Γ(η_i)的逆矩阵，B(η_i)为一个二阶Lie代数的列向量；

其中：

m^*g^*＝m₃₃g′₃₄+m₃₄g′₄₄，m′g′＝m₂₂g′₂₄+m₂₅g′₅₅

其中，c·和s·分别代表cos·和sin·函数；

其中，m_ij,{i＝1,2,...,5；j＝1,2,...,5}是UUV水动力模型中的惯性矩阵的逆矩阵-M^-1中对应的单元，通过对模型惯性矩阵及附加惯性质量矩阵之和求逆矩阵获得；f_i(x),i＝{1,2,...,10}是由UUV的数学模型通过转化为标准非线性模型而生成的列向量，

表示UUV模型中的运动学模型转换矩阵、克罗拉克向心力矩阵和阻尼矩阵，θ和ψ分别表示大地坐标系{E}下的俯仰角和艏向角。

2.根据权利要求1所述的速度状态缺失条件下的多UUV编队协调控制方法，其特征在于，反馈线性化转换，包括：

针对任一第i个UUV，选择转换坐标系：

x_i＝η_i＝[x,y,z,θ,ψ]

v_i＝[L_fh₁(η_i),L_fh₂(η_i),L_fh₃(η_i),L_fh₄(η_i),L_fh₅(η_i)]^T

其中，η_i表示第i个UUV的位置姿态信息向量，x,y,z分别表示大地坐标系{E}下x,y,z轴的位置坐标；

分别表示通过ESO观测器获取的速度状态观测值；

L_fh_j(η_i)表示一阶Lie代数：

3.根据权利要求1所述的速度状态缺失条件下的多UUV编队协调控制方法，其特征在于，还包括：

利用第i个UUV的位置观测误差向量

设计针对该UUV的ESO观测器为：

其中，β_x,β_v和β_χ为观测器的增益恒量，g(·)为连续时间函数，g(s)＝s。

Images