小型有缆无人水下机器人控制系统设计

肖朋振,刘 爽

(华东理工大学 机械与动力工程学院,上海 200237)

1 引 言

随着海洋资源开发以及水下领域作业任务的增加,水下机器人在水下作业中发挥着越来越重要的作用.其中ROV续航持久,成本相对较低,逐渐成为水下作业的重要工具.ROV工作于水下环境,具有非线性、易受环境影响等特点[1],对运动控制算法要求较高,同时要求整个控制系统要有较好的实时性和可靠性.

在水下机器人的控制算法研究中,文献[2]实现了自治水下机器人定深问题的自适应输出反馈控制;文献[3]实现了基于RBF神经网络自适应学习的滑膜控制控制;文献[4]实现了水下机器人的神经网络自适应控制等.虽然上述控制算法可以对水下机器人进行较好的控制,但对硬件平台的性能有较高要求,不适合在微控制器中运行.在水下机器人控制系统架构的相关研究中,文献[5]实现了基于VxWorks和CAN总线的水下机器人通信系统设计;文献[6]实现了基于CANopen协议和uCOS的水下机器人分布式控制系统;文献[7]实现了水下机器人通信系统的CAN总线冗余设计等.在以上的研究中,基于嵌入式实时操作系统和总线通信的分布式控制系统逐渐流行.采用基于CAN总线的分布式系统可以提高系统的鲁棒性和扩展性,但是每个节点都需要收发控制器,使得成本增加且系统结构复杂.在ROV的控制系统中引入嵌入式实时操作系统进行任务调度,可以简化软件框架设计,更合理、更有效地利用CPU的资源,获得更好地的实时性和可靠性[8,9].在上述研究中,应用较多的实时操作系统的是VxWorks,除此之外还有uCOS、uCOS-Ⅱ等,但是使用这些操作系统需要先购买其授权,也使得成本增加.

针对上述研究中存在的不足,本文从运动控制算法和系统架构方面设计了一种ROV控制系统.首先,将模糊控制和串级PID结合,实现了模糊自适应串级PID控制.模糊控制算法和PID算法对硬件平台的运算性能没有很高的要求,在32位MCU上可以轻松实现.其次,采用分布式子系统采集和处理传感器数据.传感器子系统负责采集所有的传感器数据并进行处理,避免了主MCU逐一读取每个传感器原始数据和复杂耗时的数据处理过程,提高了传感器数据的更新速率.主微控制器与传感器子系统之间采用UART通信,无需额外的收发控制器,只需要制定通讯协议即可实现二者通信,简化了系统硬件设计.最后,在MCU中采用嵌入式实时操作系统FreeRTOS进行任务调度.FreeRTOS是一款功能强大且开源免费的轻量级实时操作系统,调度策略灵活,可根据所需功能进行剪裁配置[10],完全满足ROV实时控制的需要.

2 ROV控制系统设计

ROV控制电路由主MCU、无刷直流电机、电调、电力线载波通信(PLC,Power Line Communication)模块、电压检测模块和传感器子系统等构成,结构框图如图1所示.主MCU采用ARM Cortex-M4内核的STM32F4系列,其RAM为196 KB,主频达168 MHz,拥有UART、SPI、IIC、以太网等通信接口以及定时器、ADC等丰富的外设[11].MCU不能直接驱动电机,而是通过其内部定时器产生4路PWM信号输入电调,再由电调直接驱动电机.MCU通过PLC通信模块接收外界的控制命令以及向外发送ROV的状态信息.

图1 ROV控制系统模块图Fig.1 ROV control system module

ROV运动和姿态控制首先需要获取传感器数据.由于传感器数据的采集和处理过程耗时相对较长,尤其是数据融合处理涉及大量的浮点型运算,所以本文采用传感器子系统进行这部分工作以减少对主MCU的占用.

