CN112748686B - 一种水下攀爬机器人控制系统及故障恢复方法 - Google Patents

Abstract

本发明涉及一种水下攀爬机器人控制系统及故障恢复方法，属于智能机器人领域，包括硬件部分和软件部分，硬件部分包括主控板、PC机，软件部分包括系统状态监控层、硬件接口层、控制算法层、数据中心层和主控层，硬件接口层用于与接入的硬件设备进行数据交互；控制算法层用于对交互数据进行处理分析，提供SLAM导航功能；数据中心层用于对所有数据信息及日志文件进行存储和分析；主控层用于对各层间的数据交互、运行时间以及所述硬件部分进行调度和资源分配；系统状态监控层用于监测水下攀爬机器人的外部环境、状态和故障，并自主诊断和修复，提升兼容性的同时，使水下攀爬机器人具备故障诊断与恢复的能力，解决了故障后不能自主诊断、修复的问题。

Description

一种水下攀爬机器人控制系统及故障恢复方法

技术领域

本发明涉及智能机器人技术领域，特别是涉及一种水下攀爬机器人控制系统及故障恢复方法。

背景技术

攀爬机器人在当代人类社会生产活动扮演者重要的角色，可用于大型设施表面检测、表面清理、表面涂覆等作业，还可用于帮助完成重大设施的故障诊断、故障预警、状态维护等，是代替人类在危险、复杂、狭窄等环境中完成一定作业任务的重要工具。

攀爬机器人从吸附原理上分为生物粘附式、手爪式、电磁吸附式，从移动机构上可分为足式、轮式，从工作环境上可分为陆地攀爬机器人、水下攀爬机器人，其中，水下攀爬机器人多用于水下救援、水下设施检修以及水下探索等。

但现有的水下攀爬机器人大多存在以下问题：(1)不具有故障诊断与恢复能力，当机器人在水下作业时遇到故障不能进行自主诊断和修复，经常“有去无回”；(2)硬件兼容性差，不利于自定义添加硬件设备，严重阻碍了控制系统的通用性。

针对上述问题，本文提出了一种水下攀爬机器人控制系统及故障恢复方法。

发明内容

本发明的目的是提出一种水下攀爬机器人控制系统及故障恢复方法，用于提高水下攀爬机器人的系统兼容性和通用性，并使攀爬机器人具备故障诊断与恢复的能力，提升水下攀爬机器人对严酷的水下环境的适应能力，解决现有技术中水下攀爬机器人硬件兼容性差、不具备故障诊断和自主修复能力的问题。

为实现上述目的，本发明提供了如下方案：

一种水下攀爬机器人控制系统，包括：硬件部分和软件部分，

所述硬件部分包括主控板、PC机，所述主控板为具有多种接口形式的集成开发板，所述主控板和所述PC机采用局域网无线连接；

所述软件部分包括系统状态监控层、硬件接口层、控制算法层、数据中心层和主控层；

所述数据中心层分别与所述系统状态监控层、所述硬件接口层、所述控制算法层和所述主控层互联；

所述系统状态监控层用于对所述水下攀爬机器人运行时的外部环境、运行状态和故障情况进行实时监测，将监测数据传输至所述数据中心层，并在所述水下攀爬机器人发生故障后，对故障进行自主诊断和修复；

所述硬件接口层用于与接入所述控制系统的硬件设备进行数据交互，并将交互数据传输至所述数据中心层；

所述控制算法层用于对所述交互数据进行处理分析，为所述水下攀爬机器人运行时提供SLAM导航功能；

所述数据中心层用于对所述水下攀爬机器人运行时产生的所有数据信息及日志文件进行存储和分析；

所述主控层用于对其他各层间的数据交互、运行时间以及所述硬件部分进行调度和资源分配，为各层间的数据交互指定通信协议。

可选的，所述硬件接口层包括硬件设备驱动模块、信息采集设备接口模块、驱动器设备接口模块、通信设备接口模块、输入设备接口模块；

所述硬件设备驱动模块用于驱动所述PC机和外接设备进行数据交互；所述外接设备通过所述主控板上的接口接入所述控制系统，所述外接设备包括多种监测所述水下攀爬机器人的实时数据的传感器；

所述驱动器设备接口模块用于驱动所述主控板与电机驱动器进行数据交互；

所述信息采集设备接口模块用于采集交互数据，对所述交互数据进行优化处理，将所述交互数据划分为状态信息和参数信息，并将所述状态信息和所述参数信息通过所述数据中心层的数据管理模块传输至所述系统状态监控层；

所述通信设备接口模块用于为所述PC机与所述水下攀爬机器人建立无线通讯；

所述输入设备接口模块用于接收不同类别的输入设备发出的控制信号，经所述PC机处理后，通过所述通信设备接口模块将处理后的控制信号传送给所述主控板，实现对所述水下攀爬机器人运动的控制；所述不同类别的输入设备通过操控手柄、键盘、语音、系统预置指令或手势的方式进行控制。

可选的，所述系统状态监控层包括状态监测模块、故障监测模块和数据传输模块；

所述数据传输模块用于接收所述硬件接口层的信息采集设备接口模块传输的状态信息和参数信息，并建立数据传输通道，通过所述硬件接口层的通信设备接口模块进行上传以及下载数据，包括所述状态信息、所述参数信息和所述水下攀爬机器人的控制指令信息，同时管理所述状态监测模块与所述故障监测模块的数据传输；

所述状态监测模块用于实时监测所述状态信息和所述参数信息，并将所述状态信息和所述参数信息实时显示在所述PC机的QT界面上；

所述故障监测模块用于根据所述状态信息，判断所述水下攀爬机器人在水下攀爬过程中是否发生故障，并在发生故障后进行故障诊断和自主修复。

可选的，所述故障监测模块根据所述状态信息，判断所述水下攀爬机器人在水下攀爬过程中是否发生故障，并在发生故障后进行故障诊断和自主修复，具体包括：

所述故障监测模块通过所述数据中心层的日志模块读取存储的日志文件；

从所述日志文件的索引文件中，确定一个最近的稳定点并记录所述稳定点的地址；

确定所述日志文件中，与所述地址相对应的一条最近的稳定点记录；

根据所述稳定点记录，读取所述水下攀爬机器人此时的所述参数信息，所述参数信息包括位置信息和方向信息；

根据所述位置信息和所述方向信息，对所述水下攀爬机器人进行复位；

根据所述稳定点记录，确定建立所述稳定点时所有正执行的事务队列，将所述正执行的事务队列暂时放入撤销队列，并置空重做队列；

从所述稳定点开始，对所述日志文件进行正向扫描，直到所述日志文件被扫描结束；

对所述撤销队列中的每个事务执行撤销操作，并对所述重做队列中的每个事务执行重做操作。

可选的，所述控制算法层包括信息融合模块和SLAM模块；

所述信息融合模块用于采用卡尔曼滤波算法或扩展卡尔曼滤波算法，对所述硬件接口层的信息采集设备接口模块优化处理后的数据进行信息融合，估算出所述水下攀爬机器人的状态信息；

所述SLAM模块用于利用开源ORB_SLAM算法，根据所述系统状态监控层采集的外部环境数据，生成水下三维地图并实时更新，对所述水下攀爬机器人进行定位导航。

可选的，所述数据中心层包括数据管理模块、参数动态配置模块、数据分析模块、日志模块；

所述数据管理模块用于存储、管理所述水下攀爬机器人运行时的各项数据，接收所述硬件接口层的信息采集设备接口模块采集的数据、所述控制算法层的SLAM模块传输的数据，并将所有数据发送给所述参数动态配置模块、所述数据分析模块、所述日志模块、所述控制算法层的信息融合模块以及所述系统状态监控层的数据传输模块；

所述参数动态配置模块用于对所述控制系统中可动态修改的参数进行调节、配置，所述可动态修改的参数指因采集的监测数据的格式发生变化，且需要重新配置模块间相互耦合的数据；

所述日志模块用于将所述水下攀爬机器人运行时的各项数据生成日志文件，记录所述水下攀爬机器人的参数信息和状态信息；

所述数据分析模块用于对所述参数信息和所述状态信息进行处理分析，获得所述水下攀爬机器人的运动特征，并采用强化学习的方式对所述运动特征进行强化改进，改善所述水下攀爬机器人在水下攀爬过程中的问题。

可选的，所述数据中心层采用异步通信的方式分别与所述系统主控层、所述系统状态监控层、所述硬件接口层和所述控制算法层进行互联通信。

可选的，所述异步通信方式为Topic通信，所述Topic通信在生成信息、传输信息和使用信息时对各层间进行双向解耦。

可选的，所述硬件部分还包括硬件配置模块、通讯接口模块和I/O接口模块，所述硬件配置模块、所述通讯接口模块和所述I/O接口模块集成安装于所述主控板上。

