CN112034735A

CN112034735A - 一种多auv水下协同作业的模拟实验平台

Info

Publication number: CN112034735A
Application number: CN202010894786.1A
Authority: CN
Inventors: 徐元欣; 阮亮; 马鑫奇; 李曾妮; 战元龙; 林佳祥; 裴鹏辉; 李铭凯; 郑少波; 张彩宝
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2020-08-31
Filing date: 2020-08-31
Publication date: 2020-12-04
Anticipated expiration: 2040-08-31
Also published as: CN112034735B

Abstract

本发明公开了一种多AUV水下协同作业的模拟实验平台，该平台是由多个互相通信的单AUV模拟平台扩展得到的多AUV协同平台；上位机和遥控船组件或ROV组成单AUV模拟平台；采集模拟AUV的传感器信息，每个单AUV模拟平台在上位机上搭载一个MOOS平台，由数据通信模块、融合导航模块、规划决策模块和运动控制模块组成。本发明设计实现了多AUV的消息通信方案，并利用心跳检测机制保证了可靠性；可融合速率不同的多源传感器信息，解决了传感器测量延迟问题，并且能够自动检测和剔除野值，提高导航精度；具有实验过程简便、调试效率高、设计成本低，扩展性好和可靠性高等优点；实验证明本发明具有广阔的应用前景。

Description

一种多AUV水下协同作业的模拟实验平台

技术领域

本发明属于自主水下航行器领域，尤其涉及一种多AUV水下协同作业的模拟实验平台。

背景技术

海洋占地球表面的71％，蕴藏着丰富的海洋生物资源、矿物资源和能源，是人类社会可持续发展的重要财富。随着人类越来越频繁和深入的开展海洋开发活动，在水下恶劣环境中，搭载多传感器的自主水下航行器(Autonomous Underwater Vehicle，AUV)已成为人类延伸自己感知能力的主要工具之一。

AUV进行水下作业，需要每时每刻了解自身所处的位置，才能决定下一步要进行的动作，因而高精度的融合多传感器信息的导航方法是AUV完成任务的技术保障。在导航基础上，AUV还要实现路径规划，任务决策，自主控制等功能，从而完成一个完整的水下作业。

然而AUV的研制过程较长，多AUV水下协同实验更是需要大量的财力和精力支持，因此出现了一些AUV模拟实验平台。这些实验平台通常用软件仿真或者半实物模拟AUV运动，将相应传感器数据上传到一台计算机中，运行核心算法程序，生成相应的运动指令信息再下发到模拟AUV中。但现有的AUV模拟实验平台仍存在以下不足：1.现有的模拟实验平台多是依赖纯软件来模拟AUV运动或是半实物的仿真平台；2.现有的模拟实验平台多是针对单AUV进行模拟，对多AUV协同的模拟实验平台涉及较少。

发明内容

本发明的目的在于针对现有技术的不足，提供了一种多AUV水下协同作业的模拟实验平台。

本发明的目的是通过以下技术方案来实现的：一种多AUV水下协同作业的模拟实验平台，是由多个互相通信的单AUV模拟平台扩展得到的多AUV协同平台；

所述单AUV模拟平台由上位机、模拟AUV组成；

所述模拟AUV为遥控船组件或ROV；所述遥控船组件包括遥控船、遥控器、单片机和智能终端；所述ROV包括运行机构、处理机和传感器模块；

所述智能终端用于采集传感器信息，包括角速度、加速度、旋转矩阵、GPS定位数据和姿态角；

所述ROV的传感器模块用于采集传感器信息，包括角速度、加速度、旋转矩阵、GPS定位数据、姿态角和深度；

每个单AUV模拟平台在上位机上搭载一个MOOS平台，由数据通信模块、融合导航模块、规划决策模块和运动控制模块组成；一方面每个MOOS平台的pShare进程连接MOOSDB，获取该MOOS平台中其它进程的消息；另一方面不同MOOS平台通过pShare进程两两连接互相通信；

所述数据通信模块用于通过pComm进程分别与各单AUV模拟平台的智能终端或处理机通信，获取传感器信息并通过MOOSDB发送给融合导航模块，以及实时监测连接活性；

所述融合导航模块用于通过导航滤波算法处理传感器信息，得到模拟AUV当前状态估计值并发送给规划决策模块；

所述规划决策模块用于根据各模拟AUV当前状态估计值，通过路径规划算法结合优化方法，得到各模拟AUV下一时刻的状态期望值并发送给运动控制模块；

