基于STM32 两轴深水观测云台控制系统设计

任福深 ，王 茜 ，刘 均 ，张 园

（1.东北石油大学 机械科学与工程学院，黑龙江 大庆 163318；2.陕西安然能源科技有限公司，陕西 西安 710000）

0 引言

智能视频监控技术结合图像、视频处理等技术，是实现运动目标自动检测和跟踪的关键技术[1-2]。水下机器人是海洋能源开采的重要装备，常代替人工在危险环境和人类视觉受阻的环境工作，需要借助云台实时监测水下环境、保证其安全高效的作业。

国内外云台多为单自由度或固定式，且常见的云台微控制器多为单核或双核，数据传输速度和存储量不高[3]。2014 年，中国海洋石油总公司设计了一款移动式云台，只适用于浅海水下场景的实时监控[4]。2016 年，殷莉甜针对水下云台结构的密封性和图像检测要求，设计了一款应用于水下焊接机器人的双目视觉系统，但其检测精度不高[5-6]。基于上述相关研究，本文设计了基于STM32 两轴深水观测云台结构及其控制系统，不但能实现回转和俯仰两自由度的运动，而且降低了水下机器人系统的开发成本，是水下机器人智能化研究的关键技术。

1 两自由度云台结构方案设计

图1 所示为两自由度深水观测云台的整体结构图，其主要由云台主体、数字摄像头、模拟摄像头以及深水LED 灯组成。由于水下机器人内部空间狭小，对云台结构进行了小型化设计，并采用连接板对摄像头和灯进行固定。

图1 两自由度云台整体结构图

云台电子仓和摄像机与灯分别采用水密电缆进行电气连接。云台结构采用两级O 形圈进行密封，整体密封性好，可以承受1.5 MPa的深水压力。如图2 所示，采用步进电机和角度传感器控制云台回转和俯仰两个自由度的运动，通过搭载数字摄像头、模拟摄像头和两个深水LED 灯，水下机器人可以对不同环境的深水工况进行实时监控和视频图像采集。

图2 两自由度云台电机布置

2 两自由度云台系统总体设计

2.1 云台控制系统总体方案设计

两自由度深水观测云台控制系统组成关系如表1所示，包括云台机械结构、通信和视频采集、上位机控制等6 个子系统。深水观测云台通过连接板和固定板，实现了水下高清摄像机和深水LED 灯随步进电机在俯仰和回转两个方向的同步运动。通信系统实现云台上位机和下位机的数据交换。上位机控制系统是操作员实时获取水下工况和机器人云台运动参数，并即时调控的平台。调光系统通过PWM 脉宽调制、调节深水LED 灯的亮度，以适应缺少光源的深水工况，改善拍摄环境，为采集水下高清图像和视频提供保障。

表1 两自由云台控制系统组成

2.2 云台控制系统硬件设计

2.2.1 云台测控系统组成

云台控制系统硬件主要由云台通信主控板和云台摄像头与LED 灯的控制板两部分构成。采用RS485 主从通信方式，对控制电机转向的控制命令以及大量数据进行解码、存储和调用。主控板控制系统主要由MCU、A4988 电机驱动模块、IO 接线端子、RS485 通信接口与电源电路等部分组成，如图3 所示。

图3 云台测控系统组成图

云台通信主控板采用STM32F103C8T6 芯片，它是STM32系列中具有广泛应用的常见型号，使用了高性能的ARM Cortex-M3内核[7-8]。

2.2.2 云台电机控制原理

采用28 行星减速、两相混合式步进电机使二自由度云台能够实现回转180°、俯仰±50°大范围水下环境的监控。电机的供电电压为24 V，扭矩为0.18 Nm。电机控制模块电路如图4 所示。

图4 电机控制模块电路图

设定电机转一圈，需要400 个脉冲。电机转速和脉冲频率成正比。通过控制1 s 内发送的脉冲个数和脉冲频率，调节电机转速。电机驱动芯片选用A4988，其模块电路如图5 所示。

图5 A4988 模块电路图

