CN114942646B - 异构无人系统三维空间编队控制方法 - Google Patents

Abstract

本发明属于海洋工程技术领域，公开了一种异构无人系统三维空间编队控制方法，其中异构无人系统由若干水面无人艇和若干自治水下航行器组成，异构无人系统三维空间编队控制方法包括以下步骤：建立异构无人系统三维编队通信拓扑模型；执行航向一致性控制算法；执行速度一致性控制算法；如果无人系统节点为自治水下航行器，则执行深度一致性控制算法；控制作为无人系统节点的水面无人艇按照输出航向角和输出航行速度运行；控制作为无人系统节点的自治水下航行器按照输出航向角、输出航行速度和输出航行深度运行，本发明可以有效实现编队形成、编队保持和编队变换，使异构无人系统具备良好的自愈能力和扩展能力。

Description

异构无人系统三维空间编队控制方法

技术领域

本发明属于海洋工程技术领域，尤其涉及一种异构无人系统三维空间编队控制方法。

背景技术

面对复杂多变的海洋环境，由若干水面无人艇(Unmanned Surface Vehicle，USV)和若干自治水下航行器(Autonomous Underwater Vehicle，AUV)组成的海上无人系统是执行“3D”任务(Dirty-Dull-Dangerous，即肮脏-单调-危险)时的最优选择，具有无可比拟的优势。在民用方面，作为组建海底空间站和观测网的重要工具，海上无人系统在水下地形地貌测绘、特定目标的追踪与调查、海洋数据获取等领域大有可为。传统的海上无人系统通常由单一的水面无人艇或者单一的自治水下航行器组成，相对承载能力低、覆盖范围小且信息处理能力弱。

由单一的水面无人艇或者单一的自治水下航行器组成的海上无人系统所采用的协同编队控制方法主要包括领航跟随法、基于行为法和虚拟结构法等。这些经典控制方法在三维空间编队方面均存在一定局限性，多针对单独由水面无人艇组成的无人系统或者单独由自治水下航行器组成的无人系统，尤其是在弱通信条件下，编队的自愈性和扩展性不理想。

本背景技术所公开的上述信息仅仅用于增加对本申请背景技术的理解，因此，其可能包括不构成本领域普通技术人员已知的现有技术。

发明内容

针对现有技术中为单独由水面无人艇组成的无人系统、或者单独由自治水下航行器组成的无人系统设计的协同编队控制方法，在面对三维编队的弱通信条件时，编队的自愈性和扩展性不理想的问题，设计并提供一种异构无人系统三维空间编队控制方法。

为实现上述发明目的，本发明采用下述技术方案予以实现：

一种异构无人系统三维空间编队控制方法，所述异构无人系统由若干水面无人艇和若干自治水下航行器组成；所述异构无人系统三维空间编队控制方法包括以下步骤：

步骤S11：建立异构无人系统三维编队通信拓扑模型；

在步骤S11中，异构无人系统三维编队通信拓扑模型表示为：

G＝{V，E}

其中，V＝{1，2，…，n}，V表示参与三维编队的n个无人系统节点的集合，n表示参与三维编队的无人系统节点的个数，无人系统节点是水面无人艇或自治水下航行器；E＝{(i，j)∈V×V}表示参与三维编队的n个无人系统节点之间有向信息路径组成的集合，其中i表示参与三维编队的第i个无人系统节点，i∈[1，n]；j表示参与三维编队的第j个无人系统节点，j∈[1，n]；

建立异构无人系统三维编队通信拓扑模型的邻接矩阵A_ij；邻接矩阵A_ij为n×n的n阶方阵，邻接矩阵A_ij中的元素a_ij满足：当无人系统节点i收到无人系统节点j发送的信息时，有a_ij>0；否则a_ij＝0；

建立无人系统节点i的邻居集N_i，无人系统节点i的邻居集N_i记为：

N_i＝{j∈V|a_ij>0}＝{j∈V|(j，i)∈E}；