所述运动控制模块用于根据各模拟AUV下一时刻的状态期望值，通过pRB_PID进程的控制逻辑转化为控制信息，再由pUART进程封装成数据帧后发送给模拟AUV实现运动控制；

所述pUART进程用于串口通信，将控制信息组装成帧；

所述模拟AUV的状态信息包括三维位置、速度、姿态角。

进一步地，所述pComm进程采用IO多路复用机制，包括一个主线程和两个子线程；其中，主线程用于获取数据和MOOSDB通信；一个子线程用于启动进程与MOOSDB通信；另一个子线程用于pComm与客户端通信；所述客户端为智能终端或ROV处理机；

pComm与客户端通信的子线程使用基于事件驱动的支持IO多路复用技术的网络通信服务器，支持多个客户端同时连接；在初始化后等待事件发生，包括连接事件、读事件、写事件和定时事件；

所述连接事件用于处理新客户端的网络连接并为其注册读写事件；

所述读事件用于读取客户端的传感器信息数据包，解码提取原始数据；

所述写事件用于发送命令信息，将其编码成帧后发送给客户端；

所述定时事件用于检测连接活性，客户端定时发送心跳检测数据到pComm进程，在定时器中检查最近接收的心跳包是否超时；如超时则连接失效，尝试重新连接，同时pComm进程自动清理失效连接有关的数据，为新连接创建所需资源。

进一步地，所述pComm进程还将传感器信息数据包分发到MOOS平台中的各传感器进程，并统计传输速率和延时用来验证各模块通信是否能达到实时要求；所述传感器进程包括惯导模拟传感器进程、声传感器模拟进程、其他目标模拟进程和视觉传感器进程；所述惯导模拟传感器进程用于模拟惯导传感器的数据采集，包括模拟AUV位置、速度和姿态；所述声传感器模拟进程用于模拟声传感器数据生成，包括各模拟AUV之间距离和方位；所述其他目标模拟进程用于产生其他模拟AUV的信息，包括三维位置；所述视觉传感器进程用于输出视觉处理信息，包括各模拟AUV之间相对距离和相对姿态。

进一步地，所述pRB_PID进程的控制逻辑为：首先计算模拟AUV的导航航向角与期望航向角的差值；若航向角差值的绝对值小于给定阈值，则沿原方向前进；否则，如航向角差值为正则左转，为负则右转；还需要考虑ROV的深度，计算导航深度与期望深度的差值，若深度差值为正，则下潜，为正则上浮；左转右转的转向速度和上浮下潜的速度均由PID控制算法根据航向角差值和深度差值得到。

进一步地，所述模拟AUV采用遥控船组件；由于遥控船在水面运动，模拟AUV的深度始终为0；遥控船组件的智能终端固定在遥控船上采集传感器信息，并通过WIFI发送给数据通信模块，由运动控制模块发送控制信息给单片机提取出指令数据发送给遥控器；指令数据分为两部分：第一部分为标志位，取值对应遥控船直行、左转、右转；第二部分为根据左转或右转的转向速度得到螺旋桨转速映射值的电压信息；遥控器将指令数据经过DA转换，根据电压信息改变两个控制引脚处的电压，遥控器再发送电机控制指令给遥控船，控制对应的两个螺旋桨转速；如两引脚电压相同，则对应螺旋浆转速相同，遥控船直行；如两引脚电压不同，电压较高的引脚对应的螺旋桨转速也较高，遥控船向转速较低的一边转向。

进一步地，所述遥控船组件的智能终端连接USB摄像头后拍摄水下图像，并通过WIFI传输给上位机；所述ROV连接水下声呐或水下摄像头后采集声呐数据或水下图像，并通过线缆传输给上位机。

进一步地，还包括图形交互模块，一方面是负责显示各个模块的关键数据，包括AUV的实时导航数据、摄像头的图像数据和网络传输数据；另一方面接收交互命令，包括对接任务开始与对接终止命令和对AUV进行手动操控的方向控制命令，均以按钮形式显示。

进一步地，所述对AUV进行手动操控的方向控制命令包括前进、左转、右转、后退和停止；由图形交互模块发送控制信息给单片机提取出指令数据发送给遥控器；其中，指令数据的第一部分标志位为后退时，遥控器两引脚电压取反且相同时，对应螺旋桨则向相反方向转动且转速相同。

进一步地，所述pShare进程通过水声通信模拟模块与MOOSDB进行交互；所述水声通信模拟模块的数据处理包括延迟发送、限制发送数据包大小和随机丢失小量数据包，用于模拟AUV水下通信过程中的时延大、带宽低、易受干扰的问题。

