步进控制的光栅尺全闭环EtherCAT运动控制器ZMC432CL-V2的二维螺距补偿（上）

今天，正运动小助手给大家分享一下步进控制的光栅尺全闭环EtherCAT运动控制器ZMC432CL-V2的二维螺距补偿的功能的验证。

更多关于ZMC432CL-V2运动控制器的详情点击→步进控制的光栅尺全闭环解决方案：32轴EtherCAT总线运动控制器ZMC432CL-V2查看。

01 产品简介

ZMC432CL-V2高性能多轴运动控制器是一款兼容EtherCAT总线和脉冲型的独立式运动控制器，具备高速实时反馈功能，支持脉冲全闭环控制，能够实现高精度、高响应速度的运动控制。高精度定位，有效消除机械传动误差，满足高精密加工场景应用要求。

ZMC432CL-V2硬件功能：

丰富的运动控制功能：支持直线、圆弧、空间圆弧、螺旋插补等。

硬件接口丰富：支持脉冲轴（带编码器反馈）和EtherCAT总线轴，具备24路输入和12路输出的通用IO，部分为高速IO，2路模拟量输出(DA)。

EtherCAT刷新周期最快达250us，满足高速通信需求。

支持4通道硬件比较输出、硬件定时器、运动中精准输出，适用于多通道视觉飞拍等场合。

支持掉电检测、掉电存储，多种程序加密方式，能够有效防止系统故障，保护项目工程文件数据，并提高系统的可靠性。

通过纯国产IDE开发环境RTSys进行项目开发，可实时仿真、在线跟踪以及诊断与调试，简便易用，支持多种高级上位机语言联合编程进行二次开发。

ZMC432CL-V2产品介绍视频请点击→步进控制的光栅尺全闭环解决方案：32轴EtherCAT总线运动控制器ZMC432CL-V2

02 硬件接口

03 硬件接线

1.数字量输入口IN接线

图1 输入口通用接线图

图2 输入口单端编码器接线图

2.数字量输出口接线

图3 输出口通用接线图

图4 输出口单端脉冲轴接线图

04 控制器开发

1.PC开发

支持以下语言进行开发，我们所有的开发语言和所有控制器提供的都是同一套API接口。

2.脱机开发

使用自主自研的IDE RTSys进行开发，支持Basic、梯形图开发。

05 二维螺距补偿应用场景

06 二维螺距补偿指令说明

正运动二维螺距补偿指令使用的注意点：

1.必须先设置ATYPE，然后设置补偿；修改ATYPE时，必须先关闭补偿！

2.设置补偿时，轴要位于IDLE的停止状态！

3.开始补偿点的坐标填写的是轴的MPOS坐标！

4.带编码器反馈的轴类型，不同位置补多少个脉冲，是根据编码器位置确定脉冲补偿的个数的，所以一定要保证编码器的反馈是正确的！

5.机台断使能前，需先关掉二维螺距补偿功能，否则如果断使能的情况下机台有移动，会造成dpos跟mpos的数据对不上。

6.如下图假设开始补偿点是坐标系的原点，补偿间距分别是【DisX,DisY】补偿的点是【4*4】的话，螺距补偿表存储的点位数据就是以下【点1】到【点16】的位置下的补偿数据。

07 二维螺距算法通过Basic编程快速验证效果

假设工况1：X轴和Y轴的丝杆螺距正常，但X轴和Y轴安装不垂直，假设安装夹角是135度。

步骤一：
规划开始补偿点，补偿间距，补偿的行数和列数。

'初始化二维螺距补偿的相关参数TableId = 1000
'补偿数据的Table起始点StartMposX = 0
'X轴开始补偿的位置StartMposY = 0
'Y轴开始补偿的位置DisX = 0.5
'X轴方向补偿的间距DisY = 0.5
'Y轴方向补偿的间距ColNumX = 50
'X轴方向补偿的点数RowlNumY = 50
'Y轴方向补偿的点数

步骤二：
根据该工况的测试环境，计算各个补偿点的X方向和Y方向的改补偿的脉冲数，并更新到TABLE。

'更新该工况下二维补偿表的TABLE数据
FOR i=0 to RowlNumY-1
  '先按行填写该行的每一列的数据
  FOR j=0 to ColNumX-1
    '【工况一：XY轴螺距正常，但X轴和Y轴安装不垂直，假设安装夹角是135度】
    '一定是先运动到补偿点位位置然后通过其他定位方式(如视觉)实际定位出来的差值
    IF i=0 and j=0 THEN
      Angle=135 'X轴和Y轴的安装夹角
    ENDIF
    LOCAL ActPosX,ActPosY		'实际位置
    '计算实际位置
    ActPosX = 1 + (COS(Angle*PI/180))
	  ActPosY = (SIN(Angle*PI/180))
    '计算各个补偿点需要补偿的X和Y方向的脉冲数
    Table(TableId+2*(i*ColNumX+j))  = DisX*(1+i)*(1- ActPosX) * UNITS(AxisX)
    Table(TableId+2*(i*ColNumX+j)+1) = DisY*(1+i)*(1- ActPosY) * UNITS(AxisY)
  NEXT