建立异构无人系统三维编队通信拓扑模型的拉普拉斯矩阵L_ij，拉普拉斯矩阵L_ij为：

建立修正Metropolis加权系数矩阵：

其中，N_j为三维空间编队中的无人系统节点j的邻居节点集合，m为整数且对应无人系统节点i的邻居集N_i的一个无人系统节点；

步骤S12：执行航向一致性控制算法；

在步骤S12中，航向一致性控制算法由下式表示：

其中，

表示无人系统节点i的输出航向角；ψ_i为无人系统节点i的实时航向角；n_i表示无人系统节点i的邻居集N_i中的邻居节点的个数；

表示无人系统节点i的输入航向角；b_i表示第一航向控制增益，为常数；

表示无人系统节点i的目标航向角；ψ_j表示无人系统节点i的邻居集N_i中的邻居节点j的实时航向角；W_ij为修正Metropolis加权系数矩阵；

步骤S13：执行速度一致性控制算法；

在步骤S13中，速度一致性控制算法由下式表示：

其中，

表示无人系统节点i的输出航行速度；v_i表示无人系统节点i的实时航行速度；τ_v表示速度时间常数；

表示无人系统节点i的输入航行速度；c_i表示第一速度控制增益，为常数；

表示无人系统节点i的目标航行速度；W_ij为修正Metropolis加权系数矩阵；v_j无人系统节点i的邻居集N_i中的邻居节点j的实时航行速度；

步骤S14：如果无人系统节点为自治水下航行器，则执行深度一致性控制算法；

在步骤S14中，深度一致性控制算法由下式表示：

其中，

表示无人系统节点i的输出航行深度；z_i表示无人系统节点i的实时航行深度；τ_z表示深度时间常数；

表示深度导数的时间常数；

表示无人系统节点i的实时航行深度的导数；

表示无人系统节点i的输入航行深度；

为深度控制增益，为常数；W_ij为修正Metropolis加权系数矩阵；z_j表示无人系统节点i的邻居集N_i中的邻居节点j的实时深度；γ为权重系数，为常数；

表示无人系统节点i的邻居集N_i中的邻居节点j的实时深度的导数；

控制作为无人系统节点的水面无人艇按照输出航向角和输出航行速度运行；

控制作为无人系统节点的自治水下航行器按照输出航向角、输出航行速度和输出航行深度运行。

在可选的实施方式中，在建立异构无人系统三维编队通信拓扑模型后，可选择性同步执行基于分布式一致性理论的异构无人系统三维空间编队控制算法，基于分布式一致性理论的异构无人系统三维空间编队控制算法可以由下式表示：：

其中，

表示无人系统节点i的目标y轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标y轴坐标，

表示无人系统节点i和无人系统节点j之间沿y轴的目标间距；

表示无人系统节点i的目标x轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标x轴坐标，

表示无人系统节点i和无人系统节点j之间沿x轴的目标间距；

表示无人系统节点i的目标z轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标z轴坐标，

表示无人系统节点i和无人系统节点j之间沿z轴的目标间距；

表示第二航向控制增益，为常数；

表示第二速度控制增益，为常数。

为生成更合适的一致性协议，得到更快的一致性速度，确保三维编队可以尽快成型，在执行航向一致性控制算法、速度一致性控制算法和深度一致性控制算法前对所述修正Metropolis加权系数矩阵W_ij进行实时修正得到实时Metropolis加权系数矩阵W_ij′；

无人系统节点i第k+1次一致性迭代后修正的实时Metropolis加权系数矩阵W_ij′(k+1)满足下式：

W_ij′(k+1)＝W_ij′(k)+a_ie_i(k)(ξ_j-ξ_i)+a_je_j(k)(ξ_i-ξ_j)

其中W_ij′(k)为无人系统节点i第k次一致性迭代后修正的实时Metropolis加权系数矩阵，a_i和a_j为比例因子，e_i(k)为无人系统节点i第k次一致性迭代时实时状态相对其目标状态的误差，ξ_i、ξ_j为无人系统节点i和无人系统节点j实际一致性状态的近似。

