基于CODESYS 的五轴点胶机控制系统设计①

徐建明 韩 波

(浙江工业大学信息工程学院 杭州 310023)

0 引言

制造业是国民经济的主要支柱,我国是一个制造大国,但不是一个制造强国。我国于2015 年正式提出“中国制造2025”战略,战略的核心是“智能制造”[1-2]。在实际生产制造中,大多数的点胶工作都是由人工手动完成的,手工点胶会占用很多的人力和时间且不能保证点胶的质量。为了提高点胶的工艺水平和生产效率,对点胶机器人进行研究是非常必要的[3-4]。

按照自由度进行分类,点胶机有三轴、四轴和五轴3 种。三轴点胶机的结构由X、Y、Z 3 个移动轴组成,四轴点胶机除了3 个移动轴之外多了一个旋转轴,五轴点胶机由3 个移动轴和2 个旋转轴组成。目前,对三轴和四轴点胶机的研究比较多,文献[5]研究了一种应用于半导体元器件的三轴点胶机控制系统,文献[6]使用伺服系统设计了四轴点胶机控制系统,文献[7]设计了以步进电机驱动滚珠丝杠传动的四自由度点胶直角坐标机器人。但是,三轴和四轴点胶机不能任意调整姿态,无法完成复杂的轨迹,所以五轴点胶机成为研究的热点。

机器人的控制系统一般是基于PLC(programmable logic controller)进行开发的,PLC 从硬件结构上可以分为硬PLC 和软PLC。硬PLC 是由专用的集成芯片来实现PLC 指令的执行,而软PLC 是将PLC 的控制功能封装在软件内,运行于工控机或者嵌入式控制器中[8-10]。传统的硬PLC 因其可靠性在工业控制领域得到了广泛的应用,如文献[11]采用三菱FX2N-64MT 研制了一套应用于飞机铝合金自动制孔的机器人控制系统,文献[12]基于松下FPXC60T 设计了全自动点胶机控制系统。但是随着计算机技术的发展,硬PLC 通用性和兼容性差的问题愈发明显,而软PLC 兼具PC 的开放性和PLC 的可靠性[13-15],所以软PLC 已经成为开发控制系统的最佳方式。目前,软PLC 开发平台主要有3S 公司的CODESYS 和BECKHOFF 公司的TwinCAT。

综上所述,本文采用开放性和扩展性更好的软PLC 开发方案,以3S 公司的CODESYS 作为控制系统的软件开发平台,使用工控机作为硬件平台开发出五轴点胶机控制系统。在控制系统中设计了G代码读取及解析、正逆运动学变换和PVT(position velocity time)插补等功能,同时设计了可视化界面,用于控制点胶机的运动和监控设备的状态。最后以曲面屏手机模型为实验对象,使用开发的五轴点胶机控制系统进行曲面点胶实验验证。在实验中,刀轴矢量能够始终重合于曲面法向量,点胶速度可以保持恒定,手机模型上的胶点具有均匀性和一致性,能够满足曲面屏手机点胶的工艺要求,表明所设计的五轴点胶机控制系统具有一定的工程应用价值。

1 控制系统结构方案

五轴点胶机控制系统使用一主多从的控制模式,主站为基于X86 处理器和Windows 操作系统的工控机,从站为伺服驱动器和总线耦合器,主站和从站、从站和从站之间使用EtherCAT 总线进行通信,控制系统的硬件结构图如图1 所示。工控机会在每个通信周期给伺服驱动器发送目标位置、目标速度和控制字,伺服驱动器驱动伺服电机且上传实际位置、实际速度和状态字到工控机,从而实现五轴联动控制。总线耦合器接收工控机的命令并控制I/O 模块,从而控制喷射阀的出胶动作。

图1 控制系统硬件结构图

本文使用CODESYS 软件平台开发五轴点胶机的控制系统,控制系统的设计流程图如图2 所示。整个设计流程共分为4 步,第1 步是设计正逆运动学功能块,实现工具坐标系和工件坐标系之间的变换。第2 步是设计PVT 插补功能块,将离散的位置点拟合成光滑的轨迹曲线。第3 步是设计运动控制系统,实现五轴联动控制。第4 步是设计可视化界面,用于监控运行状态和下发控制指令。

