基于PLCopen的轴组协调运动功能块的设计与实现

（合肥工业大学，合肥 230009）

0 引言

随着工业化的发展，一大批运动控制器涌现出来。许多运动控制器中都集成有PLC的功能，可以实现一些简单的控制逻辑。但是在拥有众多I/0点的复杂运动控制系统中，这些简单的PLC功能无法满足控制要求。因此，PLC和运动控制往往是独立的两套系统，两者之间可以通过I/O接口进行数据交互实现同步。这就导致了系统设计十分复杂。

PLCopen组织将PLC与运动控制结合起来，实现了PLC编程语言标准化，并推出运动控制功能块标准。标准化的功能块仅仅定义到接口和数据结构层面，功能块内部算法实现由各个厂商自行定义，采用不同功能块的组合，可以满足众多应用场合的需求。目前PLCopen的MC运动控制规范定义的功能块主要分为用于单轴运动的Part1部分，用于电子齿轮、电子凸轮的Part2部分以及轴组协调运动的Part4部分。

由于控制单轴的运动控制系统已被广泛实现，而针对轴组协调运动的轨迹规划描述较少，本文利用菲尼克斯提供的SoftPLC，所调用功能块的逻辑关系满足PLCopen定义的轴组状态机状态的切换，并通过编写轴组直线插补与圆弧插补算法，验证所设计的功能块可以被应用于传统工业机器人和CNC领域内，具有高移植性。

1 PLCopen轴组功能块的设计

1.1 功能块模型

PLCopen标准将运动控制的编程语言定义为功能块的形式，功能块作为程序组织单元的一种，执行后能够生成一个或多个输出值。并且功能块能够生成多个自身的拷贝，被称为实例。如图1所示，每个实例都拥有相关的实例名、一个包含输出和内部变量的数据结构、输入变量的值。数据结构中的所有输出变量和某些内部变量的值可能是保持的，这意味着在上一次功能块的执行过程之后的这些变量的值在下一次执行中也是有效的。因此，实例化出来的几个相同的功能块之间依旧存在着复杂的逻辑关系。

图1 功能块模型

对于功能块而言，只有其输入和输出变量是可访问的。换句话说，功能块内部变量、数据结构、逻辑控制是不透明的，用户只需要根据功能块内部算法所需要输入的速度、加速度、位移等参数进行输入配置即可，既能方便操作，也能保护运动控制功能块开发人员的知识产权。

1.2 轴组状态机的设计

在运动控制系统中的运动命令总是按照顺序给出，并会根据状态机作用于运动轴上，而运动轴始终处于状态机所定义的某种状态。单轴控制系统中轴状态机的状态切换是以单个轴为控制对象，而轴组也拥有自身的状态机，并且轴组内的单个轴也有自身的状态机，两者相互独立却也有关联。

如图2所示为PLCopen定义的轴组状态机。PLCopen将轴组状态机分为6个部分，在系统上电后，系统处于轴组禁用状态，通过调用MC_GroupEnable或者MC_GroupDisable状态机可以在轴组禁用以及轴组待机相互切换，并且在这两种状态下，都可以执行将运动轴加入轴组或者移出轴组的操作。轴组功能块只要是执行运动类功能块都将进入轴组运动中状态。轴组回零状态展示的是一系列的回零操作被执行的过程。在轴组运动或者回零过程中，通过使能MC_GroupStop功能块，运动或者回零过程会被立即打断，状态机会切换至轴组停止过程，并且在轴组停止的过程中任何功能块都不可以打断它，直到停止完成后，状态机自行切换至轴组待机。当轴组内任何一根轴出错时，系统状态会进入轴组出错停止，只有调用MC_GroupReset功能块才能离开该状态。

图2 轴组状态机

1.3 运动功能块的设计

通过C/C++将直线插补与圆弧插补算法写入轴组直线运动与圆弧运动功能块，并且调用相关功能块便可实现相对应的功能。本文功能块以绝对位置为例，基于PLCopen设计的功能块如图3、图4所示。在图3所示的直线插补功能块中，输入输出引脚外部为变量数据类型，内部为变量名称。输入变量有储存数据状态的轴组号、使能端、多维数组构成的目标位置、速度、加速度、减速度、加加速度、参考坐标系、缓冲模式、过渡模式、过渡参数。其中过渡模式与过渡参数并不常用，本文没有针对这两个输入作用进行多余阐述。输出变量主要是展示功能块运行状态的标志位，包括完成位、忙碌位、激活位、打断位、错误位以及错误ID号。

图3 直线运动功能块

图4所示的圆弧插补功能块与直线插补功能块在输入接口定义上有稍许不同，增加了圆弧模式、辅助点、终止点、圆弧方向选择。在不同圆弧模式下辅助点的定义不同，圆心模式下辅助点为圆心点而边界模式下辅助点为边界点，在圆心模式下需要选择插补方向而在边界点模式下圆弧插补方向在功能块初始化状态下即被确定。

图4 圆弧运动功能块

2 轴组协调运动算法

2.1 直线插补

为了满足待插补线段具有任意性的特点，插补首点一般不是轴组的起始点。轴组需要先执行起始点到插补首点的直线插补，再执行待插补线段的插补操作如图5所示。以插补首点到插补终点的过程为例，在开始插补前，需要将坐标系的参考点转换为a点。以插补首点为原点建立直角坐标系，如果插补终点b在坐标系的第一、四象限，由式(1)算出ab与X轴正方向的夹角θ1；如果插补终点b在a点第二、三象限，θ为ab与X轴负方向的夹角。