本发明还提出了一种水下攀爬机器人控制系统的故障恢复方法，具体包括：

读取数据中心层的日志模块中存储的日志文件；

从所述日志文件的索引文件中，确定一个最近的稳定点并记录所述稳定点的地址；

确定所述日志文件中，与所述地址相对应的一条最近的稳定点记录；

根据所述稳定点记录，读取所述水下攀爬机器人此时的参数信息，所述参数信息包括位置信息和方向信息；

根据所述位置信息和所述方向信息，对所述水下攀爬机器人进行复位；

根据所述稳定点记录，确定建立所述稳定点时所有正执行的事务队列，将所述正执行的事务队列暂时放入撤销队列，并置空重做队列；

从所述稳定点开始，对所述日志文件进行正向扫描，直到所述日志文件被扫描结束；

对所述撤销队列中的每个事务执行撤销操作，并对所述重做队列中的每个事务执行重做操作。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

1.本发明通过故障监测模块根据采集的水下攀爬机器人的状态信息和参数信息，实时监测、反馈机器人在水下攀爬过程中的故障状态，当发生故障时，利用恢复方法对故障进行自主诊断和修复，从而使水下攀爬机器人具备了故障诊断与恢复的能力，提升水下攀爬机器人对严酷的水下环境的适应能力，解决了现有技术中水下攀爬机器人在水下发生故障后不能自主修复的问题。

若在故障不可恢复时尝试对PC机发出警报，若机器人的状态可修复则依据内置的控制方法尝试进行自主修复，同时根据日志模块中记录的状态信息尝试对机器人进行状态恢复，通过读取日志文件中最近稳定点的状态，让机器人继续执行未完成的任务；若机器人在水下攀爬过程中发生碰撞、跌落等故障，导致失去对自身位置的定位时，通过控制算法层的SLAM模块控制机器人完成自主返航，从而解决了攀爬机器人在水下作业时发生故障不能进行自主诊断和修复的难题。

2.本发明软件部分的主控层采用多层可配置的形式，包含与用户进行交互的系统状态监控层、与水下攀爬机器人进行交互的硬件接口层、执行控制算法的控制算法层、完成数据配置、组织、存储与分析的数据中心层，所述主控层分别与系统状态监控层、硬件接口层、控制算法层和数据中心层进行相互通信，数据中心层分别与其他各层互联，各层以及各层中的模块采用松耦合的设计方式，可以更容易对各模块进行修改以及替换，使控制系统整体具有开放性和通用性。

3.本发明系统的硬件部分，下位机即主控板设有多种通讯接口、I/O接口，可以扩展连接更多的硬件设备，通过40PIN GPIO引脚可以增加扩展板采集多路数字信号以及模拟信号，支持RS232串行通信接口、扩展CANOpen通讯协议、EtherCAT通讯协议，支持基于ARM的32&64位Linux操作系统，如Ubuntu、Fedora以及Raspbian，可在控制板上移植安装完整的ROS操作系统，明显提高了系统的硬件兼容性，进而提升了系统的通用性。

4.本发明软件部分的硬件接口层用于和不同类别的设备进行数据交互，并向下发送主控层的控制指令，可以添加用户自定的硬件，对系统功能进行扩展，并且新添加的硬件设备仅需满足所发布的消息类型，符合控制系统的要求即可进行数据交互，进一步增强了系统的开放性与兼容性。通过硬件接口层的硬件设备驱动模块围绕设备和设备类的概念进行构建，设备由设备服务器构建，且具有唯一的标识名称，利用ROS操作系统的特性，使得各模块支持C++和Python作为编程语言，进一步提升了系统的兼容性。同时该模块可内置/外接一些机器人在完成水下攀爬动作时常用的硬件设备，包含IMU惯性测量单元、电机编码器单元、电量测量单元、水深传感单元、摄像单元以及用户自定义硬件，通过硬件设备驱动模块可以更方便地将硬件设备接入ROS操作系统进行驱动，明显增强了控制系统的开放性与兼容性。

5.本发明系统的软件部分，采用多层可配置的形式对控制系统分层解耦，增强系统的开放性和兼容性；系统状态监控层使用QT开发平台编写跨平台系统监控软件，适用于Windows、Linux以及MacOS操作系统；实时接收硬件接口层反馈的传感器数据，以3D图形可视化的方式展示机器人在水下攀爬过程中的状态信息以及在3D地图中的位置信息，提供给用户进行查看，可交互性更强、易于操作，降低用户使用门槛。同时该QT终端由硬件接口层兼容多种形式的输入设备，如遥控手柄、鼠标、键盘、语音、手势，可以让用户对机器人进行操作，进一步降低机器人的操作成本，增加操作方式的灵活性，更加方便、快捷。

6.本发明软件部分的控制算法层用于处理机器人的状态数据，信息融合模块采用卡尔曼滤波算法或扩展的卡尔曼滤波算法进行信息融合，包含多达15个变量信息，涵盖实际情况中的多数变量，并且可以不限制传感器的类型和数量，只要传感器发送的数据信息格式符合模块要求即可，进一步提升了系统的硬件兼容性。

7.本发明软件部分的数据中心层用于存储、分析机器人在运行过程中产生的所有数据信息及日志文件，参数动态配置模块通过参数服务器维护一个字符串到各类数据的键值表，在同一网络可访问的数据库中存储系统的各项配置参数；日志模块采用日志的形式对机器人在水下攀爬过程中的状态信息及环境信息进行记录可以很好的收集历史数据，采用数据挖掘以及强化学习的方式对机器人的运动进行分析并辅助决策。通过日志同步的方式，可以有效避免由于意外情况导致的日志文件丢失的情况，如，机器人跌落、进水以及损坏等，日志以任务为单位记录，并结合对日志建立稳定点和索引文件的方式进行存储，可以以此对机器人进行状态恢复，如需要对机器人进行重启等操作时，可以用最近的稳定点的状态和任务进度进行初始化，使机器人继续完成任务，而不是从头开始重新执行任务，从而提升了攀爬机器人的工作效率。

附图说明

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

图1为本发明实施例1提供的硬件部分的整体构造图；

图2为本发明实施例1提供的主控板的结构示意图；

图3为本发明实施例1提供的软件部分的整体构造图；

图4为本发明实施例1提供的硬件接口层的结构示意图；

图5为本发明实施例1提供的系统状态监控层的结构示意图；

图6为本发明实施例1提供的控制算法层的结构示意图；

图7为本发明实施例1提供的数据中心层的结构示意图；

图8为本发明实施例1提供的主控层的结构示意图；

图9为本发明实施例1提供的水下攀爬机器人控制系统的故障恢复方法的流程图。

附图标记说明：

1-硬件部分，11-主控板，12-PC机，13-硬件配置模块，14-通讯接口模块，15-I/O接口模块，2-软件部分，21-硬件接口层，211-硬件设备驱动模块，212-信息采集设备接口模块，213-驱动器设备接口模块，214-通信设备接口模块，215-输入设备接口模块，22-系统状态监控层，221-状态监测模块，222-故障监测模块，223-数据传输模块，23-控制算法层，231-信息融合模块，232-SLAM模块，24-数据中心层，241-数据管理模块，242-参数动态配置模块，243-数据分析模块，244-日志模块，25-主控层。

具体实施方式

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

本发明的目的是提供一种水下攀爬机器人控制系统及故障恢复方法，基于ROS系统设计的控制系统，采用松耦合、多层模块化的结构，使控制系统可以更方便接入自定义硬件和控制算法，提高了各模块的可替换性和可扩展性，从而有效提升了系统的硬件兼容性。当水下攀爬机器人发生故障时，通过故障监测模块根据状态监测模块的数据信息实时反馈机器人在水下攀爬过程中的故障状态，并进行自主判断，利用内置的控制方法尝试进行自主修复，从而使攀爬机器人具备了故障诊断与恢复的能力，提升了攀爬机器人对严酷的水下环境的适应能力。

实施例1

本发明提出了一种水下攀爬机器人控制系统，所述控制系统包括硬件部分1和软件部分2，所述硬件部分1包括主控板11和PC机12，所述主控板11和所述PC机12采用局域网无线连接，所述主控板11为具有多种接口形式的集成开发板。

图1示出了所述控制系统的硬件部分1的整体构造图，从图1中可以直观看出，所述硬件部分1还包括硬件配置模块13、通讯接口模块14和I/O接口模块15，所述硬件配置模块13、所述通讯接口模块14和所述I/O接口模块15集成安装于所述主控板11上，本实施例中，所述主控板11的型号优选树莓派4B的ARM嵌入式开发板，树莓派4B具有兼容性好，可扩展性强的特点。