NEXT

步骤三：
调用二维螺距补偿指令启用二维螺距补偿功能。

'开始2D螺距补偿
WAIT IDLE
PITCH2SET(1,StartMposX ,StartMposY,,DisX,DisY,ColNumX,RowlNumY,TableId)

步骤四：
发送运动指令，根据实际的编码器反馈来验证控制器实际上发的脉冲数是否正确。

'开始运动
MoveTest(1,1.1,RunMode)
MoveTest(2,1.1,RunMode)
MoveTest(2,2.1,RunMode)

步骤五：
验证效果。（PS：x轴和Y轴的脉冲当量均设置的是100000）

根据以上数据启用二维螺距补偿功能后，我们可以计算出运动到点【1,1,1】时，X轴和Y轴理论上补偿的脉冲数分别是109999.9999个脉冲和45563.49186个脉冲，根据实际编码器的反馈值发现X轴和Y轴实际补偿的脉冲数分别是109999个脉冲和45563个脉冲，因为脉冲个数是整数，所以二维螺距补偿功能正常。

假设工况2：XY轴的螺距均有异常需要补偿，但X轴和Y轴安装是垂直的。

假设X轴的激光干涉仪数据如下：

'运动到1mm处，激光干涉仪打出来实际是0.9mm处，所以需要补偿的脉冲 = 0.1*units

'运动到2mm处，激光干涉仪打出来实际是1.8mm处，所以需要补偿的脉冲 = 0.2*units

'运动到3mm处，激光干涉仪打出来实际是2.7mm处，所以需要补偿的脉冲 = 0.3*units

'Y轴丝杆工况和X轴丝杆工况类似

步骤一：
规划起始补偿点，补偿间距，补偿的行数和列数。

'初始化二维螺距补偿的相关参数
TableId = 1000 '补偿数据的Table起始点
StartMposX = 0 'X轴开始补偿的位置
StartMposY = 0 'Y轴开始补偿的位置
DisX = 0.5 'X轴方向补偿的间距
DisY = 0.5 'Y轴方向补偿的间距
ColNumX = 50 'X轴方向补偿的点数
RowlNumY = 50 'Y轴方向补偿的点数

步骤二：
根据该工况的测试环境，计算各个补偿点的X方向和Y方向的改补偿的脉冲数，并更新到TABLE。

'假设X轴的激光干涉仪数据如下：
'运动到1mm处，激光干涉仪实际是0.9mm处，所以需要补偿的脉冲 = 0.1*units
'运动到2mm处，激光干涉仪实际是1.8mm处，所以需要补偿的脉冲 = 0.2*units
'运动到3mm处，激光干涉仪实际是2.7mm处，所以需要补偿的脉冲 = 0.3*units
'Y轴丝杆工况和X轴丝杆工况类似
'更新该工况下二维补偿表的TABLE数据
FOR i=0 to RowlNumY-1
  '先按行填写该行的每一列的数据
  FOR j=0 to ColNumX-1
  '【实际测试发现运动到1mm处补偿的脉冲数不是0.1*units,是因为算法发现发0.1*units实际跑到不是0.1mm,发0.1111*units才跑0.1mm】
    Table(TableId+2*(i*ColNumX+j)) = DisX*(j+1)*0.1*UNITS(AxisX)
    Table(TableId+2*(i*ColNumX+j)+1) = DisY*(i+1)*0.1*UNITS(AxisY)
  NEXT
NEXT

步骤三：
调用二维螺距补偿指令启用二维螺距补偿功能。

'开始2D螺距补偿
WAIT IDLE
PITCH2SET(1,StartMposX ,StartMposY,,DisX,DisY,ColNumX,RowlNumY,TableId)

步骤四：
发送运动指令，根据实际的编码器反馈来验证控制器实际上发的脉冲数是否正确。

'开始运动
MoveTest(1,1,RunMode)
MoveTest(2,1,RunMode)
MoveTest(2,2,RunMode)

步骤五：
验证效果。（PS：x轴和Y轴的脉冲当量均设置的是100000）
根据以上数据启用二维螺距补偿功能后，我们可以计算出运动到点【1,1】时，X轴和Y轴理论上补偿的脉冲数分别是11111.11108个脉冲和11111.11108个脉冲，根据实际编码器的反馈值发现X轴和Y轴实际补偿的脉冲数分别是11110个脉冲和11110个脉冲，这里理论补偿值和实际补偿值误差在1个脉冲我们认为二维螺距补偿功能是正常的。

