核电站蒸汽发生器维修机器人控制系统设计

赵阿朋,王雪竹,张美玲, 康林清,樊继壮

(1.中广核研究院有限公司, 广东 深圳 518000;2.哈尔滨工业大学 机电工程学院 哈尔滨 150008)

蒸汽发生器作为压水堆核电站一回路设备中的核心装置之一,在核电站发电过程中起到一、二回路间的能量传输作用.由于其内部一回路侧的传热管在工作中长期承受高温、高压和硼酸水腐蚀,容易导致管板、传热管以及水室发生泄漏、破裂和生锈等异常状况,给核电设备安全运行带来隐患[1-2].因此为减少安全风险,避免非计划停堆,本课题组研制了一种新型串联作业机械臂,使用时可将其倒挂安装在蒸汽发生器的管板上,在其末端可搭载多种作业工具对蒸汽发生器进行视频检测、打磨、抛光等修复作业.

目前,国内外学者对于机械臂轨迹规划算法进行了深入研究并取得了众多成果.Bailón W P[3]使用八次多项式插值法进行机械臂末端轨迹规划,虽然能够得到更为光滑的运动参数曲线,但是计算过程较为复杂且轨迹精度难以保证.Altabey W A[4]利用五次多项式插值法得到了连续且光滑的运动参数曲线,在降低计算量的同时保证了轨迹精度.Gasparetto A[5]采用五次B样条插值法进行规划,得到了加速度曲线不存在拐点的运动参数曲线,但该方法的计算量较大,可能会影响轨迹精度.乔亮[6]通过总结现有方法的不足,提出了三阶参数样条插值,通过参数选取解决轨迹曲率以及拐点问题,并进行仿真验证了算法的可行性.

本文介绍了核电站蒸汽发生器的检修任务工况和作业机械臂的作业过程,在此基础上设计了作业机械臂的控制系统,根据检修任务需求,研究了作业机械臂在关节空间和笛卡尔坐标系空间下的轨迹规划算法,并基于霍夫变换圆原理实现了作业机械臂对管板圆孔的识别定位.通过实验验证了轨迹规划算法和圆孔识别定位的有效性.

1 蒸汽发生器检修工况

蒸汽发生器的整体结构由下方的半球形水室和数千根倒置的U形传热管共同组成.半球形水室与一回路侧U形传热管相连,中间有隔板隔开,形成两个四分之一的水室,上方有约厚557 mm的管板,管板上有数千个直径约为20 mm的通孔,倒置的U形管胀紧在通孔内,如图1(A)所示.在蒸汽发生器长期运行的过程中,需要定期停堆对其内部的管板、传热管以及水室进行视频检测、打磨、抛光等修复作业,由于蒸汽发生器内部具有较强核辐照剂量,采用人工作业具有极高的安全风险,因此课题组研制了一种作业机械臂,代替人工完成此项检修任务,提升作业安全性和自动化作业水平.

图1 蒸汽发生器和作业机械臂结构示意图

检修作业机械臂结构设计如图1(B)所示,采用六自由度串联结构设计方案,各个自由度均采用驱控一体的模块化关节驱动,以便于机械臂的维护和检修.

作业机械臂在执行检修任务前,如图2所示,先由地面小车抓持机械臂的末端,通过蒸汽发生器上的人孔送入水室,并将机械臂的底座吊挂安装固定在管板上,然后通过远程控制软件操控,配合多种检修工具,完成对U形管、管板以及整个水室的打磨抛光、无损检测、胀管、传热管清洁、传热管内径测量、衬管激光焊接等作业.

图2 作业机械安装、检修过程

2 作业机械臂控制系统设计

2.1 控制系统硬件设计

本文的控制系统采用分布式,将上位机和下位机分别进行控制,作业机械臂控制器为主站,进行运动学、动力学等算法计算,电机驱动器为从站,安装在各个模块关节的内部,以驱动关节转动,并实时反馈关节信息.采用EtherCAT总线来实现作业机械臂主站和从站之间的通讯,进行实时的数据传输.作业机械臂的控制系统,需要完成机械臂在蒸汽发生器水室内的位置姿态、轨迹规划以及视觉圆孔点位识别等计算,作业机械臂硬件系统框图及作业机械臂控制系统方案如图3、4所示.

图3 作业机械臂硬件系统框图

图4 作业机械臂控制系统框图