在ab段的插补过程中，可以将插补线段分割成一系列的小段，每小段分别对应轴组每个运动周期内的位移，每段位移量由每个运动周期的速度控制，通过将速度V沿X轴与Y轴分解，由式(2)算出Vx与Vy来分别执行每一轴的运动插补，通过判断插补终点在插补首点的方向，分解轴将向判断后的位置进行运动，轴组最终停在插补终点。所以只需要通过规划速度即可实现ab段的插补。

直线插补中需要注意的是由于插补点位置的数据类型为整形，而规划速度算法的精确性决定了插补脉冲的数据类型为浮点型。这就导致了在插补过程中会遗漏掉许多小数位的脉冲当量，造成插补算法的不精确。本文采用的解决办法为记录每一次的插补脉冲，分别提取整数位与小数位，将整数位的脉冲作为插补点位置脉冲进行发送而小数位的数据则加入下一次的插补脉冲中一同发送，可以保证插补数据基本不丢失。

图5 直线插补示意图

2.2 圆弧插补

相比较直线插补，圆弧插补的实现比较复杂，需要将一段圆弧转化成若干微小线段，在每个运动周期内，轴组进行直线运动，每个细小线段的最终组合逼近理论圆弧。常用的圆弧插补算法有逐点比较法、弦线法、DDA算法。本文采用改进的DDA算法，即轴组沿着圆弧切线方向进给，插补速度由角度控制，并在插补终点通过角度判别来判断是否插补结束。

在二维坐标系下，插补圆弧的选择通常可分为中心点模式与边界点模式。中心点模式即给定圆弧的中心点与插补终点，同时需要规定圆弧的插补方向，考虑到中心点可能与物体发生碰撞，所以一般情况下不能用于示教。而边界点模式则需要给定圆弧的边界点与插补终点，圆弧插补方向沿着起始点到边界点再到插补终点，由于圆弧可以抵达边界点，所以这种模式可以用于示教。本文采用边界点模式进行圆弧插补。

如图6所示为圆弧插补示意图，插补的主要过程为：1）分别算出起始点F与插补终点B与变换坐标系后原点O相对于X′正方向的角度，分别记为θF、θB，并且始终保证角度控制在[0,2π]，通过FA与AB的矢量积确定圆弧插补方向。2）插补过程中，每进行一次插补操作都需要由式(3)算出运动点此时的插补角度θ，其中：XAct为轴组实际横坐标，XO为圆弧中心点的横坐标位置。

4）当轴组切线速度V=VMax时，得到加速过程中的角度θAcc，如果进行逆时针插补，当插补角度时，进行减速运动；如果进行顺时针插补，当插补角度进行减速运动。

图6 圆弧插补示意图

3 轴组功能块的验证

本文通过德国KW公司提供的编程环境Multiprog平台来调用所编写的轴组功能块，验证所开发的直线插补与圆弧插补的准确性。整个实验平台如图7，PC机与运动控制器通过以太网连接，运动控制器采用ARMCortex A9开发板，底层执行WinCE的操作系统，在嵌入式系统运行时，应用层会执行eCLR系统，这种系统可以将微软通用的中间语言代码转换成嵌入式CPU所能执行的机器码，缩短了应用程序的执行时间。运动控制器作为EtherCAT主站，与作为从站的伺服驱动器的通讯周期为2ms。并且将EtherCAT通讯周期作为事件任务与Multiprog相关联，即EtherCAT每进行一次数据传输，Multiprog内与这个事件任务相关联的实例化功能块被执行一次。实现了通讯周期与运动控制器的执行周期同步。

图7 实验平台

为了配合轴组进行插补运动，还需要调用一些管理类功能块，如MC_ReadAxisInfo、MC_WriteAxisInfo、MC_GroupEnable、 MC_GroupStop，分别代表读取轴信息、写轴信息、轴组使能、轴组停止。本文通过调用一个轴组直线插补功能块与两个圆弧插补功能块，来实现功能块之间的协同工作，功能块主要参数设置如表1、表2、表3所示，其中位置点的单位为unit，速度单位为unit/s，加、减速度的单位为unit/s2。

轴组功能块坐标系CoordSystem选择MCS代表机器坐标系，功能块之间的打断模式BufferMode设置为Aborting，表示想要执行的运动功能块在使能后可以打断现有的运动功能块。Multiprog内置逻辑分析仪，可以实时获取运动数据，并将数据保存在文件内并用MATLAB绘制插补轨迹来分析所设计的功能块是否满足设计标准。单轴运动曲线及运动空间曲线分别如图8、图9所示，插补算法可以准确到达插补位置。

表1 圆弧功能块1参数

表2 直线功能块参数

表3 圆弧功能块2参数

图8 单轴运动曲线

图9 轴组空间运动曲线

4 结束语

本文将运动控制算法模块化，设计了轴组运动控制功能块模型。在Visual Studio 2015中通过C#开发功能块的输入输出接口以及功能块类型，再利用C++将运动控制算法封装进功能块内，在上位界面Multiprog中调用功能块并在ARM-Cortex A9开发板内执行运动控制程序，最终将轴组运动数据导入MATLAB中绘制运动轨迹。实验结果表明所设计的轴组运动功能块符合PLCopen标准的要求，这种以功能块的形式来进行运动控制可以被应用于传统工业机器人和CNC领域内，并且维护方便、可靠性高。