进一步地，所述数据通信模块基于TCP协议定义传输的数据帧格式为：

本发明的有益效果是：

(1)本发明提出了一种多AUV水下协同作业的模拟实验平台，相比现有的仿真和模拟实验平台，本平台具有实验过程简便、调试效率高、设计成本低，扩展性好和可靠性高的特点；

(2)现有技术多是针对单AUV设计的实验平台，本发明针对多AUV模拟实验平台设计的关键问题，设计实现了多AUV的消息通信方案，并利用心跳检测机制保证了可靠性；

(3)本发明实现的AUV软件平台，使用扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法，可融合速率不同的多源传感器信息，解决了传感器测量延迟问题，并且能够自动检测和剔除野值，提高导航精度。

附图说明

图1为单AUV模拟平台的总体框架示意图；其中，(a)是使用水面遥控船模拟AUV的总体框架，(b)为使用ROV模拟AUV的总体框架。

图2为多AUV之间消息通信的原理框图；

图3为一个MOOS应用的具体流程图；

图4为单AUV软件平台的整体框架示意图；

图5为通信进程pComm的流程图；

图6为融合导航进程pFusionNav的流程图；

图7为EKF迭代过程流程图；

图8为多AUV协同作业场景示意图；

图9为水池AUV回坞对接实验的场景图；

图10为一次水池AUV回坞对接实验中AUV的导航轨迹图；其中，(a)为完整轨迹图，(b)为局部放大轨迹图。

具体实施方式

本平台包括软件平台和硬件平台设计。软件平台运行在上位机中，由一组互相通信的模块组成，每个模块负责完成一部分任务，其中核心模块涉及数据通信模块，融合导航模块，路径规划模块以及自主控制模块。硬件平台由遥控船，或是遥控无人航行器(RemoteOperated Vehicle，ROV)来模拟AUV，一方面遥控船通过搭载智能终端设备(比如手机、平板电脑等)携带的传感器模块或者ROV通过其自带传感器模块来采集传感器数据，并将数据上传到通信模块，另一方面上位机将通过特定的指令来操控遥控船和ROV的航行运动。多AUV协同作业时，每个AUV可在同一个上位机中独立运行自己的软件处理平台，也可各自独占一个上位机，AUV之间通过各自的桥接模块互相进行通信。

为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。将按照单AUV总体框架，多AUV协同平台设计，单AUV硬件平台，单AUV软件平台四部分依次展开说明。

(1)单AUV总体框架

单AUV总体框架如图1所示，可以使用遥控船或者遥控无人航行器(RemoteOperated Vehicle，ROV)两种方法来模拟单AUV。

若使用遥控船模拟单AUV，总体框架如图1(a)所示。单AUV模拟实验平台由上位机、遥控船、智能终端、遥控器和单片机组成。遥控船背部固定智能终端设备(注意防水保护)，通过智能终端设备中开发的软件应用采集其携带的GPS，惯性测量单元(Inertialmeasurement unit，IMU)以及电子罗盘传感器数据，并通过WIFI网络将这些传感器数据传给岸上上位机，岸边上位机接收到传感器数据后输入给融合导航模块；融合导航模块中的导航算法根据传感器信息估计出AUV当前状态，包括三维位置、速度和姿态信息，输入给路径规划模块，路径规划模块输出期望航向和速度信息给控制器等模块，控制器模块生成实际的航行控制指令信息；这些指令信息通过串口通信方式输出给单片机，单片机程序处理后输出电压信息给遥控器，遥控器通过无线电将航行控制指令发送给遥控船的推进器，进而操控遥控船的水面航行，最终模拟出AUV的整个自主航行。

若使用ROV模拟AUV，总体框架如图1(b)所示。单AUV模拟实验平台由上位机和ROV两部分组成。ROV的上位机软件平台的处理过程与遥控船基本类似，不同的是ROV与上位机通过线缆连接，ROV中处理机采集到ROV所携带的各传感器信息，包括GPS、IMU、电子罗盘以及深度计，通过线缆传输给上位机。ROV的上位机也是通过线缆将航行控制指令发送给ROV的推进器，进而操控ROV的水面/水下航行，模拟AUV的整个自主航行过程。

此外，遥控船和ROV均添加了面向任务的扩展模块。固定在遥控船上的智能终端设备可以通过USB线连接USB水下摄像头或水下声呐，并将水下摄像头拍摄的图像数据或水下声呐接收的水声数据通过WIFI传输给上位机。ROV具有扩展接口，可以连接水下声呐或者水下摄像头，并通过线缆将水声数据或图像数据传输给上位机。

