机电装备运动控制软件功能库开发

吴继春， 许 可， 陈凌宇， 范大鹏

（1.湘潭大学 机械工程学院，湖南 湘潭 411105；2.国防科技大学 智能科学学院，湖南 长沙 410073）

1 引 言

运动控制系统开发一直是机电控制领域的关键技术，越来越多的用户在寻求标准化程度高、可移植强、开放性好、能够快速开发的工业控制系统［1-3］。

目前，可编程控制器（Programmable Logic Controller， PLC）已被广泛应用，且已成为智能制造系统的载体和核心。通过符合PLCopen规范的编程开发方式，对不同类型组件高效集成［4］，将运动规划［5］算法、高精度控制［6］算法，以及机电系统的架构［7］均以功能块的形式表达，因此如何开发功能块是关键。Krzysztof等在基于PLCopen规范的基础上，通过PLC coder快速生成，将运动轨迹生成与算法实际结合起来，并证明了该方 法的 有效性［8］。Ruiz团 队开发 了基 于IEC61131-3标准的开发工具，用于评估PLC控制器在不同轨迹下的控制性能［9］。Doaa等针对运动控制内的安全问题进行研究，设计并开发安全功能块，用于保证运动控制过程中的安全和稳定［10］。国内外自动化生产商广泛采用PLC规范编程方式，提供了可以嵌入IEC 61131-3编程软件的机械运动、CNC、机器人等一系列标准运动控制功能库［11］。但是其开发的产品开放性差，在实际系统的应用有待研究，浙江工业大学徐建明等基于Codesys开发环境和EtherCAT通讯协议设计了PLCopen规范的六轴机器人控制系统［12-13］。针对控制算法的研究，华中科技大学研究团队提出运动控制系统分层模型和功能块打断机制，实现了基于PLCopen标准的基本运动功能块［14］。王夫康等针对单轴及主/从结构多轴功能块无法实现复杂多维运动控制的问题，对轴组功能块的执行方式、参数保存及传递方法进行研究，提出结构体队列法，设计轴组功能块，完成了多轴协调运动控制［15］。华南理工大学李迪团队研究了功能块内部的加减速控制算法以及多轴直线、圆弧插补算法，并在自主设计软PLC平台上进行了实验验证［16］。

上述研究都是基于特定开发环境使用传统编程开发方式开发的功能块，增加了编程要求和难度。本文对运动控制功能块的设计和开发方式进行研究，在PLCopen规范的基础上，提出了一种运动控制功能库架构及其快速生成方法。利用功能块快速开发技术解决了众多功能块的开发问题，降低了编程难度，提高了开发效率；并以旋转双棱镜和CNC运动系统为对象进行实验，实验表明，所提出的方法能够有效减少编程难度、缩短开发周期、增强程序的可复用性，同时也验证了自主开发的功能库能够满足复杂的运动控制需要。

2 PLCopen规范介绍

PLCopen组织先后制定了符合IEC61131-3标准的运动控制、逻辑控制、安全控制、OPCUA、XML代码交换等规范。该规范在基于控制安全前提下使得PLC逻辑、机器人控制、CNC控制融合在一个编程平台中。通过XML规范能够实现基于不同开发环境中开发的程序或代码的跨平台使用。通过OPC-UA规范实现上位机与控制器及HMI组态软件之间的安全通信。

2.1 PLCopen运动控制规范

在PLCopen规范中，运动控制规范为标准化的运动控制模块开发提供了框架。该规范基于IEC标准开发环境，在开发、安装和维护的各个阶段协调不同的开发平台，使上述环节都能够满足运动控制功能块的标准化要求，从而实现“软件定义机器”的目标。该规范分为6部分，内容和功能定义如表1所示。

表1 PLCopen运动控制规范Tab.1 PLCopen motion control specification

表1中，Part1和Part2对单轴和多轴功能块的基本功能块、状态机、使用规则等进行了定义和扩展。Part3为用户提高使用引导和功能块示例。Part4对Part1和Part2在三维空间多轴运动控制功能进行扩展，Part5是针对运动的坐标系变换功能进行扩展。Part6提供液压元件和液压泵系统控制规范。上述规范为运动控制功能块的设计和开发及实现制造装备的完全自主可控开发生态提供了标准。

