基于STM32的水下机器人设计

王 雷 ，杨庆喜 ，程 勇 ，张璇琛 ，郑 磊

（1.中国科学院等离子体物理研究所，合肥 230031；2.中国科学技术大学 科学岛分院，合肥 230026）

随着陆上资源开发殆尽，越来越多的国家开始将目光放在海洋。相比海洋环境恶劣危险，人类的潜水深度有限，不能直接完成深海及海底资源的探测任务。目前的状况是只有少数国家有能力对海洋资源进行初步开采。水下机器人强度高、抗高水压、可操控，能够完成深水水域的探测和分析任务，并将探测数据返回到陆地上，已逐渐成为人类进行水下资源开发的主要工具[1]。

水下机器人按照与水面支持系统之间的联系方式不同可以分为2种：有缆水下机器人（ROV），ROV通常需要电缆从母舰接受动力，并且通过电缆传输控制信号；无缆水下机器人（AUV），通常称作自治水下机器人或者智能水下机器人，能够依靠自身的智能控制系统进行决策和控制[2]。本款水下机器人旨在设计一款在浅水水域工作的，能够实现远程无线控制和无线图像传输的水下机器人。

1 系统概述

水下机器人设计总体分为三部分：结构系统设计、控制系统设计和控制算法及软件设计，其系统层次结构如图1所示。

图1 水下机器人系统结构Fig.1 Architecture of the underwater robot

1.1 硬件结构设计

水下机器人结构设计包括外壳支撑结构、密封舱、推进器及外部天线设计。外壳支撑结构负责将水下机器人电气系统分别固定到机器人机体上，同时为保证电气系统安全运行，需要为电气系统设计防水密封舱。水下机器人为6自由度刚体，其不仅需要在6个自由度上可以平稳运行，还必须保证在外界扰动条件下正常工作。因此在推进器布置上综合考虑抗干扰和提供运动动力的问题，垂直方向上布置3个电机提供水下机器人垂直方向推力，在水平方向布置2个电机，控制水下机器人前进和转向运动。因为电磁波是横波，水是良导体，趋肤效应严重影响电磁波在水下传输，因此需要为水下机器人设计浮标天线以保证水下机器人依靠电磁波正常通信。结构设计如图2所示。

图2 水下机器人结构Fig.2 Structure of the underwater robot

1.2 控制系统设计

水下机器人需要布置各类传感器来获取自身的状态，其中水压传感器获取水下深度，姿态传感器获取姿态角，GPS获取运动空间坐标，同时加入温湿度传感器来检测密封舱密封状态。控制器作为整个水下机器人的控制中心，一方面对传感器数据进行融合，获取水下机器人的运动状态，在外界干扰条件下做出控制指令维持水下机器人机身自平衡；另一方面通过接收遥控器指令响应相应的动作。

1.3 控制算法设计

水下机器人平稳运行是水下机器人完成水下任务的关键，因此水下机器人控制算法的核心是平衡控制[3]。水下机器人运动传感器可以获取水下机器人实时姿态角，水压传感器可以获取水下机器人水下运动深度，将传感器数据作为控制算法的输入，通过PID自适应控制算法输出驱动推进器，实现水下机器人平稳运行。

2 系统电气组成和实现

水下机器人电气架构主要包括传感器、控制器和驱动器三部分[4]，系统结构框图如图3所示。

图3 水下机器人系统框图Fig.3 Block diagram of the underwater robot system

2.1 传感器

2.1.1 深度传感器

为保证水下机器人平稳运行，必须获取水下机器人在水下运动的空间姿态，包括水下深度、空间姿态角、GPS坐标等。本控制系统采用MS5803-1BA获取水下压力。MS5803压力传感器支持SPI和I2C总线通讯，是一款高精度的压力测量传感器，压力测量范围为 10～1300 mbar，其精度可达到 0.012 mbar。其水深计算公式为

式中：P为水压传感器测得的压强；Pair为当地大气压强；ρ为水密度；g为重力加速度。

2.1.2 姿态传感器

