该系统采用雷赛SMC606运动控制器和L5系列交流伺服电机控制、驱动国产SCARA机械手。如图1所示，气动手爪安装在机械手前段，其中心点不在Z轴的轴线上。

    该系统采用嵌入式工控机为主控单元，用C#语言编程，完成机器视觉定位、机械手运动轨迹规划的工作；并通过以太网向SMC606运动控制器实时发出运动控制指令，实现插头装配工作。

    和普通工控机+运动控制卡的方案相比，该系统具有稳定可靠、性价比高等特点。

图1.  SCARA机械手及手爪

一.SCARA机械手的运动学方程

1.SCARA机械手的正向运动学的解正向运动学是已知各关节的参数，求手爪的位置和姿态。

    即：已知各杆长L1、L2、L3，各关节转角θ1、θ2、θ3，求手爪在机座坐标系中的解。

    如图2所示，坐标系{0}是机座坐标系。机座坐标系又称为全局坐标系，是一个固定坐标系，它是计算机械手运动的基础。通常其原点设在机器手底部中心。

    坐标系{1}是连杆L1的坐标系。坐标系{2}是连杆L2的坐标系。坐标系{3}是工具坐标系。工具坐标系是表示工具中心点TCP（ToolCenterPoint）的位置和工具姿态的直角坐标系。工具坐标系是一个活动的坐标系，使用工具坐标系便于编写手爪抓取工件的程序。通过连杆坐标系之间的齐次变换，可得到坐标系{3}与坐标系{0}的变换关系如下：

2.SCARA机械手的反向运动学的解

    反向运动学是已知手爪的位置和姿态，求各关节的转角。即：已知手爪中心点P3在机座坐标系的位置P3（x0，y0）和转角a。求：各关节转角θ1、θ2、θ3。如图3所示。

设点P2在机座坐标系的位置为P2（M，N）。显然，

二.最佳路径的选择

1.选择终点的解

    如图4所示，终点位置连杆的参数有2个解，选用哪一个为好？需要设计一个判断准则。因为连杆L2、Z轴和手爪都安装在连杆L1上，且L1比L2长，故其负载重、运动覆盖的面积大，所以，应该让L2多运动、让L1少运动。

图4.  SCARA机械手运动路径的选择

其中：0.6为连杆L₂的加权系数。

比较total1和total2，那个小就用那个解。如图5所示，选择解1；如图6所示，选择解2。

图5.  终点选择解1的示例

图6.  终点选择解2的示例

2. 选择手爪旋转方向

    由于手爪上连接有气管，旋转角度有限制。通常手爪的旋转角θ3小于360度。如图4所示，手爪的旋转角可是dθ3也可以是dθ3’。选择手爪旋转方向的判定准则如下：

1） 在dθ3和dθ3’中选择绝对值小者a；

2） 如果½asum+a½<360^o，则转动a；然后，asum=asum+a。式中asum为手爪累计旋转角。

3） 如果½asum+a½³360^o，则a为dθ3和dθ3’中的绝对值大者；然后，asum=asum+a。

    如图7所示为正常情况下，dθ3选择旋转角度小的方向。但图8所示，dθ3选择了旋转角度大的方向。因为此时如果选择角度小的方向，累计转角就会大于360^o。

图7.  正常情况下手爪旋转选取角度小的方向

图8.  为避免手爪旋转角累计超过360^o，手爪旋转方向和正常情况相反

三.SCARA机械手结构及参数

1.Z轴及关节J4的结构，SCARA机械手外形如图9所示。Z轴及关节J4由一根丝杆花键轴构成，如图10所示。

图9.  SCARA机械手外形                          图10.  丝杆花键轴的结构

丝杆花键轴运动有以下特点：

1）当电机3控制丝杆螺母带轮左右旋转时，Z轴可上下运动。

2）当电机4控制花键套带轮左右旋转时，关节J₄（手爪）可左右旋转，同时，Z轴上下运动。即：关节J₄的旋转运动和Z轴的上下运动是耦合在一起的。

3）关节J₄（手爪）要独立进行旋转运动，电机4和电机3必须联动。联动方式为：电机3的转角与电机4的转角相同，但方向相反。使Z轴向上和向下的位移量抵消。

2. 计算各轴的脉冲当量

    已知该机械手的各轴参数如下：

    关节J₁和J₂的减速器的减速比均为1:50；关节J₄的减速器的减速比为1:20。Z轴减速器的减速比为1:2，丝杆导程为16mm。机械手上的4个电机转动一周的脉冲数都为5000个。

故：控制J₁电机运动的脉冲当量 Equival1 = 5000´50/360 = 694.44444 脉冲/度。

控制J₂电机运动的脉冲当量Equival2 = 694.44444 脉冲/度。

