EtherCAT运动控制器PT/PVT实现用户自定义轨迹规划

ZMC408CE硬件介绍

ZMC408CE是正运动推出的一款多轴高性能EtherCAT总线运动控制器，具有EtherCAT、EtherNET、RS232、CAN和U盘等通讯接口，ZMC系列运动控制器可应用于各种需要脱机或联机运行的场合。

ZMC408CE支持8轴运动控制，最多可扩展至32轴，支持直线插补、任意圆弧插补、空间圆弧、螺旋插补、电子凸轮、电子齿轮、同步跟随等功能。

ZMC408CE支持PLC、Basic、HMI组态三种编程方式。PC上位机API编程支持C#、C++、LabVIEW、Matlab、Qt、Linux、VB.Net、Python等接口。

ZMC408CE支持8轴运动控制，可采用脉冲轴（带编码器反馈）或EtherCAT总线轴，通用IO包含24路输入口和16路输出口，部分IO为高速IO，模拟量AD/DA各两路，EtherCAT最快125us的刷新周期。

ZMC408CE支持8个通道的硬件比较输出、硬件定时器、运动中精准输出，还支持8通道PWM输出，对应的输出口为OUT0-7，支持8个通道同时触发硬件比较输出。

更多关于ZMC408CE的详情介绍，点击“推荐|8通道PSO的高性能EtherCAT总线运动控制器”查看。

PCIE464M硬件介绍

PCIE464M是一款基于PCIe的PCI Express的EtherCAT总线运动控制卡，具有多项实时和高精度运动控制控制功能。

用户可直接将PCIE464M嵌入标准PC机实现高性能的EtherCAT运动控制功能，实现高精多轴同步控制，EtherCAT控制周期最小可达100us！

PCIE464M内置多路高速IO输入输出，可满足用户的多样化高速IO应用需求，如：高速色标锁存、高速PWM、多维位置比较输出PSO、视觉飞拍、速度前瞻、编码器位置检测等应用。

PCIE464M运动控制卡上自带16进16出，第三方图像处理工控机或PC无需额外配置IO数据采集卡和PLC，即可实现IPC形态的机器视觉运动控制一体机，简化硬件架构，节省成本，软硬件一体化。

更多关于PCIE464M的详情介绍，点击“PCIE464M-高速高精，超高速PCIe EthrtCAT实时运动控制卡”查看。

▶▶▶ PT指令与PVT指令

1.PT运动介绍

PT（Position-Time）运动：是一种单位时间内的运动位置控制技术，主要用于控制步进电机和伺服电机等运动设备在设定时间内实现精确的轴位置控制。位置跟时间参数一般是PC每个周期计算好对应的坐标，然后传给控制器。

PT算法：在用户定义的“位置和时间”点之间，PT算法计算出一个合适的速度曲线。PT算法保证控制卡的轨迹计算符合每一个已知的点和时间。分段速度简单的由位置和时间的差分计算出来。

PT运动适用场景：适用于近距离的点位运动或低速度的运动。它是非常简单的算法，需要很少的计算量，因此计算速度很快。在低性能的运动系统中很受欢迎。但如果点之间间隔太大，那么运动将会很粗糙，因为每一段的加速度将会显得不连续。

PT运动为保证设定的单位时间内达到规划的目标位置，控制器自动生成相应的加速度、速度曲线规划。如果在极短时间内运动大距离，脉冲频率会过高，导致电机堵转。建议分解成小段，重复发送，连续的多个周期PT运动时, 速度会自动均匀。

2.PVT运动介绍

PVT（Position-Velocity-Time）运动：是一种基于位置、速度和时间的综合控制方法，通过协调这三个方面的参数，实现对运动系统的精准控制。PVT在一段时间内驱动电机运动设置的距离，可以指定结束速度，小段内速度会自动根据前面的速度与结束速度来自动规划，尽可能连续。位置、速度跟时间参数一般是PC每个周期计算好对应的坐标，然后传给控制器。

PVT算法：在用户定义的“位置/速度/时间”点之间，PVT算法计算出合适的Jerk参数（加加速度，非恒定加速度）。这个算法保证轨迹计算合符每个已知点的位置、速度和时间。

PVT模式算法适用场景：PVT算法对于平滑轨迹和轨迹跟踪非常有效。位置轨迹点可以间隔很近，也可以间隔很大。比如说对于复杂的路径，点位需要间隔很近；对于简单的路径，点位可以间隔很大。PVT可以手动指定点位置，但是要确定好每个点的合适速度值。

3.正运动的PT指令与PVT指令

相关的指令说明可以打开RTSys软件，点击软件右上角的【帮助文档】，选择RTBasic编程手册，在第七章的7.3节特殊运动指令章节进行查看。

