两自由度并联机器人控制系统设计

谌孙杰，马 璐，吴秋平

(武汉理工大学 自动化学院，武汉 430070)



两自由度并联机器人控制系统设计

谌孙杰，马 璐，吴秋平

(武汉理工大学 自动化学院，武汉 430070)

针对机电装配、医药包装等行业自动化抓取和放置实际应用的需求，设计了一种采用直线伺服驱动技术的两自由度高速并联机器人；首先进行了机器人的机械结构设计，并且通过运动学分析，结合运用蒙特卡洛法与几何法求解出并联机器人的工作空间；其次采用MCU+CPLD(STM32F103+MaxⅡ)的双控制架构，根据工作空间结构在MCU中设计出高效的运动轨迹生成算法，并在CPLD中运用DDS技术将伺服运动参数转换成脉冲经一级差分后输出到伺服驱动器;在双控制器的通信方面合理利用CPLD中的状态机提出了双级数据缓冲方式，使得MCU写与CPLD读交错进行，可以实现数据的无缝刷新；最后进行了并联机器人系统样机的制作与调试，分析结果表明所研制的机器人控制系统能够使机器人快速抓放金属小球，实现稳定可靠运行。

并联机器人；双控制器；运动轨迹；无缝刷新

0 引言

随着人们对并联机器人的深入研究，其各方面的优势逐步显现出来。并联机器人可以提供高于串联机构的承载能力和刚度，同时还具有高速、高加速度和高精度等优点，众多研究机构和制造企业均看好其在工业制造领域的应用前景[1]。少自由度并联机器人不仅继承了并联机器人的固有优点，同时还克服了高自由度并联机器人控制困难、解算复杂的缺点，在工业上具有很高的实用价值。运动控制器按照期望的机械运动向电机驱动发出指令，在运动控制器的控制下，整个机器人系统按照一定的轨迹完成预期的动作。

传统伺服接口有模拟速度给定+编码器反馈以及脉冲+方向两种形式。前者在运动控制器内形成位置闭环，速度及电流环在驱动内；后者运动控制器以脉冲形式发出位置给定，位置、速度和电流闭环在驱动内。随着现场总线技术的快速发展，近年来采用现场总线的伺服接口也越来越普遍。运动控制行业的前景被很多公司看好，在国内外，已有多种商品化的开放式运动控制器，然而价格通常比较昂贵且功能过于复杂，在要求实现的功能在相对简单的小型伺服系统中并不实用[2]。基于以上分析，设计一款应用于低自由度并联机器人系统的运动控制器十分必要。

本文进行了一种新型实用的平面二自由度并联机器人的设计，应用于电子材料、机电装配、医药包装等行业自动化抓取和放置实际需求，采用精密直线电机直接驱动方式，取代了传统旋转伺服通过滚珠丝杆间接获取直线运动的中间环节，大大简化了系统的机械结构，降低了制造成本，同时提高了系统响应速度和时间精度[3]。设计过程中进行了并联机器人的机械结构设计和运动学分析，选用了MCU+CPLD双控制架构，研制了一种带有差分式Step+Dir通用伺服接口的运动控制器，编写了MCU伺服运算程序及CPLD脉冲发生程序，实现了样机的制作与调试，调试结果表明所设计的并联机器人能够准确快速完成抓取和放置要求，具有实用价值和创新性。

1 工作原理与运动学分析

本文所设计的两自由度并联机器人采用直线电机作为动力源，两根电机间距一定距离并且都安装有导杆，电机与导轨采用凸台轴向定位后与摆杆相连如图1所示。与左侧电机相连的两根导杆在正视图下重合，无论是左侧电机还是右侧电机，与其相连的摆杆在运动过程中始终围成底边在同一水平线上的平行四边形，这种结构确保了摆杆末梢铰链处的立方体金属块始终保持水平，使得与金属块相连的电磁铁运动到任意位置均可方便的吸放小球。

图1 并联机器人正视结构图