水下机器人控制的核心是姿态控制，为提高水下机器人的控制精度，一款性能良好的姿态传感器就显得尤为重要[5]，该水下机器人采用MPU9250作为姿态传感器。MPU9250是一款9轴的运动跟踪传感器，包括一组3轴加速度计，一组3轴陀螺仪另一组是AK8963 3轴磁力计，并在其中融合动态运动处理器DMP。通过姿态融合算法，将加速度计、陀螺仪、磁力计数据融合成四元数，再由四元数转化为欧拉角，经过卡尔曼滤波将运动噪声滤除后将欧拉角作为运动控制的输入量通过控制算法转化为电机输出达到控制水下机器人姿态的目的。

2.1.3 GPS模块

该水下机器人通过GPS来获取空间经纬度坐标，从而为实现自动返航提供支持。GPS传感器采用ATDM332D-5N系列模块，该模块基于中科微第四代低功耗GNSS SOC单芯片-AT6558，支持美国GPS和中国的北斗卫星导航系统。水下机器人内置GPS模块，可以获取到经纬度坐标，通过设置起始坐标，对比水下机器人当前坐标，结合避障算法，从而实现机器人自动返航。

2.1.4 温湿度传感器

水下机器人密封舱用来安装电子设备和检测仪器，因此密封舱需要有足够的强度和可靠的密封性。为保证水下机器人运行状态的安全可靠，在密封舱中安装温湿度传感器来检测密封性能，采用SHT10温湿度传感器检测密封舱内部温湿度状态。SHT10包括一个电容性聚合体测湿敏感元件、一个用能隙材料制成的测温元件，传感器内部有一个精度高达14位的A/D转换器，适应串行接口电路实现无缝连接。

2.2 控制器

水下机器人采用了较多的传感器，为保证姿态控制的实时性，采用单独的控制器来实现水下机器人传感器数据解析和水下机器人电机控制。水下机器人采用STM32作为控制系统控制芯片，其中采用STM32F103CBT6作为电机控制器，STM32F407VGT6作为传感器数据融合的控制器。两片控制芯片之间采用串口进行数据交换。

STM32F103CBT6产生 5路 PWM输出，分别控制2个水平推进电机和3个垂直推进电机，STM32F407VGT6则负责采集运动传感器、水压传感器、GPS、温湿度传感器、激光测距传感器及遥控器PPM输出信号并将数据汇总、解析为控制信号后通过串口发送到STM32F103控制器中。STM32F103响应控制指令，通过电子调速器来驱动无刷电机，起到控制机器人姿态的作用。

2.3 电源系统

水下机器人通过6节高倍率26650锂电池供电，单节电池容量达到5200 mA，可持续提供10 A电流放电。供电系统采用锂电池串并联方案，将3节锂电池串联后得到电机驱动所需的12 V电压，采用2组12 V电源并联提高电源的电流输出能力，同时提高水下机器人的续航能力。电源系统可以持续提供20 A放电电流，达到整个电气系统对电流电压的需求。电池供电系统架构如图4所示。

图4 电源系统架构Fig.4 Architecture of the power system

STM32采用3.3 V供电，因12 V转化到3.3 V压降较大，传统LDO稳压器如AMS1117会产生较大的热量，为提高系统的可靠性，采用DC-DC电压转化方案。电源电压转化电路采用LM2576稳压芯片来保证电源的安全可靠。电源转换电路如图5所示。

图5 LM2576电路Fig.5 LM2576 circuit

3 软件系统架构

水下机器人软件系统包括控制程序和测试软件设计两部分。

3.1 控制程序

水下机器人控制程序可分为主程序、传感器数据采集和算法程序三部分。其中主程序负责水下机器人各个电气部件的初始化，传感器数据采集获取机器人实时状态，算法程序负责将传感器数据融合，转化为控制量输出。

主程序初始化包括系统时钟、串口初始化、定时器中断初始化、以及各个传感器部件的初始化。其中用于密封舱漏水检测的温湿度传感器，可以作为整个水下机器系统的异常检测。如果没有异常，系统开始响应定时器中断程序。定时器中断程序中包含传感器数据刷新和控制算法程序，这样可以保证系统执行的实时性。之后通过串口将控制指令发送到推进器控制MCU，执行相应的动作。控制系统流程如图6所示。

