王耀东，徐建明，徐胜华

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

0 引言

随着机器人在现代工厂的广泛应用，机器人技术得到了迅猛发展，使得传统工业发生新的变革。在生产、医疗、物流等新兴领域，机器人的应用改变了整个行业的发展模式。面对现代工业发展需求的不断提高，降低生产成本、缩短开发周期和扩大应用领域尤为重要。目前，机器人控制器多采用专用的控制器，成本高，开放性和移植性差。这就需要开发出一套高开放性、可移植性和可扩展性的机器人控制系统。

控制器是机器人控制系统的核心，机器人的运动学求解、运动控制和轨迹规划等都由控制器完成。控制器大致分为四类，基于单片机的控制器［1］、基于PLC的运动控制器［2］、基于PC运动控制卡的控制器［3］以及软件型运动控制器［4］。其中基于PCI总线的运动控制卡控制方式和软件型运动控制器在当下运用最为广泛。运动控制卡采用高性能微处理器，如DSP和大规模可编程设备，因此具有比较强大的计算能力和逻辑处理能力，大多数控制卡拥有统一的接口，开放性强。而软件型运动控制器是建立在计算机强大的计算能力基础之上，只需要将开发好的软件安装在PC或者嵌入式开发板上，该平台就具有运动控制能力，且该系统开放性强，具有逻辑语言编程、CNC多轴同步插补等优点。

目前，任福深等人［5］基于PC机开发了开放式的焊接机器人控制系统，该系统运行稳定，适应性强，可以很好的完成焊接任务。马琼雄等人［6］采用基于工控机 (IPC)结合运动控制卡的结构形式，并借鉴ORC(Open Robot Controller)等典型机器人控制系统软件体系结构设计了分层模块化的软件系统，使得现有机器人控制系统只需要做少量修改就可以应用到不同场合。胡鹏等人［7］采用IPC+DSP作为六轴工业机器人的控制器，设计了一种基于可编程多轴控制器 PMAC(Programmable Multi－Axis Controller)的开放式机器人控制系统，门昌华等人［8］以IPC+PMAC作为CIN CINNATI工业机器人的控制器，设计了一种基于PMAC的开放式机器人控制系统。

本文以埃夫特公司的ER50机器人为研究对象，采用基于ARM+CoDeSys架构设计机器人运动控制器，并采用EtherCAT实时以太网通信技术，实时性强，运行速度快。该控制器具有机器人示教、在线编程、建立坐标系、关节点动控制、笛卡尔点动控制、轨迹插补规划、一键回零、各轴位置实时显示等功能。

1 控制系统结构方案

本文采用在ARM板上安装CoDeSys软件作为开发平台，构成一个支持逻辑与机器人运动控制相结合的可编程逻辑控制器。上位机采用CoDeSys软件提供的可视化模块进行设计开发，驱动器采用清能德创的CoolA8驱动器，控制总线选择EtherCAT通讯总线，机器人本体使用ER50机器人。整个系统采用模块化、总线式设计，因此整个机器人控制系统具有很强的通用性与开放性。系统架构如图1所示。

图1 机器人控制系统整体架构图

本文在CoDeSys软件平台上，基于PLCopen规范［9-10］对工业机器人运动控制器的软件部分进行设计。设计目标是基于CoDeSys准确搭建ER50机器人的正逆运动学模型，基于该模型开发一款控制精度高，运行稳定，且具有点动、示教、在线编程等功能的机器人运动控制器。首先对根据ER50机器人的DH参数建立连杆坐标系进而进行运动学建模，然后基于该运动学模型实现机器人关节空间点动，笛卡尔空间点动，基本的轨迹规划以及机器人示教和在线编程等功能，同时针对各功能设计良好的人机交互界面。

2 ER50机器人运动学建模

机器人的机械结构和运动学密切相关。ER50机器人的机械本体属于控制系统的下层，是控制系统上层机构的控制对象。机器人本体设计和动力学不在本文的研究范围之内。机器人运动学方程又与本体结构密切相关，其运动学的正解和逆解是控制系统软件发送控制指令的依据，是工业机器人的轨迹规划、轨迹跟踪以及位置伺服控制的基础。因此本文以六自由度关节型机器人ER50为例，对工业机器人的运动学模型进行分析。

