核电站法兰盖开盖机器人控制系统的设计

张浩文，刘 铮，樊绍胜，周宇霆

(长沙理工大学电气与信息工程学院，湖南长沙 410114)

0 引言

核能是一种高效清洁的能源，核电站为人类的生产生活提供了大量的能源供应，但核电站发展过程中的安全问题也越来越受关注。核电站内部有放射性污染且空间环境复杂等问题，进入核电站内部通常需要诸多防护措施，这给操作人员带来了不便和健康威胁[1-2]。

通过设计法兰盖开盖机器人，代替人工进行法兰盖螺栓拧松和法兰盖打开作业，可以减少人工更换RCV过滤器滤芯时操作人员暴露在辐射环境中的时间，对保障工作人员的健康、降低劳动强度和提高工作效率都具有重要意义。本文设计的法兰盖开盖机器人控制系统具有可靠性高、对法兰盖螺栓磨损小等特点。

1 控制系统总体结构设计

法兰盖开盖机器人总体结构设计如图1所示，包括可更换的移动底盘、机器人控制柜体和五轴机械臂。控制柜包含机器人控制系统、电源模块以及远程通信模块；五轴机械臂的作业末端是柔顺结构设计的套筒[3]。

机器人控制系统总体结构框图如图2所示。该系统主要由STM32单片机为核心组成的最小系统电路模块、CAN总线通信模块、串口通信模块、网络通信电路模块、A/D模拟量采集电路模块和供电模块组成[4]。平台移动电机与机械臂伺服电机均采用基于单电机伺服控制的方法，即每个电机是一个完整的子伺服控制系统，经CAN通信模块接收中央运动控制器的目标指令，独立完成电流、速度和位置的闭环控制。主控单元通过网络通信模块接收上位机视觉识别及视觉伺服控制算法的处理结果；对各传感器的数据采集、处理和分析，并根据分析结果执行相应的控制任务；对伺服电机驱动控制器下发运动控制指令及电机运动参数采集并将参数上传给上位机。

2 控制系统硬件设计

法兰盖开盖机器人控制系统硬件部分设计包括主控芯片最小系统电路、串口通信电路、CAN总线通信电路、网络通信电路、A/D采样电路及电源系统电路的设计。

2.1 主控芯片及其最小系统电路

机器人控制系统的主控芯片选用STM32F407ZGT6单片机，该芯片采用ARM的32位Cortex-M4内核处理器，其主频可达168 MHz，具有丰富的片上外设资源和通用I/O接口[5]，且具有符合IEEE 1588 v2标准要求的以太网MAC10/100。在本控制系统中所用到的外设包括：RS232通用异步收发传输器(读取激光测距传感器的测量结果)、CAN通信控制器(对伺服电机驱动控制器下发运动控制指令，读取电机及编码器当前状态和参数)和ADC模数转换器 (对三维力传感器以及电源的电压电流信息进行采集转换)。

为了满足控制系统的基本要求，设计了STM32F407ZGT6的最小系统，包括电源、时钟、复位和调试接口电路，如图3所示。

其中最小系统电源电路采用LM2576HV DC-DC芯片与LD1086D2M33 DC-DC芯片配合，LM2576HV为BUCK型降压转换电路，具有较宽的输入电压范围(7～60 V)，很好地适应了机器人锂电池电源在满电与亏电情况下不同的电池电压。由LM2576HV输出的5 V稳压最高可达4 A电流，满足了主控电路板其他元器件的供电需求，LD1086D2M33为3.3 V固定电压输出的BUCK型DC-DC芯片，由LD1086D2M33的BUCK型降压转换芯片实现STM32控制器的3.3 V供电。单片机的复位电路采用低电平上电复位设计，在上电瞬间，接地电容充电，单片机RESET管脚出现短暂低电平，以此产生复位信号。时钟电路外部高速时钟选择为DSX321G的8 MHz晶振，精度为20 ppm，经倍频后可达168 MHz；下载调试接口采用SWD模式，相比JTAG接口在高速模式下具有更高的可靠性并且占用更少的I/O口。主控板电源电路设计如图4所示。其中C1为铝电解电容，有效防止输入端出现较大的瞬间电压和输入电压波动；D1为肖特基二极管，其开关速度快，正向压降低，反向恢复时间短；C2为钽电容，用来输出滤波以及提高反馈环路的稳定性。

2.2 数据采集电路硬件设计

2.2.1 A/D采样电路

三维力传感器输出为与力值大小成正比的0～5 V的模拟量信号，STM32单片机片上ADC是12位逐次逼近型模数转换器，其I/O允许输入电压为0～3.3 V，故采用电阻分压法对输出电压进行处理转换。为了提高输入阻抗，以提高测量准确性，同时在单片机与被测电路之间起到隔离缓冲的作用，选择输入阻抗较高的集成运放TLC2262搭建电压跟随器，如图5所示；TLC2262为满幅电压运放，当采用5 V单电源时，其输出电压幅值可达5 V不失真，在保证电路安全且不失真的前提下可采用先进行电压跟随再分压的形式，可避免分压采样电阻对前级电路的影响。R4与C2构成低通滤波器，滤除高频尖峰干扰。