与现有技术相比，本发明的优点和积极效果是：

本发明所提供的异构无人系统三维空间编队控制方法，首先基于水面无人艇和自治水下航行器的性能差异以及通信约束，建立基于图论的稳定航行通信拓扑模型；其次，通过比较实时节点状态与目标节点状态的差异，对一致性Metropolis加权系数矩阵进行实时修正，得到更快的一致性收敛速度；最后，在航向一阶一致性、速度一阶一致性和深度一阶一致性编队算法基础上，设计三维空间编队控制方法，可以有效实现编队形成、编队保持和编队变换，使异构无人系统具备良好的自愈能力和扩展能力。

结合附图阅读本发明的具体实施方式后，本发明的其他特点和优点将变得更加清楚。

附图说明

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

图1为本发明所提供的异构无人系统三维空间编队控制方法一种具体实施例的流程图；

图2为本发明所提供的异构无人系统三维空间编队控制方法一种具体通信拓扑模型的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下将结合附图和实施例，对本发明作进一步详细说明。

需要说明的是，在本发明的描述中，术语“上”、“下”、“左”、“右”、“竖”、“横”、“内”、“外”等指示的方向或位置关系的术语是基于附图所示的方向或位置关系，这仅仅是为了便于描述，而不是指示或暗示所述装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

针对现有技术中为单独由水面无人艇组成的无人系统、或者单独由自治水下航行器组成的无人系统设计的协同编队控制方法，在面对三维编队的弱通信条件时，编队的自愈性和扩展性不理想的问题，设计并提供一种异构无人系统三维空间编队控制方法。需要说明的是，本实施方式中所述的异构无人系统由若干水面无人艇和若干自治水下航行器组成，水面无人艇和自治水下航行器均为异构无人系统的无人系统成员。水面无人艇和自治水下航行器类型不同且具有不同的状态空间模型。本实施方式所提供的异构无人系统三维空间编队控制方法的控制目标为使不同类型和状态空间模型的无人系统成员沿着相同的方向运动，保持相同的距离和较为理想的队形。

从原理上看，本实施方式所提供的异构无人系统三维空间编队控制方法基于分布式一致性理论设计，是一种编队协同控制算法，实现异构无人系统三维空间的编队形成、编队保持和编队变换。由本实施方式所提供的异构无人系统三维空间编队控制方法形成的编队具备良好的自愈能力和扩展能力。当有无人系统成员脱离编队或者加入编队时，可以自动形成新的编队并保持编队稳定。由于控制方法基于分布式一致性理论设计，异构无人系统没有中心控制节点，无需处理整个无人系统的总控制信息，降低了对于计算能力的要求，进一步降低了硬件成本。在基于分布式一致性理论设计的控制方法下，每个无人系统成员均作为一个节点，每个节点只与邻居节点通信，从而通过信息的传递来完成分配的任务。

异构无人系统三维空间编队控制方法包括以下多个步骤：

步骤S11：建立异构无人系统三维编队通信拓扑模型。

基于分布式一致性原理，三维协同编队要求每个无人系统成员通过通信拓扑与邻居节点交换状态信息数据。状态信息数据包括：实时航行位置、实时航行姿态和实时航行速度；其中实时航行位置包括但不限于实时三维坐标、实时航行姿态包括但不限于实时航向角，实时航行速度包括但不限于线速度和角速度。

示例性的，在可选的实施方式中，自治水下航行器中设置有惯性导航系统(Inertial Navigation System，INS)。惯性导航系统利用惯性敏感器件(例如加速度计和陀螺仪)、基准方向以及初始位置信息确定自治水下航行器在惯性空间中的位置、方向和速度，并将实时位置、实时姿态和实时速度提供给航行器控制器(机载控制器)。惯性导航系统可以采用现有技术中成熟的系统和算法(例如积分计算)，惯性导航系统的采样不是本发明的保护重点，在此不再进一步赘述。自治水下航行器还优选地设置声音导航测距装置(声呐)、深度计和定位模块，深度计配置为提供自治水下航行器深度信息，定位模块则可以在水面航行时获取精确的位置信息。虽然惯性导航系统作为公共参考系统可以提供全部导航数据，但是惯性导航系统的导航精度会随着时间逐渐发散，定位模块可以配置为当自治水下航行器在水面时对惯性导航系统的定位累计误差进行修正。定位模块可选的可以是GPS定位模块、北斗定位模块和/或无线电台。