图2示出了本发明实施例1提供的主控板11的结构示意图，如图2所示，本实施例采用的主控板11中，所述硬件配置模块13包括处理器为64-bit BCM2711 quad-core A72CPU@1.5GHz芯片、支持OpenGL ES 3.x的VideoCore VI graphics显卡、8GB LPDDR4 SDRAM；所述通讯接口模块14包括Full-throughpu Gigabit Ethernet端口、Dual-band 802.11ac无线网络、Bluetooth5.0；所述I/O接口模块15包括支持双4Kp60硬件解码的HEVC显示输出、2×USB3.0端口、2×USB2.0端口、CSI camera接口、DSI display接口、Power-over-Ethernet引脚、40Pin GPIO引脚、USB Type C端口供电，还可以包括MicroSD card插槽。

在本实施例的控制系统中，将所述PC机12作为上位机，将所述主控板11(型号为树莓派4B)作为下位机，在所述PC机12内预装有AMD64位的Ubuntu18.04操作系统以及嵌入式的ROS操作系统，在所述主控板11内预装有嵌入式的Ubuntu18.04操作系统以及嵌入式的ROS操作系统，构成了基于ROS操作系统的硬件部分1。将上位机和下位机通过WiFi接口连接，置于同一局域网下并相互设置对方的IP地址作为ROS_MASTER_URI，即将所述主控板11和所述PC机12采用局域网无线连接，以实现上位机与下位机的操作指令下发和信息反馈。

所述树莓派4B通过RS232通讯接口的扩展板与电机驱动器进行通讯，其中扩展板使用FT4232HL芯片由USB接口扩展多路RS232通讯接口，电机驱动器选用CopleyACJ-055型号。所述树莓派4B与驱动器设备通过发送ASCII码指令进行通讯，保证每条支路的通讯速率均为115200bps的全双工模式；采用ROS中话题(Topic)通信机制，设定信息发送频率为50Hz，在该分布式控制系统中实现数据的循环收发。所述树莓派4B通过40Pin GPIO接口连接IMU惯性测量单元、水深传感器、电量测量单元，其中所述IMU惯性测量单元选用MPU6050型号传感器采集3轴加速度数据以及3轴陀螺仪数据；所述水深传感器采用MS5837-30BA型号传感器，测量分辨率为0.2mbar(2mm)通过I²C接口与树莓派4B进行数据传输；所述电量测量单元选用50A量程宽电压的功率计。

所述PC机12通过USB接口连接2.4GHz射频发射器与PS3游戏手柄进行连接，作为上位机的输入设备操控机器人运动；同时安装有系统状态监控软件用于监控系统状态以及SLAM导航。

本实施例系统的硬件部分1，所述主控板11设有多种通讯接口、I/O接口，可以扩展连接更多的硬件设备，通过40PIN GPIO引脚可以增加扩展板采集多路数字信号以及模拟信号，支持RS232串行通信接口、扩展CANOpen通讯协议、EtherCAT通讯协议，支持基于ARM的32&64位Linux操作系统，如Ubuntu、Fedora以及Raspbian，可在控制板上移植安装完整的ROS操作系统，从而明显提高了系统的硬件兼容性，进而提升了系统的通用性。

需要说明的是，本实施例中上述硬件配置模块13、通讯接口模块14和I/O接口模块15的具体结构，仅仅是优选了一套硬件配置，上述的处理器、显卡、内存以及各种接口均可替换为其他型号的硬件，因此，硬件配置模块13、通讯接口模块14和I/O接口模块15的具体结构并不是唯一的，不应作为对本发明的保护范围的限定，根据具体情况可自行配置。

图3为本发明实施例1提供的软件部分2的整体构造图。从图3中可直观看出，在所述控制系统中，所述软件部分2采用多层可配置的结构形式，由状态监控层、硬件接口层21、控制算法层23、数据中心层24和主控层25组成。所述数据中心层24采用异步通信的方式分别与所述系统状态监控层22、所述硬件接口层21、所述控制算法层23和所述主控层25互通互联。所述异步通信方式为话题(Topic)通信机制(以下简称Topic通信)，所述Topic通信在生成信息、传输信息和使用信息时对各层间进行双向解耦。各层以及各层中的模块采用松耦合的设计方式，可以更容易对各模块进行修改以及替换，从而提升了控制系统整体的开放性和通用性。

其中，所述系统状态监控层22用于对所述水下攀爬机器人运行时的外部环境、运行状态和故障情况进行实时监测，将监测数据传输至所述数据中心层24，并在所述水下攀爬机器人发生故障后，对故障进行自主诊断和修复。所述硬件接口层21用于与接入所述控制系统的硬件设备进行数据交互，并将交互数据传输至所述数据中心层24。所述控制算法层23用于对所述交互数据进行处理分析，为所述水下攀爬机器人运行时提供SLAM导航功能。所述数据中心层24用于对所述水下攀爬机器人运行时产生的所有数据信息及日志文件进行存储和分析。所述主控层25用于对其他各层间的数据交互、运行时间以及所述硬件部分1进行调度和资源分配，为各层间的数据交互指定通信协议。

图4-图8分别为硬件接口层21、系统状态监控层22、控制算法层23、数据中心层24以及主控层25的结构示意图。

如图4所示，所述硬件接口层21包括硬件设备驱动模块211、信息采集设备接口模块212、驱动器设备接口模块213、通信设备接口模块214、输入设备接口模块215。

所述硬件设备驱动模块211用于驱动所述PC机12和外接设备进行数据交互。所述外接设备包括多种监测所述水下攀爬机器人的实时数据的传感器。

所述硬件设备驱动模块211负责上位机和不同类别的设备进行数据交互，围绕设备和设备类的概念进行构建，设备由设备服务器构建，设备服务器是实现一组设备类的进程，设备由唯一的名称进行标识，格式为<domain>/<family>/<member>。举个例子：LAB-01/PowerSupply/01，ID21/OpticsHutch/energy。每个设备都有一系列特征、属性和命令，特征由设备中的名称标识，仅具有可读属性，采用固定的数据类型，如int64、double等均可；属性由设备中的名称标识，用于提供一种配置设备的方法；命令也由名称标识，命令执行时可能接受或不接收参数，并且可能不会有返回值。设备类负责将硬件通信协议转换成ROS中Topic通信机制或服务(Service)通信机制，从而实现对外接设备的控制和监视。其中，话题(Topic)通信机制是ROS中基于发布(publish)/订阅(subscribe)模型的异步通信模式，这种方式将信息的产生和使用双方解耦，常用于不断更新的、含有较少逻辑处理的数据通信；服务(Server)通信机制是ROS中基于客户端(client)/服务器(server)模型，多用于处理ROS中的同步通信，常用于数据量较小但有强逻辑处理的数据交换。此外，已经定义了外接设备可以为典型的传感器设备，包含IMU惯性测量单元、电机编码器单元、电量测量单元、水深传感单元、摄像单元以及用户自定义硬件，通过硬件设备驱动模块211可以更方便的将硬件设备接入ROS操作系统进行驱动，从而增强了控制系统的开放性与兼容性。

所述信息采集设备接口模块212用于采集交互数据，对所述交互数据进行优化处理，将所述交互数据划分为状态信息和参数信息，并将所述状态信息和所述参数信息通过所述数据中心层24的数据管理模块241传输至所述系统状态监控层22。其中，所述状态信息用于反馈机器人自身的状态信息，包含姿态数据、电池能源的消耗数据、与上位机进行无线通讯的状态信息、控制系统内部的环境温度数据以及主控板11SoC资源占用状态等；所述参数信息用于反馈机器人外部环境的信息，包含机器人相对坐标原点的位置数据、相对坐标原点的里程计信息以及相对水面的潜水深度。通过所述数据中心层24与所述数据管理模块241进行数据交互，在所述硬件接口层21中分别接收所述硬件设备驱动模块211、所述输入设备接口模块215发送的数据，与所述驱动器设备接口模块213、所述通信设备接口模块214进行数据交互；其中数据传输采用ROS中的Topic通信方式，在该分布式控制系统中实现数据的循环收发。