(2)多AUV协同平台

扩展多个遥控船或多个ROV，将单AUV模拟平台的实现框架扩展至多AUV协同平台，并实现各单AUV模拟平台的消息通信机制。每个单AUV模拟平台在上位机中搭载一个MOOS(Mission Orientated Operating Suite)平台，多个单AUV模拟平台可单独配置自己的上位机或共用同一台上位机。本发明利用MOOS应用进行各AUV的MOOS平台之间通信的方法，并且在MOOS平台中加入了水声通信模拟模块，用来模拟AUV实际水下通信的过程，，其原理框图如图2所示。

图2显示了三台模拟AUV(AUV_1、AUV_2、AUV_3)之间消息通信的原理框图，更多台模拟AUV之间的通信与之类似。如图2所示，每台AUV上位机MOOS平台中心为MOOSDB，MOOSDB与pComm通信系统互相通信，接收遥控船或ROV的传感器信息。MOOSDB将传感器信息传给上位机中核心处理模块，包括融合导航模块、规划决策模块、运动控制模块、图形交互模块等。然后再将处理后的信息(位置、速度、姿态等)，通过水声模拟模块处理后，经过pShare进程完成各AUV的MOOS平台之间的信息交互。水声模拟模块通过延迟发送、限制发送数据包大小、随机丢失小量数据包等方法来真正模拟AUV水下通信过程中的时延大、带宽低、易受干扰等问题。图2中的pShare进程为各AUV的MOOS平台之间的通信模块，该进程利用了MOOS本身的通信模式，其工作分为两部分：一方面pShare进程与MOOSDB连接，获取同一个MOOS平台中其它进程的消息；另一方面所有pShare进程两两连接，互相发送消息，从而间接实现了不同MOOS平台之间的消息通信。各MOOS平台之间的pShare进程通信机制采用上位机通信模块中pComm进程的通信原理，采用IO多路复用机制完成多个pShare进程的通信，这里每个pShare进程的地位都是平等的，同时发送和接收消息。

pShare还需要将一条特定的消息发送到目的进程中，即路由过程；这里采用的配置文件的方法，将感兴趣的消息配置为一条路由记录，格式为<消息方向><源消息名><目的消息名><目的IP>；这样pShare便可在初始化时读取路由记录获取每条感兴趣消息的路由信息，发送到指定的MOOSDB中，目的进程也可根据消息名从MOOSDB中获取指定的消息。

(3)单AUV硬件部分

考虑到实际AUV的尺寸较大，成本较高且调试费时费力等缺点，本平台使用遥控船搭载智能终端或ROV来实现AUV的自主航行任务。使用ROV模拟AUV在硬件上基本不需要进行改造，而使用遥控船还需进行以下两点改造：

第一，模拟AUV的自主航行功能，即通过上位机发送指令，遥控船自主航行完成任务。这主要通过一个单片机(型号为STM32F103VET6)实现，单片机程序负责与上位机的通信过程，接收到上位机发送的串口数据后，触发中断，进入中断处理程序；经过成帧、解析、校验等步骤，提取出指令数据内容。指令数据分为两部分：第一部分为标志位，取值对应遥控船直行、左转、右转和后退等操作；第二部分为螺旋桨转速映射值，这些映射值经过DA转换改变遥控器两个控制引脚处的电压，进而操控对应两个螺旋桨的转速，实现对遥控船的控制，具体的控制策略为：两引脚电压相同，则对应螺旋浆转速相同，直行；两引脚电压不同，引脚电压较高的对应的螺旋桨转速也较高，往转速较低的方向转向；假设左边引脚电压高，则左螺旋浆转速高于右螺旋桨，右转；左转类似；两引脚电压取反且相同时，对应螺旋桨则向相反方向转动且转速相同，后退。

第二，AUV进行自主运动需要实时获取自身状态信息，包括位置，速度和姿态角等，为此在遥控船上绑上一个智能终端设备(比如手机、平板电脑等)，利用智能终端内置的传感器来获取一些测量数据，这些数据经过WIFI传输到上位机软件平台，进一步处理即可获取遥控船的导航信息。

(4)单AUV软件平台

