水下清洗机器人控制系统软件设计

2022-12-23刘捷黄燕民陈宏轩罗友高唐国元

电子设计工程 2022年24期

刘捷，黄燕民，陈宏轩，罗友高，孙涛，唐国元

（1.武汉第二船舶设计研究所，湖北武汉 430060；2.华中科技大学船舶与海洋工程学院，湖北武汉 430074）

随着海洋装备技术的发展与应用，许多具有特殊功能和用途的水下机器人被研制出来，并广泛应用在水下资源勘探、深水作业、救援打捞、渔业养殖等领域[1-2]。水下清洗机器人作为一种特殊的服务型机器人，主要用于对目标作业物表面附着的脏物进行清洗，常见的清洗方式有射流式、刷洗式以及混合式[3]。

设计一款交互性好、实时性高、功能完备的软件控制系统对提高清洗机器人水下作业效率十分重要。该文结合实际使用场景，基于开发高效的Python语言和梯形图语言，采取模块化设计方式，完成了水下清洗机器人控制系统软件的整体设计开发。

1 软件系统结构

清洗机器人软件系统整体由上位机控制终端和下位机执行终端两部分组成，根据任务属性不同，具体可分为应用层、决策层和驱动层[4]。应用层主要负责人机交互功能，利用上位机软件界面实时监控从下位机采集并上传的清洗机器人运行状态信息，如惯性导航模块测量的姿态数据、深度计测量的当前工作水深信息以及网络摄像头视频画面等，同时还负责将操作人员在软件界面输入的操作指令发送给决策层执行；决策层负责解算下位机发送的数据信息和操作人员输入的指令信息，将通过一系列运算得到的控制指令发送给下位机执行；驱动层则负责执行由上位机发出的控制指令，同时采集和上传清洗机器人的水下运行数据。清洗机器人软件系统结构如图1 所示。

图1 清洗机器人软件系统结构

清洗机器人控制系统软件采用模块化设计，将执行不同功能的程序块分隔开来，以此降低彼此间的耦合，提高软件开发效率。软件主要分为通信模块、运动控制模块、视频画面显示模块、运行位姿显示模块、手柄、推进器和传感器数据显示模块。通信模块用于上、下位机之间的数据交换，这也是软件系统能够控制清洗机器人水下位姿的前提条件；运动控制模块会根据程序设定好的控制算法，按照自主模式或是手动模式将控制指令发送给下位机执行；视频画面显示模块则是将清洗机器人本体搭载的两个网络摄像头视频信号处理转换为图像信息，显示在软件界面，为操作人员提供决策参考；运行位姿显示模块将根据下位机回传的传感器数据，在软件界面绘制实时位姿图，直观反映清洗机器人水下运行状态；手柄、推进器和传感器数据显示模块则在软件界面显示了具体的数值信息。

2 上位机程序设计

上位机作为控制系统的核心，是实现水下清洗机器人能够按照预期轨迹、姿态运行的关键。该文水下清洗机器人控制系统上位机软件使用Python 语言进行编写，该语言开发简洁、可扩展性强[5]，用来编写清洗机器人上位机软件十分合适。软件开发工具选择PyCharm，用户界面则采用跨平台界面库PyQt5作为开发框架。

2.1 设计方案

清洗机器人上位机软件具体需要实现的几个功能分别是系统通信、运动控制、网络视频信号读取和显示、实时位姿展示等，因而需要对这几个功能分别进行程序设计。上位机软件结构如图2 所示。

图2 上位机软件结构

2.2 关键功能实现