2.2 功能块的实现原理

运动控制功能块的实现依赖3个基本要素，即运动控制功能块逻辑控制与算法实现、轴状态机以及轴信息集合体。其中，轴状态机表示轴当前状态及可能存在的状态转换关系；轴信息集合体则是包含了当前系统中被控轴的参数信息。当单个功能块执行时通过读取轴信息结构体中关于轴的实时信息来改变运行状态。多个功能块共同运行时，功能块之间、功能块与轴信息结构体之间实时进行参数传递。通过参数的实时读取改变内部函数的输出，进而得到功能块的输出，并将该输出下发至伺服机构执行。通过实时采集执行机构的参数和理想输入对比，对轴的状态进行实时调整。

功能块实现流程如图1所示，在PLC编程开发软件上对功能块或函数进行开发，通过调用功能块搭建运动控制模块；在编译完成后进行仿真或直接下发指令至PLC实时内核中，与轴信息结构体进行参数传递，进而改变轴状态机。在功能块执行的过程中，通过对比理想输出和实际输出对功能块参数进行实时修改，为避免由于功能块内部逻辑算法错误导致系统运行错误，可预先通过外部指令对系统进行调试。上述流程在PLCopen规范下，使得软件开发不再受硬件约束，用户可以使用同一程序控制不同的外设或机电设备，且只需要对功能块的输入参数进行修改，有效地提升了程序的复用性和快速移植性。

图1 PLCopen功能块实现流程Fig.1 Implementation process of PLCopen function block

功能块的执行是建立在状态机上的，且在工作时轴只能处于一种状态，通过功能块的执行情况实现轴状态的转换。根据PLCopen规范，将状态机分为单轴和轴组两类，存在8种状态，包括禁能、错误停止、静止状态机等3种非运行状态机和停止运动、回零运动、同步运动、离散运动和连续运动状态等5种运行状态机。轴的状态改变分为两种情况：一是功能块正常执行引起的改变；二是功能块执行错误或者系统发生故障所引起的状态改变。因此，在功能块开发时状态机的正常转换是实现精确运动控制的前提。

2.3 功能块的分层结构设计

PLCopen规范只规定到外部定义，对功能块内部的逻辑框架没有给出定义，因此需要用户根据工程实际需要开发程序和逻辑代码，形成标准化的算法结构和控制流程。如图2所示，一个标准的功能块分为内部逻辑和外部接口两部分，其中外部接口包括输入输出变量（包括变量类型）、功能块名称和类型。内部逻辑分为接口层、预处理层、轴状态层和逻辑算法（运动控制核心）。

图2 功能块分层结构Fig.2 Functional block hierarchical structure

虽然PLCopen规范为实现运动控制的基本功能块制定了一套通用标准，详尽规定了每个功能块实例所实现的具体功能、输入输出接口参数含义和数据类型、完成时序图等，而对于功能块的内部执行逻辑和核心控制算法则需要开发者具体实现。根据功能块的通用模型，本文设计功能块内部代码的分层结构，确保功能块内部逻辑实现与外部接口分开，又能通过外部接口实现与系统硬件驱动的连接，保证控制软件不再依赖硬件。

3 基于PLCopen规范的功能块开发方法

3.1 运动控制功能块的开发

功能块是控制系统的关键，也是“软件定义机器”的核心一环，随着一系列研究的进行和标准的提出，其开发方式也逐渐丰富。目前，开发方式主要有两种，一是传统的在开发环境中采用IEC标准语言进行程序编写的方式。由于开发软件计算能力等因素的限制，开发含大量专业数学运算的功能块会增加编程难度，如机器人的正逆运动学求解功能块的开发等。因此，该方法还存在一定的局限性。二是基于PLCopen XML规范，借助其他开发软件或数学软件采用PLC coder自动生成的方式生成代码，将符合软件兼容性的代码导入目标开发环境，解决大量复杂的数学运算功能块的开发问题。该方法能够通过模型生成代码，并且可在生成软件上进行仿真和代码检查，其开发效率对比传统开发方式有着显著提升，适合复杂功能块的快速开发。