图2 控制系统设计流程图

2 运动控制功能块设计

2.1 运动学功能块设计

Denavit-Hartenberg(DH)参数法是对机器人进行运动学建模的经典方法,机器人的每个连杆可以用4 个运动学参数来描述,其中2 个参数用于描述连杆本身,另外2 个参数用于描述连杆之间的连接关系[16-19]。对于五轴点胶机,找出各关节轴并标出这些轴线的延长线。找出关节轴i 和i +1 之间的公垂线或者关节轴i 和i +1 的交点,以关节轴i 和i +1 的交点或者公垂线与关节轴i 的交点作为连杆坐标系{i} 的原点。规定Zi轴沿关节轴i 的指向,Xi轴沿公垂线的指向,如果关节轴i 和i+1 相交,则Xi轴垂直于关节轴i 和i +1 所在的平面,按照右手定则确定Yi轴。以此步骤建立坐标系,可以得到如图3所示的五轴点胶机连杆坐标系分布图。

图3 连杆坐标系分布图

根据连杆坐标系分布图可以推导出五轴点胶机的DH 参数,DH 参数表如表1 所示,其中ai-1表示连杆长度,αi-1表示连杆扭转角,di表示连杆偏距,θi表示关节角。

表1 DH 参数表

已知连杆坐标系变换矩阵ii-1T 的一般表达式为(sθ 表示sinθ,cθ 表示cosθ):

式(2)即为五轴点胶机的正运动学公式,下面使用代数法求解逆运动学,观察式(2),使用双变量反正切函数arctan2 计算θ1和θ2,则有:

当已知θ1和θ2时,令

则可以得到矩阵形式的线性方程组:

因为方程组系数矩阵的秩与增广矩阵的秩相等且等于方程组中未知数的个数,所以方程组有唯一解:

在CODESYS 中创建正运动学功能块Forward Transformation,正运动学功能块可以根据输入的关节变量θ1、θ2、d1、d2、d3计算出工具中心点(tool center point,TCP)在工件坐标系下的位置和姿态,正运动学功能块的设计流程图如图4 所示。

图4 正运动学功能块设计流程图

创建逆运动学功能块Inverse Transformation,当输入TCP 的位置和用XYZ 固定角描述的姿态时,逆运动学功能块可以计算出关节变量的值,逆运动学功能块的设计流程图如图5 所示。

图5 逆运动学功能块设计流程图

2.2 PVT 插补功能块设计

机器人的轨迹规划主要研究如何生成一系列连续的运动路径点,运动路径点的信息包括时间、位置和速度等。一般情况下轨迹规划是在关节空间中进行的,主要研究关节变量与时间的函数关系,确保函数的一阶二阶导数连续且可导,从而避免速度和加速度的突变,使机器人的运动更加平稳[20-21]。

PVT 代表位置(position)、速度(velocity)和时间(time),用来表示运动轨迹的相关信息。当使用PVT 运动模式时,控制命令必须指定路径点的位置、速度以及完成路径的时间。但在实际应用中,点在某个位置的瞬时速度难以确定,因为轨迹点是离散点,速度无法通过求导的方式进行计算,另外通过Δs/Δt 求出的速度是平均速度,而不是轨迹点的瞬时速度。三次样条插值可以在只知道点的位置和时间的情况下计算出点的瞬时速度,从而解决上述问题。PVT 插补的数学原理是分段3 次Hermite 插值和3 次样条插值算法[22]。

设[P0,P1,…,Pn] 为运动轨迹中的一系列轨迹点,则在区间[Pi,Pi+1] 中,根据运动控制命令指定的轨迹点信息可以得到Pi和Pi+1点处的位置p、速度v 和从Pi运动到Pi+1所需要的时间t。设F(t)在点tj(j=0,1,…,n) 处的值为F(tj)=pj,F′(tj)=vj,则在区间[Pi,Pi+1] 中

函数F(t) 同样满足3 次样条函数定义,所以可以利用3 次样条函数二阶导数连续的性质来计算节点tj处的瞬时速度。因为F(t) 在区间[P0,P1,…,Pn] 上二阶导数连续,所以在节点tj(j=1,2,…,n-1) 处满足连续性条件F″(tj-0)=F″(tj+0)。对F(t) 求二次导数,可得:

已知v0=vn=0,则式(10)可以表示为对角占优的三对角线方程组

使用追赶法[23]可以解出vj(j=1,2,…,n -1),将vj代入式(8),可计算出任意时刻的位置和速度。将上述插值算法封装为PVT 插补功能块,PVT插补功能块可以在输入位置数组和时间数组的情况下计算出任意时刻的位置值和速度值,PVT 插补功能块的设计流程图如图6 所示。

图6 PVT 插补功能块设计流程图

3 运动控制系统及可视化界面设计

3.1 运动控制系统设计

本文使用SM3_CNC 库来实现点胶机的运动控制系统,运动控制系统的设计流程如图7 所示。(1)将功能块SMC_ReadNCFile2 实例化,在SMC_ReadNCFile2 的输入接口sFileName 中指定文件的路径和名称,在fDefaultVel 中指定默认速度。功能块SMC_ReadNCFile2 的输出接口名称为sentences,其数据类型是SMC_GSentenceQueue。在经过功能块的处理后,sentences 中的数据是字符串形式的G代码语句队列。(2)将功能块SMC_NCInterpreter实例化,把前面sentences 中的数据传递给功能块SMC_NCInterpreter,解释器就可以将G 代码语句解析为路径点队列。路径点队列由多个SMC_GEOINFO 对象组成,每一个对象的信息包括运动类型iMoveType、路径起始点piStartPos、路径终点piDestPos、路径速度dVel 和路径长度dLength 等。因此,路径点队列包含了运动轨迹的所有信息。(3)创建结构体GeoBuffer,其中包含空的位置数组和时间数组。获得路径点队列的内存地址,读取所有的SMC_GEOINFO 对象,然后根据每个对象中的路径起始点piStartPos、路径终点piDestPos、路径速度dVel 和路径长度dLength 计算出对应的位置值和时间值,最后将位置和时间放入到结构体GeoBuffer 中。(4)调用逆运动学功能块Inverse Transformation 将GeoBuffer 中的相对于工件坐标系的位置变换到关节坐标系中,将变换后的关节值保存到结构体Axis-ValueBuffer 中。(5)将AxisValueBuffer 中的5 个关节值数组传递给功能块PVT,就可以对数组中的数据点进行拟合,获得光滑的关节轨迹曲线。(6)调用功能块SMC_FollowPosition,在每个通信周期将关节值实时地下发给伺服驱动器,伺服驱动器就可以对伺服电机进行精确地控制,从而实现五轴联动控制。

图7 运动控制系统设计流程图

3.2 可视化界面设计

可视化界面是控制系统和用户之间进行交互的媒介,可视化界面的主要功能是取代传统的控制面板和显示仪表,用于监控设备运行状况和下发控制指令。CODESYS Visualization 模块使用控件的编程方式,基本的控件元素包括指示灯、按钮、文字和框架等。

五轴点胶机控制系统的可视化界面主要包括4个部分,分别是主界面、运动控制界面、程序编辑界面和点胶控制界面,可视化界面的结构图如图8 所示。在主界面中可以通过开关对电机轴进行使能,通过按钮对设备进行点动控制和实现回零功能,通过文本框显示实时的坐标值。运动控制界面的主要功能是指定G 代码文件的路径、读取文件信息并解析、设置运动的相关参数、显示G 代码的运行轨迹和显示设备运行的状态。程序编辑界面由图形框和文本框组成,图形框提供G 代码文件的图形预览,可以通过控制面板调整图形的显示效果。文本框显示文件的G 代码,可以通过按钮对G 代码进行编辑、保存、新建和删除操作。点胶控制界面主要包括阀门的使能控制、阀门工作状态和阀门压力的显示、通断电时间设置以及手动自动模式切换。

图8 可视化界面结构图

4 实验验证