1）系统通信：清洗机器人控制系统具备手动控制和自动控制两种工作模式，而无论处在何种工作模式下，其水面系统和水下系统之间的数据交互都是以脐带缆作为传输介质并通过以太网通信连接实现的。清洗机器人手动控制模式是通过使用两台操纵手柄来实现的，操纵手柄经USB 串行线建立与工控机的物理连接。在Python 开发环境中，使用第三方库函数来实现这两种通信。在PyCharm 开发工具中安装“pySerial”和“HslCommunication”两个模块，使用“from ** import **”语句获取模块中的调用方法。在建立的串口通信线程中，对串口参数进行初始化，按照手柄通信协议配置相应的串口号、波特率、奇偶校验等串口属性，最后使用“serial.Serial()”方法对所配置的串口对象进行实例化，在确认串口正确打开后，对实例化的串口对象使用“read()”方法便可以接收来自操纵手柄的数据，数据接收格式则可以根据需要自由设定。

清洗机器人使用西门子S7-1500PLC 作为水下控制器，该型控制器支持多种数据通信协议，其中较有特色的是西门子特有不开放的S7 通信协议，这种通信协议不要求服务器方编写通信程序，使用便捷[6]。该文清洗机器人水面、水下控制系统通信使用支持S7 通信协议的开源模块“HslCommunication”来实现。在程序中导入该模块的两个类“SiemensS7Net、SiemensPLCS”，并创建S7-1500PLC 连接对象类，使用“SiemensS7Net()”方法对S7-1500PLC 实例化，在线程中建立上位机与S7-1500PLC 的长连接。“Connect Server().IsSuccess”方法用于判断系统连接状态，当在程序中调用“start()”方法开启通信连接线程后，前者将返回一个表征通信连接状态的布尔型变量，当该连接状态为“TRUE”，则表示水面、水下控制系统已成功建立长连接，此时上位机便可以通过建立的以太网通信连接对下位机进行连续读写数据操作。

2）运动控制：清洗机器人水下定深、定艏向控制是通过运行程序中编写的增量式PID 算法实现的。增量式PID 算法是一种递推式算法，计算式如式（1）所示：

式中，u(k-1) 为控制系统上一次输出，e(k)、e(k-1)、e(k-2)分别为最近三次偏差值，Kp为比例系数，Ki为积分系数，Kd为微分系数。该算法可有效减少偏差迭代次数，提高运算速度的同时减少了对计算机资源的占用[7-9]。

在上位机程序中，建立一个PID 算法的类，在类的初始化方法中分别对比例、积分、微分系数以及三次偏差值进行初始化，通过将操纵手柄输入的航向角和深度值与下位机上传的角度、深度数据进行比较得到偏差值，每收到一次数据调用一次PID算法进行数据更新，程序自动将计算得到的输出变化量与上一次系统输出值进行叠加，得到最终输出控制量。程序将根据设定好的控制循环周期，对下位机进行数据写入操作。为验证程序中增量式PID算法的有效性，手动设定艏向角为360°，程序控制周期T为100 ms，输出响应如图3 所示。可见所写代码能够起到实际控制作用，后续只需调整算法参数即可。

图3 增量式PID算法程序输出曲线

3）网络视频信号读取和显示：上位机使用跨平台的计算机视觉库OpenCV 来对网络摄像头视频流信号进行处理，提取出帧图像并在软件界面中使用“QLabel”控件来显示。OpenCV-Python 库丰富的函数和算法使得软件开发效率显著提高[10]。需要注意的是OpenCV 是按照BGR 格式读取图像信息的，而在软件界面，图像信息是以RGB 格式显示的，因而需要在程序中调用“cvtColor()”和“QImage()”两个函数，将提取的帧图片转换为RGB 格式进行显示。通过在界面主线程引入定时器定时刷新界面或创建图像处理线程的方式来解决界面显示视频画面卡顿的问题。

