一种分布式异构多UUV协同控制模块及协同控制系统

技术领域

本发明属于多水下无人航行器协同控制技术领域，涉及一种分布式异构多UUV协同控制模块及协同控制系统。

背景技术

具有自主规划、感知与导航等能力的水下无人航行器(Unmanned UnderwaterVehicle，UUV)是海洋科学考察、海洋开发利用与保卫海洋安全的有利工具。但是由于作业范围、作业能力的限制，单UUV已经无法胜任日益复杂的海洋任务。多UUV系统因其较高的冗余性、容错性以及鲁棒性，可增大水下侦察搜索范围，提高反应速度与协同效果。

多UUV协同控制系统(亦称多UUV系统、UUV集群系统)可分为同构协同控制系统和异构协同控制系统。同构系统中各UUV平台结构一致、性能一致、输入输出接口一致，协同控制较为简单，目前已经取得了较多研究成果；随着研究的深入以及需求多元化，由单一类型UUV平台组成的集群系统已经无法完成多元化任务，因此，异构多UUV协同控制系统被广泛关注。

异构系统中存在不同类型的UUV，这些UUV往往结构不统一、性能不统一、输入输出接口也没有统一标准的协议，更没有可供解析的统一标准指令集，这给多UUV协同控制系统的设计与应用带来了挑战。一是面向结构不一致、性能不一致、控制参数不一致的异构UUV平台以及水声弱通信环境，传统的集中式控制模式已经无法应用；二是面向没有统一标准的输入输出接口协议，各UUV之间信息交换变得困难，若要实现协同控制，必须建立标准的接口协议；三是面向不同厂家设计生产的UUV，没有统一标准的通用指令集，这使得系统在任务解析以及控制指令输出时必须考虑多套指令集。如果有通用指令集，则可实现统一标准下的任务指令与控制指令的解析，便于系统的设计与应用；四是面向不同厂家生产的UUV平台，若基于机载控制器设计协同控制算法，技术封锁以及接口开放程度带来的开发困难也会给系统的设计、调试以及拓展性能带来不小的挑战。

发明内容

为解决上述技术问题，本发明的目的是提供一种分布式异构多UUV协同控制模块及协同控制系统，以实现统一标准下的多UUV协同控制系统设计、开发与应用。

本发明提供一种分布式异构多UUV协同控制模块，所述协同控制模块搭载在一台UUV上，每台搭载协同控制模块的UUV为一个分布式节点，所述协同控制模块包括：协同算法子模块以及分别与协同算法子模块相连接的通信子模块、使命解析子模块、故障检测与处理子模块以及数据记录子模块；

所述通信子模块基于统一标准的输入输出接口协议，实现协同控制模块与所搭载的UUV进行通信，接收所搭载的UUV发送的本节点状态信息、邻居节点状态信息以及使命任务信息，向所搭载的UUV输出更新后的本节点状态信息和协同控制指令信息；

所述使命解析子模块与通信子模块连接，用于对使命任务信息进行解析获得任务指令并发送给协同算法子模块和故障检测与处理子模块；

所述故障检测与处理子模块根据任务指令、邻居节点以及本节点的历史状态信息、来自协同算法子模块的故障检测关键信息，检测所述协同控制模块、所搭载的UUV以及UUV搭载的外围设备是否存在故障以及故障类型，并输出故障处理策略；

所述协同算法子模块根据任务指令、本节点状态信息、邻居节点状态信息以及故障处理策略计算协同控制指令信息；

所述数据记录子模块用于存储本节点状态信息、邻居节点状态信息、协同控制指令信息以及故障处理策略。

进一步的，所述协同算法子模块是协同控制模块的核心部分，其集成编队航行控制算法、协同探测算法、协同目标跟踪算法、协同任务分配算法，以完成复杂多样的协同控制任务；

所述协同算法子模块计算的协同控制指令信息通过通信子模块输出到所搭载的UUV，再由UUV的底层控制系统跟踪进而完成协同任务。

进一步的，所述使命任务信息由母船/岸基指挥控制中心下发，包含各UUV的角色身份、通信拓扑结构设定、目标海域以及目标任务，使命任务信息基于统一标准的指令集，能实现通用统一标准的任务指令的下发和解析；

所述本节点状态信息是协同控制模块所搭载的UUV的航行状态信息，包括UTC时间基准、速度信息与位姿信息；

所述邻居节点状态信息包括该邻居节点的角色身份、UTC时间基准、速度信息、位姿信息、所处任务阶段和故障类型；

所述更新后的本节点状态信息用于输出给其他节点，即为其他节点所接收的邻居节点状态信息；