▶▶▶ ZBasic快速验证自定义轨迹规划指令MOVE_PTABS

1.MOVE_PTABS指令的介绍。

注意距离参数dis的单位不是脉冲数，dis*units才表示具体的脉冲数。

时间参数ticks的单位是伺服周期，控制器的伺服周期可以通过RTSys软件的在线命令输入?*max进行打印。

如下图SERVO_PERIOD:1000 min:500 max:4000表示当前控制器的伺服周期是1000us，伺服周期的可调范围是500us到4000us。

控制器伺服周期可以通过在线命令发送SERVO_PERIOD=500把伺服周期设置成500us，控制器断电重启后生效。

2.如何通过move_ptabs指令来封装接口来实现单轴Sin曲线的位置规划。

（1）接口详情

'主函数逻辑
'轴参数初始化
BASE(0,1,2)
DPOS=0,0,0
MPOS=0,0,0
UNITS=100,100,100
MAX_SPEED=10000000,10000000,10000000
'停止线程和轴运动
STOPTASK 1
RAPIDSTOP(2)
WAIT IDLE
'清除控制器报警
DATUM(0)
'开始轨迹规划
'单轴Sin曲线，振幅10，周期100个SERVO_PERIOD，初相角 0°
Runtask 1,MySingleAxisSin(0,10,Pi/50,0)
END
'/**********************************************************************
'函数功能：单轴sin函数的轨迹
'函数模型：y=Amp*sin(Rad*x+Ipa)
' 输  入 ：Amp(振幅)、Rad(角速度ω，弧度制)、Ipa(初相角φ，弧度制)
' 输  入 ：AxisNum(运动的轴)
' 输  出 ：
' 备  注 ：周期=2*Pi/Rad*10（控制周期）
'**********************************************************************/
GLOBAL SUB MySingleAxisSin(AxisNum,Amp,Rad,Ipa)
   LOCAL lvTime
   lvTime=0
   '选择要运动的轴 
   BASE(AxisNum)
   WHILE 1 
       '通过PT运动下发运动指令
       MOVE_PTABS(10,Amp*SIN(Rad*lvTime+Ipa))
       lvTime=lvTime+1
       DELAY(1)
   WEND
ENDSUB

（2）测试结果

3.如何通过move_ptabs指令来封装接口实现多轴的Sin曲线的位置规划。

（1）接口详情

'主函数逻辑
'轴参数初始化
BASE(0,1,2)
DPOS=0,0,0
MPOS=0,0,0
UNITS=100,100,100
MAX_SPEED=10000000,10000000,10000000
'停止线程和轴运动
STOPTASK 1
RAPIDSTOP(2)
WAIT IDLE
'清除控制器报警
DATUM(0)
'开始轨迹规划
'多轴的Sin曲线规划
Runtask 1,MyMultAxisSin(10,Pi/50,0,10,Pi/100,0,10,Pi/200,0)
END
'/**********************************************************************
'函数功能：0、1、2轴的3轴sin函数的轨迹  
'函数模型：y1=A1mp*sin(Rad1*x+Ipa1)
'     ：y2=A2mp*sin(Rad2*x+Ipa2)
'     ：y3=A3mp*sin(Rad3*x+Ipa3)
' 输  入 ：Amp(振幅)、Rad(角速度ω，弧度制)、Ipa(初相角φ，弧度制)
' 输  出 ：
' 备  注 ：周期=2*Pi/Rad*10（控制周期）
'**********************************************************************/
GLOBAL SUB MyMultAxisSin(Amp1,Rad1,Ipa1,Amp2,Rad2,Ipa2,Amp3,Rad3,Ipa3)
   LOCAL lvTime
   lvTime=0
   '选择要运动的轴
   BASE(0,1,2)
   WHILE 1 
       MOVE_PTABS(10,Amp1*SIN(Rad1*lvTime+Ipa1),Amp2*SIN(Rad2*lvTime+Ipa2),Amp3*SIN(Rad3*lvTime+Ipa3))
       lvTime=lvTime+1
       DELAY(1)
   WEND
ENDSUB

（2）测试结果

4.如何在move_ptabs指令后插入move_op指令来实现运动中对输出口的操作。

（1）假设需要在每执行Spasce个周期就需要操作一下输出口0，接口详情如下。