主站是整个作业机械臂控制系统的核心,需要进行大量的数据计算,包括位姿、轨迹规划、圆孔定位等一些重要的算法计算.其通过EtherCAT总线接受来自上位机传递的控制命令,进行计算求解之后,通过EtherCAT总线将控制指令发送给各个模块关节的驱动器,从而驱动各个关节完成相对应的指令,同时将各个模块关节所处的转动角度等状态带回给上位机,得到一个实时的反馈.因此对主站的计算能力要求很高,同时还需要丰富的接口,来满足实际的需求.本文采用纳博特科技研发的NRC系列控制器,尺寸为45 mm×230 mm×165 mm.该控制器配备了Intel下的Bay Trail Celeron处理器,8GB系统内存,以及丰富的外设接口.

2.2 控制系统上位机设计

由前文可知,作业机械臂由六个模块关节、连接件、控制器和上位机组成,机械臂的运动模式由上位机软件进行控制,因此上位机软件在作业机械臂控制系统中占重要位置.

本文上位机软件系统采用Qt编写,主要用于为操作者提供友好全面的作业机械臂软件操作界面,用户通过该软件对作业机械臂进行远程操作和实时控制,从而实现的对蒸发发生器管板及水室的检查和修复,如图5所示.

图5 上位机软件控制界面

上位机软件界面主要包括了通过IP连接机械臂、设置运动速度、点动、多关节联动、圆孔视觉识别与定位、圆弧与直线规划运动、获取当前关节位置与紧急下电等功能模块.操作上位机软件时,首先通过EtherCAT连接到各个模块关节,点击连接,等待响应,连接成功之后,上位机与主站建立了联系,可以通过上位机发送指令,对机械臂进行运动控制.本文设计的上位机包含以下功能模块:

1)回原点

回原点操作即是复位的操作,作业机械臂在搭建过程中,关节之间有一个相对位置.为了操作方便,在每次执行完任务之后,可以执行回原点的操作,方便下一次作业,同时也方便观察机械臂的状态,防止出现碰撞.

2)单关节点动

单关节点动是作业机械臂运动控制的基本功能.选择点动的关节,输入转动的关节角度,然后点击动作按钮即可.通过修改速度,可以改变关节点动的速度,速度变化范围为0～10°/s.点击获取点位按钮,可以实时得到关节的变化角度.

3)多关节联动

多关节联动功能是同时控制六个模块关节运动,可以实现作业机械臂末端点到点的运动.首先给定各个关节转动的角度,并设置目标速度,然后点击运动按钮,即可实现关节联动功能.同时,点击当前位置按钮,上位机软件还能通过读取绝对值编码器值,反馈出各个关节目前所处的位置状态.

4)逆运动学求解

在该功能模块中,输入作业机械臂末端在基坐标系下的位置坐标和姿态角度,即横滚、俯仰与偏转角度,运用机器人的逆运动学公式求解出各个关节需要转动的角度,在软件中显示出来.点击开始运动按钮之后,作业机械臂即从当前位置运动到所给定的位姿.

5)圆孔识别定位

作业机械臂末端运动到管板正下方,通过相机抓拍一张管板管孔深度彩色图片,基于霍夫变换圆检测技术,可得到各个圆孔圆心在相机坐标系下的位置,通过位姿矩阵变换,得到管孔圆心在作业机械臂基坐标系下的位置坐标和姿态,运用所推导的逆运动学求解公式得到机械臂末端工具运动到管孔圆心的各个关节角度变量,然后点击圆孔定位.

6)轨迹规划运动

在使用前需要转到连续运动模式,该功能模块分成两部分,一部分是机械臂轨迹规划仿真,另一部分是样机轨迹规划运动.首先,通过RemoteAPI将Qt与仿真软件Vrep进行通讯,将求解得到轨迹规划数据传给Vrep,控制作业机械臂模型进行运动,通过观察机械臂在水室中的运动状态,主要观察是否发生碰撞、关节运动是否异常、末端状态等.在保证仿真验证不出问题的前提下,再将轨迹规划数据通过EtherCAT总线传给机械臂进行运动控制.

3 作业机械臂运动轨迹规划

根据检修任务需求,可以将作业机械臂轨迹运动形式分为点对点运动和连续路径运动.点对点运动用于管板圆孔的检修作业,通过关节空间轨迹规划算法实现;连续路径运动用于管板和水室内壁加工面的检测、焊接等作业,通过笛卡尔坐标系空间轨迹规划算法实现[7-9].

3.1 关节空间轨迹规划算法

关节空间轨迹规划算法需要给定作业机械臂末端作业工具的起始位姿和目标位姿,利用逆运动学求解公式得到各个关节起点和终点的角度,对各个关节起点和终点的角度进行插补,得到关节变化的连续轨迹.由于需要对速度和加速度进行约束,本文选择五次多项式插值规划算法.