2.1 ER50机器人连杆坐标系建立

对于ER50机器人相邻坐标系之间的坐标变换，一般先对全部连杆从基座到机器人末端执行器依次建立相邻连杆i－ 1 与 i之间的变换关系。坐标系 Oi－1Xi－1Yi－1Zi－1的两次旋转和两次平移可以变换到坐标系OiXiYiZi，图2为相邻连杆坐标系的建立:

1) 绕轴 Zi－1旋转 θi角，使得 Xi－1轴与轴 Xi处于同一个平面内;

2) 沿Zi－1轴平移di个距离，使得轴Xi－1与轴Xi共线;

3) 沿 Xi－1轴 平 移 ai个 距 离，使 得 坐 标 系Oi－1Xi－1Yi－1Zi－1和 OiXiYiZi坐标原点重合;

4)绕Xi轴旋转αi角，使两坐标系重合。

图2 相邻连杆坐标系与参数

齐次变换矩阵可表示为:

其中:Trans为平移变换矩阵，Rot为旋转变换矩阵，具体可表示为:

ER50机器人的D－H参数如表1所示，其中di为关节轴i上的连杆偏距，ai－1为关节轴i－1和关节轴i之间的公垂线长度 (连杆长度)，αi－1表示关节轴i－1和关节轴i之间连杆转角。由于机器人本体的机械传动耦合关系，四关节的转动将影响五、六关节的转动，五关节的转动又会带动六关节的旋转，关节耦合比如表2所示，本文设计的机器人运动控制器通过软件编程补偿了关节耦合对机器人控制产生的误差。由机器人的连杆参数和关节的旋转参数确定机器人操作臂末端坐标系在基坐标中位姿关系，建立ER50机器人连杆坐标系如图3所示，根据该连杆坐标系关系建立ER50机器人的正逆运动学模型。

表1 ER50机器人D－H参数表

表2 ER50机器人关节耦合比

图3 ER50机器人连杆坐标系

2.2 ER50机器人运动学模块封装

2.2.1 正运动学功能块封装

本文正逆运动学求解方式借鉴《机器人学导论》［11］书中所提出的方法。将正运动学公式用程序代码表述并封装成功能块，程序功能块如图4(a)所示，config为机器人的D－H参数配置，drive0至drive5为关节角当前位置信息，Position是经过正运动学得出的笛卡尔空间位姿。armState、elbowState、handState分别是第一关节、第三关节和第五关节的状态。

2.2.2 逆运动学功能块封装

机器人逆运动学是指已知机器人工具末端的位置和姿态，通过逆运动学变换求出机器人各个关节轴对应的角度。由于本文设计对象为六轴串联型机器人，所以根据逆运动学方程变换最多可求出八组逆解，根据关节变化量最小的原则选取最优逆解，逆运动学变换功能块在调用时的实例图如图4(b)所示。模块中_pre表示上一次变换后各关节轴对应的关节角，pos表示需要变换的机器人末端位姿。dq表示经功能块变换后得出的各关节轴的角度。

3 机器人运动控制器功能开发

图4 运动学功能封装

在CoDeSys软件平台上开发机器人运动控制器的各个功能，本文主要从示教模式、点动模式以及在线编程模式几个方面来阐述该运动控制器的软件开发过程。

3.1 示教模式

3.1.1 示教流程

通过使用笛卡尔空间点动或关节空间点动功能控制机器人工具末端运动到需要示教的位置点，记录关节坐标以及笛卡尔空间末端执行器位姿信息。示教编程时，可根据不同运动类型选择不同运动模式，供选择的模式有MoveL、MoveJ以及MoveC三种运动模式，在文件程序运行时判断指令的ASCLL码值，然后再调用对应运动功能块。示教流程如图6所示。控制流程实现了Wait()函数，采用定时器TON功能块实现，定时时间可通过参数传入。