单AUV软件平台是基于开源软件MOOS进行二次开发完成的，搭载在上位机中；MOOS最初是由Paul Newman于2001年在美国麻省理工学院做博士后时编写的软件，广泛应用于自主机器人特别是自主水下航行器领域。从拓扑结构看，软件平台中所有应用的信息交互以MOOS数据库(MOOS Database，MOOSDB)为中心，呈现出星型拓扑结构。用户编写的应用与MOOSDB之间的通信是通过发布订阅(Publish-Subscribe)模式来完成的，一个应用的具体流程如图3所示：应用启动后，该应用首先与MOOSDB连接，然后通过OnStartUp()函数完成应用初始化的任务。此时，该应用创建了一个子线程用于该应用与MOOSDB通信，该子线程有两个工作：一为从MOOSDB中取出该应用订阅的消息，并存放在收件箱InBox中；二为从发件箱OutBox中取出该应用发布的消息到MOOSDB中，如果取出的消息中有应用退出信号，则整个应用运行结束。除了子线程外，该应用的主线程用于该应用的数据处理，主要包括两个核心函数:OnNewMail()函数和Iterate()函数。OnNewMail()函数实现从收件箱InBox取出消息，并做数据解析处理。Iterate()函数为该应用核心功能，处理从MOOSDB接收到的消息，并将处理过的消息发送给发件箱OutBox。单AUV的软件平台是由上位机中运行的一组相互通信的应用组成，每个应用都由接收消息、主循环完成运算、发送消息的重复迭代组成，迭代的频率由Apptick时钟决定。

单AUV的软件平台整体框架设计如图4所示，整个框架按照功能划分为数据通信、融合导航、规划决策和运动控制与图形交互这五个模块。数据通信模块为pComm进程，该进程用于上位机与遥控船绑定的智能终端或者ROV处理机通信；融合导航模块为pFusionNav进程；规划决策模块包括pPathPlan进程和pHelmIvP进程以及预先定义的任务文件；运动控制模块为pRB_PID进程；图形交互模块为pVisonGUI进程；图中还有pINS、pUSBL、pMobileObject、pVision四个进程，这四个进程为传感器进程，pComm进程采集的传感器信息经过传感器进程后到达融合导航模块；其中惯导模拟传感器进程pINS，用于模拟惯导传感器的数据(模拟AUV位置、速度、姿态)采集；声传感器模拟进程pUSBL，用于模拟声传感器数据(各模拟AUV之间距离、方位)生成；其他目标模拟进程pMobileObject，用于产生其他模拟AUV的信息(三维位置)；视觉传感器进程pVision，用于输出视觉处理信息(各模拟AUV之间相对距离，相对姿态)，下面将按照数据流动的方向依次介绍每个模块的功能，重点是数据通信与融合导航模块。

(4.1)数据通信模块

数据通信模块中的pComm进程负责与遥控船上绑定的智能终端或者ROV处理机进行通信，采集各类传感器原始数据并将数据分发给各类传感器进程。根据多AUV协同作业的需求，pComm进程必须与多个智能终端或者ROV处理机建立连接，以便采集每个模拟AUV数据进行分析，并实时监测连接活性。智能终端和ROV处理机传输的数据包括角速度、加速度、旋转矩阵、GPS定位数据、姿态角和心跳检测数据。数据通信模块基于TCP协议完成，定义传输的数据帧格式如表1所示。

表1：数据通信模块基于TCP协议定义传输的数据帧格式

考虑到导航模块对数据速率的要求，pComm进程与智能终端中开发的软件应用或者ROV处理机以一定频率进行数据通信；智能终端或者ROV处理机中的传感器数据以较高频率上传，而pComm进程以较低频率采集传感器数据，并做时间对齐，分发到各传感器进程。

pComm进程的主要流程如图5所示，该进程在完成初始化后，共运行三个线程，包括一个主线程和两个子线程；其中，主线程用于获取数据和MOOSDB通信；一个子线程用于启动该进程与MOOSDB通信；另一个子线程用于pComm与智能终端通信，该线程使用基于事件驱动的支持IO多路复用技术的网络通信服务器，同时支持多个客户端连接。pComm与智能终端通信的线程在初始化后等待事件发生，每种事件设置不同的回调函数处理，这些回调函数是用户处理的核心；共有以下四种事件：连接事件用来处理新客户端的网络连接，并为其注册读写事件；读事件用来读取智能终端或ROV处理机的传感器数据包，按表1定义的数据帧格式数据包解码，提取出原始数据；写事件用来发送命令信息，将其编码成帧后发送给智能终端或ROV处理机；定时事件用来检测连接活性，每隔一段时间(默认为2s)，智能终端或ROV处理机定时发送心跳包到上位机中pComm通信进程，在定时器中检查最近接收的心跳包是否超时来判断连接的有效性。若连接失效，智能终端开发的软件应用或ROV处理机会自动检测到这种情况，尝试一定次数的重新连接，而pComm进程也会自动清理失效连接有关的数据，为新连接创建所需资源，这样就保证了通信的可靠性。此外，pComm进程还完成了相关数据包分发到上位机MOOS平台中的各传感器进程，并统计传输速率和延时用来验证上位机与模拟AUV之间通信是否能达到实时要求。