传感器子系统主要由MCU、惯性测量单元(IMU,Inertial Measurement Unit)、水温水压传感器等组成.IMU测量的数据经过融合得到的姿态角数据主要用于ROV的姿态控制.水压传感器用来获取水压数据,通过换算得到深度信息用于ROV的深度控制.传感器子系统MCU和主MCU型号相同,分别通过IIC和SPI通信接口获取IMU和水温水压传感器的原始数据,然后对原始数据进行处理并通过UART接口与主MCU进行数据传输.

3 ROV运动与姿态控制

3.1 ROV运动形式

ROV的动力来源于电机驱动螺旋桨旋转产生的反推力,电机的放置位置、放置方向和数目直接决定了ROV可以实现的运动形式.本文研究的ROV搭载有四个电机,放置位置和放置方向的示意图如图2所示.电机转速用字母n表示,同时规定面对电机的输出轴,轴顺时针转动时转速n为正,逆时针转动时转速n为负.如果ROV在多个维度上运动,则电机的实际转速是各维度对电机转速控制作用的叠加.假设某一时刻控制算法对ROV的进退、偏航、升潜和俯仰维度的控制量输出分别是uv、ua、ud和uβ,四个电机的转速分别用n1、n2、n3和、n4表示,则满足公式(1):

(1)

在公式(1)中,a、b、c、d分别是进退、偏航、升潜和俯仰维度的响应系数,如果该系数设为零,则表示电机对该维度的控制不作响应.ROV控制算法的输入量是被控维度的设定值和传感器测量值,输出是该维度的控制量,然后根据公式(1)把各维度控制量转化为对电机的控制量从而对电机进行控制.

图2 ROV电机位置和方向图Fig.2 Motors position and direction

3.2 ROV控制算法

PID算法是目前应用最为广泛的一种控制算法,它根据设定值和反馈值构成控制偏差,将偏差的比例、积分和微分通过线性组合构成控制量,对被控对象进行控制[12].在ROV的PID控制中,只考虑了对姿态角的闭环控制作用,对角速度并没有反馈控制.角度-角速度串级PID是在角度PID的基础上增加一个角速度环PID控制器,即外环是角度PID,内环是角速度环PID,二者共同组成一个控制器[13].外环PID输入的是角度设定值和测量值,输出是角速度的期望值;内环PID输入的是角速度的期望值和测量值,输出是该维度的控制量.角度-角速度串级PID对角度和角速度都有反馈机制,拥有更好的稳定性和响应速度.

但ROV是一个非线性系统,且难以对其构建精确的模型,上述控制算法不能够满足要求.模糊控制(Fuzzy Control)是模拟人的模糊推理和决策过程的一种非线性控制方法,不依赖于精确的数学模型,易于实现对不确定性系统及强非线性系统的有效控制[14].本文采用模糊控制器根据设定(期望)值和测量值的偏差e以及该偏差的变化率ec,输出PID控制器的比例、积分、微分系数,然后再进行串级PID控制.在角度-角速度串级PID控制中,由于内环角速度环直接输出各维度的控制量,所以采用模糊控制器输出内环PID控制器的控制数,外环PID控制器的系数则采用固定值,其控制原理图如图3所示.

在PID控制中,积分项主要用来消除稳态误差,所以模糊控制器只根据误差e来选取积分系数.模糊控制器选取积分系数的规则是当误差e大于某个误差阈值 EI 时,积分系数设为零,即取消积分作用,此时控制器的首要任务是消除误差,只采用PD控制以尽快消除误差;当误差e小于EI时,为消除稳态误差,积分系数采用一个固定的值KI.误差阈值EI和积分系数KI需要根据实际经验并在实验的基础上选取.模糊控制器根据误差e和误差变化率ec来选取比例系数和微分系数的值,并根据选取规则制定成表格存储在微控制器的ROM中,比例系数和微分系数的选取规则见表1.表中的NB、NM、NS、ZO、PS、PM、PB是变量e、ec、比例系数、微分系数的模糊表示方法,分别表示负大、负中、负小、零、正小、正中和正大,它们对应的具体值以及表格的制定依赖于实际调试经验并需要通过实验不断修改和验证.在实现模糊控制器时可以采用宏定义的方式定义NB、NM、NS等对应的具体值,以便于修改和调试.