示例性的，水面无人艇中设置有定位模块，定位模块可选的可以是GPS定位模块、北斗定位模块和/或无线电台，定位模块可以采样水面无人艇的实时位置、实时航向角和实时航行速度。

在本实施方式中，利用通信拓扑来表示异构无人系统中集群的通信关系，将每个无人系统成员看作通信拓扑中的一个节点，将无人系统成员之间的信息交互关系看作通信拓扑的边。

在本实施方式中，异构无人系统三维编队通信拓扑模型可以表示为：

G＝{V，E}

其中V＝{1，2，…，n}，V表示参与三维编队的n个无人系统节点的集合，n表示参与三维编队的无人系统节点的个数，无人系统节点可以是水面无人艇或自治水下航行器；E＝{(i，j)∈V×V}表示参与三维编队的n个无人系统节点之间有向信息路径组成的集合，其中i表示参与三维编队的第i个无人系统节点，i∈[1，n]；j表示参与三维编队的第j个无人系统节点，j∈[1，n]。

如图2提供的示例，其中共有6个参与三维编队的无人系统节点，其中三台为水面无人艇(如图2中U1、U2和U3所示)，三台为自治水下航行器(如图2中U4、U5和U6所示)；图2中的异构无人系统中6个无人系统节点的集合可以表示为：V＝{1，2，3，4，5，6}；与之对应的，参与三维编队的n个无人系统节点之间有向信息路径组成的集合可以表示为：

E

用邻接矩阵A_ij来表示并存储异构无人系统三维编队通信拓扑模型，邻接矩阵A_ij中的元素a_ij表示无人系统节点之间的通信状态，邻接矩阵A_ij中的元素a_ij满足：当无人系统节点i收到无人系统节点j发送的信息时，有a_ij>0；否则a_ij＝0。无人系统节点i和无人系统节点j之间的有向信息路径可以赋予设定权重，若设定权重为1时，a_ij＝1。同时设置a_ii＝0。在本实施方式中，邻接矩阵A_ij优选为带权邻接矩阵，A_ij为n×n的n阶方阵。

对于三维空间编队中的无人系统节点i，其邻居节点集合可以记为N_i，n_i表示邻居集N_i中的元素个数，即目标无人系统节点i的邻居的个数。

无人系统节点i的邻居集N_i记为：

N_i＝{j∈V|a_ij>0}＝{j∈V|(j，i)∈E}

若设定权重为1，则无人系统节点i的邻居N_i记为：

N_i＝{j∈V|a_ij＝1}＝{j∈V|(j，i)∈E}

无人系统节点i的邻居节点属于无人系统节点集合V且满足a_ij＝1，同时满足该节点与无人系统节点i的有向信息路径在有向信息路径组成的集合E中。

示例性的，对于无人系统节点1，有N₁＝{6}；即无人系统节点1的入度n₁＝1。

延续如图2所示的示例，可以得对应的邻接矩阵为：

在邻接矩阵A_ij的基础上，进一步可以得到异构无人系统三维编队通信拓扑模型的拉普拉斯(Laplacian)矩阵L_ij。由于在本实施方式中，邻接矩阵A_ij为优选为带权邻接矩阵，所以相应的，拉普拉斯矩阵也为加权拉普拉斯矩阵

拉普拉斯矩阵L_ij满足：