(4.2)融合导航模块

融合导航模块主要负责加工各类传感器数据，对模拟AUV状态进行实时估计。各个传感器进程会根据需要进行数据预处理，包括时间配准和空间配准，生成相应的传感器测量数据，这些传感器包括惯性导航系统、USBL传感器以及视觉引导系统等。融合导航进程pFusionNav负责接收各个传感器进程的实时测量数据，并实现相应的导航滤波算法，得到AUV状态的估计值，供规划决策模块使用。如图6所示，pFusionNav进程启动后，先进行初始化工作，并创建一个子线程用于与MOOSDB通信，主线程用于实现融合导航模块核心功能。其中，OnNewMail()函数收取各传感器的测量信息，整理后作为当前的测量向量；Iterate()函数执行EKF算法的迭代过程，并且导航进程还实现了对测量信息的野值检测，判断标准为：若测量值的新息协方差超过阈值，则直接舍弃当前测量值。同时，如果Iterate()函数产生了进程退出信号，则该进程运行结束。

其中，导航算法采用EKF算法；EKF是一种典型的非线性滤波方法，采用函数泰勒展开并略去二阶及更高阶项的方式去近似非线性函数，将非线性问题线性化，然后再使用标准的卡尔曼滤波进行状态估计；EKF算法优点是形式简单，在非线性不强的系统中估计性能较好。EKF算法的状态输入量主要包括AUV自身的绝对位置，姿态和速度以及AUV与移动目标的相对位置和相对姿态。如图7所示，EKF的一次递推过如下：

(4.2.1)时间更新

P_k|k-1＝F_k-1P_k-1F'_k-1+Q_k-1

式中，

为上一时刻状态的估计值，

为当前时刻状态的预测值，f(·)为非线性状态转移函数，P_k|k-1为当前时刻预测状态协方差矩阵，F_k-1为过程雅克比矩阵，P_k-1为上一时刻状态估计的协方差矩阵，F'_k-1为过程雅克比矩阵的转置，Q_k-1为状态转移函数的噪声方差矩阵(4.2.2)测量更新

K_k＝P_k|k-1H'_k(H_kP_k|k-1H'_k+R_k)^-1

P_k＝(I-K_kH_k)P_k|k-1

式中，

为测量预测值，h(·)为非线性函数；K_k为卡尔曼增益，H_k为测量雅克比矩阵，H'_k为测量雅克比矩阵的转置，z_k为测量向量，P_k为后验误差协方差矩阵，R_k为测量方程噪声方差矩阵，

为当前时刻状态估计值，I为单位矩阵。。

(4.3)规划决策模块

规划决策模块负责分析AUV当前执行的任务，采用一定的路径规划算法，并结合一些优化方法(如蚁群算法、遗传算法等)给出AUV下一时刻的期望值，包括航向角，速度和深度信息。

pPathPlan进程主要负责路径规划过程，假设一个AUV需要执行回坞对接任务，该进程将这个任务分成多个阶段完成，在每个阶段pPathPlan会根据当前的AUV及目标状态，规划出基本的定点航行任务，定向航行任务或是其它的自定义任务，直到AUV满足各阶段任务完成的要求(比如定点航行任务达到目标点的要求)会进入下一阶段，最终完成整个回坞对接任务。

pHelmIvp进程根据当前任务给出决策信息，首先它读取预先写好的任务配置文件设初始化和参数设置，比如AUV的起点设置。随后它将接收到pPathPlan进程发送的任务信息，对于定点航行任务是连续的定位点，对于定向任务是目标航向角，若是自定义任务，也需按照一定规则编写处理逻辑即可。经过对多个任务的优化计算，该进程会最终给出AUV下一时刻的期望航向角，速度和深度。

(4.4)运动控制模块

运动控制模块负责接收AUV下一时刻的期望值。其中，若使用遥控船模拟AUV，AUV接收到的下一时刻期望值经过pRB_PID进程中的控制算法转化为控制信息，再由pUART串口进程将控制信息封装成数据帧，发送给遥控器，pUART进程负责串口通信，将控制信息组装成帧，帧格式类似于数据通信模块给出的表1数据帧格式，包括8位大小的帧头，用于识别帧信息，16位大小的数据，以及8位大小的检验码，用于检验帧数据的正确性。这些数据经过串口进入单片机后，会触发中断，在中断处理程序中进行解析数据，改变遥控器两个控制引脚处的电压，然后遥控器通过无线电将电机控制指令发送给遥控船，从而实现对遥控船的自主控制。