步进电机是将电脉冲信号转变为角位移或线位移的开环控制元件。步进电机的脉冲数和脉冲频率分别决定了其转动的速度和总角度位移，所有形式的运动方式改变均可以在很少的脉冲数量下完成。云台电机控制如图6 所示，通过改变相应IO 口电平即可控制电机的转向。使用角度传感器记录电机转的圈数，计数随角度传感器的转向而增加或减少。随着电机轴的旋转给出一个精准的角度信号。电位器测一圈，确定云台的当前位置。

图6 云台电机控制图

2.2.3 云台通信方案设计

水下装置对通信系统的可靠性、实时性以及稳定性具有很高的要求[9]。云台通信方案如图7 所示，采用电力载波（电缆）的方式实现上位机控制命令和下位机的控制模块进行通信。通过RS485 通信电路控制云台、摄像头和灯等外部设备。下位机接收到上位机指令后，获取并实时调控云台当前姿态位置和转动速度。

图7 云台通信方案示意图

2.2.4 云台摄像头和灯的控制原理

两自由度深水观测云台搭载的水下摄像头与深水LED 灯组成了水下机器人视觉系统，深水LED 灯辅助摄像头拍摄、对深海光线不足的环境进行补光，是水下机器人进行深海环境图像观测与采集的重要依据。云台选用两个高清模拟摄像头和数字摄像头。模拟摄像头固定安装在机械臂正下方，是导航和发现目标的依据。

摄像头和灯的控制原理如图8 所示。镜头通过电荷耦合器件（CCD）将深水图像传递给数据处理器（DSP），之后再发送给摄像头和灯的主控制器（MCU），通过以太网传递到地面上位机，上位机的视频采集卡对传输过来的视频进行储存。数字摄像头采集画面清晰度高；模拟摄像头通过两根视频线将模拟信号直接传输到地面模拟显示器上，数据传输速度快，便于对深水工况进行实时监控，获取高质量水下图像。

图8 云台摄像头和灯的控制原理图

2.3 云台位置PID 控制算法

云台的运动特性[10]对拍摄图像的清晰度有关键影响，在选取云台的控制方式之前，需要对云台的位置，回转速度v1、俯仰速度v2以及云台俯仰方向的最大倾角αmax等参数进行标定。

PID 控制[11]是一种线性控制方法，其控制原理框图如图9 所示。

图9 PID 控制原理图

PID 控制结合了比例控制、积分控制和微分控制，其表达式如式（1）所示：

其基本控制思路为：当设定位置和目标实际位置差值较大时，采用合适的比例系数，使云台快速移动到指定位置，对目标进行跟踪和图像视频采集，以达到良好的监控效果。

位置式PID 算法和增量式PID 算法应用广泛，适用于不同的控制场景[12]。位置式PID 算法，计算时对e（t）进行累加。本文选用增量式PID 控制每次输出较上次输出的增量，不会出现误差累积，其动态方程如式（2）和式（3）所示：

式中，e（k）为本次偏差，e（k-1）为上一次的偏差，e（k-2）为上上次的偏差；KP为比例系数；TI为积分时间常数；TD为微分时间常数。

操作员通过地面上位机发送运动控制指令，设置云台期望的运动目标位置。根据当前位置和期望位置的误差，由运动控制中断程序进行步进电机运动控制脉冲的计算和产生，采用角度传感器对云台转动的角位移进行计数，并转换为闭环坐标值，保存在寄存器中。在运动控制中断程序内调用增量PID 控制算法模块，通过改变定时器中速度系数，从而控制发步进电机运动控制脉冲的快慢。当目标位置和实际位置相距较远时电机快速运动，相距较近时运动慢一些。云台增量式PID 控制流程图如10 所示。

图10 云台PID 控制流程图

3 云台控制系统软件设计

在Windows 操作系统上使用Visual Studio 软件，选用面向对象的C# 语言对上位机控制界面进行开发[13]。云台上位机控制界面主要有串口通信、云台姿态控制、视频监控、深水LED 灯调光等功能。云台下位机调试软件Keil5的主要作用是：对控制器的通信端口进行初始化、配置定时器，对控制命令和数据进行解析、数模转换，使用Modbus 实现数据的双向传输，实现各个子系统功能。

3.1 控制系统和DMA 数据采集