即所构建得加权拉普拉斯矩阵L_ij同样为一个n×n的n阶方阵，拉普拉斯矩阵L_ij对角线上的元素为相应行邻接矩阵A_ij中相应元素a_ij之和，非对角线上的元素为相应位置的邻接矩阵A_ij中相应元素a_ij的值的取反。

延续如图2的示例，可得拉普拉斯矩阵L_ij为：

对于其中的一个无人系统节点，可以在某一个时刻接收到邻居节点集合中随机的一个邻居节点发送的信息，由于本实施方式所提出的异构无人系统三维编队通信拓扑模型是一个分布式系统，提出以下修正Metropolis加权系数矩阵以体现无人系统节点间的采样信号的随机游走，利用无偏差采样算法增加无人系统节点之间的转移概率。修正Metropolis加权系数矩阵可以表示为：

其中，N_j为三维空间编队中的无人系统节点j的邻居节点集合，m为整数且对应无人系统节点i的邻居集N_i的一个无人系统节点。

修正Metropolis加权系数矩阵中的元素w_ij表示无人系统节点i到其邻居节点集合中选取无人系统节点j进行数据采样(即无人系统节点i接收到无人系统节点j发送的信息)的转移概率。

延续如图2的示例，根据上式，可以计算出，无人系统节点3到其邻居节点集合{1，2}中选取无人系统节点1进行数据采样(即接收到无人系统节点1发送的信息)的转移概率为0.5，即修正Metropolis加权系数矩阵中的元素w₃₁为0.5；类似的，无人系统节点3到其邻居节点集合{1，2}中选取无人系统节点2进行数据采样(即接收到无人系统节点2发送的信息)的转移概率也为0.5，即修正Metropolis加权系数矩阵中的元素w₃₂为0.5，逐一计算出各无人系统节点与其邻居节点间的转移概率后，即可以得到修正Metropolis加权系数矩阵W_ij。

步骤S12：执行航向一致性控制算法。

选用航向角作为异构无人系统三维编队航行的第一参考状态。航向角决定了每个无人系统节点的航行方向。

航向一致性控制算法的控制目标为：在异构无人系统三维编队形成和三维编队保持的过程中，确保异构无人系统中的所有无人系统节点的实时航向角收敛于同一航向角，也即水面无人艇和自治水下航行器均收敛于同一航向角。

航向一致性控制算法可以由下式表示：

其中，

表示无人系统节点i的输出航向角，ψ_i为无人系统节点i的实时航向角，n_i表示无人系统节点i的邻居集N_i中的邻居节点的个数，

表示无人系统节点i的输入航向角；b_i表示第一航向控制增益，

表示无人系统节点i的目标航向角，ψ_j表示无人系统节点i的邻居集N_i中的邻居节点j的实时航向角；W_ij为修正Metropolis加权系数矩阵；第一航向控制增益b_i可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用，目标航向角

可以预先设定，或者通过远程通信获得并可以根据实际情况进行调整。

步骤S13：执行速度一致性控制算法。

选用航行速度作为异构无人系统三维编队航行的第二参考状态。

速度一致性控制算法的控制目标为：在异构无人系统三维编队形成和三维编队保持的过程中，确保异构无人系统中的所有无人系统节点的实时航行速度收敛于同一航行速度，也即水面无人艇和自治水下航行器均收敛于同一航行速度。

速度一致性控制算法可以由下式表示：

其中，

表示无人系统节点i的输出航行速度，v_i表示无人系统节点i的实时航行速度，τ_v表示速度时间常数，

表示无人系统节点i的输入航行速度，c_i表示第一速度控制增益，

表示无人系统节点i的目标航行速度，W_ij为修正Metropolis加权系数矩阵；v_j无人系统节点i的邻居集N_i中的邻居节点j的实时航行速度；速度时间常数τ_v可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用；第一速度控制增益c_i可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用；目标航行速度

可以预先设定，或者通过远程通信获得并可以根据实际情况进行调整。

步骤S14：执行深度一致性控制算法。

选用深度作为异构无人系统三维编队航行的第三参考状态。