图3为功能块的快速开发流程。在预处理阶段，对工程要求进行分析，确定目标被控对象及相关参数。基于数学软件进行特性分析并建立控制模型和子系统。在建立子系统后需要对它进行兼容性检测，如不符合标准，则对模型和子系统进行调整；反之，则对目标模型进行仿真以验证模型是否符合目标工程要求。对模型进行仿真能够有效地减少开发错误，及时对模型和参数进行修改。模型仿真通过后根据目标开发环境生成相应PLC coder。

图3 功能块快速开发流程Fig.3 Rapid development process of function blocks

上述方法生成的PLC Coder包括注释信息和主代码，注释信息包括生成软件和目标软件的版本信息和代码生成的时间。主代码包括功能块名称、变量和主程序，各部分都是采用符合IEC标准的ST语言。基于XML规范，用户在工程中将ST代码导入即可使用，极大地增加了功能块的开发效率，减少了开发过程中的错误。本文基于上述开发方法，开发了机器人的正逆运动学功能块以及插补功能块等复杂的运动控制功能块。

3.2 运动控制功能库架构

虽然国外软件上已形成了一套标准化的功能库，但是由于功能库被封装，用户不能更改内部程序和算法，这不利于自主可控PAC系统的快速开发和重构。而目前国产软件的开发工具并没有专业的运动控制功能库。因此，对于功能库的开发是自主可控PAC系统开发及“软件定义机器”的关键环节之一。

本文基于国产PAC软件建立了标准化的运动控制库架构，其架构如图4所示，运动控制功能库由工程和库信息模块、变量和指令模块和功能块/函数模块三部分构成。其中，信息模块用于库名称和库信息的设置；变量和指令模块对驱动接口和数据类型等进行定义；此处为PLCopen规范下的自定义数据类型，如全局变量、结构体、枚举等。功能块/函数模块是功能库的核心，不同的运动控制库对应不同的功能块集合。用户针对目标运动将所需功能块封装至同一库内，可以直接调用。

图4 功能库结构树Fig.4 Structure tree of function library

运动控制功能库是功能块和函数的集合，可用于实现单轴运动、多轴运动和插补运动等功能。如图5所示，库内的功能块根据轴数分为单轴、多轴/轴组功能块，且每一类根据功能属性又分为轴运动控制类和轴管理类功能块。其中，运动控制类功能块用于驱动单轴或多轴运动，如MC_Interpolated（插补）功能块和Home（回零）功能块等；管理类功能块用于轴管理、参数配置和状态监控，如MC_Power（使能）功能块和Read-Status（轴状态读取）功能块等。

图5 功能块类型Fig.5 Function block types

4 运动控制功能块实例

4.1 轴管理类功能块

MC_Power功能块用于控制运动，控制从站伺服状态机的切换，使驱动器上电使能，同时监测系统的工作状态，其接口定义如表2所示。Axis为轴信息结构体，用于读取轴信息。Enable用于给输出轴使能，Enable为True时，驱动器就绪且状态Status显示为True。同时轴的状态机由禁能状态转换为静止状态，等待控制指令输入。如果从站出现故障状态机或者驱动器返回伺服报警信号，则状态Status仍为False，并通过Valid，ErrorID等输出接口将错误信息输出给用户，表明系统存在硬件问题。此时，轴将转换到错误停止状态，需要等待硬件问题修复后才可继续使用系统。

表2 MC_Power功能块接口定义Tab.2 Interface definition of MC_ Power function block

4.2 单轴运动控制功能块

单轴运动控制功能块是运动控制库中的基本元素。常用的单轴运动控制功能有MC_Home、速度控制、绝对运动（MC_MoveAbsolute）、相对运动（MC_MoveRelative）等，通过加减速控制实现上述功能。

MC_MoveAbsolute和 MC_MoveRelative运动功能块是常用的单轴运动控制功能块，绝对运动功能块用于以指定参数运动到指定的绝对位置，而相对位置则是运动一段相对距离。不同的参数会得到不同的运动轨迹。因此，速度规划是此类功能块的核心，本文针对这类功能块进行设计和开发。功能块实例如图5所示，两功能块的输出和输入变量与MC_Home相似，其中Distance表示位置增量输入，Position表示目标位置输入，Jerk为加加速度输入。Buffer_Mode用于选择轨迹类型，本文设计了3种速度类型。输入为0时系统以输入的速度匀速运行，1和2分别对应梯形和S型速度模式。