'主函数逻辑
'轴参数初始化
BASE(0,1,2)
DPOS=0,0,0
MPOS=0,0,0
UNITS=100,100,100
MAX_SPEED=10000000,10000000,10000000
'停止线程和轴运动
STOPTASK 1
RAPIDSTOP(2)
WAIT IDLE
'清除控制器报警
DATUM(0)
'开始轨迹规划
'单在Sin曲线轨迹规划插入对输出口的操作指令
MySingleAxisSinOP(0,10,Pi/50,0,0.25)
END
'/**********************************************************************
'函数功能：单轴sin函数的轨迹+MOVE_OP 
'函数模型：y=Amp*sin(Rad*x+Ipa)
' 输  入 ：Amp(振幅)、Rad(角速度ω，弧度制)、Ipa(初相角φ，弧度制)
' 输  入 ：AxisNum(运动的轴)，Space（间隔多少个周期操作一下输出口）
' 输  出 ：
' 备  注 ：周期=2*Pi/Rad*10（控制周期）
'**********************************************************************/
GLOBAL SUB MySingleAxisSinOP(AxisNum,Amp,Rad,Ipa,Space)
   LOCAL lvTime,lvOpFlag
   lvTime=0
   lvOpFlag=0
   MOVE_OP(0,OFF)
   WHILE 1 
       MOVE_PTABS(10,Amp*SIN(Rad*lvTime+Ipa))
       '检测是否执行到Space个周期，执行到需要打开/关闭输出口
       IF lvTime MOD(Space*2*Pi/Rad) = 0THEN
           IF lvOpFlag=1 THEN
               MOVE_OP(0,OFF)
               lvOpFlag=0
           ELSE
               MOVE_OP(0,ON)
               lvOpFlag=1
           ENDIF
       ENDIF
       lvTime=lvTime+1
       DELAY(1)
   WEND
ENDSUB

（2）测试结果

5.如何通过硬件定时器对输出口进行操作。

（1）硬件定时器指令介绍

MOVE_HWTIMER(mode, cyclonetime, optime, reptimes, opstate, opnum )

①mode：0-停止硬件定时器，1-动态修改参数（不修改启动设置），2-启动（启动后不可重复开启）

②cyclonetime：周期时间，us单位

③optime：有效时间，us单位

④reptimes：重复次数，启动模式，reptimes =0时，软关闭HW_TIMER，原来的脉冲没有完成的，会继续输出完成；-1时无限输出，除非主动关闭

⑤opstate：输出缺省状态，输出口变为非此状态后开始计时（输出口初始状态OFF。一般此参数设为OFF，将输出口变为ON状态后开始计时）

⑥opnum：输出口编号，必须能硬件比较输出的口。

（2）测试代码详情

'主函数逻辑
'轴参数初始化
BASE(0,1,2)
DPOS=0,0,0
MPOS=0,0,0
UNITS=100,100,100
MAX_SPEED=10000000,10000000,10000000
'停止线程和轴运动
STOPTASK 1
RAPIDSTOP(2)
WAIT IDLE
'清除控制器报警
DATUM(0)
'开始轨迹规划
'单在Sin曲线轨迹规划插入对输出口的操作指令
MySingleAxisSinOP(0,10,Pi/50,0,0.25)
END
'/******************************************************************************  
'函数功能：单轴sin函数的轨迹+输出口多次输出
'函数模型：y=10*sin(Pi/50*x+0)
' 输  入 ：AxisNum(运动的轴)
' 输  出 ：
'具体需求：要求Sin函数每隔0.5个周期打开一次输出口,并且输出口的以脉冲的形式输出4个脉冲
'******************************************************************************/
GLOBAL SUB MySingSinOP_HwTime(AxisNum)
   LOCAL lvTime,Space
   lvTime=0
   Space=0.5
   BASE(AxisNum)
   MOVE_OP(0,OFF)
   MOVE_HWTIMER(0, 60000, 30000, 4, OFF, 0)
   MOVE_HWTIMER(2, 60000, 30000, 4, OFF, 0)
   WHILE 1 
       MOVE_PTABS(10,10*SIN(PI*lvTime/50+0))
       '检测是否执行到Space个周期，执行到需要打开/关闭输出口,mod是求余指令
       IF lvTime MOD (50) = 0THEN
           MOVE_OP(0,ON)
       ENDIF
       lvTime=lvTime+1
       DELAY(1)
   WEND
ENDSUB

（3）测试结果

▶▶▶ Qt进行自定义轨迹规划的算法验证

1.新建QT项目，并添加正运动的动态库到项目里面。

（1）新建Qt项目。

图2-1 新建Qt项目 图2-2 选择Qt编译套件(kits) 图2-3 选择基类

（2）将函数库相关的文件复制到新建的项目中。

图2-4 库文件复制

（3）向新建的项目里面添加函数库的静态库。（zmotion.lib）

图2-5 添加函数库1 图2-6 添加函数库2 图2-7 添加函数库3