pRB_PID进程根据当前的期望值结合PID(Proportional IntegralDifferential，比例积分微分)控制算法进行控制。若使用遥控船模拟AUV，此时控制比较简单，不需要考虑深度，只需给出两个螺旋桨的转速信息即可；若使用ROV模拟AUV，pRB_PID控制进程还需要考虑深度信息，具体的控制逻辑与遥控船类似。

使用遥控船时pRB_PID进程的控制逻辑为：

(4.4.1)计算导航航向角与期望航向角的差值；

(4.4.2)若航向角差值的绝对值小于给定阈值(本实施例为1)，则前进；否则，如差值为正则左转，为负则右转；

(4.4.3)遥控船左右转的转向速度由PID算法给出。

使用ROV时pRB_PID的控制逻辑为：

(4.4.1)计算导航航向角与期望航向角的差值，计算导航深度与期望深度的差值；

(4.4.2)若航向角差值的绝对值小于给定阈值(本实施例为1)，则前进；否则，如差值为正则左转，为负则右转；若深度差值为正，则下潜，为正则上浮。

(4.4.3)ROV左右转转向速度以及上浮下潜的速度由PID算法给出。

(4.5)图形交互模块

图形交互模块pVisionGUI进程主要有两个作用：一方面是负责显示各个模块的关键数据，展示给用户，如AUV的实时导航数据(位置、速度和姿态)，摄像头的图像数据，网络传输数据(网络连接情况、传输速率等)等；另一方面接收用户下达的交互命令，如对接任务开始与对接终止命令；以及对AUV进行手动操控的方向控制命令，如前进、左转、右转、后退和停止命令；这些以按钮形式显示；由图形交互模块发送控制信息给单片机提取出指令数据发送给遥控器；其中，指令数据的第一部分标志位为后退时，遥控器两引脚电压取反且相同时，对应螺旋桨则向相反方向转动且转速相同。

综上所述，就是多AUV水下协同的模拟实验平台设计完整过程。下面以水池AUV回坞对接任务为场景，介绍本平台的具体实验应用。图9为实验场景，其中AUV(使用遥控船模拟)下边固定一个摄像机用于捕获图像，作用距离5～10米。对接目标下边固定一个灯架，十字形对称布放四个LED灯源。图中虚线为移动目标的航行轨迹，实线为AUV的航行轨迹。另外，在岸边还有两台计算机，分别运行AUV与移动目标的软件平台。移动目标的软件较为简单，只需要实时导航定位其位置，航向等信息，然后做定向航行。而AUV则需要定期获取对接目标的相对位置和相对航向角，并结合视觉导引提高精度，整个对接过程AUV的轨迹通过路径规划进程计算得到。图10展示了整个对接过程AUV的导航轨迹，图中三角为AUV的起点，圆圈为AUV的终点。十字线为惯性导航系统的输出轨迹，点线为EKF算法的估计轨迹，可以看出EKF算法具有较高的估计精度和可靠性。

Claims

1.一种多AUV水下协同作业的模拟实验平台，其特征在于，是由多个互相通信的单AUV模拟平台扩展得到的多AUV协同平台；

所述单AUV模拟平台由上位机、模拟AUV等组成。

所述模拟AUV为遥控船组件或ROV；所述遥控船组件包括遥控船、遥控器、单片机和智能终端；所述ROV包括运行机构、处理机和传感器模块。

所述智能终端用于采集传感器信息，包括角速度、加速度、旋转矩阵、GPS定位数据和姿态角。

所述ROV的传感器模块用于采集传感器信息，包括角速度、加速度、旋转矩阵、GPS定位数据、姿态角和深度。

每个单AUV模拟平台在上位机上搭载一个MOOS平台，由数据通信模块、融合导航模块、规划决策模块和运动控制模块组成；一方面每个MOOS平台的pShare进程连接MOOSDB，获取该MOOS平台中其它进程的消息；另一方面不同MOOS平台通过pShare进程两两连接互相通信。

所述数据通信模块用于通过pComm进程分别与各单AUV模拟平台的智能终端或处理机通信，获取传感器信息并通过MOOSDB发送给融合导航模块，以及实时监测连接活性。

所述融合导航模块用于通过导航滤波算法处理传感器信息，得到模拟AUV当前状态估计值并发送给规划决策模块。