所述信息采集设备接口模块212的具体交互数据包括：(1)输入设备接口模块215发送的数据包括操作手柄、键盘、语音、手势等操作设备发送的模拟量和数字量，分类到信息采集设备接口模块212中参数数据的控制指令单元并映射为单位数值，用于操作机器人运动。(2)硬件设备驱动模块211发送的数据，包括IMU惯性测量单元、水深传感测量单元、摄像单元、电机编码器单元、电量测量单元、用户自定义硬件等发送的模拟量和数字量，将IMU惯性测量单元、水深传感测量单元、摄像单元、电机编码单元发送的数据分类到信息采集设备接口模块212中参数数据的姿态数据单元，将电量测量单元发送的数据分类到电池能源数据单元，用于监控机器人在水下攀爬过程中的状态信息。(3)驱动器设备接口模块213发送的数据，包括执行元件的实际位置、实际速度、实际加速度、温度数据，所述信息采集设备接口模块212接收的数据包括位置指令、速度指令、加速度指令以及驱动器支持的其他控制指令，分类到信息采集设备接口模块212中参数数据的控制指令单元并映射为单位数值，用于驱动执行机构。(4)信息采集设备接口模块212将收集的状态数据和参数数据通过通信设备接口模块214发送到数据中心层24中的数据管理模块241，其中，通信设备接口模块214通过高频信号发射单元与低频信号发射单元在上位机与下位机之间传输。

所述驱动器设备接口模块213用于驱动所述主控板11与电机驱动器进行数据交互，支持RS232串行通信接口、CANOpen通信协议、EtherCAT通信协议。其中，所述RS232串行通信接口，通过RS232通讯接口的扩展板，使用FT4232HL芯片由USB接口扩展多RS232通讯接口，使所述主控板11与驱动器设备通过发送ASCII码指令进行通讯，保证每条支路的通讯速率均为115200bps的全双工模式；采用ROS中话题(Topic)通信机制，设定信息发送频率，在该分布式控制系统中实现数据的循环收发。所述CANOpen通讯协议，使用MCP2515型芯片由树莓派4B的GPIO引脚扩展CAN通讯接口，使主控板11可以通过CANOpen协议与电机驱动器进行通讯。所述EtherCAT通讯协议，使用树莓派4B的Full-throughpu Gigabit Ethernet端口安装IgH EtherCAT Master for Liunx开源软件，实现EtherCAT的主站功能，实现与支持EtherCAT通讯协议的电机驱动器进行通讯。

所述通信设备接口模块214用于为所述PC机12与所述水下攀爬机器人建立无线通讯，支持433、477、858、915MHz频段以及2.4GHz和5.8GHz频段信号发射，包括低频信号发射单元、高频信号发射单元。其中，所述低频信号发射单元，采用LoRa扩频技术，通过硬件部分1I/O模块的40Pin GPIO接口模块扩展433、477、858、915MHz频段进行数据传输，通信距离达5Km，支持自动中继扩大通信距离。所述高频信号发射单元，使用树莓派4B的Dual-band802.11ac无线网络接口，发射2.4GHz和5GHz频段信号，扩大无线通讯的带宽以及降低延迟。通过所述通信设备接口模块214选择高频、低频信号发射单元，使机器人与PC机12之间可以根据不同的环境需求选择不同的通讯方式，解决了机器人建立无线通讯的不同需求，提升了面对不同环境的适应性能力。

所述输入设备接口模块215用于接收不同类别的输入设备发出的控制信号，经所述PC机12处理后，通过所述通信设备接口模块214将处理后的控制信号传送给所述主控板11，实现对所述水下攀爬机器人运动的控制。所述不同类别的输入设备通过操控手柄、键盘、语音、系统预置指令或手势的方式进行控制。其中，操作手柄输入方式，通过ROS操作系统中/Joy_node节点将按键采集的数据转换成Twist消息进行发布，实现机器人的操作。键盘输入方式，通过ROS操作系统中定义/Key_node节点循环采集键盘按键触发信息并转换成Twist消息进行发布，实现机器人的操作。语音输入方式，通过ROS操作系统中安装科大讯飞SDK包，进行语音识别，通过触发内置指令完成相应动作，实现机器人的操作。手势输入方式，通过ROS操作系统中订阅IMU惯性测量单元发布的姿态信息并转换成Twist消息进行发布，实现机器人的操作。通过所述硬件接口层21兼容遥控手柄、鼠标、键盘、语音和手势等多种形式的输入设备，可以让用户以简单化、多样化的操作方式对机器人进行操作，降低了机器人的操作成本，增加操作方式的灵活性。

本实施例中，所述硬件接口层21主要用于和不同类别的外接设备进行数据交互，并向下发送主控层25的控制指令，可以添加用户自定的硬件，对系统功能进行扩展，并且新添加的硬件设备仅需满足所发布的消息类型、符合控制系统的要求即可进行数据交互，增强了系统的开放性与兼容性。通过硬件接口层21的硬件设备驱动模块211围绕设备和设备类的概念进行构建，设备由设备服务器构建，且具有唯一的标识名称，利用ROS操作系统的特性，使得各模块支持C++和Python作为编程语言，进一步提升了系统的兼容性。同时该模块内置一些机器人在完成水下攀爬动作时常用的硬件设备，包含IMU惯性测量单元、电机编码器单元、电量测量单元、水深传感单元、摄像单元以及用户自定义硬件，通过硬件设备驱动模块211可以更方便地将硬件设备接入ROS操作系统进行驱动，进一步增强了控制系统的开放性与兼容性。

本实施例中，所述通信设备接口模块214支持不同频段射频信号与所述PC机12建立无线通讯，使用USB外置网卡用于发送2.4GHz和5.8GHz频率波段进行通讯，其中外置网卡与树莓派4B通过USB3.0接口进行连接，并在Linux系统下编译安装驱动，信号发射天线以浮漂的形式安置在水面，通过信号传输电缆与机器人本体进行连接，连接端口采用灌胶密封的形式，通过通信设备接口模块214使得机器人的所述主控板11与所述PC机12之间进行信号传输。

所述通信设备接口模块214与所述信息采集设备接口模块212相连，通过USB3.0接口连接外置网卡单元发射频率为2.4GHz与5.8GHz频段信号进行通讯。所述硬件设备驱动模块211与信息采集设备接口模块212相连，传输传感器采集的数据信息，具体包括：(1)水深传感测量单元与硬件设备驱动模块211相连，通过I²C接口连接并设置通讯速率为100Hz，定时将采集的机器人深度数据传输到信息采集设备接口模块212；(2)IMU惯性测量单元与硬件设备驱动模块211相连，通过SPI接口连接并设置通讯速率为200Hz，定时将采集的四元数信息传输到信息采集设备接口模块212；(3)摄像单元与硬件设备驱动模块211相连，通过CSI camera接口连接并录制720P压缩视频，定时将信息传输到信息采集设备接口模块212，在QT系统监控软件中显示；(4)电量测量单元与硬件设备驱动模块211的相连，通过TTL接口连接，定时将采集的电池基本信息传输到信息采集设备接口模块212，电池基本信息包括：电池电量、电流大小、电压大小、电池温度信息；并接收树莓派4B的电源指令，控制电源通电断电动作；(5)电机编码单元与硬件设备驱动模块211相连，通过RS232串行通信接口连接并设置通讯速率为115200bps，实时将信息传输到信息采集设备接口模块212。驱动器设备接口模块213与信息采集设备接口模块212相连，通过RS232串行通信接口连接并设置通讯速率为115200bps，实时接收控制算法层23的控制指令。输入设备接口模块215与信息采集设备接口模块212相连，通过2.4GHz与5.8GHz频段信号进行通讯，实时发送PS3手柄的控制指令。

如图5所示，所述系统状态监控层22包括状态监测模块221、故障监测模块222和数据传输模块223。

其中，所述数据传输模块223用于接收所述硬件接口层21的信息采集设备接口模块212传输的所述状态信息和所述参数信息，并建立数据传输通道，通过所述硬件接口层21的通信设备接口模块214进行上传以及下载数据，包括所述状态信息、所述参数信息和所述水下攀爬机器人的控制指令信息，同时管理所述状态监测模块221与所述故障监测模块222的数据传输。所述状态监测模块221用于实时监测所述状态信息和所述参数信息，并将所述状态信息和所述参数信息实时显示在所述PC机12的QT界面上，其中所述状态信息和所述参数信息由硬件接口层21的信息采集设备接口模块212传送。

所述故障监测模块222用于根据所述状态信息，判断所述水下攀爬机器人在水下攀爬过程中是否发生故障，并在发生故障后进行故障诊断，依据内置的控制方法进行自主修复。

所述数据传输模块223将接收的所述状态信息和所述参数信息发送给所述故障监测模块222，若所述状态信息中的实际位置、实际速度、实际加速度、温度数据发生异常，即偏差量超过所述主控层25中设定的阈值，则在所述PC机12的QT界面显示警告弹窗，直到故障修复并重启。

使用QT开发平台编写跨平台系统监控软件，应用于所述PC机12的Linux系统中，该监控软件具有命令输入窗口，可发送控制指令到所述主控板11操作机器人运动，还具有状态检测窗口，可以实时显示信息采集设备接口模块212中的状态数据，还具有故障检测窗口，用于检测机器人在水下攀爬过程中遇到的故障问题。