（4）添加函数库相关的头文件到项目中。

图2-8 添加头文件1

（5）声明相关头文件，并定义连接句柄。

2.正运动动态库相关接口说明。

（1）运动PC函数库编程手册所在路径。

（2）上位机如何连接控制器。

（3）上位机如何下发自定义的轨迹规划参数。

3.Qt自定义轨迹规划案例讲解。

（1）案例Ui讲解。

该例程除了可以下发Sin曲线的轨迹外，还可以按照设定的角度间隔操作输出口输出，打开输出口后硬件定时器将生效，会按照设定的周期去重复操作输出口，重复操作输出口的次数及界面上的脉冲持续个数。

（2）案例代码讲解。

①连接控制器。

//连接控制器
void MainWindow::on_LinkButton_clicked()
{
   int err=0;
   if(g_handle == NULL)
   {
       QString ControllerIp = ui->IPcomboBox->currentText();
       QByteArray IpStr = ControllerIp.toLocal8Bit();
       err = ZAux_OpenEth(IpStr.data(),&g_handle);
       if(err!=0)
       {
           QMessageBox::about(this, "提示","连接失败请确认IP正确！");
       }
   }
   else
   {
       QMessageBox::about(this, "提示", "句柄非空请先断开链接！");
   }
}

②启动自定义轨迹规划算法测试。

//点击启动按钮，如果条件满足讲会讲标志位RunFlag设置为从而触发定时器下发自定义的轨迹
void MainWindow::on_StartRun_clicked()
{
   if(g_handle!=0)
   {
       if(RunFlag==0)
       {
           //初始化轴参数
           //例程为方便测试，直接把规划位置DPOS清空
           ZAux_Direct_SetDpos(g_handle,0,0);
           //例程为方便测试，直接把编码器位置MPOS清空
           ZAux_Direct_SetMpos(g_handle,0,0);       
           ZAux_Direct_SetUnits(g_handle,0,1000);
           //关闭硬件定时器
           ZAux_Direct_HwTimer(g_handle,0,10000,5000,1,0,0);
           //启动硬件定时器功能，脉冲周期是50ms,高电平持续时间是25ms   
           ZAux_Direct_HwTimer(g_handle,2,50000,25000,ui->PluseNum->text().toInt(),0,0);
           //启动自定义轨迹规划算法测试
           CurAanle=0;
           RunFlag=1;
       }
       else
       {
           QMessageBox::about(this, "提示", "运行中请先停止");
       }
   }
}
//定时器的超时服务子函数
void MainWindow::MyTimeOut()
{
   if(g_handle!=NULL)
   {
       //如果启动条件满足则开始下发运动指令
       if(RunFlag==1)
       {
           int AxisList[1]={0};         //轴列表
           uint32 TicksList[1]={10};    //时间列表
           float PosList[1]={0};        //位置列表
           float OpCycTime=0;           //操作输出口的周期时间
           int RemainBuff=0;            //剩余缓冲器
           int A = ui->ParaAmp->text().toInt(); //更新Sin曲线的振幅
           double Rad =  qDegreesToRadians(ui->ParaRad->text().toDouble());//更新Sin曲线的角速度
           double Ipa =  qDegreesToRadians(ui->ParaIpa->text().toDouble());//更新Sin曲线的初相角
           OpCycTime = (2*M_PI/Rad*(ui->Angle->text().toInt()/360.0));
           //获取剩余轴缓冲器数目
           ZAux_Direct_GetRemain_Buffer(g_handle,0,&RemainBuff);
           //如果剩余缓冲器够下发新的运动指令
           if(RemainBuff>10)
           {
               for(int i=0;i< 10;i++)
               {
                   // 计算正弦值
                   PosList[0] = A*qSin(Rad*CurAanle+Ipa);
                   //下发正弦曲线
                   ZAux_Direct_MultiMovePtAbs(g_handle,1,1,AxisList,TicksList,PosList);
                   //如果到达打开输出口的角度，则下发打开输出口的指令
                   if(((int)(CurAanle/OpCycTime))*OpCycTime==CurAanle)
                   {
                       ZAux_Direct_MoveOp(g_handle,0,0,1);
                   }
                   CurAanle=CurAanle+1;
               }
           }
       }
   }
}

（3）通过RTSys的示波器功能接口自定义轨迹的运动效果。

本次，正运动技术EtherCAT运动控制器PT/PVT实现用户自定义轨迹规划，就分享到这里。

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

本文由正运动技术原创，欢迎大家转载，共同学习，一起提高中国智能制造水平。文章版权归正运动技术所有，如有转载请注明文章来源。

审核编辑 黄宇