假设工况3：X轴和Y轴的丝杆螺距均有异常需要补偿，且X轴和Y轴的安装夹角是135度。

假设X轴的激光干涉仪数据如下：

'运动到1mm处，激光干涉仪打出来实际是0.9mm处，所以需要补偿的脉冲 = 0.1*units

'运动到2mm处，激光干涉仪打出来实际是1.8mm处，所以需要补偿的脉冲 = 0.2*units

'运动到3mm处，激光干涉仪打出来实际是2.7mm处，所以需要补偿的脉冲 = 0.3*units

'Y轴丝杆工况和X轴丝杆工况类似

步骤一：
规划起始补偿点，补偿间距，补偿的行数和列数。

'初始化二维螺距补偿的相关参数
TableId = 1000 '补偿数据的Table起始点
StartMposX = 0 'X轴开始补偿的位置
StartMposY = 0 'Y轴开始补偿的位置
DisX = 0.5 'X轴方向补偿的间距
DisY = 0.5 'Y轴方向补偿的间距
ColNumX = 50 'X轴方向补偿的点数
RowlNumY = 50 'Y轴方向补偿的点数

步骤二：
根据该工况的测试环境，计算各个补偿点的X方向和Y方向的改补偿的脉冲数，并更新到TABLE。

'假设X轴的激光干涉仪数据如下：
'运动到1mm处，激光干涉仪实际是0.9mm处，所以需要补偿的脉冲 = 0.1*units
'运动到2mm处，激光干涉仪实际是1.8mm处，所以需要补偿的脉冲 = 0.2*units'运动到3mm处，激光干涉仪实际是2.7mm处，所以需要补偿的脉冲 = 0.3*units
'Y轴工况和X轴工况类似
'更新该工况下二维补偿表的TABLE数据
FOR i=0 to RowlNumY-1
  '先按行填写该行的每一列的数据
  FOR j=0 to ColNumX-1
  '【工况三：XY轴螺距异常，XY的夹角是135度】
  'X轴螺距有问题，实际是10000个脉冲跑0.5mm
  'Y轴螺距也有问题，实际是10000个脉冲跑0.9mm
  IF i=0 and j=0 THEN
    Angle=135'XY轴的安装夹角
  ENDIF
  LOCAL ActPosX,ActPosY '实际位置
  'X轴补偿数据
  ActPosX = 0.5*(j+1)+ (i+1)*0.9*(COS(Angle*PI/180))
  Table(TableId+2*(i*ColNumX+j)) = DisX*((1+j)-ActPosX)*UNITS(AxisX)
  'Y轴补偿数据
  ActPosY = 0.9*(SIN(Angle*PI/180))
  Table(TableId+2*(i*ColNumX+j)+1) = DisY*(1+i)*(1- ActPosY)*UNITS(AxisY)
  NEXT
NEXT

步骤三：
调用二维螺距补偿指令启用二维螺距补偿功能。

'开始2D螺距补偿
WAIT IDLE
PITCH2SET(1,StartMposX ,StartMposY,DisX,DisY,ColNumX,RowlNumY,TableId)

步骤四：
发送运动指令，根据实际的编码器反馈来验证控制器实际上发的脉冲数是否正确。

'开始运动
MoveTest(1,1,RunMode)
MoveTest(2,1,RunMode)
MoveTest(2,2,RunMode)

步骤五：
验证效果。（PS：x轴和Y轴的脉冲当量均设置的是100000）
根据以上数据启用二维螺距补偿功能后，运动到点【1,1】时，根据实际发的脉冲数推算出实际运动的位置是【0.99999,0.99999】和【1,1】的脉冲误差也是在一个脉冲以内，所以二维螺距补偿功能正常。

MoveTest函数的实现：