所述数据管理模块241对所述系统状态监控层22交互的数据进行管理，其中，所述数据传输模块223与所述数据中心层24的所述数据管理模块241进行数据交互，用于获取信息采集设备接口模块212的所述状态信息和所述参数信息；通过硬件接口层21的通信设备接口模块214向上位机发送PS3手柄的控制指令；所述状态监测模块221将所述数据传输模块223获取的所述状态信息和所述参数信息，以3D图形可视化的方式展示机器人在水下攀爬过程中的状态信息以及在3D地图中的位置信息，提供给用户进行查看。所述故障监测模块222对所述数据传输模块223获取的所述状态信息和所述参数信息进行分析处理，实时反馈机器人在水下攀爬过程中的故障状态，并进行自主判断，若机器人状态不可恢复时尝试对所述PC机12发出警报；若机器人的状态可修复则依据内置的控制方法尝试进行自主修复，同时由所述数据管理模块241中获取所述日志模块244中记录的状态信息尝试对机器人进行状态恢复，通过读取日志文件中最近稳定点的状态，让机器人继续执行未完成的任务；若机器人在水下攀爬过程中发生碰撞、跌落等故障，导致失去对自身位置的定位时，通过所述控制算法层23的所述SLAM模块232为机器人提供SLAM导航功能，控制机器人完成自主返航，从而实现故障诊断和自主修复。

所述故障监测模块222根据所述状态信息，判断所述水下攀爬机器人在水下攀爬过程中是否发生故障，并在发生故障后进行故障诊断，依据内置的控制方法进行自主修复，具体包括：

所述故障监测模块222通过所述数据中心层24中的日志模块244读取所述日志模块244内存储的日志文件；从所述日志文件的索引文件中，确定一个最近的稳定点并记录所述稳定点的地址；确定所述日志文件中，与所述地址相对应的一条最近的稳定点记录；根据所述稳定点记录，读取所述水下攀爬机器人此时的所述参数信息，所述参数信息包括位置信息和方向信息；根据所述位置信息和所述方向信息，对所述水下攀爬机器人进行复位；根据所述稳定点记录，确定建立所述稳定点时所有正执行的事务队列，将所述正执行的事务队列暂时放入撤销队列，并置空重做队列；从所述稳定点开始，对所述日志文件进行正向扫描，直到所述日志文件被扫描结束；对所述撤销队列中的每个事务执行撤销操作，并对所述重做队列中的每个事务执行重做操作。

本实施例中，通过所述故障监测模块222根据采集的水下攀爬机器人的状态信息和参数信息，实时监测、反馈机器人在水下攀爬过程中的故障状态，当发生故障时，可对故障进行自主诊断和修复，从而使攀爬机器人具备了故障诊断与恢复的能力，提升了水下攀爬机器人对严酷的水下环境的适应能力，解决了现有技术中水下攀爬机器人在水下发生故障后不能自主修复的问题。

如图6所示，所述控制算法层23包括信息融合模块231和SLAM模块232。所述信息融合模块231用于采用卡尔曼滤波算法或扩展卡尔曼滤波算法，对所述硬件接口层21的信息采集设备接口模块212优化处理后的数据进行信息融合，估算出所述水下攀爬机器人的状态信息。

在嵌入式ROS操作系统中安装robot_localization功能包，将传感器检测设备采集的状态数据分类为以下15种变量信息，发送到robot_localization节点订阅的话题中，并订阅该节点发布的话题，获取机器人在相对空间中的位置信息；

所述15种变量信息具体为：[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama，ddot_x，ddot_y，ddot_z]，以机器人启动位置作为坐标原点，在相对空间中的建立右手笛卡尔直角坐标系：其中[x，y，z]为该坐标系下沿三个直角坐标轴方向的移动位移；[alpha，beta，gama]为该坐标系下绕三个直角坐标轴方向的转动位移；[dot_x，dot_y，dot_z]为该坐标系下沿三个直角坐标轴方向的移动速度；[dot_alpha，dot_beta，dot_gama]为该坐标系下沿三个直角坐标轴方向的转动速度；[ddot_x，ddot_y，ddot_z]为该坐标系下沿三个直角坐标轴方向的移动加速度。

具体的，若所述状态信息和所述参数信息包括IMU惯性测量单元、水深传感测量单元以及电机编码器单元的监测数据，则采用卡尔曼滤波算法将[alpha，beta，gama，dot_alpha，dot_beta，dot_gama]、[z]与[x，y]数据进行融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama]数据。

若所述状态信息和所述参数信息包括IMU惯性测量单元、电机编码器单元的监测数据，则采用卡尔曼滤波算法将[alpha，beta，gama，dot_alpha，dot_beta，dot_gama]与[x，y]数据进行融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama]数据。

若所述状态信息和所述参数信息包括水深传感器单元、电机编码器单元的监测数据，则采用卡尔曼滤波算法将[z]与[x，y]数据以及相应的一阶微分数值进行融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，dot_x，dot_y，dot_z]数据。

若所述状态信息和所述参数信息包括多个IMU惯性测量单元的监测数据，则采用平均滤波算法将多组[alpha，beta，gama，dot_alpha，dot_beta，dot_gama]数据进行数据融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama]数据。

所述信息融合模块231在所述控制算法层23中向所述SLAM模块232发送数据，包括机器人在水下攀爬过程相对空间中位置、速度、加速度、角速度、角加速度数据；采用的通信协议和算法种类均由主控层25指定。

应说明的是，若采用ORB_SLAM算法，则使用所述状态信息和所述参数信息中的摄像单元的监测数据。若采用cartographer算法，则使用所述状态信息和所述参数信息中的激光雷达单元的监测数据，其中所述激光雷达数据需要用户通过硬件设备驱动模块211中用户自定义单元进行接入，并将相应的数据格式转换成点云数据。应说明的是，所述用户自定义单元可以接入各种监测单元或传感器装置，具体接入的设备可视实际需求而定。

所述SLAM模块232通过所述数据中心层24向所述数据管理模块241发送数据，包括机器人的实际位置、实际速度、实际加速度、修正位置指令、修正速度指令、修正加速度指令等。具体的，若机器人通过所述硬件接口层21中的所述输入设备接口模块215对机器人发送控制指令，则所述SLAM模块232仅发送机器人的实际位置、实际速度、实际加速度。若机器人通过所述主控层25指定机器人的期望轨迹数据，则由所述SLAM模块232发送机器人的实际位置、实际速度、实际加速度、修正位置指令、修正速度指令、修正加速度指令。

所述SLAM模块232用于利用开源ORB_SLAM算法，根据系统状态监控层22采集的外部环境数据，生成水下三维地图并实时更新，对所述水下攀爬机器人进行定位导航。具体的，使用开源ORB_SLAM算法，利用单目相机接口采集的数据实时更新三维地图，其中ORB_SLAM包订阅/camera/image_raw数据，为输入单目相机采集的图像信息，并输出/PointCloud2类型的消息数据，为包含地图的所有关键点以及/tf格式的里程计数据。在ROS操作系统中订阅该话题即可创建实时地图，并在上位机的系统状态监测软件上以3D图形的形式显示出来，更直观的将机器人在3D空间中的状态信息实时进行展示。

本实施例中，所述控制算法层23主要用于处理机器人的状态数据，信息融合模块231采用卡尔曼滤波算法或扩展的卡尔曼滤波算法进行信息融合，包含多达15个变量信息，涵盖实际情况中的多数变量，并且可以不限制传感器的类型和数量，只要传感器发送的数据信息格式符合模块要求即可，进一步提升了系统的硬件兼容性。

如图7所示，所述数据中心层24包括数据管理模块241、参数动态配置模块242、数据分析模块243、日志模块244。

所述数据管理模块241用于存储、管理所述水下攀爬机器人运行时的各项数据，接收所述硬件接口层21的信息采集设备接口模块212、所述控制算法层23的SLAM模块232传输的数据，通过控制算法层23向所述信息融合模块231发送数据，通过所述系统状态监控层22向数据传输模块223发送数据；所述数据管理模块241在所述数据中心层24中分别向参数动态配置模块242、数据分析模块243、日志模块244发送数据。其中，数据传输采用ROS中的Topic通信机制，在该分布式控制系统中实现数据的循环收发。

所述参数动态配置模块242用于对所述控制系统中可动态修改的参数进行调节、配置，所述可动态修改的参数指因采集的监测数据的格式发生变化，且需要重新配置模块间相互耦合的数据，例如因硬件接口、传感器采集数据格式等发生变化需要重新配置模块间相互耦合的数据。通过参数服务器维护一个字符串到各类数据的键值表，在同一网络可访问的数据库中存储系统的各项配置参数，ROS系统中的各个节点可以对参数服务器进行读写，其中数据格式可通过YAML字符串进行配置，可以减轻参数配置的复杂性。