五次多项式的通式为式(1),有6个待定系数.给定作业机械臂末端工具在起始点和目标点下各个关节的角度、速度以及加速度,根据五次多项式的一阶导、二阶导,如式(2)所示,可列出六个方程,如式(3)所示,从而求解得到五次多项式.

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

(2)

(3)

联立式(1)～(3)可求解待定系数分别为:

同样的,得到各个关节的角度、速度和加速度对时间的函数之后,通过仿真验证五次多项式插值轨迹规划在关节空间的结果,如图6所示.

图6 五次多项式插值关节角度、速度、加速度变化曲线

从五次多项式插值的结果来看,作业机械臂的关节角度变化较为平滑,速度与加速度的变化也较为连续,没有出现突变现象,有利于作业机械臂的运动平稳进行.

3.2 笛卡尔坐标系空间轨迹规划算法

在关节空间下的轨迹规划无法保证作业机械臂末端作业工具按照特定路径进行运动,因此,若要对作业机械臂末端作业工具的位姿进行绝对的约束,需要在笛卡尔坐标系空间内实现轨迹规划.

3.2.1 直线轨迹规划

图7 空间直线轨迹

设作业机械臂末端工具沿直线插补的速度为v,直线插补周期为Ts,总运动时长为T,根据直线长度L,求解总的直线插补步数N,如式(4)～(6)所示:

(4)

T=L/v

(5)

N=round(T/Ts)+1

(6)

其中:round表示对内部的数值进行取整.

规定λ为归一化位置变量,取值范围为[0,1],当λ取0和1时,分别对应为作业机械臂末端工具在起始点和目标点位置.F函数表示λ在时间上的映射,如式(7)所示,对应的变化曲线如图8所示.

图8 归一化变化曲线

(7)

3.2.2 圆弧轨迹规划

在一个三维空间内,已知三个不共线的坐标点,可以求得经过这三个坐标点的圆弧轨迹,并采用插补方法计算出在圆弧上各个插值点的三维坐标值.给定空间中的三个坐标点分别为Q1=[x1y1z1]、Q2=[x2y2z2]、Q3=[x3y3z3],其在空间中的位置如图9所示.

图9 空间圆弧轨迹

给定的三个坐标点在平面o1x1y1内确定了一个外接圆,根据外接圆公式,得到圆的方程:

(8)

圆心的位置可以通过平面o1x1y1与线段Q1Q2的垂直平分面和与线段Q1Q3的垂直平分面的交点确定,线段Q1Q2的垂直平分面的方程如式(9).

(9)

线段Q1Q3的垂直平分面的方程如式(10):

(10)

联立式(8)～(10)可求解圆弧圆心坐标和半径为:

(11)

(12)

根据圆弧圆心坐标及其半径值,运用插值算法,可计算出圆弧上各个插值点在o1x1y1z1坐标系中的坐标P(1).通过齐次坐标变换,得出插值点相对于基坐标系下的三维坐标值P(0),如式(13).

P(0)=0T1P(1)

(13)

其中:0T1为o1x1y1z1坐标系相对于o0x0y0z0坐标系的变换矩阵.

4 管板圆孔识别定位方法

管板位于水室上方,表面均布有上千个直径约为20 mm的通孔.根据检修任务要求,需要作业机械臂对管板圆孔进行识别定位,进而执行检修作业.首先利用深度相机对管板进行拍摄,根据霍夫圆环变换识别图像中的圆孔,通过坐标变换得到基坐标系下的圆孔圆心坐标.

4.1 霍夫变换圆环检测

图像中的圆孔识别是利用霍夫变换圆环检测实现的,该算法是将识别到图像中的每一个像素点作为圆上的一个点,进行投票方式来得到该图像中的形状集合,并根据实际情况设定一个权重从而定位圆的位置[10],如图10所示.

图10 霍夫变换圆环检测原理

假设在X-Y平面内有一个圆,其圆心为(a,b),半径为r,则该圆可用(x-a)2+(y-b)2=r2方程来表示,即为二元一次函数f(x,y)=r2.圆方程也可写成为(a-x)2+(b-y)2-r2=0,其表示a-b-r空间中函数,此时x、y为常量,也可以写成三元一次函数f(a,b,r)=0.此变换的含义就是将X-Y平面内圆环上的一个点对应为a-b-r三维空间中的一条曲线,如图8所示.圆上各点对应的曲线在a-b-r三维空间中会相交于一点(a,b,r),该点就是X-Y平面上所定义的圆的圆心(a,b)和半径r.前面提到的权重就是在a-b-r三维空间内相交曲线的数量,当累加的相交曲线数量达到该权重的比例时,就认为相交的那个点对应X-Y平面上的一个圆.