并联机器人机械系统的正常运行离不开精确可靠的控制系统，本文所设计的两自由度机器人用于抓放物体的末端执行器的运动状态由两直线电机共同决定，因此直线电机的控制是整个系统的控制关键。通常伺服控制系统由伺服电机、驱动器、控制器和检测装置组成，其中MCU需要完成运动轨迹计算及与上位机的通信，结合其控制任务及控制精度要求可选用STM32F103单片机。然而要发出一定带宽且能实时更新的脉冲去驱动直线电机仅采用单片机很难实现，因此本文所述的运动控制系统采用了MCU+CPLD双控制架构作为运动控制器，系统整体框图如图2所示。运动控制器要求能够根据运动控制指令，在线规划电机运动轨迹，计算出电机运行过程中每一时刻的具体位置、速度等参数；将电机的位置给定包括位移增量以及移动方向以脉冲形式发给伺服驱动器，去驱动并联机器人，所述的并联机器人由伺服电机和机械传动机构组成。系统运行时，控制器接受检测装置检测到的机器人实时运行状态同时与上位机进行通信，使整个系统能够实时接收上位机的管理控制，并能在突发情况下进行紧急处理。

图2 两自由度并联机器人控制系统整体框图

作为整个两自由度并联机器人控制系统的核心，运动控制器主要功能是根据提前规划的轨迹做运算，并将运算结果以脉冲方式输出位置与方向给定去分别驱动两个直流电机。要做轨迹规划就必须对并联机器人进行运动学分析，建立运动学方程。如图1所示，左右两侧电机的动子下部端点分别为M1、M2，A、B为动子的延长线与摆杆的交点，两转动传动副末梢闭链结构交点为C，并联机器人搬运的小球下半球面与水平面的切点为点D。根据这些关键点建立坐标系，结合实际距离参数，可以确定各关键点坐标。本文中涉及的运动学分析为运动学逆问题，即根据末端执行器在参考坐标系下的位置和姿态求解关节的位置，也就是已知摆杆L1、L2交点C的位置，求解两个电机动子下部端点M1、M2的具体位置，机器人运动学的逆解方程为：

(1)

式中,L为摆杆的长度，x、y分别表示横坐标和纵坐标。与此同时，还要确定机器人关节变量与末端执行器之间位置随时间的变化率，跟据所求的运动学逆解方程，对其进行时间微分，即可求得速度雅克比矩阵逆为：

(2)

进行机器人的轨迹规划时，要考虑机器人末端参考点所能到达的实际空间，还要进行机器人工作空间的求解。本文运用MATLAB仿真软件，使用蒙特卡洛法求得机器人理想工作空间，再考虑机器人避障问题(实际情况下受台阶限制)结合几何法[4-6]，求得实际工作空间(出去阴影部分)如图3所示。

图3 并联机器人实际工作空间

2 双控制器系统设计

本文所描述的控制系统采用单片机+可编程逻辑器件双控制器的设计，总体系统如图2所示。单片机负责与上位机通信、开关量的控制以及根据预设运动轨迹，计算出相应的脉冲频率给定，每1 ms(伺服周期)刷新一次频率给定值；可编程逻辑器件在接收到单片机的给定后，将其转化为对应频率的方波信号，这种双控制器架构，充分发挥了单片机的灵活性与可编程逻辑器件的高速性。结合系统的需求选用了具有丰富的存储及外设资源和强大处理能力的STM32F103RET6单片机作为主控制器。主控制器单片机和MaxIIEPM570最小系统的应用已经很成熟，此处不再赘述[7]。

单片机作为控制器系统的核心，需要根据第2节中所述的运动学分析，进行运动轨迹的合理规划和软件算法实现。机器人的所有运行轨迹均被限制在如图3所示的工作空间内，机器人利用电磁铁吸放金属小球的过程来模拟实际工业生产过程，其运动轨迹包括直线运动与圆弧运动。其中轨迹1、3、4为不包含x方向的直线运动，电机动子端点M1、M2的运动轨迹完全相同且为竖直方向直线运动，进行软件编程时，需根据预设给定位移，求解出运动轨迹中的加速、匀速(部分轨迹存在)和减速3个阶段之间的转折时间点及各时间段内运动方向；包含x方向的运动包括直线运动轨迹2和圆弧运动轨迹5，运用运动学分析方程分别求出轨迹2、5下的M1、M2位移时间曲线如图4所示，这种情况下若采用软件解算法远比不含x分量的直线轨迹复杂，因此本文采用一种在位移时间曲线上均匀采样去拟合逼近原曲线的方法。

图4 轨迹2、5下M1、M2位移时间曲线