所述协同控制指令信息为协同算法子模块根据任务指令、本节点状态信息、邻居节点状态信息以及故障处理策略综合计算的本节点的控制指令信息；控制指令信息包括所搭载的UUV需要跟踪的期望速度、航向信息以及深度或高度信息，其基于统一标准的控制指令集，可实现异构UUV平台控制指令的解析与跟踪。

进一步的，所述使命任务信息基于XML文件体现，其中目标任务包括需要跟踪的航路关键点信息、期望速度信息、期望深度或高度信息、期望队形信息、探测区域信息、期望任务时间信息，规定了多UUV系统期望任务及执行方式。

进一步的，所述来自协同算法子模块的故障检测关键信息包括：本节点的速度信息和位姿信息、邻居节点的速度信息和位姿信息、协同控制指令信息以及当前任务阶段信息。

进一步的，所诉协同控制模块具有一个以太网接口和一个串口，便于不同模式下的数据传输。

进一步的，所述协同控制模块支持SSH或telent协议，支持TCP/IP网络通信协议。

进一步的，所述协同控制模块支持NTP对时机制，支持程序网络远程升级，支持日志输出机制。

本发明还提供一种分布式异构多UUV协同控制系统，包括：母船/岸基指挥控制中心、中继设备以及多台搭载协同控制模块的UUV；所述协同控制模块固定在UUV平台密封舱内，由UUV电源管理模块供电，通过以太网协议或者串行通信协议与UUV通信模块交换数据，不参与UUV底层控制；母船/岸基指挥控制中心通过无线电与中继设备通信，多台搭载协同控制模块的UUV与中继设备通信以接收母船/岸基指挥控制中心的使命任务信息，各台UUV通过水声通信交换数据，以完成协同任务。

本发明的一种分布式异构多UUV协同控制模块及协同控制系统至少具有以下有益效果：

1、提供分布式的协同控制体系结构，可适应异构UUV平台与弱通信环境带来的挑战；

2、提供通用统一标准的输入输出接口协议，便于协同控制系统基于统一标准协议交换数据，降低了系统中不同标准通信协议带来的数据交换困难；

3、提供统一标准的通用指令集，使得各UUV可在统一标准下制定使命任务、解析出使命任务指令与协同控制指令信息，降低了系统设计开发、调试与应用的难度；

4、将上述标准输入输出接口协议、标准通用指令集以及故障检测等封装成协同控制模块，该模块搭载在UUV内部，与UUV本体通信模块交换数据，不参与UUV的底层控制，降低了存在不同类型、不同接口开放程度异构UUV的协同控制系统的设计与开发难度。

附图说明

图1为本发明的分布式异构多UUV协同控制模块的接口示意图；

图2为本发明的分布式异构多UUV协同控制模块的各软件子模块的拓扑关系图；

图3为故障检测流程图；

图4为协同探测任务中各阶段流程图；

图5为编队航行控制算法的队形信息定义图；

图6为协同控制模块与UUV连接关系示意图；

图7为分布式多UUV协同控制系统的示意图。

具体实施方式

本发明设计了一种分布式异构多UUV协同控制模块，搭载在一台UUV上，每台搭载协同控制模块的UUV为一个分布式节点。所述协同控制模块其硬件体现为嵌入式核心控制单元，其中固化Linux操作系统，上电自启动。

如图1所示，每个协同控制模块独立封装，预留安装定位口、通信口与供电口。协同控制模块具有一个以太网接口和一个串口，便于不同模式下的数据传输。所述协同控制模块支持SSH或telent协议，支持TCP/IP网络通信协议。所述协同控制模块支持NTP对时机制，支持程序网络远程升级，支持日志输出机制。

如图2所示，所述协同控制模块包括：协同算法子模块以及分别与协同算法子模块相连接的通信子模块、使命解析子模块、故障检测与处理子模块以及数据记录子模块。

所述通信子模块基于统一标准的输入输出接口协议，实现协同控制模块与所搭载的UUV进行通信，其所有信息均与UUV交互或者通过UUV转发。通信子模块接收所搭载的UUV发送的本节点状态信息、邻居节点状态信息以及使命任务信息，向所搭载的UUV输出更新后的本节点状态信息和协同控制指令信息。

其中，使命任务信息由母船/岸基指挥控制中心下发，包含各UUV的角色身份、通信拓扑结构设定、目标海域以及目标任务，使命任务信息是初始时对任务的总体描述，其基于统一标准的指令集，能实现通用统一标准的任务指令的下发和解析。

所述本节点状态信息是协同控制模块所搭载的UUV的航行状态信息，包括UTC时间基准、速度信息与位姿信息。用于完成协同控制。