4.2 圆心三维坐标获取

使用深度相机拍取一张管板的深度/彩色图并进行灰度处理,利用霍夫变换圆检测算法得到圆孔的位置信息.由于此位置信息是相对于像素坐标系而言的,需要通过坐标变换,得到圆孔圆心在基坐标系下的位置坐标[11].

由深度相机得到的深度/彩色图,因为彩色图像数据和深度图像数据是相对于不同的坐标系生成的,彩色图像数据的原点在RGB摄像头中心,深度图像数据的原点在红外摄像头中心,两摄像头存在一定距离,两者的坐标系会有一定的误差.因此需要进行深度图像的匹配,将彩色图像与深度图像进行对齐,使其在同一个坐标系下进行位置计算.

由像素坐标系下的位置转换成相机坐标系下的位置这个过程,运用小孔成像原理,如图11所示,为了理解清晰,将实际点与成像点放置在同一方向上,实际应是关于Oc对称的.f为相机坐标系下的焦距,m为在焦平面上的成像点,其坐标为Px(-x,-y,-z),M为相机坐标系下的一个点,坐标为Pc(xc,yc,zc).由相似三角形关系可得:

图11 小孔成像模型

(14)

一般像素坐标系的原点位于图像左上角,为了计算方便,将像素坐标系(Px)相对于相机坐标系(Pc)进行了缩放和原点平移,像素坐标系为Px(u,v):

(15)

其中:fx,fy,cx,cy的单位为像素,cx,cy是像素坐标系平移的尺寸.f为像距,dX,dY分别表示X和Y方向上的一个像素点在相机感光板上的物理长度,即fx=f/dX,fy=f/dY.则(u,v)可以写成矩阵方程为:

(16)

通过矩阵运算,点在相机坐标系下的位置为:

(17)

同样的,该点位置只是相对于相机坐标系而言的,需要将相机坐标系(Pc)变换到基坐标系(P0)下,从而得到该点在基坐标系下的位置为:

(18)

其中:R为一个3×3的旋转矩阵,T是一个3×1的平移矩阵.联立两式即可得到像素坐标系下m点的位置相对于基坐标系下的位置为:

(19)

5 实验测试

为了验证轨迹规划算法和圆孔识别定位方法的有效性, 搭建了实验平台进行实验验证. 利用EtherCAT信号线借口串联各个模块,电缆及信号线均为中空走线,作业机械臂样机如图12所示.

图12 作业机械臂样机

5.1 轨迹规划运动实验

通过上位机发送指令,机械臂各个关节之间进行配合实现联动控制,分别完成在笛卡尔坐标系下的直线和圆弧轨迹规划,如图13所示.

图13 作业机械臂轨迹规划实验

作业机械臂末端绘制的轨迹如图14所示.绘制结果没有明显的抖动痕迹,且运动精度较为精确,运动过程比较稳定.给了确定作业机械臂末端轨迹规划的精度,直线与圆弧进行了多组实验,每组进行十次测试,求取平均值,结果如表1、2所示.经过计算,作业机械臂末端直线轨迹规划误差不超过0.5%,圆弧轨迹规划误差不超过1%.

表1 直线理论尺寸与实际尺寸

表2 圆弧理论尺寸与实际尺寸

图14 机械臂末端轨迹

5.2 管板圆孔识别定位实验

实验所用的管板模型如图15(A),圆孔直径为19.28 mm.相机位于管板平面的正上方,相机到管板的垂直距离为300 mm.经过霍夫变换圆检测处理,得到如图15(B)所示的圆孔处理图像.从霍夫圆检测识别到的圆心位置与实际的圆心位置进行对比发现,识别得到的圆心位置误差不超过1.38 mm,并能够准确将末端作业工具插入到管板圆孔中,如图15(C)所示.

图15 管板圆孔识别定位实验

在实验的过程中,误差有多个方面原因:1)光源与识别背景,光线太强或背景过于复杂,会造成较大的识别误差,甚至导致识别失败;2)相机视平面应与管板平行,若成倾斜状态,会导致识别到的圆孔为椭圆形,降低识别到圆心个数.

6 结 论

1)根据核电站蒸汽发生器内部结构及检修任务要求,分别在关节空间和笛卡尔坐标系空间下对自研的六自由度机械臂进行了轨迹规划,并完成了机械臂末端工具对管板圆孔的识别定位.

2)搭建了作业机械臂样机并进行性能测试,机械臂能够完成直线和圆弧轨迹规划,误差分别不超过0.5%和1%;能够识别管板圆孔坐标并定位,识别的圆心误差不超过1.38 mm.