具体来说，首先判断当前无人系统节点是否为自治水下航行器；如果为自治水下航行器，则执行深度一致性控制算法。

深度一致性控制算法的控制目标为：使作为无人系统节点的多个自治水下航行器集结在同一深度航行(定深航行)。换言之，对于无人系统节点i和无人系统节点j，定深航行应满足无人系统节点i的航行深度趋近于无人系统节点j，无人系统节点i的航行深度的导数趋近于无人系统节点j的航行深度。

深度一致性控制算法可以由下式表示：

其中，

表示无人系统节点i的输出航行深度，z_i表示无人系统节点i的实时航行深度，τ_z表示深度时间常数，

表示深度导数的时间常数，

表示无人系统节点i的实时航行深度的导数，

表示无人系统节点i的输入航行深度；

为深度控制增益；W_ij为修正Metropolis加权系数矩阵；z_j表示无人系统节点i的邻居集N_i中的邻居节点j的实时深度，γ为权重系数，

表示无人系统节点i的邻居集N_i中的邻居节点j的实时深度的导数；深度时间常数τ_z可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用；深度导数的时间常数

可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用；深度控制增益

可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用；权重系数γ可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用。

在本实施方式中，采用分布式的信息交换策略，假设异构无人系统三维编队的通信拓扑中至少存在一簇有向生成树，即每个无人系统节点都有可以和其进行信息交互的邻居。然后利用无人系统模型横纵向解耦的特性，将异构无人系统集群的三维空间编队队形控制分解为水平和垂直的队形控制。

在可选的实施方式中，在建立异构无人系统三维编队通信拓扑模型后，可选择性同步执行基于分布式一致性理论的异构无人系统三维空间编队控制方法。

建立三维坐标系，在可选的实施方式中，基于分布式一致性理论的异构无人系统三维空间编队控制方法，即另一种可选的航向一致性控制算法、速度一致性控制算法和深度一致性控制算法可以由下式表示：

其中，

表示无人系统节点i的目标y轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标y轴坐标，

表示无人系统节点i和无人系统节点j之间沿y轴的目标间距；

表示无人系统节点i的目标x轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标x轴坐标，

表示无人系统节点i和无人系统节点j之间沿x轴的目标间距；

表示无人系统节点i的目标z轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标z轴坐标，

表示无人系统节点i和无人系统节点j之间沿z轴的目标间距；

表示第二航向控制增益，为常数；

表示第二速度控制增益，为常数；第二航向控制增益

和第二速度控制增益

可以在实验条件下提前利用模拟软件测试得到，并以常数的形式预先存储以供随时调用；上述目标值可以预先设定，或者通过远程通信获得并可以根据实际情况进行调整。

在执行航向一致性、速度一致性和深度一致性的算法和/或执行基于分布式一致性理论的控制方法之后，作为无人系统节点的水面无人艇按照输出航向角和输出航行速度运行，作为无人系统节点的自治水下航行器按照输出航向角、输出航行速度和输出航行深度运行。判定执行航向一致性、速度一致性和深度一致性算法和/或执行基于分布式一致性理论的异构无人系统三维空间编队控制方法可以依据实时通信质量或者实时任务需求的检测参数，进一步进行手动或自动选择。在同步执行两种算法后，可以随机执行一者输出的输出航向角、输出航行速度和输出航行深度。

原理上说，一致性算法可以迭代无数次。但在实际应用中，考虑通信带宽的约束，在有限的时间间隔(k→k+1时间范畴内)，一致性算法只能够迭代有限次数n_d<∞，甚至在考虑通信延迟的情况下，只能迭代依次，即n_d＝1；n_d代表迭代次数。

在本发明的一些实施方式中，考虑到每个无人系统节点的邻居节点集合不同，通过比较实时节点状态与目标节点状态，计算实时节点状态和目标节点状态的差异，进一步对修正Metropolis加权系数矩阵W_ij进行实时修正，无人系统节点i第k+1次一致性迭代后修正的实时Metropolis加权系数矩阵W_ij ^′(k+1)满足下式：