对曲线进行拟合时，可均匀取n个采样点，每两点之间直线连接如图5所示，曲线S在t时间内位移与曲线S′在t′时间内位移相同，只是所用时间不同。因此所设计的算法在时域上具有伸缩性，对某具体轨迹均匀采样n个点，无论这条轨迹需要多长时间完成，都可用同一组数据表示完成运算，大大简化了软件解算程序。需要注意的是，进行软件编程时，一段连续的曲线运动轨迹在计算机内无法实现连续变化，因此我们需要将连续时域的轨迹作离散化的处理。以一次曲线为例，若运动控制器的伺服周期设为1 ms，其轨迹离散处理方法如图6所示。伺服周期决定了计算机更新伺服给定的最小分辨率，并且一个伺服周期内直线电机的速度保持最新给定值不变，那么在每1 ms的伺服周期中，要使用一个恒定的速度等效替代连续域中变化的速度，并保证最终完成的位移效果相同，可以遵行面积相等的等效替代原则。

图5 轨迹时域伸缩拟合示意图

图6 轨迹离散处理示意图

然而，通过MCU的软件解算得到的各类轨迹伺服参数仍然无法直接去驱动电机，还需要通过CPLD将控制参数转化为脉冲信号输出到驱动器，这里就涉及到MCU与CPLD之间的通信程序设计。

一个伺服周期中，MCU要向CPLD传输80 bit数据，如果采用并行通信方式需要大量IO资源，因此本文采用SPI同步串行的通信方式。利用CPLD内部的逻辑块设计生成一个SPI接口，它的功能相当于一个串入并出的移位寄存器。每根电机的运动参数信息为40 bit，因此可以开发一个40位的移位寄存器。并联机器人伺服系统对实时性要求很高，每个伺服周期内必须在前一组数据脉冲发送完成前完成下一组数据更新，本文设计了一种双级数据缓冲方式，其数据更新时序图如图7所示。

图7 双缓冲数据更新时序图

在CPLD脉冲发生周期进行到一半时，MCU通过SPI发送数据到一级缓存，一个周期脉冲发完时CPLD更新频率给定信号FRE和计数值CMP等参数，且通过CPLD中状态机的合理调度使得MCU的伺服运算周期与CPLD的脉冲更新周期在长度上相等，仅仅是错开了半个周期。这种将MCU传输数据与CPLD取数据在时间上交错的双级缓存方式，消除了伺服周期内由于脉冲更新产生的死区的影响，完成了数据的无缝刷新，提高了系统实时性。CPLD接收到MCU发送的实时数据，运用其高速的时序逻辑处理能力，需将这些参数转化为频率随时间动态变化的脉冲，这就需要DDS信号发生模块。DDS信号发生模块具有基准时钟频率fDDS、查表计数器cnt和相位步进量M3个重要参数，每来一个基准时钟沿，cnt的值加上M，cnt通过查表得出一个对应的具体数值作为输出如表1所示。

表1 M=1时DDS查表对应

表1表示当DDS的基准时钟频率fDDS=1且相位步进量M=1时，每间隔一秒来一个时钟沿，查表计数器加1，再通过表中对应得到某一时刻的输出值d，由表中数据可知输出信号的频率与相位步进量M成正比。任意设一个频率给定值FRE=1 001 Hz，本文所设计的DDS模块的输出通过示波器实测的显示结果如图8所示，示波器显示采样瞬间脉冲频率为1 002 Hz，频率平均值为1 000.99 Hz，由此可见实际输出频率与理想给定频率十分接近。

图8 FRE=1 001 Hz时DDS输出实测波形

DDS信号发生模块的输出经由一级差分处理后，送伺服驱动器，驱动电机完成控制器软件运算规划的运动轨迹，最终实现小球的搬运工作。控制器系统设计软件部分还包括MCU与上位机通信程序设计及简单的IO口开关量控制[8-9],由于篇幅原因这里不做详细说明。

3 实验样机调试与结果分析

通过前面两个部分的分析，分别运用C语言和VHDL语言对运动控制器中的MCU(STM32F103RET6)和CPLD(MAXIIEPM570)编程实现控制功能，还同时通过在PC机上运用LinMot-Talk软件对伺服驱动进行配置，设置驱动的工作模式并能在线实时显示位置反馈曲线，这使得驱动器PID参数的调节更加直观。将Step+Dir伺服接口接至显波器即可清晰地观察机器人走每个动作时，脉冲信号的具体发生状态。加速度aacc值为正数、零和负数时，分别表示运动学分析中的加速、匀速和减速运动过程。表示图9(a)所示为加速度aacc=6 m/s2时，DDS输出的脉冲波形，脉冲频率在快速逐渐增大；图9(b)所示为加速度aacc=-1 m/s2时，DDS输出脉冲波形，脉冲的频率在逐渐的减小。分析实验波形可知，当加速度为正值时，机器人的动子端点做加速运动，脉冲频率是一个递增的过程，且加速度的值越大，加速过程也就越快，加速时间越短。加速度的绝对值越大，脉冲频率的变化(增大或减小)就越快，加速或减速的过程也就越短。所得到的脉冲信号经过差分驱动芯片进行差分处理，幅值也被一定程度地放大，使得控制器最终的输出电平为与伺服驱动器兼容的422差分电平。