图3 ROV模糊自适应串级PID控制原理图Fig.3 ROV self-adaptive fuzzy cascade PID

表1 控制参数选取规则表Table 1 Control parameters election rules

4 传感器子系统软件设计

传感器子系统软件设计包括传感器数据采集处理程序、子系统与主MCU通信程序两部分.子系统程序是典型的前后台系统模式,整个应用程序无限循环地采集并处理传感器数据,这是后台行为;在UART通信中断服务程序中与主MCU通信,这是前台行为.传感器子系统进行软件设计时既要保证通信响应的实时性,即任何时刻主MCU的数据请求都要在第一时间得到响应,又要保证返回数据的完整性.

4.1 UART通信程序设计

主MCU与子系统之间采用请求应答的通信模式.主MCU通过UART向子系统发送传感器数据请求,子系统通过UART中断接收该请求并在中断服务程序中将处理好的传感器数据一次性返回,中断方式使得响应延迟降低.

微控制器UART通信以字节为单位进行数据传输,在此基础上需要用户制定通信协议.主MCU发送的数据请求报文固定为两个字节的十六进制数0XAA和0X01,子系统在中断服务程序中收到数据请求报文0XAA和0X01后立即返回传感器数据报文给主MCU.返回报文由两个字节的帧头、40个字节的数据和两个字节的CRC校验位构成.返回报文首先是两个字节开头的帧0XBB和0X2A,0X2A换成十进制是42,表示这一帧报文后面还有42个字节.数据包括三轴角速度、三轴姿态角、水温和深度数据,每个数据均为浮点型,共需要32个字节,同时为方便以后扩展,又增加了8个保留字节,所以数据共有40个字节.数据之后是两个字节的CRC校验位,供接收方校验的数据的正确性.

4.2 传感器数据采集处理程序设计

子系统后台程序无限循环地采集和处理传感器的数据,并把处理好的数据按照通信协议规定的格式存到数组中,这个数组就是待发送的一帧报文.当收到主MCU的数据请求时,子系统在中断服务程序中直接发送该数组,中断返回后继续执行后台程序.

在上述程序模型下,如果子系统在存放数据到数组的过程中数据请求到来,中断响应将打断这一过程,导致返回的报文不完整.接收方收到该报文后也会因为CRC校验出错而丢弃该报文.如果在存放数据形成一帧报文之前暂时关闭中断响应,此方法虽然保护了报文没有被中断破坏,但是如果在此时主MCU发出数据请求,则该请求不会得到响应.为此,本文借鉴了图形图象处理编程中的双缓冲技术,设计了子系统传感器数据采集处理程序,程序流程如图4所示.

图4 传感器子系统程序流程图Fig.4 Sensor subsystem program flow chart

在图形图象处理编程中,双缓冲是指将计算的结果先放在一个中间缓冲区内,需要显示时直接把中间缓冲区的的内容复制到显示缓冲区[15].因此,本文采用编号分别为0和1的两个数组存放报文,同时设置一个指针变量Ptr,在任意时刻Ptr指向这两个数组中的一个,即Ptr只能在0和1中取值.与图形图像编程中双缓冲区不同的是这两个数组是并列使用的,不存在一个数组的数据复制到另一个数组的过程.在一次采集循环中,子系统直接把处理好的传感器数据存Ptr所指向的数组,然后改变Ptr的值使其指向另一个数组,则下次循环的数据会被存到另一个数组,如此以实现两个数组交替使用.当主MCU的数据请求中断到来时,子系统在中断服务程序中直接发送Ptr未指向的那个数组,保证了报文的完整性.

