小型水下机器人运动控制系统设计

侯茂林，马春燕，庞 健，安宝宝

（太原理工大学电气与动力工程学院，山西 太原 030024）

0 引 言

水下机器人（Remotely Operated Vehicle, ROV）是能通过人的远程操控完成长时间、大范围以及危险区域水下作业任务的重要工具，能够完成一些复杂水域的探测和分析任务。由于水下环境复杂，考虑到人身安全，水下机器人可以到达人类无法到达的区域并获取该水域的信息和数据，并将获取的信息和数据上传到地面，已逐渐成为人类进行水下资源开发的主要工具[1-2]。在近海域、狭窄环境和水下地形复杂的环境以及执行一些特殊任务时，机器人的优势就会明显突出。水下机器人在水下大坝检测、水下搜救、水下环境监测、城市地下管道检测等多领域投入使用并发挥着越来越重要的作用[3]。

针对小型水下机器人具有较深厚技术积累的主要是美国、日本、英国等一些科技水平较发达的国家。美国SeaBotix 公司的LBV150 水下机器人是一款非常稳定的小型水下机器人，能够实现自动操作。Nekton 公司的“巡逻兵”总长为0.92 m，直径为0.09 m，重量为4.5 kg，装有温盐深、叶绿素、含氧量等传感器，主要用于海洋环境监测及多水下探测器的协调控制研究[4]。REMUS-100 是由美国Hydroid 公司研制的一款低成本水下机器人，是一种结构紧凑、重量轻的水下机器人，可搭载多种传感器，满足不同任务需求[5-6]。小型水下机器人具有节能、成本低、运动灵活、可在浅水和地形狭窄处工作等优点，加大水下机器人的小型化设计以及多功能的水下机器人的技术研究，可以解决国内民用小型水下机器人应用的困境，未来也具有更广泛的应用空间[7-8]。本文首先使用Solidworks 设计一款小型水下机器人的结构，基于该结构使用STM32 微控制器完成控制系统设计，提出一种模糊串级PID 的控制方法。最后进行水下试验，验证了小型水下机器人控制系统的有效性和良好的水下运动性能。

1 水下机器人结构设计

所设计小型水下机器人具有体积小、结构紧凑、便于安装和携带等特点，结构上具有对称性和较小阻力，并且各部分独立设计，可以轻松实现结构的扩展[9-10]。水下机器人的主要特点是：外部使用侧连接板来固定推进器，同时侧连接板为未来挂载其他设备预留了空间；运动控制、测量电路等可通过一个电路板实现，节省了内部空间，有比较好的空间利用率。水下机器人结构参数如表1 所示。

表1 水下机器人结构参数

水下机器人由本体框架、耐压电子舱、控制系统、运动系统等模块组成。通过Solidworks 完成水下机器人的机械结构设计以及模型的绘制。水下机器人Solidworks 模型如图1 所示，框架通过合理的布局，使得水下机器人保证一定的稳定性，并且重心和浮心保持在同一垂直线上。

图1 水下机器人模型

本体框架是水下机器人的本体，用来安装水下机器人的电子舱、推进器等配件。由于在水下工作，水下机器人主框架选用重量较轻、耐腐蚀、强度和硬度较高的尼龙材料。采用2 个厚度为8 mm、一体成型的侧板和4 个半圆管扣件将水下机器人组装成一体，侧板上预留安装孔位以方便推进器和传感器的搭载。

耐压电子舱位于机体中间，用于封装控制器、电源板等电子器件，是整个ROV 系统的浮力提供者。为减小水下机器人运动时的阻力，降低能量消耗，舱体采用圆柱形管作为主体。根据水下机器人的工作环境要求，圆柱形密封舱和前端摄像头导流罩使用机械强度较高、耐磨、耐腐蚀性的PMMA。PMMA 具有很高的透明度，视觉清晰，可以达到92%的透光率，可以保证摄像头正常工作以及观察舱内工作情况。水下机器人的密封对整个系统至关重要，目前，耐压壳体的密封大多使用静密封，即使用O 型圈固定在舱体和法兰之间，防止发生泄露。