所述邻居节点状态信息包括该邻居节点的角色身份、UTC时间基准、速度信息、位姿信息、所处任务阶段和故障类型。是辨别邻居节点以及邻居节点在关键时刻用于协同控制的关键信息。

所述更新后的本节点状态信息用于输出给其他节点，即为其他节点所接收的邻居节点状态信息。例如：将本节点记为A，B为其邻居节点，则节点A所输出的状态信息即为节点B输入的邻居节点状态信息。

所述协同控制指令信息为协同算法子模块根据任务指令、本节点状态信息、邻居节点状态信息以及故障处理策略综合计算的本节点的控制指令信息；控制指令信息包括所搭载的UUV需要跟踪的期望速度、航向信息以及深度或高度信息，其基于统一标准的控制指令集，可实现异构UUV平台控制指令的解析与跟踪。

如图1所示，所述使命解析子模块与通信子模块连接，用于对使命任务信息进行解析获得任务指令并发送给协同算法子模块和故障检测与处理子模块。所述使命任务信息基于XML文件体现，其中目标任务包括需要跟踪的航路关键点信息、期望速度信息、期望深度或高度信息、期望队形信息、探测区域信息、期望任务时间信息，规定了多UUV系统期望任务及执行方式。

所述故障检测与处理子模块根据任务指令、邻居节点以及本节点的历史状态信息、来自协同算法子模块的故障检测关键信息，检测所述协同控制模块、所搭载的UUV以及UUV搭载的外围设备是否存在故障以及故障类型，并输出故障处理策略。其中，来自协同算法子模块的故障检测关键信息包括：本节点的速度信息和位姿信息、邻居节点的速度信息和位姿信息、协同控制指令信息以及当前任务阶段信息。具体的故障检测与处理子模块检测的故障列表如下：

故障检测程序流程如图3所示。故障检测与处理子模块首先初始化故障列表，然后根据上表预设故障阈值，周期性逐项检测上表中的故障，若发生故障，则开启故障处理过程。

所述数据记录子模块是协同控制模块的数据管理中心，其一方面记录系统运行日志以及关键输入输出、关键中间数据信息，便于后期的数据处理、系统运行分析与任务效能评估等；另一方面为各子模块提供历史关键数据。数据记录子模块中存储有本节点状态信息、邻居节点状态信息、协同控制指令信息以及故障处理策略等。

所述协同算法子模块根据任务指令、本节点状态信息、邻居节点状态信息以及故障处理策略计算协同控制指令信息。所述协同算法子模块是协同控制模块的核心部分，其集成协同探测算法、协同目标跟踪算法、协同任务分配算法，编队航行控制算法以完成复杂多样的协同控制任务。所述协同算法子模块计算的协同控制指令信息通过通信子模块输出到所搭载的UUV，再由UUV的底层控制系统跟踪进而完成协同任务。

下面，以协同探测任务为例对协同算法子模块的工作过程进行说明。

协同探测任务包括三个阶段，分别是航渡阶段、作业阶段和返航阶段。首先由母船/岸基指挥控制中心通过通信网络将配置好的使命任务信息下发到各UUV的协同控制模块中，然后解析获得任务指令。各UUV布放下水后，在布放等待区等待，母船/岸基指挥控制中心下发任务开始指令后，UUV进入航渡阶段，按照解析出的航渡阶段的任务指令包括：航路关键点信息、期望速度信息、期望深度信息、期望队形信息航渡到作业区，其中关键点是指组成航路的经纬度坐标点。到达作业区后，自主切换到作业阶段，开始按照解析出的作业阶段的任务指令以编队的方式完成对作业区的协同探测任务。当对作业区的协同探测任务结束后，将自主返航到布放等待区。

协同探测任务中各阶段的流程图如图4所示。首先各UUV中的协同控制模块根据当前阶段获取相关的任务指令，然后判断是否有航路关键点，如果有则执行编队航行控制，执行关键点航行控制；如果没有航路关键点，则判断是否需要定点航行，如果需要定点航行，则执行定点航行控制。接下来判断是否切换阶段，是则切换阶段，本阶段程序执行结束，进入下一阶段，否则执行定点航行控制。

以下是编队航行控制算法的一个实施例：

执行协同探测任务的多UUV中，有一台UUV是领航者，其余UUV是跟随者。领航者UUV根据航路关键点信息生成航行路径，并以期望的速度和深度跟踪此路径航行。为避免领航者UUV在路径跟踪时反向航行，领航者UUV的航向控制量通过下式表达：

其中，

其中，d为领航者UUV到期望路径的距离，d

领航者UUV输出的速度和深度则是任务指令中的期望速度信息和期望深度信息。

