水下机器人运动控制系统设计与分析①

张欢欢,田军委,熊靖武,赵彦飞,史珂路

(西安工业大学 电子信息工程学院,西安 710021)

随着海洋科学研究的进一步开展,人类探索海洋的难度与深度也越来越大.水下带缆遥控机器人(Remotely Operated Vchicle,ROV)因其安全、高效、续航力强而日益成为开发海洋资源的重要工具,是最早得到开发和目前应用最成熟的无人潜水器[1–3].

国内对ROV的研究起于20世纪70年代末,相比于欧美国家和日本处于落后水平,目前我国比较先进的ROV是“海龙号”深海机器人,它在前人的基础上解决了脐带缆断裂的问题.2002年,中科院沈阳自动化研究所成功研制了中国第一台能够进行水下作业的ROV——“CISTAR”,随后提出了一种基于水平面模糊避障规划和垂直面模糊避障规划相结合的三维实时避障规划方法.2014 年,“海马号”ROV 搭乘“海洋六号”综合科学考察,分三个航段在南海进行海上实验,标志着我国全面掌控了大深度无人遥控潜水器的各项关键技术[4,5].

本文以不断发展的ROV为研究对象,设计了基于CAN总线ROV的运动控制系统,以实现ROV参数采集、上浮下潜、转向、悬停等姿态的控制.要达到以上运动要求,指令信息传递的载体则显得极为重要,它起着控制箱与水下机器人信息传递的作用[6–9].

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

1.1 水下机器人结构组成

水下机器人控制系统中,操作人员只要下达指令,水下机器人就会做出相应的动作.水下机器人主要有水上控制平台和水下执行机构两部分,水上控制平台主要是一个控制箱,能够对机器人下达指令和接收水下机器人不同时刻的状态数据.水上控制平台下达控制指令,其中CAN总线作为控制箱与水下机器人信息传递的桥梁,水下主控器则会控制各执行机构作出相应动作.

姿态检测系统输出当前水下机器人的实时运动姿态和位置信息,与指定的位置信息进行比较,经过相关的程序和算法计算后,可以得到相应的位置误差,对水下机器人进行操作,主控器下达PWM指令控制驱动系统,调整推进器的速度,进而调整水下机器人的工作状态,使水下机器人到达预定的目标位置.

水下机器人的动力部分是安装在四个不同位置的推进器,它是由相应的驱动器进行控制,驱动器接收到控制信息就会驱动推进器,按照我们的要求做出合理的调整,推动机器人运动,硬件框图如图1所示.

水下机器人的稳定性可以被定义为在不存在修正行为的条件下,在受到扰动后能够回到平衡状态的能力,在运动控制系统硬件方案的设计中,整体方案的响应速度对水下机器人的稳定性控制是有一定联系的,基于此,对水下机器人运动控制系统进行设计.

图1 机器人本体硬件框图

1.2 水下机器人运动控制系统

如图2,涵盖了整个运动控制系统中的主控单元、通讯单元、检测单元、驱动单元及辅助单元,整个系统由主控单元中的运动控制器STM32F407来控制;CAN总线实现水下机器人与水上操控箱之间的双向通讯,是信息传递的桥梁;整个控制系统中的检测模块有9轴姿态传感器、漏水检测传感器、水深传感器以及温度传感器;扩展的接口包括无线通讯接口、GPS扩展预留接口、声呐预留接口及陀螺仪预留接口,使得之后机器人功能更加完善;动力单元是安装在四个不同位置的推进器及调重心步进电机,分别由电调及步进电机驱动板进行驱动;辅助单元包括摄像头、LED灯及激光器,LED灯由控制器经过控制板进行控制,继电器控制整个水下运动控制系统的运行.

图2 水下机器人运动控制系统

如图3为水下运动控制系统实体图,目前已完成了对于主控板、控制继电器、激光器以及摄像头等器件的安装及连接,出于节省空间的考虑,将各个传感器如姿态传感器、温湿度传感器以及电调都置于底板上,将单片机用螺柱支撑起来,整个控制系统呈现为二层结构.

图3 水下运动控制实体图

1.3 控制系统各模块软件设计1.3.1 软件设计