所述参数动态配置模块242在对参数进行动态配置时，首先使用Python编程语言配置YAML文件，导入dynamic_reconfigure安装包提供的参数生成器(parametergenerator)，然后定义需要进行动态配置的参数，部分关键代码如下：

gen.add("int_param"，int_t，0，"An Integerparameter"，50，0，100)

gen.add("double_param"，double_t，0，"A double parameter"，5，0，1)

gen.add("str_param"，str_t，0，"A string parameter"，"Hello World")

gen.add("bool_param"，bool_t，0，"A Booleanparameter"，True)；

然后初始化ROS节点，然后创建一个参数动态配置的服务端实例，参数配置的类型与配置文件中描述的类型相同。该服务器实例会监听客户端的参数配置请求。最后定义回调函数，并将回调函数和服务端绑定。当客户端请求修改参数时，服务器即可跳转到回调函数进行处理。

所述日志模块244用于将所述水下攀爬机器人运行时的各项数据生成日志文件，记录所述水下攀爬机器人的参数信息和状态信息。

在所述日志模块244中，日志以事务为单位进行记录，并结合对日志建立稳定点和索引文件的方式进行存储，将日志记录的信息同时向所述PC端的ROS操作系统与嵌入式端ROS操作系统写入进行日志同步，从而可以有效避免由于意外情况导致日志文件丢失的情况。

需要说明的是，本实施例中，日志记录的方法为：

首先把日志缓冲区中所有的现有日志记录写入磁盘的日志文件上；然后定时检测机器人的状态，并在日志文件中写入一个稳定点记录；再然后把当前缓冲区的所有关节机械动作序列进行记录；最后把稳定点记录在日志文件中的地址记入索引文件中。

应该注意的是，所述日志模块244生成的日志文件，可用于：(1)记录所述水下攀爬机器人在水下攀爬过程中的状态信息；(2)用于所述水下攀爬机器人在水下攀爬过程中的故障恢复，所述故障恢复主要是指机器人对自身的定位信息的恢复，例如当机器人在运动过程中突然断电时，采集机器人位置信息的传感器如惯性测量元件、电机编码器等都会被重置，并且这两种传感器测量的信息都是相对位置信息(相对于出发点)，断电后，通过所述日志模块244所记录的轨迹数据可以获得出发点的信息，从而继续完成指定的动作。除此之外，机器人的执行机构可能在断电时正处于某一执行位置，由于电机的编码器是相对编码器，不是绝对编码器，断电后会置零，通过读取日志文件中记录的位置信息，也可以继续完成动作；(3)用于数据分析模块243，对历史数据进行数据挖掘以及强化学习，改进机器人在水下攀爬过程中的问题，主要是用于算法的分析研究。例如，添加PID算法用于电机的速度控制，通过所述日志模块244记录的机器人运动的速度数据，可以分析PID算法的实际效果，从而进一步优化PID的参数设置。同样，在使用不同控制算法的时候，都需要记录实际的运动数据，从而优化算法，达到更好的控制效果。

所述数据分析模块243用于对所述参数信息和所述状态信息进行处理分析，获得所述水下攀爬机器人的运动特征，并采用强化学习的方式对所述运动特征进行强化改进，改善所述水下攀爬机器人在水下攀爬过程中的问题。

本实施例中，所述数据中心层24主要用于存储、分析机器人在运行过程中产生的所有数据信息及日志文件，参数动态配置模块242通过参数服务器维护一个字符串到各类数据的键值表，在同一网络可访问的数据库中存储系统的各项配置参数；日志模块244采用日志的形式对机器人在水下攀爬过程中的状态信息及环境信息进行记录可以很好的收集历史数据，采用数据挖掘以及强化学习的方式对机器人的运动进行分析并辅助决策；通过日志同步的方式，可以有效避免由于意外情况导致日志文件丢失，如，机器人跌落、进水以及损坏等；日志以任务为单位记录，并结合对日志建立稳定点和索引文件的方式进行存储，可以以此对机器人进行状态恢复，如需要对机器人进行重启等操作时，可以用最近的稳定点的状态和任务进度进行初始化，使机器人继续完成任务，而不是从头开始重新执行任务，从而提升了攀爬机器人的工作效率。

如图8所示，本实施例中，所述主控层25主要用于控制所述数据中心层24与所述系统状态监控层22、所述硬件接口层21和所述控制算法层23相互通信的过程，对其他各层间的数据交互、运行时间以及所述硬件部分1进行调度和资源分配，为各层间的数据交互指定通信协议。

所述主控层25用于调度各层间的数据交互，运行时间，系统CPU、RAM等计算资源分配，具体为：在Linux中安装Xenomai来保障控制系统数据传输的实时性；通过ROS参数动态配置机制对系统中数据发布频率以及数据缓冲队列长度进行动态调整，保证数据传输的稳定性；对各层之间的数据传输设置不同的优先级，从而进行协调保证数据传输的可靠性。

应该说明的是，本实施例中，所述主控层25负责：(1)设定数据中心层24与其他各层之间的数据交互通信协议，默认为ROS中的Topic通讯协议；(2)设定各层的模块之间的数据交互通信协议，默认为ROS中Topic通讯协议；(3)设定机器人允许的误差阈值，如实际位置、实际速度、实际加速度、温度等状态信息与控制量中位置指令、速度指令、加速度指令产生的偏差；(4)设定执行机构采用的通信协议，包括RS232串行通信接口，默认通讯速率为9600bps、CANOpen通信协议，默认通讯速率为1MBps、EtherCAT通讯协议；(5)设定输入设备与驱动设备的连接状况，包括输入设备与驱动设备同时与上位机或下位相连，或输入设备与驱动设备分别与上位机和下位机相连；(6)设定硬件接口层21中信息采集设备接口模块212包含的状态数据种类，包括IMU惯性测量单元、水深传感测量单元以及电机编码器单元的数据；IMU惯性测量单元、电机编码器单元的数据；水深传感器单元、电机编码器单元的数据；多个IMU惯性测量单元的数据等组合形式；(7)设定SLAM模块232使用的算法，包括ORB_SLAM算法、cartographer算法等。

此外，所述主控层25还控制所述硬件接口层21与所述树莓派4B、所述PC机12的硬件接口进行数据交互，具体包括：(1)管理所述树莓派4B与所述PC机12之间WiFi信号频率，当距离较近时设置信号频率为5.8GHz，保证数据传输带宽以及延迟最低，当信号强度变弱时，调控发射信号频率为2.4GHz以适应更远的传输距离；(2)管理所述树莓派4B、所述PC机12与所述硬件设备驱动模块211的通讯协议，包括：所述信息采集设备接口模块212通过USB3.0接口连接外置网卡单元发射频率为2.4GHz与5.8GHz频段信号进行通讯，采用Topic通信用于所述树莓派4B与所述PC机12之间的数据传输；各个测量单元通过I²C接口连接并设置通讯速率为100Hz，采用Topic通信定时将采集的机器人深度数据传输到信息采集设备接口模块212；所述IMU惯性测量单元通过SPI接口连接并设置通讯速率为200Hz，采用Topic通信定时将采集的四元数信息传输到信息采集设备接口模块212；所述摄像单元通过CSIcamera接口连接并录制720P压缩视频，采用Topic通信定时将信息传输到信息采集设备接口模块212；所述电量测量单元通过TTL接口连接，采用Topic通信定时将采集的电池基本信息传输到信息采集设备接口模块212，并接收树莓派4B的电源指令，控制电源通电断电动作；所述电机编码单元通过RS232串行通信接口连接并设置通讯速率为115200bps，采用动作(Action)通信机制实时将信息传输到信息采集设备接口模块212。所述驱动器设备接口模块213通过RS232串行通信接口连接并设置通讯速率为115200bps，采用动作(Action)通信机制实时接收控制指令。所述输入设备接口模块215通过2.4GHz与5.8GHz频段信号进行通讯，采用动作(Action)通信机制实时发送控制指令。

所述软件部分2中主控层25以节点的形式应用于PC机12的ROS操作系统与嵌入式ROS操作系统中，数据传输采用ROS中Topic通信机制，在该分布式控制系统中实现数据的循环收发；

在所述软件部分2中，所述数据中心层24应用于所述PC机12的ROS操作系统与嵌入式ROS操作系统中，具体为：

所述数据管理模块241以节点的形式安装于嵌入式ROS操作系统中；

所述参数动态配置模块242以节点的形式安装于嵌入式ROS操作系统中；

所述日志模块244以节点的形式安装于嵌入式ROS操作系统中；

所述数据分析模块243以节点的形式安装于所述PC机12的ROS操作系统中；