跟随者UUV则根据本节点状态信息、邻居节点状态信息以及使命任务信息解析获得的相关任务指令计算本节点的控制指令。其中航向控制量的计算方法和领航者UUV路径跟踪计算过程相同。跟随者UUV的速度和深度计算过程如下。

首先，根据邻居节点状态信息计算跟随者UUV节点i的期望位置：

其中，

然后，在二维平面构建距离与角度的反馈误差，以计算期望速度。

根据下式计算距离误差e

其中p

根据下式计算角度误差

其中

考虑到欠驱动UUV系统转弯半径大，若期望位置在跟随者节点i前方，即跟随者节点i载体坐标系的一、二象限时，需要以比领航者更高的速度去追赶，若期望位置在其后方，即位于载体坐标系的三、四象限时，需要以比领航者更低的速度去等待。根据

深度指令为预置的期望指令，如下式：

至此，可以得到跟随者节点i完整的控制律：

对于定点航行控制，则可以使得UUV以当前点为圆心执行圆周运动。

本发明还设计一种分布式异构多UUV协同控制系统，包括：母船/岸基指挥控制中心、中继设备以及多台搭载协同控制模块的UUV。

如图6所示，所述协同控制模块固定在UUV平台密封舱内，由UUV电源管理模块供电，通过以太网协议或者串行通信协议与UUV通信模块交换数据，不参与UUV底层控制；母船/岸基指挥控制中心通过无线电与中继设备通信，多台搭载协同控制模块的UUV分别与中继设备通信以接收母船/岸基指挥控制中心的使命任务信息，各台UUV通过水声通信交换数据，以完成协同任务。

组成集群的各UUV均搭载本发明设计的协同控制模块，每台搭载协同控制模块的UUV为一个分布式节点，协同控制模块为该节点协同控制层的计算与控制中心。母船/岸基指挥控制中心是多UUV系统的中央指挥与监控系统，其通过无线电与中继设备通信；中继设备一半在水上一半在水下，水上支持无线电通信，水下支持水声通信，内部可实现水声通信和无线电通信协议的转换；无线中继与各水下UUV平台通信，以打通母船/岸基指挥控制中心与多UUV系统的通信链路。

UUV还搭载必要的姿态传感、探测、通信等设备，以满足多UUV系统的基本任务需求。协同控制模块不搭载上述设备，所需信息均来自UUV本体。UUV接收来自母船/岸基指挥中心的指挥控制信息，各UUV之间通过水声通信交换数据，以完成协同任务。

图7为典型的应用场景，UUV1为主UUV，其与母船/岸基指挥控制中心通信，UUV2-UUV4与UUV1通信，UUV5与UUV3通信。指挥控制人员基于统一标准的任务指令集制定好协同控制任务，体现为具有标准格式的使命任务文件，其中包含了UUV1-UUV5五台UUV的组群以及拓扑关系。还包含了各UUV的多个阶段的使命任务信息，如关键路径点信息。

母船/岸基指挥控制中心将使命任务文件发送给UUV1，UUV1按照既定的通信拓扑关系将使命文件发送给各UUV。集群内各UUV收到使命文件，并校验完成后，将使命文件送到使命解析子模块进行使命解析。同时，各UUV根据既定通信拓扑结构将本节点状态信息以及收到的下一级的状态信息向上一级汇报。如图7，UUV5将本节点状态信息上报给UUV3，UUV3将本节点以及UUV5状态信息上报给UUV1。UUV2、UUV4将本节点状态信息上报给UUV1。UUV1将所有节点状态信息上报给母船/岸基指挥控制中心，母船/岸基指挥控制中心收到各节点状态信息后，解析其当前状态。如各UUV状态均显示成功收到无误的使命任务，则指挥控制中心下发执行第一阶段任务指令；如存在UUV状态为未收到或收到有误的使命任务信息，则指挥控制中心重新发送使命任务。

各UUV中的协同控制模块接收到执行第一阶段任务指令后，则从使命任务解析子模块中获取第一阶段任务指令，并按照图7的通信拓扑结构进行数据传输，并将所有UUV状态信息上报给指挥控制中心。各类信息的发送/接收频率由协同控制算法实现以及指挥控制中心既定要求决定。各UUV中的协同控制模块按照图2各软件子模块的拓扑关系以及该阶段既定任务指令执行协同任务，实时输出协同控制指令到UUV本体，以完成既定任务。当该阶段完成或故障时，将信息上报给指挥控制中心，指挥控制中心给出切换下阶段任务或故障处理指令，以此往复直至全阶段任务完成或终止。

以上所述仅为本发明的较佳实施例，并不用以限制本发明的思想，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。