水下机器人的运动控制系统包括网络通信模块、串口通信模块、传感器数据采集模块、相应数据处理模块、控制器模块.主控器对传感器的信息处理和水下机器人推力分配算法等统一在实时操作系统的架构下进行管理,从而有效提高动力定位控制系统的实时性和可靠性.实时操作系统接收采集到的数据和来自串口通信的数据,保证数据运行的高效性和实时性.

根据水下机器人运动控制系统功能结构,将系统软件进行模块化设计,使主程序分成若干个子程序模块分别进行编写、设计和调试,这样可以提高编程的效率和准确性,易于查找错误,使程序更具可读性.本课题的运动控制系统软件部分主要由5个模块组成,其结构框图如图4所示.

图4 运动控制系统软件结构框图

1.3.2 各模块流程框图

主程序开始时,首先是对STM32F407单片机进行初始化,然后通过传感器分别采集水下机器人外部周围水介质温度数据信息、内部温湿度数据信息和水深数据信息,而通过采用中断的方式,一方面是对水下机器人姿态信息的采集,另一方面是接收外部中断控制指令,控制推进器,从而实现对水下机器人的姿态控制,主程序流程图如图5所示.

图5 主程序流程图

温湿度采集模块作为检测水下机器人内部是否发生泄漏的传感器,在保障水下机器人安全运行方面有着极其重要的作用.温湿度采集模块选用的SHT30-DIS温湿度传感器,其测量分辨率分别为16 bit(温度),16 bit(湿度),其程序流程图如图6所示.

图6 DHT30 数据采集程序流程图

压力采集模块主要是对水下机器人当前位置水压力及水温数据的采集,它也是一个模拟量的采集过程,需要将其模拟量转化为数字量,得到压力值,再通过计算将压力值转化为距离(深度)信息,从而定位水下机器人的位置,选择供电电压为3.3 V的MS5837型号的压力传感器,水深测量分辨率高达2 mm.模拟量采集程序流程图如图7所示.

图7 模拟量采集程序流程图

姿态传感器主要是采集水下机器人当前的姿态信息,该模块与单片机之间通过串口协议进行数据的传递,每当给模块发出一个中断信号,数据采集就会更新一次,从而不断记录姿态信息,如图8,图9,本文选用的是JY901电子陀螺仪(三维角度传感器,原理图如图10),它集成了高精度的陀螺仪、加速度计、地磁场传感器,采用高性能的微处理器和先进的数字滤波技术,能有效降低测量噪声,提高测量精度,模块内部集成了姿态解算器,配合动态卡尔曼滤波算法,能够在动态环境下准确输出模块的当前姿态,在本文中即为水下机器人的姿态信息,图11为其上位机显示界面,界面内容包含有数据读取窗口、图像数据显示窗口、三维方向指示窗口和磁场指示窗口,从数据读取窗口可以分别读取到当前模块的时间、三维角速度、三维角度、三维加速度、三维磁场数据,同时可以在图像数据显示窗口各个参量变化趋势.

图8 接收中断

图9 数据读取

图10 JY901 电路原理图

图11 姿态传感器上位机界面显示结果

2 推进器推力及测试

2.1 推进器推力分布

图12为推进器推力分示意图,其中(a)为竖直方向的推进器分布图,(b)为水平方向的推进器分布图,如图(c),F1、F2为水平方向的推进器转动时ROV所受的力,F3、F4为竖直方向的推进器转动时ROV所受的力.

图12 推进器推力分布示意图

水平和竖直面的推进器分布采用的是双推进器平行分布,推力和力矩计算如下:

2.2 推进器测试

图13为推进器测试时实物接线图,其中电调为50 HZ PWM 信号控制,1-2 ms PWM 范围,1.5 ms为中位停转,驱动电机占空比为5%-10%,其中7.5%为中位停转信号.

图13 推进器测试

如表1、表2,表明此电调可作为推动推进器的装置,推进器空转的电压、启动电流与转动电流随转速的增大而增加.

表1 推进器正转数据

表2 推进器反转数据

3 倾角与下潜深度控制方案及仿真分析

3.1 倾角与下潜深度控制方案