控制系统主要负责水下机器人的运行，是机器人的核心部分，由图像系统、运动控制系统、通信系统、能源系统等组成。

水下机器人使用水下直流无刷电机驱动螺旋桨产生的反推力作为动力，ROV 采用6 个水下推进器为其提供动力，推进器置于两侧连接板上，布置位置与方向如图2 所示。

图2 推进器布置位置和方向

推进器对称布置，通过控制推进器的转速可以实现前进、升降、转向、悬停等动作。为避免水下机器人绕竖直方向中心线产生自旋运动，对角线上的2 个竖直推进器旋转方向一致，而不在对角线上的2 个竖直推进器旋转方向相反，水平方向上2 个推进器旋转的方向相反，防止水平推进器产生横滚力矩[12]。

2 水下机器人控制系统

水下机器人的控制系统首先要完成最基本的运动控制，其次还能够处理传感器数据，接收地面上位机的指令，实现水下信息采集、图像视频传输等功能[13]。

2.1 硬件设计

水下机器人控制系统的硬件电路架构如图3 所示。该架构由STM32 控制板负责控制水下机器人运动以及传感器数据采集，采用树莓派完成水下机器人视频传输，以太网通信模块配合光纤完成水下机器人与地面之间的有线通信，单片机通过串口实现与树莓派之间的数据传输，电源管理模块为各模块工作提供所需电压。

图3 水下机器人硬件电路总体架构

水下机器人在运动过程中需要采集运动姿态信息和传感器数据，并有效接收远程指令的调控，因此对处理器的速度和运算能力有较高要求。综合考虑市场上的微控制器，将意法半导体的STM32F407 单片机作为主控芯片。 STM32F407 单片机基于高性能ARM®Cortex®-M4 32 位RISC 核心，运行高达168 MHz频率，其强大的运行速度和数据处理能力可以满足多数进行简单控制的机器人系统的需求。

水下机器人的供电系统使用24 节9CLGHE4 的18640 电芯，6S 高倍率锂电池呈圆柱外观，能够达到40 A 的放电能力，容量为10 400 mA·h，带有一定的保护功能。24 V 电压经转换后可以输出5 V 和12 V 电压为主控和各传感器供电。水下机器人控制系统功能的实现需要一种远距离通信方式。为保证水下控制和视频传输的实时性，采用光纤通信方式。光纤通信是一种以光导纤维为传输介质、在媒介中传输光信号的远距离通信方式，具有传输速度快、距离远、抗扰能力强等优点，采用光纤收发器实现数据传输过程中的光电信号相互转换。

传感器模块主要负责对水下实时信息的采集。传感器数据采集包括温度、深度、姿态角等。水下机器人机身尾部通过穿线螺丝安装MS5837 压力传感器，其同时具有深度和温度测量功能。通过I2C 协议与单片机通信，将传感器采集的数据经过滤波、校正和温度补偿后得到准确的压力值，从而计算出水下机器人的深度，同时传感器具有优异的防水性能。

为提高水下机器人姿态信息获取的准确性，姿态传感器选用MPU9250，其集成了陀螺仪、加速度计和磁力计，通过SPI 通信方式与单片机通信，具有良好的动态响应特性。根据水下机器人的结构和功能设计，要求单片机输出7 路脉冲宽度调制信号，其中6 路用于推进器转速控制来实现水下机器人姿态的控制调整，1 路用来控制舵机实现摄像头的二自由度运动，增加水下摄像头的拍摄角度。

2.2 软件流程

水下机器人的主程序控制流程如图4 所示。

图4 主程序控制流程