GLOBAL SUB MoveTest(Pos1,Pos2,RunMode)
  MOVEABS(Pos1,Pos2)
  DELAY(1500)
  ?"运动前编码器数据",TempVarX,TempVarY
  ?"运动后坐标：【"+TOSTR(Pos1,4,1)+","+TOSTR(Pos2,4,1)+"】，当前XY轴的ENCODER:",ENCODER(AxisX),ENCODER(AxisY)
  ?"X轴实际补偿值",ENCODER(AxisX)-TempVarX-DPOS(AxisX)*UNITS(AxisX)
  ?"Y轴实际补偿值",ENCODER(AxisY)-TempVarY-DPOS(AxisY)*UNITS(AxisY)
  IF RunMode=0 THEN '工况1
    ?"X轴理论补偿值",-POS2*(COS(Angle*pi/180)/SIN(Angle*pi/180))*UNITS(AxisX),'"和实际有差值，可能是脉冲当量的问题"
    ?"Y轴理论补偿值",POS2*(1/(SIN(Angle*pi/180)) -1)*UNITS(AxisY),'"和实际有差值，可能是脉冲当量的问题"
  ELSEIF RunMode=1 THEN '工况2
    ?"X轴理论补偿值",POS1*0.1/0.9*UNITS(AxisX),'"和实际有差值，可能是脉冲当量的问题"
    ?"Y轴理论补偿值",POS2*0.1/0.9*UNITS(AxisY),'"和实际有差值，可能是脉冲当量的问题"
  ELSEIF RunMode=2 THEN '工况3
    ?"X轴理论停止位：",((ENCODER(AxisX)-TempVarX)*0.5 +(ENCODER(AxisY)-TempVarY)*0.9*COS(Angle*pi/180))/UNITS(AxisX)
    ?"Y轴理论停止位：",((ENCODER(AxisY)-TempVarY)*0.9*SIN(Angle*pi/180))/UNITS(AxisX)
  ENDIF
ENDSUB

08

二维螺距补偿功能现场实战

客户有一个激光切割机的机台需要使用二维螺距补偿指令来解决机台在不同位置受力不同机台形变不同导致的精度丢失问题。

步骤一：
以轴的MPOS坐标【0,0】作为二维补偿的开始补偿的位置。

步骤二：
以轴的MPOS坐标【40,40】作为第一个圆的圆心，然后以阵列的方式通过激光在板子是画出4行4列的16个圆，阵列X轴方向间距为40mm，Y轴方向间距为40mm。

步骤三：
通过视觉找出这16个圆的圆心，并把坐标填入到TABLE寄存器待处理。

startposx=0 '开始补偿点X坐标
startposy=0 '开始补偿点Y坐标
disonex=40 'X轴的补偿点间距
disoney=40 'Y轴的补偿点间距
maxpointx=4 'X轴方向补偿点列数
maxpointy=4 'Y轴方向补偿点行数
tableindex=1000 '存储补偿点数据的TABLE起始地址

步骤四：
通过视觉找出这16个圆的圆心，并把坐标填入到TABLE寄存器待处理。

步骤五：
计算各个补偿点的X和Y方向的脉冲补偿值=(理论位置-实际位置)*Units。

DIM i,j '
更新二维螺距补偿表
For i=0 to maxpointy-1
  For j=0 to maxpointx-1
    TABLE(tableindex+2*(j+i*maxpointx)) = UNITS(0)*((40+40*j) - TABLE(tableindex+2*(j+i*maxpointx)))
    TABLE(tableindex+2*(j+i*maxpointx)+1) = UNITS(1)*((40+40*i) - TABLE(tableindex+2*(j+i*maxpointx)+1))
  NEXT
NEXT

步骤六：
调用二维螺距补偿指令启用二维螺距补偿功能。

'启用二维螺距补偿
PITCH2SET(enable,startposx,startposy,disonex,disoney,maxpointx,maxpointy,tableindex)

步骤七：
发送运动指令，读取编码器的原始值验证二维螺距补偿是否正常。

'开始运动检测是否补偿成功
MOVEABS(40,40)
WAIT IDLE
DELAY(500)
?"当前坐标:",MPOS(0),MPOS(1)
?"编码器原始值:",MPOS_ORI(0),MPOS_ORI(1)
?"补偿值：",PITCH2_DIST(0),PITCH2_DIST(1)
MOVEABS(80,80)
WAIT IDLE
DELAY(1000)
?"当前坐标:",MPOS(0),MPOS(1)
?"编码器原始值:",MPOS_ORI(0),MPOS_ORI(1)
?"补偿值：",PITCH2_DIST(0),PITCH2_DIST(1)
MOVEABS(120,120)
WAIT IDLE
DELAY(1000)
?"当前坐标:",MPOS(0),MPOS(1)
?"编码器原始值:",MPOS_ORI(0),MPOS_ORI(1)
?"补偿值：",PITCH2_DIST(0),PITCH2_DIST(1)

步骤八：
测试的实际效果。

完整代码获取地址

▼

本次，正运动技术步进控制的光栅尺全闭环EtherCAT运动控制器ZMC432CL-V2快速入门：二维螺距补偿(上)，就分享到这里。

更多精彩内容请关注“正运动小助手”公众号，需要相关开发环境与例程代码，请咨询正运动技术销售工程师：400-089-8936。

审核编辑 黄宇