调整和保持下潜深度是水下机器人的基本航行能力之一,本文中水下机器人用垂直方向的两个推进器调节深度,通过调整推进器转速调节ROV的深度,从而实现其下潜及上浮运动,所以要考虑机器人浮力变化的过程,也就是要注重推力计算.推进器转速v=0时,浮力F=0,ROV 在水面保持平衡,处于正浮力状态.当v增大到一定值不变时,浮力减少到0,进入水下平衡状态,继续增大v,机器人下潜.同时调节四个不同位置推进器的v,产生各个角度相异的浮力可以使机器人具有不同的水下姿态,俯仰倾斜,左右倾斜,以至于扩大照明系统的可视范围,增加探测方位及面积,正常情况下机器人的倾斜角度无需控制,只需保持水平与竖直方向两个推进器的v一致即可.

倾角控制的目的是在机器人初始下潜以及探测阶段,控制机器人的航行姿态,在水下大部分时间,保持水平即可,其中水平倾角θ以及竖直倾角σ的控制是通过调节推进器的v产生浮力差,即产生上下左右旋转的偏颇力矩.

如图14,ROV的动力系统是一个典型的闭环反馈控制系统,其核心是带有算法的控制器,首先给出姿态指令,然后将测量系统(传感器)实时测得的ROV位置经融合处理后与给定值进行比较获得误差信号,并将其进行简单的坐标变换处理后作为控制器的输入,控制器根据相应算法计算并且输出推力或推力矩指令,推进器根据指令使得ROV保持在给定深度.

3.2 ROV运动模型及仿真分析

如图15采用两种右手直角坐标系即固定坐标系E-ξηζ和运动坐标系O-xyz.

把水下机器人的运动坐标系的零点相对于固定坐标系的速度U在O-xyz坐标系上的三个投影的分量非别是u(横向速度)、v(纵向速度)和w(垂向速度);水下机器人绕固定坐标系的原点的角速度Ω在O-xyz坐标系上的三个投影分量分别是p(横摇角速度)、q(纵摇角速度)和r(摇舷角速度);我们把作用在水下机器人本体结构上的外力F在O-xyz坐标系上的三个分量称为X(纵向力)、Y(横向力)和Z(垂向力);对应的把水下机器人的三个力矩的分量称为K(横摇力矩)、M(纵倾力矩)和N(摇艏力矩).我们规定把坐标轴的正半轴作为水下机器人的运动速度和力的正向,按照右手螺旋规则确定角速度和力矩服的正向.用运动坐标系来表示水下机器人的六个自由度的运动和力(力矩).

图14 ROV 定位控制系统框图

图15 水下机器人的空间运动坐标系

参考文献[10],根据刚体动力学理论相关知识,利用牛顿—欧拉方程建立水下机器人空间六自由度运动的一般方程,由一般方程可得ROV纵垂面运动简化方程为:

其中,m为水下机器人质量;u、w、q为 (角)速度;、、为(角)加速度;xG、zG是水下机器人的重心坐标参数;X、Z、M是力(矩);Iy为水下机器人围绕运动坐标系的三个坐标轴的转动惯量.

参考文献[11],在将ROV结构参数确定好之后,考虑到推进器推力参数,ROV所受浮力与重力,水下惯性力系数及粘性阻力系数等因素影响,代入式(3)简化后可得纵倾角速度的传递函数为:

由(4)式,可以得到纵倾角的传递函数:

进而可以得到深度传递函数为:

利用MATLAB中的simulink模块进行PID控制器的设计及仿真.纵倾角闭环控制系统方框图如图16.在输入单位阶跃信号时,利用试凑法对水下机器人的数学模型进行调节,最后选择了一组较为理想的调节参数,其中Kp=7、Ki=0.01,Kd=3,得单位阶跃输入下的响应曲线结果如图17所示.

图17 纵倾角单位阶跃响应曲线

深度值的闭环控制系统如图18所示.

图18 深度的闭环控制系统

Kp=15、Ki=0.01,Kd=8,得单位阶跃输入下的响应曲线结果如图19所示.

图19 深度的单位阶跃响应曲线

由系统仿真图可得,运动控制系统具有较快的响应速度,响应时间短等优点,证实了整个系统的可靠性.

4 结论

本文建立了水下机器人的运动控制系统,设计了基于STM32F407的水下机器人嵌入式控制器并介绍了各个功能模块的具体实现方法,进行了纵倾角及深度仿真分析.在后期的研究中会进一步对水下机器人进行算法控制,提高其精确度,并会将机器人密封性完善后进行水下实时监测控制.