图6 控制程序执行流程Fig.6 Control program execution flow chart

水下机器人控制算法采用数字PID控制器。数字PID控制器具有结构简单、参数整定方便、能满足大多数控制性能要求等优点[6]。水下机器人控制的核心是姿态控制，因此PID控制效果直接决定了机器人能否平稳运行。水下机器人控制对象包括俯仰角（pitch）控制、横滚角（roll）控制、偏航角（yaw）控制、深度控制，另外水下机器人需要对遥控器控制指令做出响应。

3.2 测试软件

图7 上位机界面Fig.7 Host computer interface

测试软件采用LabVIEW编写，上位机界面如图7所示。LabVIEW是美国国家仪器公司开发的图形化编辑语言，因其简单快捷，极大提高了编程效率，降低硬件测试平台开发的难度。通过LabVIEW中的VISA驱动，搭建出基于串口通信的上位机测试软件，并通过2块433 MHz无线数传模块建立起上位机和水下机器人之间的通信，实时显示水下机器人运动状态，同时可以便捷地对PID参数进行在线修改，提高了PID参数调试的效率。

水下机器人采用Keil5软件对STM32控制器进行编程，通过ST-Link下载器将程序下载到控制器，使用LabVIEW上位机在线对PID参数进行修改。上位机姿态角显示如图8所示。

图8 上位机姿态角显示Fig.8 Attitude angle display by LabVIEW

4 系统测试

4.1 水下机器人静态调试

鉴于水下机器人的应用场景，因此必须对水下机器人进行水下调试，找出水下机器人最优的PID参数[7]。在静水水域对水下机器人的姿态角PID参数进行调试，采用的方法是试验凑试法，先对比例参数进行整定，通过上位机观察系统的响应曲线，直至得到反应快、超调小的响应曲线。若在比例控制下稳态误差没有办法满足要求，再加入积分环节。PI控制可以消除系统稳态误差，加入D控制会提高系统的动态响应性能。水下测试如图9所示，经验证水下机器人可以达到良好的静态性能，在水下可以做到长时间悬浮。

图9 水下机器人静态调试Fig.9 Underwater robot static debugging

4.2 水下机器人动态调试

在保证水下机器人静态性能的前提下，对水下机器人的运动性能进行测试，主要测试水下机器人运行过程中的姿态纠正和定深控制以及长距离运行中的航行角控制，经测试其在水中的机动性和指向性都是可以满足设计要求的。在长距离测试中同样测试了密封舱的防水性能和视频的传输，均得到良好的性能。

5 结语

本文介绍了一款基于STM32的水下机器人系统架构，采用STM32作为传感器数据的处理中心并控制推进器达到机器人精确控制的目的。水下机器人内置的433 MHz无线模块可以将水下机器人的运动状态反馈到上位机进行显示，同时上位机可以在线修改水下机器人参数。

经测试，所设计的系统可以达到良好的运动性能，满足设计要求，可以实现远距离无线控制，并将实时图像远距离传送到APP进行显示。系统长时间运行可靠，具有较强的安全性，可以作为浅层水域探测的工具。

[1]邢进.水下机器人运动姿态控制技术的研究[D].山东：山东科技大学，2008.

[2]姜婵娟.遥控式水下机器人PID运动控制算法优化研究[D].黑龙江：哈尔滨工程大学，2010.

[3]刘琼.微型航姿系统的设计与姿态解算算法研究[D].重庆：重庆邮电大学，2016.

[4]Vasilescu I，Varshavskaya P，Kotay K，et al.Autonomous modular optical underwater robot（AMOUR）design，Prototype and feasibility study[C]//IEEE International Conference on Robotics and Automation，IEEE，2006：1603-1609.

[5]刘镇，姜学智.PID控制器参数整定方法综述[J].电力系统自动化，1997（8）：79-83.

[6]Youakim D，Ridao P，Palomeras N，et al.Autonomous underwater free-floating manipulation using moveIt[J].IEEE Robotics&Automation Magazine，2017（99）：1.

[7]李晔，刘建成，沈明学.Dynamics model of underwater robot motion control in 6 degrees of frccdom[J].哈尔滨工业大学学报：英文版，2005，12（4）：456-459.