图中，主程序启动后声明产生任务调度表，添加任务需要在任务表添加新任务代码。任务项需要说明任务函数名、运行时间单位以及运行时长，控制器可以有效估计运行时间。主程序在完成任务调度表的声明后，需要获取任务的地址和数量，加载设备的默认参数，进行任务初始化，确定任务调度循环的时间间隔。完成上述步骤后，程序进入任务的循环调度中，有序执行任务。在任务的循环调度中，判断当前设备的工作运行模式，在对相应的运行模式进行初始化配置，自动模式配置下完成对设备位置、姿态和电机的控制，手动模式下接收遥控指令，控制推进器电机，实现对水下机器人实时控制功能。

3 水下机器人运动控制算法

水下环境复杂多变，高效的运动控制是水下机器人完成各种任务的基础。水下机器人使用的控制方法有PID 控制、滑模控制、神经网络控制等。但复杂的水下环境对水下机器人的实时性有较高的要求，先进控制虽然有较大优势，但是在调节过程中运算复杂、参数整定困难、水下环境中适应性不强，因此大多只留在仿真计算上，在实际工程中应用很少[14]。

目前实际应用中大多采用PID 及其优化算法，数字PID 控制算法一般有两种：位置型控制算法和增量型控制算法。本文采用增量型控制算法，具体表达式为：

式中：u(t)为PID 控制器输出控制量；e(t)为控制系统输入期望值和系统实际输出值之间的差值；KP、TI、TD分别为控制器的比例、积分及微分系数。将式（1）离散化后可得离散PID 的表达式：

式中：T为采样周期；u(k)为第k时刻PID 控制器的输出值；e(k)为第k时刻控制系统输入量和反馈值之间的偏差。由此可以得到第k- 1 次的输出u(k- 1)，继而得到第k次输出的增量Δu(k)的表达式：

串级PID 控制就是将两个PID 控制器串级连接，分为内环和外环控制回路，外环控制主变量输出内环的控制量，内环可以提高系统的动态性能和稳定性，从而提高系统的控制效果。由于水下机器人是一种时变的非线性系统，难以对其构建精确的模型，本文采用模糊串级PID 控制。模糊控制（Fuzzy Control）是模拟人类推理与决策的智能控制方法，不依赖于精确的数学模型，在水下机器人这种无法建立准确数学模型的非线性控制系统中有很大的优势。模糊控制器根据误差和误差的变化率输出PID 参数的调节量，然后与初始参数相加得到系统的控制参数，最后再进行串级PID 控制。控制原理图如图5 所示，用模糊控制器输出内环控制器参数的变化量，外环控制器的系数采用初始参数。

图5 模糊串级PID 控制原理图

4 水下机器人实验测试

为了测试控制系统对水下机器人的运动和姿态控制效果，在图6 所示的水池中进行机器人运动实验。在水下进行直线运动、旋转运动以及定深运动，观察水下机器人的运行状态。

图6 实验环境

机器人在水中直线-转向-直线运动过程中偏航角变化如图7 所示。0～11 s 时，水下机器人保持初始航向角120°，11～18 s 时，连续转向最后航向角仍然保持在120°附近，在整个水下运动过程中，机器人可以接收上位机命令稳定运行。

图7 偏航角曲线

深度曲线如图8 所示。由图8 可知，0～20 s 时，水下机器人稳定下潜，没有出现较大的抖动，20 s 以后稳定地悬停在水下，深度保持在1 m 左右。由水下实验结果可知，设计的控制器可基本实现水下机器人的功能。在长时间的水下测试中同样测试了ROV 的密封性。

图8 深度曲线

为测试水下机器人图像系统，在水中放置仿真鱼模型，摄像头可以清晰地拍到仿真鱼，摄像头拍摄的画面如图9 所示。

图9 摄像头水下图像

5 结 语

本文设计一种水下机器人结构，并采用Solidworks软件建立3D 模型；其次，提出一种基于STM32 微控制器的水下机器人系统架构和模糊串级PID 的控制算法。水下试验结果表明：水下机器人可以在水池中实现姿态控制和定深控制，控制系统具有良好的运动控制能力；水下摄像头可以清楚地拍摄到水下画面并传输到上位机。从而验证了水下机器人控制系统的实现效果较为稳定可靠，可以作为浅层水域探测的工具。