5 实时操作系统与主MCU任务调度

主MCU需要负责各个任务的调度运行以及任务间的通信,对软件设计的实时性和稳定性要求较高,传统的前后台系统已经不能满足要求.实时操作系统FreeRTOS功能强大,遵循MIT授权模式[16],可免费用于商业产品,因此本文采用FreeRTOS进行任务调度.

配置FreeRTOS采用抢占式任务调度方式,即高优先级任务就绪后可以抢占低优先级任务获得CPU的运行权,低优先级任务只有在高优先级任务挂起或阻塞时才得以运行[17].需要操作系统调度的任务有与传感器子系统通信任务、运动与姿态控制任务、与外界通信任务和电池电压检测任务等,任务的优先级根据实时性要求分别设为4、3、2、1.

主MCU与子系统通信任务以50Hz的固定频率运行以获取传感器数据,然后通过信号量同步运动与姿态控制任务.子系统通信任务挂起后控制任务便成为就绪态中优先级最高的任务,立刻得到运行.与外界通信任务以10Hz的频率运行,该任务主要是向外界反馈当前ROV的状态信息.电池电压检测任务的实时性要求相对较低,其运行周期设为1到2秒即可.

6 实验与分析

为了验证整个控制系统的实际控制效果,搭建了图5所示的实验环境并进行了多组实验.首先,为了验证传感器子系统方案的可行性以及整个系统之间通信的可靠性,选取ROV的偏航角α和Z轴角速度ω作为观察变量,发送控制命令使其在原位置匀速旋转至少一周,然后使其停止,记录α和ω的变化曲线如图6和图7所示.在发送命令前偏航角α稳定在某个值附近,角速度ω稳定在0附近,发送命令后,ROV的角速度ω从0开始上升并稳定在40 附近,偏航角α则以稳定斜率变化;发送停止命令后,角速度ω逐渐回到0附近,偏航角α最后稳定在某一角度值.由于偏航角α的变化范围是0～360度,所以α在0度或360度附近会发生突变.在图6中,一共有两次突变,这两次突变之间ROV以匀速转过了360度,时间间隔约为9 s,可计算出其角速度为40 度每秒,这与图7中的第5秒至20秒的角速度负40一致.

图5 实验环境图

图6 偏航角曲线图Fig.6 Yaw angle value curve

其次,为了验证整个控制系统对ROV的运动和姿态控制能力,选取偏航角α和深度d作为控制变量,测试ROV对阶跃输入的响应能力,实验结果如图8 和图9所示.在深度响应实验中,ROV初始深度略微超过0值,此时水面刚好没过ROV顶部,随后设定深度值90.

深度值从0附近逐渐增加到90同时出现了微小的超调量,最终收敛稳定在90附近.在偏航角响应实验中,先给设定偏航角为5度使其稳定,然后再设定偏航角为160度.ROV收到控制命令后迅速发生偏转,在初始阶段偏航角以固定的斜率上升,表明ROV在匀速转动.在120度左右时偏航角上升斜率逐渐变缓至水平,ROV转动速度逐渐下降为0,最终偏航角达到设定值且没有出现明显的超调量.

图8 深度响应曲线图Fig.8 Depth response curve

图9 偏航角响应曲线图Fig.9 Yaw angle response curve

7 小 结

本文从ROV的控制算法、传感器数据采集和任务调度等方面介绍了一种控制系统方案的实现,重点叙述了模糊串级PID控制算法的实现和传感器子系统的软件设计,最后通过实验对该控制系统方案进行了测试验证.模糊加串级PID控制算法不需要精确的数学模型,易于实现并在实际控制中表现良好.采用子系统采集处理传感器数据,提高了传感器数据的更新率,使得系统模块化.通过实时操作系统FreeRTOS进行任务调度则可以提高系统的实时性,简化软件框架设计.最终实验表明该方案可以较好的满足对ROV的控制要求.