4）实时位姿展示：为了帮助操作人员更为直观地了解清洗机器人在水下的运动姿态以及当前工作水深情况，基于Matplotlib 库在软件界面建立两个动态图表窗口，其中一个用来展示清洗机器人的三维姿态，另一个用于生成清洗机器人下潜深度随时间变化的二维曲线图。Matplotlib 是一个堪比MATLAB图像绘制功能的强大图形库[11-12]，通过编写代码可以十分轻松地进行2D、3D 图形的绘制。为了能够在PyQt 界面实现动态绘图，首先需要在程序中建立一个画布的类，从Matplotlib 库引入FigureCanvasQTAgg类，并在新建画布类中继承它。使用“figure()”生成图像窗口，“add_subplot()”函数用于在该图像窗口添加一个子图，通过关键字“projection=‘3d’”将子图设为3D 图形属性，之后便可以将该画布类作为控件添加到软件界面。绘制图形及更改视图需要用到“plot()”和“view_init()”两个函数。

2.3 界面设计

在Python 中使用PyQt 库开发GUI（图形用户界面）可选择使用手写代码的方式设计软件界面，也可使用QtDesigner 这一界面辅助工具对界面进行设计。相比前者要手写大量代码，后者的引入则只需拖动控件来设计界面，再用软件生成代码便可完成界面设计，减少了程序设计的工作量[13]，故而该文软件界面设计采用这一方法。

3 下位机程序设计

采用了PLC 作为下位机,其软件不涉及复杂的逻辑运算，只是接收并执行来自上位机的指令以及上传水下运行数据，该文主要针对一些开发过程中的实际问题分析其解决办法。

3.1 问题描述

1）连接保护：由于上位机软件中使用西门子专门的S7 协议来与下位机通信，而这种通信机制是单向连接的[6]，清洗机器人水下运行过程中一旦失去与上位机的连接，可能会陷入失控运行状态，故需要在程序中设置通信连接保护机制。

2）模拟量信号干扰和噪声：清洗机器人以七台推进器作为水下动力，为达到良好的控制效果，需要对推进器进行转速的闭环控制。该文水下控制器S7-1500PLC 扩展了一个模拟量采集模块，最多支持八个模拟量信号同时采集。但模拟量信号采集易受信号源以及传输过程干扰等因素影响，准确性有所下降，因而有必要对信号干扰进行去除，获得可信度较高的采集数据。

3）推进器正反转状态：该文清洗机器人所选配的推进器，在未知转速控制量的前提下，无法通过模拟量反馈信号直接判断推进器当前转向，只能根据反馈的模拟量信号计算得到转速信息。在水下运行过程中，清洗机器人两种工作模式下的水平姿态都是自动控制的，假设某一时刻推进器转速为反转400 rpm，下一时刻控制量为正转400 rpm，此时控制偏差应为800 rpm，但实际计算偏差却为零，导致推进器保持原输出不变，清洗机器人水下运动“失控”。因此需要在软件层解决这一问题。

3.2 解决方案

1）针对连接保护问题，采取在下位机主程序块OB1中，利用系统时钟存储器与加计数器（CTU）组成一个接通延时时长为2 s 的定时器。系统通信已建立期间，上位机发出指令，保持加计数器始终处在复位状态，由于上位机指令输出频率远大于定时时长，因而在系统保持连接期间，下位机程序中这一组合定时器始终不能接通输出。一旦系统通信断开，2 s 时间一到，组合定时器接通输出，推进器输出复位和急停程序立即响应，从而保证清洗机器人水下运行安全。

2）针对模拟量信号去干扰和噪声的问题，常用方法为硬件滤波和软件滤波。所使用的S7-1500PLC扩展模拟量模块自身具备一定的滤波能力，使用方便，只需在组态下载设备时开启这一功能即可。而单纯依靠PLC 有限的硬件滤波功能并不能解决推进器转速模拟量信号波动的问题，所以还需要考虑使用软件滤波。但是由于控制器本身的局限性，PLC无法运行复杂的滤波算法，文献[14]则提出了一种用于PLC 软件滤波的改进型加权平均滤波算法，计算公式如式（2）所示。该算法使用结构化控制语言（SCL）进行编写，相比使用广泛的梯形图语言（LAD），更适合处理复杂运算，且编程条理清晰[15-16]。