W_ij′(k+1)＝W_ij′(k)+a_ie_i(k)(ξ_j-ξ_i)+a_je_j(k)(ξ_i-ξ_j)

其中W_ij′(k)为无人系统节点i第k次一致性迭代后修正的实时Metropolis加权系数矩阵，W_ij′(k)的初始值可以为W_ij，a_i和a_j为比例因子，为提前设定的常数，e_i(k)为无人系统节点i第k次一致性迭代时实时状态相对其目标状态的误差，例如示例性的包括实时航向角和目标航向角之间的误差、实时航行速度和目标航行速度之间的误差、和/或实时航行深度和目标航行深度之间的误差；上述误差值可以择一使用，也可以赋予相应的权重并计算和值，ξ_i、ξ_j为无人系统节点i和无人系统节点j实际一致性状态的近似，为现有技术中所公开的模拟软件的计算值，对于本实施方式来说，即为常数值。

在每一次一致性迭代时，修正实时Metropolis加权系数矩阵，可以生成更合适的一致性协议，以得到更快的一致性速度，确保三维编队可以尽快成型。

在可选的实施方式中，每一个无人系统节点中均设置至少一台机载控制器。机载控制器可选的与岸基控制器通信连接。机载控制器和岸基控制器中均设置有存储单元、处理器、输入/输出接口、通信接口等元件，其中存储单元可以包括易失性存储器和/或非易失性存储器，存储单元配置为存储与上述控制方法相关联的指令或数据。处理器可以是专用处理或中央处理器等。处理器可以访问存储单元以执行在存储单元中所存储的指令以实现相关功能。通信接口配置支持一种或多种无线通信协议，以与其它记载控制器和岸基控制器实现交互连接、数据传输并接收控制指令。输入/输出接口可以是串行接口或者光纤，例如与不同的传感器连接采样无人系统节点的各路运行数据。

本发明所提供的异构无人系统三维空间编队控制方法，首先基于水面无人艇和自治水下航行器的性能差异以及通信约束，建立基于图论的稳定航行通信拓扑模型；其次，通过比较实时节点状态与目标节点状态的差异，对一致性Metropolis加权系数矩阵进行实时修正，得到更快的一致性收敛速度；最后，在航向一阶一致性、速度一阶一致性和深度一阶一致性编队算法基础上，设计三维空间编队控制方法，可以有效实现编队形成、编队保持和编队变换，使异构无人系统具备良好的自愈能力和扩展能力。

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

Claims (3)

1.一种异构无人系统三维空间编队控制方法，其特征在于，

所述异构无人系统由若干水面无人艇和若干自治水下航行器组成；

所述异构无人系统三维空间编队控制方法包括以下步骤：

步骤S11：建立异构无人系统三维编队通信拓扑模型；

在步骤S11中，异构无人系统三维编队通信拓扑模型表示为：

G＝{V，E}

其中，V＝{1，2，…，n}，V表示参与三维编队的n个无人系统节点的集合，n表示参与三维编队的无人系统节点的个数，无人系统节点是水面无人艇或自治水下航行器；E＝{(i，j)∈V×V}表示参与三维编队的n个无人系统节点之间有向信息路径组成的集合，其中i表示参与三维编队的第i个无人系统节点，i∈[1，n]；j表示参与三维编队的第j个无人系统节点，j∈[1，n]；

建立异构无人系统三维编队通信拓扑模型的邻接矩阵A_ij；邻接矩阵A_ij为n×n的n阶方阵，邻接矩阵A_ij中的元素a_ij满足：当无人系统节点i收到无人系统节点j发送的信息时，有a_ij>0；否则a_ij＝0；

建立无人系统节点i的邻居集N_i，无人系统节点i的邻居集N_i记为：

N_i＝{j∈V|a_ij>0}＝{j∈V|(j，i)∈E}；

建立异构无人系统三维编队通信拓扑模型的拉普拉斯矩阵L_ij，拉普拉斯矩阵L_ij为：