为验证两功能块是否能够完成目标运动，对两功能块进行仿真，在Beremize软件中调用两功能块并将相对运动功能块与绝对运动功能块相连，运动参数设置如表3所示。

表3 MC_MoveAbsolute和MC_MoveRelative的参数Tab.3 Parameter of MC_MoveAbsolute and MC_ Moverelative

仿真结果如图6所示，两功能块都是使用S型加减速方式。在运行至3 s左右时，对相对功能块的执行状态进行打断并使绝对功能块运行，可以看到两功能块能够完成预期运动，表明两功能块能够实现轴的相对运动和绝对运动。

图6 功能块执行结果Fig.6 Result of function block execution

4.3 多轴运动控制功能块

多轴运动的实现形式包括多轴同步协同或者实现空间任意轨迹，本文针对多轴运动开发了一系列运动控制功能块，如两轴和三轴直线插补、圆弧插补等。图5所示为MC_Interpolator插补功能块，其输入输出与相对运动功能块相似，通过上升沿触发，可以实现圆弧和直线插补，以用户指定参数进行插补运动控制。其中，输入变量Mode为插补模式，其值为0，1和2，分别代表匀速插补、梯形速度模式和S型速度模式。多轴圆弧插补算法采用在Matlab编程仿真，然后通过PLC Coder自动生成代码的方式实现，完成了从当前位置点P1（15，0，5）经过中间边界点P2（14，14，10）到 达 终 点P3（3，1，-10）的 空间圆弧插补，采用梯形加减速算法，插补速度为30 (°)/s，加速度为45 (°)/s2。插补结果如图7所示。

图7 插补功能块测试曲线Fig.7 Test curves of interpolation function block

5 仿真与实验

本文以旋转双棱镜和CNC系统为控制对象，开发了运动控制功能库，并通过自主搭建的实验平台对上述功能库进行验证。

5.1 自主可控PAC系统实验平台

如图8所示，系统由主站、从站、实时以太网总线、上位机软件和用户监控终端等构成。实时以太网采用高性能NCUC 2.0总线协议，通信周期为1 ms，逻辑编程软件和HMI监控软件运行于PC上位端，与系统主站进行Modbus或TCP/IP通信，完成控制程序代码下载和数据实时交互。

图8 国产PLC控制测试系统架构Fig.8 Architecture of domestic PLC control test system

主站是整个PLC系统的核心与“大脑”，主要用于完成系统实时任务调度、编译环境的逻辑信息运算、对各节点从站的指令下达与通信控制、HMI数据交互等功能。本文使用的主站为基于国产处理器M7的嵌入式主站，嵌入式主站基于“FPGA+ARM架构”，无操作系统，程序执行代码直接运行于嵌入式芯片上；从站节点模块主要是根据主站的指令进行外设操作，包含传感器的采集和输出，并通过以太网总线控制器与主站实现数据交换和通信。以太网总线通信控制器位于各从站模块中，主要实现各节点之间的高带宽、高效率、高速度的数据传输通信；伺服驱动装置和外设是系统完成运动控制的执行机构；系统软件主要包括IEC-61131-3标准软件集成开发环境和界面监控软件两部分，软件集成开发环境主要用于实现系统硬件组态、可视化的系统功能设计和逻辑编程、逻辑代码的编译、程序可执行文件的生成和下载等。本文采用的开发环境为国产Beremize软件，监控软件为Qtouch软件。界面监控软件主要用于人机交互界面的设计和开发，完成与主站的通信，保证对主站数据的实时监控。

5.2 旋转双棱镜控制实验

典型旋转双棱镜成像系统原理如图9（a）所示，两棱镜Π1，Π2的横截面为直角三角形，两直角边面相互平行且垂直于共同旋转轴Z轴。为实现一定空间角度内所有指向的全覆盖，两棱镜必须达到完全匹配，故通常让两棱镜具有相同的折射系数和顶角，即n1=n2=n，α1=α2=α。两棱镜旋转角θ1，θ2以X轴为测量起始。视轴方向以极坐标中的偏转角Φ和方位角Θ描述。通过对点光源进行折射，改变两个楔镜的相对旋转角度，光的传播路径随之改变，从而达到对光束方向的控制。因此，根据上述原理，本实验以两棱镜光源指向控制为目标，验证所开发的功能块以及国产软硬件对在旋转双棱镜上的应用效果，控制系统如图9（b）所示。