图5 示教流程图

手动操作机器人到各个示教点后保存示教点。再应用示教点指令生成示教程序，保存程序到.txt文件中，示教采集点和示教编程界面分别如图6(a)、6(b)所示。本系统根据机器人语言常用规范实现了一些基本指令，组成本系统示教再现模块的机器人指令集。机器人的指令集包括动作指令集、控制指令集。动作指令集包括 MoveC，MoveL，MoveJ，控制指令集有 Wait、Jump。MoveC、MoveL是圆弧插补和直线插补，MoveJ是笛卡尔空间点到点的轨迹规划。在选择插补方式时设定对应运动速度v=5，执行语句“MoveL［p0，p1］;”则机器人末端将从一个点p0沿直线轨迹运动到当前点位置p1，其速度为5 mm/s。通过Read按钮可以读取到示教文件上的各个示教点信息，可以在线编辑修改示教文件，以及插入、删除示教点。如果机器人在运行过程中出现问题，为了安全保障，可以选择Stop按钮，紧急停止所有操作。

图6 示教界面

3.2 点动模式

3.2.1 关节空间点动

关节空间点动模式采用MC_Jog功能块实现，可以单独控制关节轴的正反转，以及运行时的速度、加速度，也可以根据需要独立设置，Axis为电机轴对应的控制字。机器人控制系统单轴模式的程序图如图7所示。MC_Power功能块用于控制驱动器的使能状态，是控制伺服驱动器电源模块，上电使能是电机轴运动的必要前提。该模块中，Axis为关节轴控制字，对应于具体的关节轴电机驱动器，通过置Enable、bRegulatorOn和bDriveStart三个输入控制字为true即可使能伺服驱动器电源。MC_Reset实现电机轴的复位操作，用于电机轴从ErrorStop状态到StandStill状态的转换。

图7 单轴模式程序图

3.2.2 笛卡尔空间点动

笛卡尔空间点动，设计此功能的目的在于使用示教器示教或需要平移末端工具点时，能更加快速到达目标点，使用时更加方便和快捷。此功能设计有6个功能按键，分为X+、X－、Y+、Y－、Z+、Z－，分别控制机器人工具末端沿着世界坐标系中的X、Y、Z坐标轴移动。以X轴为例，当点击X+时，工具末端会沿着X轴正方向移动。此功能的设计原理是通过功能按键来控制末端位置的坐标值增加或减小，然后再根据机器人坐标系模型中的逆运动学方程，将末端位置转换为各个关节角度，再将转换出的各个关节角度作为各轴连接的电机的设定位置，从而控制各轴同步运动到相应的位置，达到预期目的。

3.2.3 点动模式界面设计

主要包含常用增删改查的操作，另包括：数据资源灾备(自动备份，异常情况下，保证数据资源安全)、数据库信息标识、日志信息查看、索引自动创建查看、授权信息、执行计划、服务性能、SQL审核、故障错误信息、系统升级、系统调用及部署等。

点动界面包含两种点动模式，分别为关节空间点动和笛卡尔点动，每种模式单独设计一个界面，界面包含点动控制的各功能按键、保障安全的急停按键、模式之间相互切换的切换按键、显示轴运动状态的状态显示灯、实时显示各轴关节位置或工具末端位姿数据的数据状态栏等。在关节空间下，通过控制各个关节来实现手动运行如图8(a);在运行速度上为了安全没有提供在线设置运行速度，而是提供了三种运行速度模式，分为Slow、Middle、High三种，这样操作者可以根据需求在线选择运行速度。另一种是在笛卡尔空间进行手动操作如图8(b)，可以手动移动在空间中的位置以及姿态，通过三维坐标的修改按钮，以及欧拉角的旋转按钮进行增加欧拉角度或者减少，从而改变末端姿态，使其运行到期望位置。

图8 点动界面

3.3 在线编程模式

3.3.1 在线编程实现