建立修正Metropolis加权系数矩阵：

其中，N_j为三维空间编队中的无人系统节点j的邻居节点集合，m为整数且对应无人系统节点i的邻居集N_i的一个无人系统节点；

步骤S12：执行航向一致性控制算法；

在步骤S12中，航向一致性控制算法可由下式表示：

其中，

表示无人系统节点i的输出航向角；ψ_i为无人系统节点i的实时航向角；n_i表示无人系统节点i的邻居集N_i中的邻居节点的个数；

表示无人系统节点i的输入航向角；b_i表示第一航向控制增益，为常数；

表示无人系统节点i的目标航向角；ψ_j表示无人系统节点i的邻居集N_i中的邻居节点j的实时航向角；W_ij为修正Metropolis加权系数矩阵；

步骤S13：执行速度一致性控制算法；

在步骤S13中，速度一致性控制算法可由下式表示：

其中，

表示无人系统节点i的输出航行速度；v_i表示无人系统节点i的实时航行速度；τ_v表示速度时间常数；

表示无人系统节点i的输入航行速度；c_i表示第一速度控制增益，为常数；

表示无人系统节点i的目标航行速度；W_ij为修正Metropolis加权系数矩阵；v_j无人系统节点i的邻居集N_i中的邻居节点j的实时航行速度；

步骤S14：如果无人系统节点为自治水下航行器，则执行深度一致性控制算法；

在步骤S14中，深度一致性控制算法可由下式表示：

其中，

表示无人系统节点i的输出航行深度；z_i表示无人系统节点i的实时航行深度；τ_z表示深度时间常数；

表示深度导数的时间常数；

表示无人系统节点i的实时航行深度的导数；

表示无人系统节点i的输入航行深度；

为深度控制增益，为常数；W_ij为修正Metropolis加权系数矩阵；z_j表示无人系统节点i的邻居集N_i中的邻居节点j的实时深度；γ为权重系数，为常数；

表示无人系统节点i的邻居集N_i中的邻居节点j的实时深度的导数；

控制作为无人系统节点的水面无人艇按照输出航向角和输出航行速度运行；

控制作为无人系统节点的自治水下航行器按照输出航向角、输出航行速度和输出航行深度运行。

2.根据权利要求1所述的异构无人系统三维空间编队控制方法，其特征在于，

所述航向一致性控制算法、速度一致性控制算法和深度一致性控制算法还可由下式表示：

其中，

表示无人系统节点i的目标y轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标y轴坐标，

表示无人系统节点i和无人系统节点j之间沿y轴的目标间距；

表示无人系统节点i的目标x轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标x轴坐标，

表示无人系统节点i和无人系统节点j之间沿x轴的目标间距；

表示无人系统节点i的目标z轴坐标，

表示无人系统节点i的邻居集N_i中的邻居节点j的目标z轴坐标，

表示无人系统节点i和无人系统节点j之间沿z轴的目标间距；

表示第二航向控制增益，为常数；

表示第二速度控制增益，为常数。

3.根据权利要求1或2所述的异构无人系统三维空间编队控制方法，其特征在于，

在执行航向一致性控制算法、速度一致性控制算法和深度一致性控制算法前对所述修正Metropolis加权系数矩阵W_ij进行实时修正得到实时Metropolis加权系数矩阵W_ij′；

无人系统节点i第k+1次一致性迭代后修正的实时Metropolis加权系数矩阵W_ij′(k+1)满足下式：

W_ij′(k+1)＝W_ij′(k)+aiei(k)(ξ_j-ξ_i)+a_je_j(k)(ξ_i-ξ_j)

其中W_ij′(k)为无人系统节点i第k次一致性迭代后修正的实时Metropolis加权系数矩阵，a_i和a_j为比例因子，e_i(k)为无人系统节点i第k次一致性迭代时实时状态相对其目标状态的误差，ξ_i、ξ_j为无人系统节点i和无人系统节点j实际一致性状态的近似。

Images