基于Ｐｙｔｈｏｎ与Ｓ７欗
１５００的清洗机器人运动控制系统
ＴｈｅＭｏｔｉｏｎＣｏｎｔｒｏｌＳｙｓｔｅｍｏｆＣｌｅａｎｉｎｇＲｏｂｏｔ
ＢａｓｅｄｏｎＰｙｔｈｏｎａｎｄＳ７１５００
黄燕民１
　陈宏轩１
　罗友高２
　刘　捷２
　孙　涛２
　唐国元
１
（１华中科技大学船舶与海洋工程学院，武汉　４３００７４；２武汉第二船舶设计研究所，武汉　４３００６０）
摘　要牶针对一种养殖网箱水下清洗机器人，分析了清洗机器人的系统组成和控制原理，提出了其
运动控制方法，通过两台操纵手柄之间的配合来完成清洗机器人位姿控制。对清洗机器人水面、水下控制系统涉及的串行口通信和以太网通信问题分别进行了分析，在Ｐｙｔｈｏｎ环境下利用第三方库函数解决了水面、水下控制系统之间的通信问题。对清洗机器人控制系统软件设计部分提出了设计要点并给出了解决办法。基于以上方法开发了机器人实际控制系统。水池试验表明，所开发的清洗机器人控制系统具有控制简单方便、响应快速、水下运行平稳的特点。
关键词牶清洗机器人　
控制系统　Ｓ７１５００　Ｐｙｔｈｏｎ　软件设计
ＤＯＩ牶１０．１６４１３／ｊ．ｃｎｋｉ．ｉｓｓｎ．１００７０８０ｘ．２０２２．ｚ２．０１０
Ａｂｓｔｒａｃｔ牶Ａｉｍｉｎｇａｔａｎｕｎｄｅｒｗａｔｅｒｃｌｅａｎｉｎｇｒｏｂｏｔｆｏｒａｑｕａｃｕｌｔｕｒｅｃａｇｅｓ，ｔｈｅｐａｐｅｒａｎａｌｙｚｅｓｔｈｅｓｙｓｔｅｍｃｏｍｐｏｓｉｔｉｏｎａｎｄｃｏｎｔｒｏｌｐｒｉｎｃｉｐｌｅｏｆｔｈｅｃｌｅａｎｉｎｇｒｏｂｏｔ，ａｍｏｔｉｏｎｃｏｎｔｒｏｌｍｅｔｈｏｄｏｆｔｈｅｃｌｅａｎｉｎｇｒｏｂｏｔｉｓｐｒｅｓｅｎｔｅｄ．Ｗｅｃａｎｃｏｎｔｒｏｌｔｈｅｐｏｓｉｔｉｏｎａｎｄｔｈｅｐｏｓｅｏｆｔｈｅｃｌｅａｎｉｎｇｒｏｂｏｔｔｈｒｏｕｇｈｔｈｅｃｏｏｐｅｒａｔｉｏｎｂｅ
ｔｗｅｅｎｔｈｅｊｏｙｓｔｉｃｋｓ．ＴｈｅｓｅｒｉａｌｐｏｒｔｃｏｍｍｕｎｉｃａｔｉｏｎａｎｄＥｔｈｅｒｎｅｔｃｏｍｍｕｎｉｃａｔｉｏｎｐｒｏｂｌｅｍｓｉｎｔｈｅｓｕｒｆａｃｅａｎｄｕｎｄｅｒｗａｔｅｒｃｏｎｔｒｏｌｓｙｓｔｅｍｏｆｔｈｅｃｌｅａｎｉｎｇｒｏｂｏｔａｒｅａｎａｌｙｚｅｄｒｅｓｐｅｃｔｉｖｅｌｙ．ＴｈｅｃｏｍｍｕｎｉｃａｔｉｏｎｐｒｏｂｌｅｍｂｅｔｗｅｅｎｔｈｅｓｕｒｆａｃｅａｎｄｕｎｄｅｒｗａｔｅｒｃｏｎｔｒｏｌｓｙｓｔｅｍｉｓｓｏｌｖｅｄｂｙｕｓｉｎｇｔｈｅＴｈｉｒｄＰａｒｔｙＬｉｂｒａｒｙｏｆＰｙｔｈｏｎ．Ｆｏｒａｐａｒｔｏｆｔｈｅｓｏｆｔｗａｒｅｄｅｓｉｇｎｏｆｔｈｅｃｌｅａｎｉｎｇｒｏｂｏｔｃｏｎｔｒｏｌｓｙｓｔｅｍ，ｔｈｅｋｅｙｐｏｉｎｔｓａｎｄｔｈｅｓｏｌｕｔｉｏｎｓａｒｅｇｉｖｅｎ．Ｂａｓｅｄｏｎｔｈｅａｂｏｖｅｍｅｔｈｏｄｓ，ｔｈｅａｃｔｕａｌｃｏｎｔｒｏｌｓｙｓｔｅｍｏｆｔｈｅｃｌｅａｎｉｎｇｒｏｂｏｔｉｓｄｅｖｅｌｏｐｅｄ．Ｔｈｅｐｏｏｌｔｅｓｔｓｈｏｗｓｔｈａｔｔｈｅｄｅｖｅｌｏｐｅｄｃｏｎｔｒｏｌｓｙｓｔｅｍｈａｓｔｈｅｃｈａｒａｃｔｅｒｉｓｔｉｃｓｏｆｓｉｍｐｌｅａｎｄｃｏｎｖｅｎｉｅｎｔｃｏｎｔｒｏｌ，ｑｕｉｃｋｒｅｓｐｏｎｓｅａｎｄｓｔａｂｌｅｒｕｎｎｉｎｇｕｎｄｅｒｗａｔｅｒ．
Ｋｅｙｗｏｒｄｓ牶ｃｌｅａｎｉｎｇｒｏｂｏｔ　ｃｏｎｔｒｏｌｓｙｓｔｅｍ　Ｓ７
１５００　Ｐｙｔｈｏｎ　ｓｏｆｔｗａｒｅｄｅｓｉｇｎ
９
７２０２２ ４
５
机电一体化　
作者简介：黄燕民　１９９６年生，硕士在读。主要研究方向为水下机器人。
陈宏轩　１９９８年生，硕士在读。主要研究方向为水下控制技术。罗友高　１９６６年生，博士，研究员。主要研究方向为机械制造自动化。刘　捷　１９９４年生，学士。主要研究方向为机械控制技术。孙　涛　１９７５年生，硕士，高级工程师。主要研究方向为嵌入式系统。唐国元（通讯作者）　１９７３年生，博士，副教授，博士生导师。主要研究方向为智能船舶、水下机器人及作业系统。
０　引　言
随着海产品市场的不断扩大，以及在受过度捕捞、水质环境污染等问题导致的近海鱼类资源慢慢枯竭，近海人工养殖容量趋近饱和的情况下，人们开始将目光转向深远海网箱养殖。目前在大型深水网箱养殖方面技术较为成熟的有挪威、美国、加拿大、日本、意大利等国，而这其中又以挪威为主要代表，其网箱养殖成效最好［１］。网箱养殖普遍存在养殖密度大、网孔易脏堵，以及易被海生物附着，造成水体交换不畅而诱发鱼病等问题［２］。
许多国家网箱养殖配套设备和技术发展较早，因而也较为成熟，我国在这方面起步较晚，当前养殖网箱仍以人工潜水清洗为主［３］。相比人工清洗，使用水下清洗机器人清洗网箱效率高、成本低，且操作人员只需在水面操纵台进行控制，人员安全系数得到显著提高。因此，发展清洗机器人技术有着十分重要的意义。
１　系统组成与控制原理
１．１　系统组成
清洗机器人系统由水面系统和水下系统两部分构成，如图１所示。其中水面系统包括：计算机控制柜、高压泵站；水下系统包括：Ｓ７１５００ＰＬＣ、网络摄像头、深度计、惯性导航模块、倾角仪模块等。脐带缆为水面、水下控制系统提供通信支持。计算机控制柜内置４００ＶＤＣ开关电源；通过脐带缆为水下控制系统输送电力；高压泵站通过高压水管为机器人输送用于清洗作业的高压水。
计算机控制柜主要实现人机交互功能，接收并解析由下位机上传的数据，读取网络摄像头视频流信号，以可视化形式为操作人员提供决策参考，并根据操作指令为水下实时运动控制提供强大算力，不断向下位机发送正确的控制指令。Ｓ７１５００ＰＬＣ作为执行器，也是水下运动控制系统的核心。通过自身扩展的模拟量模块，ＰＬＣ不仅能够控制七台推进器的正、反转，实现清洗机器人的水下运动控制，还能够通过模拟量通道采集推进器转速信息
，从而实现推进器转速闭环控制。此
图１　清洗机器人控制系统组成
外，ＰＬＣ搭载的两路ＲＳ２３２串口模块，用来接收传感器的角度、加速度、深度等信息。１．２　控制原理
清洗机器人使用７台推进器为水下运动提供动力（推进器布置如图２）。单台推进器正、反转推力最大值均为４２ｋｇ，确保了清洗机器人在执行网箱清洗任务时，能够克服高压水对载体的反作用力以进行稳定可靠的运动。清洗机器人水下运动控制包括沿ｘｙｚ轴进退、侧移、升沉以及分别绕此三轴横滚、俯仰、艏摇共６个自由度的控制。由图２所示，水平推进器Ｔ４、Ｔ５、Ｔ６和Ｔ７分别呈１５°布置，用来实现清洗机器人进退、侧移和艏摇运动，纵向推进器Ｔ１、Ｔ２和Ｔ３垂直ｘＯｙ平面呈９０°布置，用来实现升沉、横滚、俯仰运动
［４］。
图２　清洗机器人推进器布置图
０
８　机电一体化２０２２ ４５
清洗机器人工作模式有两种：手动模式和自动模式，这里仅就手动模式进行阐述。计算机控制柜采用研华工控机作为上位机，通过自身ＵＳＢ接口外接两台操纵手柄，每台手柄均可对外输出ｘｙｚ三轴模拟量以及两路开关量。通过在网箱中设置标志物的方式，结合清洗机器人本体前后搭载的两个网络摄像头，以及照明灯、惯性导航模块等外部传感器，可以方便地从计算机控制柜显示屏中获取清洗机器人的方向、位姿信息。将其中一台操纵手柄功能定义为清洗机器人前后运动控制，另一台则定义为清洗机器人的航向、深度控制，这样在两台操纵手柄的协同作业下，可以方便地开展水下清洗作业。
手动模式控制流程：系统上电后初始化，自检无误后开始运行，上位机接收和解析手柄数据，通过计算获得清洗机器人期望位姿所需的推力，根据水平和垂直方向推力分配矩阵计算出不同推进器所需要的推力。将推力换算为推进器对应转速，发送给Ｓ７１５００ＰＬＣ执行，ＰＬＣ经模拟量输出通道输出控制电压驱动推进器动作，推动机器人完成相应的水下运动。
２　系统通信
清洗机器人水下作业有赖于系统上、下位机之间建立稳定可靠的通信连接。因此有必要对系统所涉及的通信方式进行分析。本文清洗机器人控制系统涉及的通信主要是串行口通信和以太网通信。
２．１　串行口通信
串行口通信主要有两部分：一是水面系统两台操纵手柄与上位机之间的通信；二是水下系统传感器与Ｓ７１５００ＰＬＣ之间的通信。清洗机器人搭载的惯导模块、倾角仪模块以及深度计支持的通信协议涵盖ＲＳ２３２、ＲＳ４２２、ＲＳ４８５三种不同接口通信协议。本文所用Ｓ７１５００ＰＬＣ扩展的两路串口通信模块ＣＭＰｔＰＢＡ均为ＲＳ２３２接口基本型，最大通信距离１５ｍ。由于数据传输距离短，ＲＳ２３２通信速率可满足控制系统要求，因此将两路串口分别用于接收惯导和深度计数据。因倾角仪模块仅支持ＲＳ４８５通信，虽然深度计也支持该输出协议，这样在扩展模块不足的情况下可以考虑采取主从节点轮询的方式来进行一对多通信［５］，但这需要Ｓ７１５００ＰＬＣ串口模块硬件支持。基于现有条件，选用串转网模块，将两台不同装配方向的倾角仪与串转网模块按照ＲＳ４８５接口进行连接，再用一根网线将串转网模块与交换机连接，最终实现Ｓ７１５００ＰＬＣ与两台倾角仪的数据交换。值得注意的是：在将Ｓ７１５００ＰＬＣ串口模块分别与惯导模块、深度计按ＲＳ２３２接口连接时，要把彼此公共地端（ＧＮＤ）同时与２４Ｖ直流电源负极连接在一起，来消除公共地线电位差影响以及可能出现的外部干扰。按照ＲＳ４８５方式连接时，若传输距离较长则应在ＲＳ４８５网络首尾匹配终端电阻，防止信号反射造成信号失真［６］。实际连接如图３、图４所示
。
图３　ＣＭＰｔＰＲＳ２３２ＢＡ
串口接线示意图
图４　ＲＳ４８５主从轮询接线示意图
ＣＭＰｔＰＲＳ２３２ＢＡ串口模块支持自由口通信协议以及３９６４（Ｒ）通信协议，后者涉及数据校验，通信伙伴在数据交换前后需要握手应答和结
１
８
２０２２ ４５机电一体化　
束确认，实际应用较少。惯导模块和深度计均已设置为主动输出模式，所以Ｓ７１５００ＰＬＣ只需要被动接收数据即可，故串口协议选择自由口通信。该协议使用简单，还可根据需要自由规定通信双方数据协议。实际组态硬件时，需要注意将“组态消息接收—将接收的帧装入缓存区：”一栏设置为“１”，并去掉勾选“防止覆盖操作”一项，以减少因程序执行周期造成的串口数据在ＣＰＵ内更新延迟，从而满足清洗机器人对获得传感器数据实时性的要求。
Ｓ７１５００ＰＬＣ通过调用“Ｒｅｃｅｉｖｅ＿Ｐ２Ｐ”通信指令来接收串口数据。为该指令选择正确端口号，并在ＤＢ块（数据块）新建一个以字节为单位的数组“Ａｒｒａｙ［０．．  ］ｏｆＢｙｔｅ”，拖放在“ｂｕｆｆｅｒ（接收缓存区）”用于存放接收到的数据，同时取消该数据块的“优化的块访问”，按照绝对地址进行寻址。
２．２　以太网通信
Ｓ７１５００ＰＬＣ通信功能强大，ＣＰＵ集成了一个ＰＲＯＦＩＮＥＴ接口，支持ＴＣＰ／ＩＰ、ＩＳＯｏｎＴＣＰ、ＵＤＰ和Ｓ７通信协议，可以非常方便地与其他设备建立网络链接。该接口采用直通网线或交叉网线两种连接方式直连ＣＰＵ或经过交换机连接ＣＰＵ［７］。在清洗机器人控制系统中，上位机与Ｓ７１５００ＰＬＣ通过脐带缆和交换机建立以太网连接，因而属于交叉连接类型。通过在通信两端增加信号放大模块的方式解决了以太网因传输距离远导致信号衰减而无法通信的问题。
两台倾角仪通过ＲＳ４８５串转网模块，经交换机与Ｓ７１５００ＰＬＣ建立以太网链接，对两台倾角仪仍采用主从轮询方式获取数据。调用开放式用户通信指令“ＴＳＥＮＤ＿Ｃ”和“ＴＲＣＶ＿Ｃ”，组态网络连接参数，连接伙伴选择“未指定”，连接类型为ＴＣＰ连接，输入连接对象ＩＰ和端口号，并将ＰＬＣ设置为主动连接。这样Ｓ７１５００ＰＬＣ作为客户端将不同站地址的１６进制报文轮流发送给两台倾角仪，当倾角仪识别到报文中地址码与本站地址符合，将根据报文中的查询功能码做出应答，应答数据通过以太网链接存入ＰＬＣ指定存储区，从而完成通信过程。
上位机使用基于ＧｉｔＨｕｂ开源组件“ＨｓｌＣｏｍｍｕｎｉｃａｔｉｏｎ．ｐｙ”，实现与Ｓ７１５００ＰＬＣ正常通信。该组件为Ｐｙｔｈｏｎ提供了一个ＡＰＩ（应用程序接口），同时支持Ｓ７协议和Ｆｅｔｃｈ／Ｗｒｉｔｅ协议，但前者使用起来更为便捷，能够满足实际需要，并且无需在ＰＬＣ端进行过多配置，仅需将Ｓ７１５００ＰＬＣ“链接机制”下的“允许来自远程对象的ＰＵＴ／ＧＥＴ通信访问”勾选上，就能够实现上位机与Ｓ７１５００ＰＬＣ的以太网通信。
３　软件设计
清洗机器人控制系统软件设计包括上位机软件和下位机软件设计。上位机基于Ｐｙｔｈｏｎ语言进行开发，该语言易学易懂，可读性强，且库函数种类丰富，通过简单调用，就能够开发出新的功能，十分适合上位机程序编写。下位机软件在西门子专门的ＴＩＡ博途软件平台进行开发，该平台集成度高，提供在线诊断、状态监控和调试功能。
图５为清洗机器人控制界面。该界面左下角包含启动、急停、系统运行状态显示以及运行数据记录功能；右侧则提供了一个从程序界面直接输入，调试推进器的功能，以及推进器转速、手柄数据实时显示，
直接由鼠标点击控制清洗机器人运动的按键功能。界面上方为两个网络摄像头视
图５　清洗机器人控制界面
２
８　机电一体化２０２２ ４５
频画面显示区，以及一个动态展示清洗机器人姿态的窗口，这一功能是根据窗口下方传感器实时数据变化来实现的。
３．１　上位机设计
上位机程序界面基于Ｐｙｃｈａｒｍ＋ＰｙＱｔ５进行编写。受Ｓ７１５００ＰＬＣ扩展串口模块限制，惯导模块数据输出频率设为２０Ｈｚ，为充分发挥惯导性能，采取Ｓ７１５００ＰＬＣ基于ＴＣＰ／ＩＰ协议作为客户端主动发送数据给上位机的方式，因而需要在上位机建立一个ＴＣＰ服务器用于接收。利用“ｉｍｐｏｒｔｓｏｃｋｅｔ”指令导入套接字模块，通过继承ＰｙＱｔ５自有线程管理ＱＴｈｒｅａｄ类，创建一个ＴＣＰ服务端线程类，并在类方法“ｉｎｉｔ”中对程序初始化，使用“ｓｏｃｋｅｔ．ｓｏｃｋｅｔ（）”方法创建ＴＣＰ套接字和实例化对象，设置计算机本地网络ＩＰ地址与Ｓ７１５００ＰＬＣ处在同一网络层，并用“ｂｉｎｄ（）”函数将实例化的套接字对象与计算机ＩＰ以及端口号绑定，“ｌｉｓｔｅｎ（）”启动被动连接，再对实例化套接字对象使用“ａｃｃｅｐｔ（）”指令来接收ＰＬＣ端连接请求。当ＰＬＣ发起主动连接后，该函数将返回一个新的服务套接字以及客户端网络地址，这样对该服务套接字调用“ｒｅｃｖ
（）”指令，使用“实例化对象．ｓｔａｒｔ（）”命令启动线程，就可以正常接收来自Ｓ７１５００ＰＬＣ客户端主动发送的数据了。Ｐｙｔｈｏｎ接收手柄串口数据需要导入“ｐｙｓｅｒｉａｌ”模块，使用“ｓｅｒｉａｌ．Ｓｅｒｉａｌ（）”方法来创建连接对象，并用“ＩｓＯｐｅｎ（）”方法来判断串口是否打开，在程序中设置跳转执行条件，只有串口打开成功才可进行下一步操作。因程序需要循环监听串口数据，因此也通过继承ＱＴｈｒｅａｄ类来创建线程，线程启动方法均放在界面程序中启动。
上位机通过Ｓ７协议向下位机写入数据，需要从“ＨｓｌＣｏｍｍｕｎｉｃａｔｉｏｎ”模块导入“ＳｉｅｍｅｎｓＳ７Ｎｅｔ”“ＳｉｅｍｅｎｓＰＬＣＳ”两个方法。首先通过“ＳｉｅｍｅｎｓＳ７Ｎｅｔ（）”方法实例化Ｓ７１５００ＰＬＣ连接对象，建立与ＰＬＣ端的长连接，从而方便连续读写数据。“ＣｏｎｎｅｃｔＳｅｒｖｅｒ（）．ＩｓＳｕｃｃｅｓｓ”方法用于判断与Ｓ７１５００ＰＬＣ是否成功建立通信连接。由于操纵手柄从数据接收到处理后下发是运行在不同线程中的两个类，因而需要实现两个不同线程间的数据交换，这里使用ＰｙＱｔ５特殊的“信号”和“槽”功能来实现。使用“ｐｙｑｔＳｉｇｎａｌ”建立信号并设置触发信号条件，当某一信号达到触发条件，使用“ｅｍｉｔ（）”指令来将数据发送到指定功能的槽函数，这样便可以实现不同线程间的通信，也就达到了利用操纵手柄间接控制清洗机器人水下运动的目的。
３．２　下位机设计
Ｓ７１５００ＰＬＣ基于ＴＣＰ／ＩＰ协议主动向上位机发送惯导数据需要精确定时，使用循环中断组织块来完成。在
程序块中添加ＯＢ３０（循环中断组织块），将其循环时间设置为２５０００ｕｓ，产生一个周期为５０ｍｓ的脉冲信号，利用这个脉冲信号作为发送命令，当扫描信号为上升沿时，“ＴＳＥＮＤ＿Ｃ”指令将向上位机发送数据。
在ＯＢ１００（启动组织块）中，存放程序初始化指令“ＩＮＩＴ＿ＲＤ”，每次系统上电启动前进行一次初始化，同时将两台倾角仪轮询指令也存放在这里。需要注意的是初始化指令“ＩＮＩＴ＿ＲＤ”必须在初始化倾角仪轮询指令之前，否则轮询指令初始化不成功，这和ＰＬＣ工作原理有关。
上位机通过Ｓ７协议与Ｓ７１５００ＰＬＣ建立主动通信，由于这种特殊通信方式Ｓ７１５００ＰＬＣ不能主动感知当前系统连接状态，一旦出现某种原因导致系统离线等突发情况，清洗机器人水下运动将会失控，因而有必要采取连接保护措施。利用前面ＯＢ３０中Ｓ７１５００ＰＬＣ与上位机所建立的ＴＣＰ主动连接，“ＴＳＥＮＤ＿Ｃ”指令ｅｒｒｏｒ（错误标志位）作为连接信号，Ｓ７１５００ＰＬＣ一旦与上位机失去连接，该标志位状态变为“１（Ｂｏｏｌ型）”。在主程序块ＯＢ１中添加比较状态指令，当检测到连接标志为“１”，将立即触发急停信号，水下控制系统进入连接等待，若连接成功，系统恢复正常工作。Ｓ７１５００ＰＬＣ程序流程如图６所示。
３
８
孙涛２０２２ ４５机电一体化