以曲面屏手机模型为实验对象,以刀轴矢量始终重合于曲面法向量和点胶速度保持匀速为工艺要求验证五轴点胶机控制系统的功能。在SolidWorks中建立曲面手机模型的三维图,将手机模型的三维图导入到Mastercam 软件中,使用Mastercam 生成轨迹的步骤为:(1)将模型底面的中点设置为机床原点,同时建立机床参考坐标系。(2)使用曲线工具在曲面上创建曲面切线,生成的切线即为点胶轨迹。(3)将模型表面的中点设置为对刀点,同时将刀具进入点和退出点设为X=0,Y=0,Z=30。(4)创建多轴加工刀路,在弹出的对话框中将曲线类型设为3D 曲线,补正方式设为关闭,刀轴控制方式设为曲面。操作完成后可以得到运动轨迹,运动轨迹如图9所示。图9 中的坐标系表示机床原点和机床参考坐标系,箭头表示运动的方向。

图9 运动轨迹图

点胶过程中的刀轴矢量图如图10 所示,图中的直线表示刀轴矢量。从图10 中可以看出,在点胶过程中刀轴矢量是始终重合于曲面法向量的,因此需要五轴联动才能完成点胶。

图10 刀轴矢量图

后处理主要用来定义怎样生成G 代码,并且使生成的G 代码满足数控系统的需求。后处理文件是可编辑的文本文件,其内容可以通过代码编辑器进行修改。使用编辑器打开Generic FANUC 5x Router 后处理文件,在文本中查找“#Machine rotary routine settings”,找到如图11 所示的用于修改机床类型的代码片段。根据机床类型修改mtype 变量的初始值,双转台类型的mtype 值为0,摆头加转台类型为1,双摆头类型为2。本文的五轴点胶机为双转台类型,所以mtype 的值需要更改为0。

图11 修改机床类型的代码片段

同样地,在文本中查找“# Motion NC output”,找到如图12 所示的用于修改G 代码输出格式的代码片段,prapidout、plinout 和pcirout 代码段分别定义了快速定位、直线插补和圆弧插补的G 代码输出格式,变量xout、yout、zout、p_out 和s_out 分别定义了X 轴、Y 轴、Z 轴、主旋转轴和次旋转轴的输出值,对其进行更改就可以获得适用于五轴点胶机控制系统的G 代码输出格式。

图12 修改G 代码输出格式的代码片段

使用修改后的后处理文件对点胶运动轨迹曲线进行处理,可以得到运动轨迹的G 代码文件。将G代码文件导入到点胶机控制系统中,控制系统就可以自动读取和解析G 代码。在运动控制界面中下发运行指令,点胶机就可以自动运行。

TCP 的运动速度记录图如图13 所示,其横坐标表示时间值,纵坐标表示TCP 运动速度值。从图中可以看出,除启动和停止阶段外,TCP 运动速度是恒定的,完成一次点胶需要花费约32 s 的时间,这个时间能够满足生产效率的要求。喷射阀出胶频率和TCP 运动速度的关系图如图14 所示。喷射阀出胶频率与TCP 运动速度是相关联的,TCP 运动速度增加时喷射阀出胶频率也会增加,反之亦然,这样可以保证点胶效果的均匀和一致,喷射阀出胶的最大频率为50 Hz。在本次实验中TCP 运动速度为20 mm/s,喷射阀出胶频率为12 Hz。

图13 TCP 运动速度记录图

图14 喷射阀出胶频率和TCP 运动速度关系图

点胶完成后的手机模型实物图如图15 所示,可以看出,手机模型上的胶点具有均匀性和一致性。综上所述,本文设计的五轴点胶机控制系统能够保证刀轴矢量始终重合于曲面法向量,可以满足点胶速度恒定的工艺要求,因此具有一定的工程应用价值。

图15 手机模型实物图

5 结论

本文采用CODESYS 作为开发平台,使用基于X86 +Windows 的工控机作为硬件平台设计了五轴点胶机控制系统。基于SM3_CNC 库中的功能块开发了G 代码读取以及解析功能,使用Denavit-Hartenberg 参数法建立了运动学模型并且设计了正逆运动学功能块,使用分段3 次Hermite 插值和3 次样条插值算法设计了PVT 功能块。同时设计了可视化界面,用于控制点胶机的运动和监控运行状态。以曲面屏手机模型作为实验对象,使用五轴点胶机控制系统进行曲面点胶实验验证。实验表明,点胶机的点胶速度保持恒定,手机模型上的胶点均匀且一致,证明了所设计的五轴点胶机控制系统具有工程应用价值。