在线编程支持新建.txt编程文件，或者加载之前已有的编程文件并再此基础上进行修改，然后保存。运动指令主要有MoveJ、MoveC和MoveL三种，当在线编程时目标位置以及中间点位置要在机器人运动范围内以及运动指令要符合特定语法，如 MoveJ(10，20，30，10，10，5，5);并以 “;”结束，前6个参数表示目标位姿，最后一个参数表示运动速度。如果不符合这种语法在编译时会报错。错误类型主要有:指令不符合规范、文件为空、指令没有以分号结束等。编译完成无误情况下，可以通过运行按钮执行文件中的指令。控制指令有Wait指令，暂停一定时间。在线编程流程图如图9所示。

图9 在线编程流程图

3.3.2 在线编程界面

在线编程界面如图10所示，在线编程界面包含当前坐标系显示栏、机器人运动状态显示栏、程序编译错误显示栏、程序文件管理模块、程序文本编辑及修改栏、界面切换功能键、程序操作功能键及文件操作功能键。程序操作功能键包括对程序编译和运行，文件操作功能键包括文件新建、打开、保存、关闭和删除。通过编程界面可在线管理程序代码，存储在控制器中的程序文件也会在文件管理模块中显示，也可以新建.txt类型的程序文件。通过对程序文件修改保存后，可点击编译按钮，对程序进行编译，观察编写的程序是否有语法错误，编译无误后，可点击运行按钮，机器人就会按照程序功能控制机器人按照预定的轨迹到达目标位置点。通过查看机器人状态显示栏查看机器人运动状态。

图10 在线编程界面

4 实验结果与分析

在ER50六轴工业机器人上对设计的运动控制的在线示教、在线编程功能进行测试。实验对象如图11。

使机器人姿态保持不变，通过运动控制器的示教功能示教机器人位置，保存4个点如下:

P0(496．213，170．285，612．158，16，23，121)

P1(728．610，296．926，786．103，16，23，121)

P2(950，196．734，1723．02，16，23，121)

P3(1246．207，287．654，1810．143，16，23，121)

图11 ER50机器人本体+运动控制器

再通过在线编程功能执行语句“MoveL［P0，P1］;”实现点 P0到 P1的直线插补，以及 “MoveC［P1，P2，P3］;”实现点P1、P2、P3之间的圆弧插补。机器人末端位置的三维坐标记录如图12所示。将机器人运行数据保存起来并在matlab中对插值数据进行绘图，得到规划的轨迹曲线图如图13所示，将实际定位的P0、P1、P2、P3点的坐标与示教定位点坐标进行误差对比，数据如表3所示。

图12 机器人末端坐标位置

图13 机器人末端三维轨迹

表3 定位精度表

通过本文设计的控制器示教功能和在线编程功能实现了点P0到P1的直线插补，点P1，P2和P3的圆弧插补，从图12、13可以看出P0到P1点保持严格的直线运行，P1、P2、P3点间的轨迹平滑，足以说明该控制器具有很好的示教、在线编程功能以及运行稳定性;通过表3可以看出，运行点P0、P1、P2、P3与其示教点误差分别为0.1312 mm、0.0947 mm、0.1466 mm、0.1341 mm，完全满足工业生产中的加工精度要求，可见该控制器对ER50机器人的正逆运动学模型建立准确。

该机器人控制器功能众多，本文仅对直线、圆弧插补功能进行测试，从测试过程可以看出，本文设计的这款基于CoDeSys软件平台的运动控制器运行稳定、控制精度高，同时具备了丰富的使用功能和人机交互界面，满足大多数机器人平台的运动控制要求。

5 结束语

本文研究了一种基于ARM+CoDeSys架构的运动控制器。以ER50六自由度工业机器人为控制对象，结合Ether-CAT通信技术和PLCopen规范，研发了一款具备在线示教、在线编程、关节空间与笛卡尔空间点动以及轨迹插补规划等功能于一体的机器人运动控制器。该款控制器控制精度高，运行稳定，具有很好的开放性、可移植性和低成本等优势，已在六轴工业机器人、SCARA机器人以及三轴机器人等平台工厂化使用，并取得了预期控制效果。