控制J₄电机运动的脉冲当量Equival4 = 5000´20/360 = 277.77778 脉冲/度。

控制Z电机运动的脉冲当量Equival3 = 5000´2/16 = 625.0 脉冲/毫米。

四．  SMC606控制SCARA机械手的程序

1. 控制界面的设计

    SMC606控制SCARA机械手运动分为两类：手动控制和自动控制。手动控制又分XYZ坐标下的位置控制和3个关节旋转轴的位置控制。如图10所示。

图10.  SCARA机械手控制界面

    手动控制有2种方式：连动和点动。连动即连续运动，按键按下后运动开始，按键抬起后运动停止。点动即点击一次按键，相应的电机运动一段距离。且每次运动的速度和距离也可选择为高、低和长、中、短。

2. 程序的初始化

1）在SMC606运动控制器资料光盘中的动态库目录下找到文件LTSMC.CS、LTSMC.dll，并拷贝到C#工程文件目录中的BIN/DEBUG内。

2）用鼠标右键点击“解决方案资源管理器”的工程名，然后点击“添加”、“现有项”，找到上述目录中的LTSMC.cs文件，添加到工程中。

3）在Form1.cs头文件中添加SMC606运动控制器的命名空间 using Leadshine;

4）在Form1_Load函数中连接SMC606运动控制器。指令如下所示。

注意：与SMC606连接的PC机的IP地址前3段必须为192.168.5。

3. 手动控制的程序

1）单轴运动的点动程序

    关节J₁、J₂和Z轴的点动过程都是单轴运动，采用点位运动指令Pmove很容易实现此功能。控制关节J₁正方向点动的程序如下所示，关节J₂和Z轴的点动程序类似。其中运动距离Dist0和运动速度Max_Vel0由界面中的“点动距离”和“速度”确定。

2）关节J₄（手爪）旋转的点动程序

    如本文三.2节所述，关节J₄做旋转运动时，Z轴也会运动。为了实现J₄独立运动，必须同时让Z轴反向运动。采用2轴插补运动可实现此功能。详见如下程序。

3）X和Y方向的点动程序

    如本文一.2节所述，当手爪仅向X方向运动时，关节J₁、J₂、J₄都会运动；由本文三.2节所述，关节J₄运动时，Z轴也会运动。故X方向的点动，实际上涉及到4个轴的运动，采用4轴插补运动可实现该功能。X和Y方向的点动程序类似。仅给出X正方向点动程序如下。

4）关节J₁、J₂、Z轴的连动程序

    关节J₁、J₂和Z轴的连动过程都是单轴运动，采用速度控制指令Vmove很容易实现此功能。控制关节J₁正方向点动的程序如下所示，关节J₂和Z轴的连动程序类似。

5）关节J₄的连动程序

    当按键按下后，用连续运动指令vmove控制关节J₄按一定的速度持续运动；同时，用点位运动指令pmove控制Z轴跟随关节J₄运动；并且打开一个定时器，每0.2秒自动检查一次位置误差，然后用在线变位指令调整Z轴的目标位置。当按键抬起后，关节J₄运动停止；然后，检查Z轴位置，最后一次在线调整Z轴的目标位置，使其误差为零。程序如下所示。

6）X、Y轴的连动程序

    以X轴正方向连动为例：当按键按下后，计算位移量dx，然后计算终点处4轴的参数，调用4轴插补运动函数，且有加速过程；指令脉冲发送完后，继续向前运动dx，但4轴插补运动为匀速过程。当按键抬起后，再向前运动最后一个dx，但4轴插补运动有减速过程。这样连动过程就比较连续、平稳。程序如下所示。

4. 机械手位置动画显示程序

    如图10所示，程序由一个定时器控制，每隔0.2秒在控制界面上显示一次机械手连杆L₁、L₂和手爪的位置。为提高动画显示质量，避免画面闪烁，坐标轴及网格、机械手运动范围不是每次重画一遍，而是直接调用事先画好的一个图片。动画程序如下所示。

5. 机械手自动控制程序

    根据机械手动作要求，编写自动控制程序并不复杂。点位运动的指令可参照本文三.2节中介绍的“X和Y方向的点动程序”内容。对于复杂的轨迹控制，可调用连续插补、样条差值（PVT函数）等指令。重点是机器视觉与运动控制的混合编程。因篇幅受限，相关内容将在后续文章中介绍。

五．  小结

    SMC606运动控制器的运动控制指令的种类多、功能强。只要充分理解SCARA机械手的运动学正解和反解的算法、灵活应用SMC606的指令，控制SCARA机械手达到快、稳、准的要求并不难。

六．  参考文献

[1] 郭洪红. 工业机器人技术（第三版）. 西安：西安电子科技大学出版社，2016

[2] 张爱红. 工业机器人应用与编程技术. 北京：电子工业出版社，2015