FreeRTOS 是源码公开、可在ARM 等多款处理器上移植的实时操作系统[14]。本文的云台控制系统选用STM32F103 控制芯片。在FreeRTOS 操作系统上设置不同优先级的多任务，优先级高的任务先运行。在实现高效利用CPU 资源的同时，保证了系统的实时性和可靠性[15]。

DMA（Direct Memory Access）技术用来提供在外设和存储器之间、存储器与存储器之间的高速数据传输。在这种方式下，不需要用到CPU，可以直接读写系统存储器[16]。

云台控制系统对云台实际回转和俯仰电机位置进行采集，根据设定值和实际值之差，执行运动控制中断程序内调用的增量PID 控制算法，控制发送步进电机运动控制脉冲的快慢。通过步进电机控制程序，输出修正后的步进电机驱动信号，调整云台位置，实现云台位置的精确调控。云台与上位机采用RS485 通信、DMA 中断进行数据采集，收到的数据存入缓冲区，主程序按照标志位处理。根据标志位控制电机位置主线程如图11 所示，定时器中断流程图如图12 所示。

图11 根据标志位控制电机位置主线程

图12 定时器中断流程图

3.2 Modbus 通信协议

Modbus 是一种串行通信协议[17]，现在是工业电子设备之间常用的连接方式，支持RS232 和RS485 电气接口。常用的Modbus 串行传输模式有ASCII 和RTU两种[18]。根据云台和上位机的通信数据量，云台控制系统采用Modbus RTU 模式，采用二进制表示数据，数据以帧的方式进行传输。串口通信参数设置为：1 位起始位、8 位数据位、1 位停止位，无校验，数据传输的波特率为38 400。

当云台下位机控制板收到上位机发送的控制命令时，先判断发送的数据地址是否对应下位机地址，若地址一致则读取信息并对收到的数据进行CRC 校验。若CRC 效验正确，则执行相应的任务。拷贝地址和命令、功能码，并将执行后的数据和CRC 校验码一起回送给上位机。若发送的数据地址与下位机地址不一致则返回错误。如果CRC 校验出错，则不返回任何信息。发送与接收数据都进行CRC 校验，确保了发送数据的准确性[19]。

4 系统通信测试与实验分析

4.1 通信测试和结果

云台实物调试如图13 所示。通过Keil5 软件对程序编译无误后，采用ST LINK 仿真器将程序下载到云台控制板中。采用RS485 一端连接电脑上位机，另一端连接云台，使用MODBUS POLL 软件调试电机转向。当上位机发送控制命令给云台控制板时，观察到云台在回转和俯仰两自由度上，能够根据相应指令，做出对应的运动。

图13 云台调试实物图

Modbus 数据帧包含的内容有：地址域+功能码+数据+差错校验。

当通过上位机给下位机发送（0x03）读寄存器指令（30 03 00 12 00 02 60 2F）后，下位机（云台）返回数据为（30 03 02 F1 37 C1 C6）。其 中F1 37 为返回数据，转换为二进制，表示水平转速为241 r/min。通过测速仪测得水平电机的转速为239 r/min，结果表明云台监控系统通信正常。

4.2 海洋实验结果

图14 所示为云台搭载在水下机器人上。在水下机器人进行作业时，地面工控机通过电缆将控制命令发送到机器人电子仓内控制板，再转发到云台控制板。图15所示云台控制系统完全满足水下机器人对云台性能的要求，实现水下工况的实时监测和图片采集。

图14 水下机器人装置

图15 工控机上云台摄像头拍摄的画面

5 结论

本文研制了一种两自由度深水观测云台结构及其控制系统，通过集摄像头和深水LED 灯于一体，为机器人在100 m 水深作业提供实时性的监控平台。

设计了基于FreeRTOS的云台控制系统，采用DMA 中断和USART 通信，实现实时多任务操作的同时，便于代码模块化编写，系统响应快。采用STM32 控制板结合PID 算法，应用RS485 通信和Modbus 通信协议，实现回转和俯仰步进电机的位置控制。通过通信测试和海洋实验，证明了本文所设计的基于STM32 两轴深水观测云台控制系统运行平稳，能够实现对深水高质量图片和视频的实时采集。

相比国内外高昂的云台装置，本文所设计的云台结构和控制系统均为自主研发，开发成本低，具有很强的实用价值和广阔的市场。