所述规划决策模块用于根据各模拟AUV当前状态估计值，通过路径规划算法结合优化方法，得到各模拟AUV下一时刻的状态期望值并发送给运动控制模块。

所述pUART进程用于串口通信，将控制信息组装成帧。

所述模拟AUV的状态信息包括三维位置、速度、姿态角。

2.如权利要求1所述多AUV水下协同作业的模拟实验平台，其特征在于，所述pComm进程采用IO多路复用机制，包括一个主线程和两个子线程；其中，主线程用于获取数据和MOOSDB通信；一个子线程用于启动进程与MOOSDB通信；另一个子线程用于pComm与客户端通信；所述客户端为智能终端或ROV处理机；

3.如权利要求2所述多AUV水下协同作业的模拟实验平台，其特征在于，所述pComm进程还将传感器信息数据包分发到MOOS平台中的各传感器进程，并统计传输速率和延时用来验证各模块通信是否能达到实时要求；所述传感器进程包括惯导模拟传感器进程、声传感器模拟进程、其他目标模拟进程和视觉传感器进程；所述惯导模拟传感器进程用于模拟惯导传感器的数据采集，包括模拟AUV位置、速度和姿态；所述声传感器模拟进程用于模拟声传感器数据生成，包括各模拟AUV之间距离和方位；所述其他目标模拟进程用于产生其他模拟AUV的信息，包括三维位置；所述视觉传感器进程用于输出视觉处理信息，包括各模拟AUV之间相对距离和相对姿态。

4.如权利要求1所述多AUV水下协同作业的模拟实验平台，其特征在于，所述pRB_PID进程的控制逻辑为：首先计算模拟AUV的导航航向角与期望航向角的差值；若航向角差值的绝对值小于给定阈值，则沿原方向前进；否则，如航向角差值为正则左转，为负则右转；还需要考虑ROV的深度，计算导航深度与期望深度的差值，若深度差值为正，则下潜，为正则上浮；左转右转的转向速度和上浮下潜的速度均由PID控制算法根据航向角差值和深度差值得到。

5.如权利要求4所述多AUV水下协同作业的模拟实验平台，其特征在于，所述模拟AUV采用遥控船组件；由于遥控船在水面运动，模拟AUV的深度始终为0；遥控船组件的智能终端固定在遥控船上采集传感器信息，并通过WIFI发送给数据通信模块，由运动控制模块发送控制信息给单片机提取出指令数据发送给遥控器；指令数据分为两部分：第一部分为标志位，取值对应遥控船直行、左转、右转；第二部分为根据左转或右转的转向速度得到螺旋桨转速映射值的电压信息；遥控器将指令数据经过DA转换，根据电压信息改变两个控制引脚处的电压，遥控器再发送电机控制指令给遥控船，控制对应的两个螺旋桨转速；如两引脚电压相同，则对应螺旋浆转速相同，遥控船直行；如两引脚电压不同，电压较高的引脚对应的螺旋桨转速也较高，遥控船向转速较低的一边转向。

6.如权利要求5所述多AUV水下协同作业的模拟实验平台，其特征在于，所述遥控船组件的智能终端连接USB摄像头后拍摄水下图像，并通过WIFI传输给上位机；所述ROV连接水下声呐或水下摄像头后采集声呐数据或水下图像，并通过线缆传输给上位机。

7.如权利要求6所述多AUV水下协同作业的模拟实验平台，其特征在于，还包括图形交互模块，一方面是负责显示各个模块的关键数据，包括AUV的实时导航数据、摄像头的图像数据和网络传输数据；另一方面接收交互命令，包括对接任务开始与对接终止命令和对AUV进行手动操控的方向控制命令，均以按钮形式显示。

8.如权利要求7所述多AUV水下协同作业的模拟实验平台，其特征在于，所述对AUV进行手动操控的方向控制命令包括前进、左转、右转、后退和停止；由图形交互模块发送控制信息给单片机提取出指令数据发送给遥控器；其中，指令数据的第一部分标志位为后退时，遥控器两引脚电压取反且相同时，对应螺旋桨则向相反方向转动且转速相同。

9.如权利要求1所述多AUV水下协同作业的模拟实验平台，其特征在于，所述pShare进程通过水声通信模拟模块与MOOSDB进行交互；所述水声通信模拟模块的数据处理包括延迟发送、限制发送数据包大小和随机丢失小量数据包，用于模拟AUV水下通信过程中的时延大、带宽低、易受干扰的问题。

10.如权利要求1所述多AUV水下协同作业的模拟实验平台，其特征在于，所述数据通信模块基于TCP协议定义传输的数据帧格式为：