在采集电池电压信号时，将R2和R3构成的分压电路前置以保护后级电路。

2.2.2 RS232通信电路

RS232C是常用的串行通信接口标准之一，采用负逻辑电平，即DC-15～-3 V表示逻辑1，DC3～15 V表示逻辑0，实际使用中-9～-7 V为合理的驱动电平[6]。而STM32控制器的信号电平为TTL电平，激光距离传感器与控制器之间的RS232通信需要进行电平转换，因此采用MAX3232电平转换IC进行设计，MAX3232采用3.3 V单电源供电，配备高精度钽电容完成电平转换，设计电路如图6所示。

3 控制系统软件设计

本控制系统的软件包括下位机底层控制软件和上位机监测软件[7]。其中系统底层采用C语言在KEIL MDK-ARM集成开发环境进行程序设计。上位机在Visual Studio IDE中使用C++语言开发。

3.1 下位机底层软件设计

底层控制软件在STM32微控制器上移植的μcos-Ⅲ实时操作系统，它是Micrium公司推出的第三代产品，相比于前代产品，专门针对Cortex-arm4进行了优化，具有更短的中断切入时间和任务切换时间[8]，能够满足机器人的控制任务。

在μcos-Ⅲ实时操作系统中，以多线程的方式处理不同模块的任务，任务模块包括用于上位机监控的网络通信模块、用于机械臂控制的CAN总线模块以及传感器信号采集模块。在主线程中实现对以上模块的初始化以及创建默认子线程。

根据不同的任务模块分别创建对应的子线程，其中网络通信子线程用于与上位机建立TCP通信，将机器人当前状态实时反馈给上位机，对接收到的上位机通信指令进行解析并执行相应功能的子线程；CAN总线子线程用于对机械臂及平台进行运动控制并读取电机参数；传感器信号采集子线程包括激光距离传感器和三维力传感器的数据采集及预处理。下位机程序流程图如图7所示。

3.2 上位机监测与控制软件

法兰盖开盖机器人上位机监测与控制软件在Windows系统中使用Visual Studio集成开发环境完成设计，其界面如图8所示：界面上方为监控视频显示区，第一个视频窗口为法兰盖及螺栓的视觉检测结果显示、机械臂视觉伺服初步定位结果显示；第二个视频窗口为螺栓对中控制结果显示区。向下依次为状态显示区(关节电机状态参数、传感器参数以及电源状态参数)、系统控制区(启停控制)、自动控制区和手动操作区。

4 基于双传感器的螺栓柔顺对中控制

传统的拧螺栓作业末端通常采用具有被动柔顺结构的机械末端来被动调整套筒与螺栓对准时的微小误差。被动柔顺拧螺栓时，由于套筒相对于螺栓有一定偏角，电机带动套筒的拧紧力被分解，电机需要更大的功率，其次也会对螺栓和套筒造成较大的磨损。基于以上缺点和法兰盖开盖作业特定环境，提出了基于激光距离传感器和三维力传感器的双传感器末端柔顺控制。

导纳控制算法是根据机械臂与环境的作用力来调整末端的位姿来达到柔顺控制的效果。位姿的调整分为末端姿态的调整与位置的调整。为简化控制算法，便于算法的实现，本文采用激光距离传感器和三维力传感器分别对姿态和位置进行调整。

4.1 五自由度机械臂作业平台的简化

机械臂平台简化示意图在Matlab中进行运动学建模，如图9所示。其中前移平台可以为套筒提供向螺栓进给的距离，关节5为旋转电机，依靠旋转驱动套筒拧松螺栓。关节1用来调整末端与法兰盖的左右夹角，关节4用来调整与法兰盖的上下夹角，故调节关节1和关节4即完成对末端姿态的调节；关节2和关节3联动可完成平面内的位置调整。

4.2 基于激光距离传感器的末端姿态调节

法兰盖可视为竖直平面，螺栓与法兰盖为平面垂直关系，末端姿态调整时需将末端套筒截面调整为与法兰盖平面平行(套筒可无偏角套入螺栓)。采用套筒侧面的激光测距仪随套筒旋转一周，测量圆周各点与法兰盘的距离，即可测算末端与法兰盖的姿态偏差。

旋转电机带动套筒转动一周为360°，转动套筒，调整激光测距的起点在套筒圆周的竖直方向，记此点为0°。测距图像如图10所示，当测得距离最小值时套筒转动的角度为θZ，则测得最大值时转动角度为θZ+180°。