图10所示为旋转双棱镜运动控制系统，系统主要由国产PAC系统、旋转双棱镜、视觉采集系统组成以及平行光源组成。所使用光源波长为650 nm，视觉采集系统由海康摄像头及视图处理软件组成。

图9 旋转双棱镜运动模型与控制系统Fig.9 Motion model and control system of rotating double prism

图9（b）中，轴管理和从站参数设置功能块用于对电机使能以及从站内部控制器的参数设置；双棱镜运动学功能块用于进行光束正向问题和逆向问题的解算；运动指令转换功能块用于对指令进行转换，以便从站执行；传感器反馈模块用于接收电机编码器反馈的实际运动参数；从站状态反馈功能块用于监视和调整从站的状态。

实验结果如图11所示，通过视觉处理系统可以获得光点的轨迹，由光点轨迹可以看出经过双棱镜系统的点射光源在经过反射后始终保持一个椭圆轨迹，表明所开发的功能块能够实现双棱镜光束指向的正、逆向问题求解，且所开发的软硬件系统能够完成双棱镜的光束指向控制。因此，该系统可以应用于自由空间光通信、红外对抗、激光指示器、激光雷达和光纤光开关等精密光学系统中。

图10 旋转双棱镜实验平台Fig.10 Rotating double prism experimental platform

图11 光点轨迹Fig.11 Light path

5.3 CNC运动控制实验

本文搭建了CNC运动控制实验平台和控制模块，如图12所示。图12（b）中，MC_power为轴使能功能块，MC_Interpolator为插补运动控制功能块，MC_Home功能块为复位功能块，负责轴的急停和轴的回零运动等。

本文以图13镟轮机璇修轨迹为插补对象进行实验，并以S型加减速模式运动，通过各段的曲率阈值，可以得到各段参数配置如下：Jerk=1.6×103mm/s3，Acceleration=20 mm/s2，Velocity=10 mm/s，各段的起始速度为vs1=0 mm/s，vs2=2 mm/s，vs3=4 mm/s，vs4=3 mm/s，vs5=5 mm/s，vs6=4 mm/s，vs7=3 mm/s，vs8=2 mm/s，vs9=2 mm/s；运动终点速度为ve9=0 mm/s，Mode=2。

图12 CNC功能库实验系统Fig.12 CNC functional library experimental system

图13 目标璇修轨迹Fig.13 Target repair trajectory

图14 加速度与速度曲线Fig.14 Acceleration and velocity curves

实际的加速度和速度曲线如图14所示。X和Y轴的实际位移曲线如图15（a）所示，两轴同步运动，可以实现多段的S型加减速运动。HMI反馈轨迹如图15（b）所示，该功能块可以正常完成圆弧插补和直线插补运动，与实际轨迹相比，在无负载的情况下两者之间的误差在0.3 mm。上述结果表明，自主开发的CNC运动控制功能库能够实现多种运动规划，模拟简单的CNC直线插补和圆弧插补运动，实现多电机的同步运动，初步验证了该运动控制库的有效性。

图15 插补轨迹Fig.15 Interpolation trajectory

6 结 论

本文针对PLC上位机软件中功能块和库不开放以及国产软件内部功能块缺乏等问题，基于XML规范，采用PLC coder技术自动生成符合IEC标准的结构化文本的方式快速开发功能块，能够解决上位机软件中大量含复杂计算的功能块的开发需要。基于功能块的基本模型设计功能块的分层结构，针对功能块在国产开发环境中的关键技术进行了研究，通过快速开发方法开发一系列运动控制功能库，如常用的机器人和CNC功能库等。通过功能库快速搭建了旋转双棱镜和CNC运动控制模块，能够有效节约开发时间。在PLC上位机软件中，利用快速生成的旋转双棱镜和CNC运动控制库搭建控制系统，通过自主搭建的实验平台，验证了该控制系统的有效性。实验结果表明，该方法能够快速生成基于PLCopen的运动控制功能库，解决了传统PLC软件功能库不完善的问题。该控制系统能够实现自主开发和控制，满足工业控制的需求。