以上分析结果表明，本文所设计的双架构运动控制器能满足伺服过程中的各种加减速要求，并为伺服系统提供稳定可靠的脉冲给定，所述的并联机器人运动控制系统，在实际应用中，只需要通过设置几个伺服参数，根据具体情况，可以直接调用所设计的指令，实现对所有带Step+Dir接口的伺服电机的控制，具有较强的通用性。

图9 不同加速度值时的加减速脉冲波形

4 结束语

通过对两自由度并联机器人控制系统的理论研究，在实际样机测试达到了预期的控制效果。本设计仅对两自由度做出了研究，对多自由度的研究是并联机器人系统的发展方向。代替MCU+CPLD双控制架构的解决方案是采用单颗FPGA芯片作为控制器的架构，使系统的控制板更加细小，节约成本。随着工业化进程的不断发展，两自由度甚至多自由度的并联机器人在工业领域将会得到更多的应用，能极大地提高生产效率，具有很高的实用价值。

[1] 田 涛. 一种高速拾取并联机器人的设计与实现 [D]. 大连: 大连理工大学, 2013.

[2] 田家林. 基于ARM与FPGA的运动控制系统的设计[D]. 成都:西南石油大学,2006.

[3] 施 俊. 直线感应电机双边驱动电梯的控制系统研究 [D]. 杭州: 浙江大学, 2004.

[4] Stan S, Maties V, Balan R. New Methodology and Evaluation of Micro Parallel Robots for Optimal Design[A]. The 16th IEEE International Symposium on Robot and Human interactive Communication[C].2007: 894-899.

[5] Stan S, Maties V, Balan R. Workspace optimal design of a 2 DOF micro parallel robot using genetic algorithms and simulated annealing optimization methods[A]. International Conference on Mechatronics and Automation[C].2007: 1108-1113.

[6] 洪乃刚,等. 电力电子和电力拖动控制系统的MATLAB仿真[M]. 北京: 机械工业出版社,2006.

[7] 齐 珊. 多轴运动控制系统设计[D]. 天津: 天津大学,2008.

[8] 蒙博宇. STM32自学笔记[M]. 北京: 北京航空航天大学出版社, 2012.

[9] 黄 任. AVR单片机与CPLD/FPGA综合应用入门[M].北京: 北京航空航天大学出版社,2004.

Design of Control System for Parallel Robot with Two Degree of Freedom

Chen Sunjie，Ma Lu，Wu Qiuping

(School of Automation,Wuhan University of Technology, Wuhan 430070,China)

To fit the demand of automatic pick and place in the manufacture process of medicine packaging and electromechanical assembling,a high-speed 2-DOF parallel robot using linear servo technology is designed in this thesis. Firstly, the mechanical structure design of the robot is introduced, according to the results of kinematics analysis, the workspace of the 2-DOF parallel robot is obtained via Monte-Carlo method and geometrical method. Secondly, a control system consist of MCU and CPLD(STM32F103+MaxII)is adopted, an efficient approach to generating motion curve programs in MCU is proposed based on workspace. And we use DDS(Direct Digital Frequency Synthesizer) to generate square wave in accordance with motion parameters, then we transform square wave to differential signals which will be sent to servo driver. In the design of communication between the two controllers, a two stage data buffer is presented by rational use of state machine in CPLD, which can realize the interleave of MCU read and CPLD write to achieve seamless refresh of data. Finally we complete the manufacture and adjustment of the robot demo, the results shows that the performance of the 2-DOF parallel robot system is stable and reliable, at the same time the robot can pick and place the prill quickly.

parallel robot;dual controller;motion curve;seamless refresh

2015-12-08；

2016-01-07。

谌孙杰(1991-)，男，贵州凯里人，硕士研究生，主要从事嵌入式方向的研究。

1671-4598(2016)06-0110-03DOI：10.16526/j.cnki.11-4762/tp

C

A