设距离最小值为L1，最大值为L2，则根据几何关系可得套筒与法兰盖平面夹角大小为θX：

(1)

式中R为激光绕套筒中心轴的旋转半径。

在基座标下，已知机械臂末端姿态为

(2)

将末端调整至与法兰盖平面平行需调整角度按Z-Y-X型动态欧拉角可记为Euler(θZ、0，θX)，可得旋转矩阵：

(3)

式中:cZ、cX、sZ、sX分别表示cosθZ、cosθX、sinθZ、sinθX。

可求得待调整的机械臂末端姿态为T′=TR，再由逆运动学可解出关节1和关节4角度。

4.3 基于三维力传感器的末端套筒位置调节

在末端调平与初步对准螺栓后，平台前移使机械臂整体向螺栓进给，末端套筒在其X轴(左右方向)与Y轴(上下方向)的期望受力为0，仅在Z轴的进给方向受力FR。所以期望受力F0=(0,0,FR)。若三维力传感器测得套筒受力为F=(FX,FY,FZ)，则采用直接力反馈控制算法对套筒位置调整，增加末端套筒柔顺性。

4.3.1 位置环PD控制

在力反馈控制前，需要有独立的位置控制器作为内环控制，本文采用目前实际应用最为广泛的PD位置控制器：

(4)

式中：τ为关节力矩；e为关节角位移偏差Δθ。

由于调节第二、三关节电机即可调整位置，故机械臂模型可简化为二连杆结构，示意图如图11所示，关节角位移分别为θ1、θ2，两关节转矩分别为τ1、τ2，质量分别为m1、m2，杆长度分别为l1、l2。

利用拉格朗日法对机械臂动力学建模[9]：

(m1+m2)l1gcosθ1

(5)

改写为矩阵形式，则有：

(6)

可以解得：

(7)

建立PD位置控制的算法框图如图12所示。

4.3.2 直接力反馈控制

末端套筒除了在进给方向的受力外，在平面内受力为F=(FX，FY)。偏差越大，受力越大，故设定末端套筒受力与位置偏差成正比。力反馈控制器为

τ=KθC

(8)

式中：θC为位置修正量；K为力矩反馈系数。

建立直接力反馈控制算法框图如图13所示。

力反馈控制器的输入为

(9)

式中J为力雅可比矩阵，表示为

以二连杆机构为对象在Simulink仿真平台进行直接力反馈控制仿真，如图14所示。

设关节1和关节2的起点位置速度均为0，在t=0时刻，给定位置为幅值0.98的阶跃信号，实际位置为1。通过给定不同的力矩反馈系数K，来调节力反馈控制达到最佳效果，图15为不同K值情况下的位置输出结果。

由仿真结果可知，当1/K值太小时调节时间过长，不具有实用性；当1/K值太大时超调量较大，不能用于拧松作业。本文取1/K为3.7。

5 试验结果分析与样机测试

为测试法兰盖开盖机器人实际作业效果，按核电站实际环境搭建了法兰盘模型。法兰盘中心距地面1.5 m，螺栓预紧力矩为150 N·m。以此为作业对象进行试验。

5.1 螺栓柔顺对中控制试验

仅靠末端套筒的被动柔顺结构裹覆螺栓进行螺栓对中和拧松时，由于套筒中心轴未与螺栓中心轴重合，套筒拧松力矩被分解，套筒驱动电机输出扭矩需大于150 N·m才可拧松螺栓，且中心轴偏离越远，所需输出扭矩越大。驱动电机转矩系数为10.1 N·m/A，实验中末端套筒从同一位姿开始进行主动/被动柔顺对中控制对比实验，通过监测螺栓拧松时峰值电流的大小来评价主动柔顺对中控制的结果。实验数据如表1所示。

表1 主动/被动柔顺对中控制电机峰值电流

由表1数据可知，主动柔顺控制减小了驱动电流，减少了套筒与螺栓之间的磨损。

5.2 机器人实际作业效果

在完成螺栓柔顺对中控制试验，取得了更好的柔顺对中效果后，再对机器人整机作业效果进行试验。检测上下位机通信的稳定性、传感器及电机数据能否及时上传、机械臂作业空间是否与周围环境存在干涉。试验结果表明，法兰盖开盖机器人能安全有效地完成法兰盖螺栓拧松及开盖任务。实际作业效果如图16所示。

6 结束语

设计了基于STM32微处理器的法兰盖开盖机器人控制系统并进行了测试，采用上下位机结合的结构，完成传感器数据采集及分析、机械臂运动控制及作业过程的实时监控。提出了基于双传感器的螺栓柔顺对中控制，简化了柔顺控制算法的设计，且相较于被动柔顺结构具有更好的柔顺效果。实验结果表明该机器人控制系统能有效完成法兰盖开盖任务，降低工作人员的劳动强度和辐射风险。