所述数据中心层24与各层之间的数据传输采用ROS中Topic通信机制，在该分布式控制系统中实现数据的循环收发。

在所述软件部分2中，所述系统状态监控层22使用QT开发平台编写跨平台系统监控软件，安装于PC机12的Linux系统中，通过所述PC机12的WiFi通讯接口与所述树莓派4B的WiFi通讯接口组成的局域网络进行数据交互，采用ROS中的Topic通信机制，在该分布式控制系统中实现数据的循环收发；

在所述软件部分2中，所述控制算法层23应用于所述PC机12的ROS操作系统与嵌入式ROS操作系统中，具体为：

所述信息融合模块231以节点的形式安装于嵌入式ROS操作系统中，所述SLAM模块232以节点的形式安装于所述PC机12的ROS操作系统中；

其中，数据传输采用ROS中Topic通信机制，在该分布式控制系统中实现数据的循环收发。

在所述软件部分2中，所述硬件接口层21应用于嵌入式ROS操作系统中，其中：

所述硬件设备驱动模块211与所述硬件部分1中的I/O接口模块15相连，具体为：所述硬件设备驱动模块211连接的外接设备可以包括IMU惯性测量单元、电机编码器单元和电量测量单元，也可以包括摄像单元和水深传感单元等其他各种传感器检测单元，同时还预设有用户自定义硬件接口，以根据用户的需求接入到所述控制系统中，实现对所述控制系统的某一特定指标的实时监测，从而进一步提升了系统的硬件兼容性。

具体的，所述IMU惯性测量单元与40Pin GPIO引脚相连，所述电机编码器单元兼容40Pin GPIO引脚相连读取PWM脉冲、兼容扩展CAN Open通讯协议进行通讯、兼容与USB接口相连转换RS232通讯协议进行通讯，所述电量测量单元与USB转TTL端口相连读取电量相关信息，所述水深传感单元与40Pin GPIO引脚相连读取水压数据映射为水下深度数据，所述摄像单元与CSI camera接口相连传输图像以及视频数据，所述用户自定义硬件可通过硬件接口层21与I/O模块中接口相连并将信息传入信息采集设备接口模块212。

需要说明的是，上面提及的各个测量单元、传感器都不是唯一的，其种类、用途不应作为对本发明的保护范围的限定，各种类型、用途的传感器或测量单元，均可根据实际情况进行选择。

所述驱动器设备接口模块213与所述硬件部分1中的I/O接口模块15相连，具体为：所述驱动器设备接口模块213与40Pin GPIO引脚相连，读取PWM脉冲，还扩展CAN Open通讯协议进行通讯，还与USB接口相连，转换RS232通讯协议进行通讯，还与Full-throughpuGigabit Ethernet端口相连，扩展EtherCAT通信协议进行通讯，从而与电机驱动器进行数据交互。

所述通信设备接口模块214与所述硬件部分1中的I/O接口模块15相连，具体为：所述通信设备接口模块214与40Pin GPIO引脚相连通过LoRa扩频技术扩展433、477、858、915MHz频段，与USB端口相连扩展2.4GHz和5.8GHz频段信号。所述输入设备接口模块215与所述PC机12通过USB及其扩展接口与输入设备相连。所述信息采集设备接口模块212以节点的形式安装于所述PC机12的ROS操作系统中与嵌入式的ROS操作系统中，其中数据传输采用ROS中Topic通信机制，在该分布式控制系统中实现数据的循环收发。

此外，输入设备与驱动设备是否同时与PC机12或主控板11相连，也由所述主控制指定。若输入设备接口模块215所接入的输入设备与驱动器设备接口模块213所接入的驱动设备同时与PC机12相连或同时与主控板11相连，则将参数数据中控制指令单元的数据经通信设备接口模块214直接转发到驱动器设备接口模块213。若输入设备接口模块215所接入的输入设备与驱动器设备接口模块213所接入的驱动设备分别与PC机12和主控板11相连，则将参数数据中控制指令单元的数据经通信设备接口模块214的高、低频信号发射单元转发到驱动器设备接口模块213，使其在PC机12和主控板11之间进行数据交互；同时，将高、低频信号发射单元通讯的信号强度、带宽、延迟等信息发送到信息采集设备接口模块212，分类到状态数据中通讯状态单元，用于监控机器人的通讯状态信息。

本发明系统的软件部分2，采用多层可配置的形式对控制系统分层解耦，从而增强系统的开放性和兼容性。系统状态监控层22使用QT开发平台编写跨平台系统监控软件，适用于Windows、Linux以及MacOS操作系统，实时接收硬件接口层21反馈的传感器数据，并以3D图形可视化的方式展示机器人在水下攀爬过程中的状态信息以及在3D地图中的位置信息，提供给用户进行查看，可交互性更强、易于操作，降低用户使用门槛。

此外，如图9所示，本实施例还提供了一种水下攀爬机器人控制系统的故障恢复方法，具体步骤包括：

S1.读取数据中心层24的日志模块244中存储的日志文件；

S2.从所述日志文件的索引文件中，确定一个最近的稳定点并记录所述稳定点的地址；

S3.确定所述日志文件中，与所述地址相对应的一条最近的稳定点记录；

S4.根据所述稳定点记录，读取所述水下攀爬机器人此时的参数信息，所述参数信息包括位置信息和方向信息；

S5.根据所述位置信息和所述方向信息，对所述水下攀爬机器人进行复位；

S6.根据所述稳定点记录，确定建立所述稳定点时所有正执行的事务队列，将所述正执行的事务队列暂时放入撤销队列，并置空重做队列；

S7.从所述稳定点开始，对所述日志文件进行正向扫描，直到所述日志文件被扫描结束；

S8.对所述撤销队列中的每个事务执行撤销操作，并对所述重做队列中的每个事务执行重做操作。

本实施例中，通过故障监测模块222根据采集的水下攀爬机器人的状态信息和参数信息，实时监测、反馈机器人在水下攀爬过程中的故障状态，当发生故障时，利用所述恢复方法对故障进行自主诊断和修复，从而使攀爬机器人具备了故障诊断与恢复的能力，提升了水下攀爬机器人对严酷的水下环境的适应能力，解决了现有技术中水下攀爬机器人在水下发生故障后不能自主修复的问题。

应说明的是，若故障不可恢复，则对所述PC机12发出报警，以提醒后台操作人员机器人发生了故障且该故障不可恢复。若机器人的状态可修复，则依据内置的控制方法尝试进行自主修复，同时根据日志模块244中记录的状态信息尝试对机器人进行状态恢复，通过读取日志文件中最近稳定点的状态，让机器人继续执行未完成的任务；若机器人在水下攀爬过程中发生碰撞、跌落等故障，导致失去对自身位置的定位时，通过控制算法层23的SLAM模块232控制机器人完成自主返航，从而解决了攀爬机器人在水下作业时发生故障不能进行自主诊断和修复的难题。

本发明提出了一种水下攀爬机器人控制系统及故障恢复方法，基于ROS系统设计的控制系统，采用松耦合、模块化的设计思想，使控制系统可以更方便接入自定义硬件和控制算法，提高了各模块的可替换性和可扩展性，从而有效提升了系统的硬件兼容性。当水下攀爬机器人发生故障时，通过故障监测模块222根据状态监测模块221的数据信息实时反馈机器人在水下攀爬过程中的故障状态，并进行自主判断，利用内置的控制方法尝试进行自主修复，从而使攀爬机器人具备了故障诊断与恢复的能力，提升了攀爬机器人对严酷的水下环境的适应能力。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种水下攀爬机器人控制系统，其特征在于，包括：硬件部分和软件部分，

所述硬件部分包括主控板、PC机，所述主控板为具有多种接口形式的集成开发板，所述主控板和所述PC机采用局域网无线连接；

所述软件部分包括系统状态监控层、硬件接口层、控制算法层、数据中心层和主控层；

所述数据中心层分别与所述系统状态监控层、所述硬件接口层、所述控制算法层和所述主控层互联；

所述系统状态监控层用于对所述水下攀爬机器人运行时的外部环境、运行状态和故障情况进行实时监测，将监测数据传输至所述数据中心层，并在所述水下攀爬机器人发生故障后，对故障进行自主诊断和修复；

所述硬件接口层用于与接入所述控制系统的硬件设备进行数据交互，并将交互数据传输至所述数据中心层；

所述控制算法层用于对所述交互数据进行处理分析，为所述水下攀爬机器人运行时提供SLAM导航功能；

所述控制算法层包括信息融合模块，所述信息融合模块用于采用卡尔曼滤波算法或扩展卡尔曼滤波算法，对所述硬件接口层的信息采集设备接口模块优化处理后的数据进行信息融合，估算出所述水下攀爬机器人的状态信息；

在嵌入式ROS操作系统中安装robot_localization功能包，将传感器检测设备采集的状态数据分类为以下15种变量信息，发送到robot_localization节点订阅的话题中，并订阅该节点发布的话题，获取机器人在相对空间中的位置信息；

所述15种变量信息具体为：[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama，ddot_x，ddot_y，ddot_z]，以机器人启动位置作为坐标原点，在相对空间中的建立右手笛卡尔直角坐标系：其中[x，y，z]为该坐标系下沿三个直角坐标轴方向的移动位移；[alpha，beta，gama]为该坐标系下绕三个直角坐标轴方向的转动位移；[dot_x，dot_y，dot_z]为该坐标系下沿三个直角坐标轴方向的移动速度；[dot_alpha，dot_beta，dot_gama]为该坐标系下沿三个直角坐标轴方向的转动速度；[ddot_x，ddot_y，ddot_z]为该坐标系下沿三个直角坐标轴方向的移动加速度；

具体的，若状态信息和参数信息包括IMU惯性测量单元、水深传感测量单元以及电机编码器单元的监测数据，则采用卡尔曼滤波算法将[alpha，beta，gama，dot_alpha，dot_beta，dot_gama]、[z]与[x，y]数据进行融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama]数据；

若所述状态信息和所述参数信息包括IMU惯性测量单元、电机编码器单元的监测数据，则采用卡尔曼滤波算法将[alpha，beta，gama，dot_alpha，dot_beta，dot_gama]与[x，y]数据进行融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama]数据；

若所述状态信息和所述参数信息包括水深传感器单元、电机编码器单元的监测数据，则采用卡尔曼滤波算法将[z]与[x，y]数据以及相应的一阶微分数值进行融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，dot_x，dot_y，dot_z]数据；

若所述状态信息和所述参数信息包括多个IMU惯性测量单元的监测数据，则采用平均滤波算法将多组[alpha，beta，gama，dot_alpha，dot_beta，dot_gama]数据进行数据融合，获得机器人在水下攀爬过程的相对空间中[x，y，z，alpha，beta，gama，dot_x，dot_y，dot_z，dot_alpha，dot_beta，dot_gama]数据；

所述数据中心层用于对所述水下攀爬机器人运行时产生的所有数据信息及日志文件进行存储和分析；

所述数据中心层包括数据分析模块，所述数据分析模块用于对所述参数信息和所述状态信息进行处理分析，获得所述水下攀爬机器人的运动特征，并采用强化学习的方式对所述运动特征进行强化改进，改善所述水下攀爬机器人在水下攀爬过程中的问题；

所述主控层用于对其他各层间的数据交互、运行时间以及所述硬件部分进行调度和资源分配，为各层间的数据交互指定通信协议。

2.根据权利要求1所述的水下攀爬机器人控制系统，其特征在于，所述硬件接口层包括硬件设备驱动模块、信息采集设备接口模块、驱动器设备接口模块、通信设备接口模块、输入设备接口模块；

所述硬件设备驱动模块用于驱动所述PC机和外接设备进行数据交互；所述外接设备通过所述主控板上的接口接入所述控制系统，所述外接设备包括多种监测所述水下攀爬机器人的实时数据的传感器；

所述驱动器设备接口模块用于驱动所述主控板与电机驱动器进行数据交互；

所述信息采集设备接口模块用于采集交互数据，对所述交互数据进行优化处理，将所述交互数据划分为状态信息和参数信息，并将所述状态信息和所述参数信息通过所述数据中心层的数据管理模块传输至所述系统状态监控层；

所述通信设备接口模块用于为所述PC机与所述水下攀爬机器人建立无线通讯；

所述输入设备接口模块用于接收不同类别的输入设备发出的控制信号，经所述PC机处理后，通过所述通信设备接口模块将处理后的控制信号传送给所述主控板，实现对所述水下攀爬机器人运动的控制；所述不同类别的输入设备通过操控手柄、键盘、语音、系统预置指令或手势的方式进行控制。

3.根据权利要求1所述的水下攀爬机器人控制系统，其特征在于，所述系统状态监控层包括状态监测模块、故障监测模块和数据传输模块；

所述数据传输模块用于接收所述硬件接口层的信息采集设备接口模块传输的状态信息和参数信息，并建立数据传输通道，通过所述硬件接口层的通信设备接口模块进行上传以及下载数据，包括所述状态信息、所述参数信息和所述水下攀爬机器人的控制指令信息，同时管理所述状态监测模块与所述故障监测模块的数据传输；

所述状态监测模块用于实时监测所述状态信息和所述参数信息，并将所述状态信息和所述参数信息实时显示在所述PC机的QT界面上；

所述故障监测模块用于根据所述状态信息，判断所述水下攀爬机器人在水下攀爬过程中是否发生故障，并在发生故障后进行故障诊断和自主修复。

4.根据权利要求3所述的水下攀爬机器人控制系统，其特征在于，所述故障监测模块根据所述状态信息，判断所述水下攀爬机器人在水下攀爬过程中是否发生故障，并在发生故障后进行故障诊断和自主修复，具体包括：

所述故障监测模块通过所述数据中心层的日志模块读取存储的日志文件；

从所述日志文件的索引文件中，确定一个最近的稳定点并记录所述稳定点的地址；

确定所述日志文件中，与所述地址相对应的一条最近的稳定点记录；

根据所述稳定点记录，读取所述水下攀爬机器人此时的所述参数信息，所述参数信息包括位置信息和方向信息；

根据所述位置信息和所述方向信息，对所述水下攀爬机器人进行复位；

根据所述稳定点记录，确定建立所述稳定点时所有正执行的事务队列，将所述正执行的事务队列暂时放入撤销队列，并置空重做队列；

从所述稳定点开始，对所述日志文件进行正向扫描，直到所述日志文件被扫描结束；

对所述撤销队列中的每个事务执行撤销操作，并对所述重做队列中的每个事务执行重做操作。

5.根据权利要求1所述的水下攀爬机器人控制系统，其特征在于，所述控制算法层还包括SLAM模块；

所述SLAM模块用于利用开源ORB_SLAM算法，根据所述系统状态监控层采集的外部环境数据，生成水下三维地图并实时更新，对所述水下攀爬机器人进行定位导航。

6.根据权利要求1所述的水下攀爬机器人控制系统，其特征在于，所述数据中心层还包括数据管理模块、参数动态配置模块、日志模块；

所述数据管理模块用于存储、管理所述水下攀爬机器人运行时的各项数据，接收所述硬件接口层的信息采集设备接口模块采集的数据、所述控制算法层的SLAM模块传输的数据，并将所有数据发送给所述参数动态配置模块、所述数据分析模块、所述日志模块、所述控制算法层的信息融合模块以及所述系统状态监控层的数据传输模块；

所述参数动态配置模块用于对所述控制系统中可动态修改的参数进行调节、配置，所述可动态修改的参数指因采集的监测数据的格式发生变化，且需要重新配置模块间相互耦合的数据；

所述日志模块用于将所述水下攀爬机器人运行时的各项数据生成日志文件，记录所述水下攀爬机器人的参数信息和状态信息。

7.根据权利要求1所述的水下攀爬机器人控制系统，其特征在于，所述数据中心层采用异步通信的方式分别与所述主控层、所述系统状态监控层、所述硬件接口层和所述控制算法层进行互联通信。

8.根据权利要求7所述的水下攀爬机器人控制系统，其特征在于，所述异步通信方式为Topic通信，所述Topic通信在生成信息、传输信息和使用信息时对各层间进行双向解耦。

9.根据权利要求1所述的水下攀爬机器人控制系统，其特征在于，所述硬件部分还包括硬件配置模块、通讯接口模块和I/O接口模块，所述硬件配置模块、所述通讯接口模块和所述I/O接口模块集成安装于所述主控板上。

10.一种根据权利要求1～9任一项所述的水下攀爬机器人控制系统的故障恢复方法，其特征在于，具体包括：

读取数据中心层的日志模块中存储的日志文件；

从所述日志文件的索引文件中，确定一个最近的稳定点并记录所述稳定点的地址；

确定所述日志文件中，与所述地址相对应的一条最近的稳定点记录；

根据所述稳定点记录，读取所述水下攀爬机器人此时的参数信息，所述参数信息包括位置信息和方向信息；

根据所述位置信息和所述方向信息，对所述水下攀爬机器人进行复位；

根据所述稳定点记录，确定建立所述稳定点时所有正执行的事务队列，将所述正执行的事务队列暂时放入撤销队列，并置空重做队列；

从所述稳定点开始，对所述日志文件进行正向扫描，直到所述日志文件被扫描结束；

对所述撤销队列中的每个事务执行撤销操作，并对所述重做队列中的每个事务执行重做操作。

Images