一种基于SCARA机械手的运动轨迹规划方法及系统

摘要

本发明所提供的一种基于SCARA机械手的运动轨迹规划方法及系统，方法包括：上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将轨迹规划方式和运动参数发送至SCARA机械手的运动控制器；运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；SCARA机械手接收实际运动轨迹信息，并根据实际运动轨迹信息进行运动。本发明具有直线插补，圆弧插补，飞行捕捉三种不同运动轨迹算法，具有响应快，精度高特点。

权利要求

1.一种基于SCARA机械手的运动轨迹规划方法，其特征在于，所述方法包括步骤：

A、上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；

B、所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；

C、所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动；

所述步骤A中的所述轨迹规划方式是直线插补轨迹规划、圆弧插补轨迹规划和飞行捕获平滑轨迹规划；

所述步骤A中当所述轨迹规划方式是直线插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标和插补精度值；当所述轨迹规划方式是圆弧插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向；当所述轨迹规划方式是飞行捕获平滑轨迹规划时，则所述运动参数包括起点坐标、触发点坐标、终点坐标、触发脉宽和平滑度；

所述步骤B中具体包括：

B1、当所述轨迹规划方式是直线插补轨迹规划，则所述运动控制器中的DSP根据直线插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行B4；

B2、当所述轨迹规划方式是圆弧插补轨迹规划，则所述运动控制器中的DSP根据圆弧插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行B4；

B3、当所述轨迹规划方式是飞行捕获平滑轨迹规划，则所述运动控制器中的DSP根据飞行捕获平滑算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行B4；

B4、所述DSP将所述多个插补点的物理坐标转化为相对应的SCARA机械手大小臂的转角，并根据预定的梯形曲线加减速方法计算多个插补点的速度；

B5、所述DSP将所述多个插补点对应的SCARA机械手大小臂的转角及速度发送至所述运动控制器中的运动控制芯片，运动控制芯片根据所述SCARA机械手大小臂的转角及速度控制SCARA机械手运动；

所述步骤B1具体包括：

B11、所述DSP获取所述运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向间距ΔX及纵向间距ΔY；

B12、判断|ΔX|是否大于或等于|ΔY|，当大于或等于时则执行步骤B13，当小于时则执行步骤B14；

B13、根据n1＝floor(|ΔX|/ΔL+1)计算第一插补次数n1，并根据ΔLN1＝ΔX/n1计算当前第一插补精度ΔLN1，并根据ΔXi＝ΔLN1确定第一横向增量ΔXi，根据ΔYi＝ΔLN1*ΔY/ΔX确定第一纵向增量ΔYi，根据Xi＝Xi-1+ΔXi确定实际运动轨迹中n1个插补点的横坐标，根据Yi＝Yi-1+ΔYi确定实际运动轨迹中n1个插补点的纵坐标；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数；

B14、根据n2＝floor(|ΔY|/ΔL+1)计算第二插补次数n2，并根据ΔLN2＝ΔY/n2计算当前第二插补精度ΔLN2，并根据ΔXj＝ΔLN2*ΔX/ΔY确定第二横向增量ΔXj，根据ΔYj＝ΔLN2确定第二纵向增量ΔYj，根据Xj＝Xj-1+ΔXj确定实际运动轨迹中n2个插补点的横坐标，根据Yj＝Yj-1+ΔYj确定实际运动轨迹中n2个插补点的纵坐标；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数。

2.根据权利要求1所述基于SCARA机械手的运动轨迹规划方法，其特征在于，所述步骤B2具体包括：

B21、所述DSP获取所述运动参数中的起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)、圆弧插补精度值DfStep及圆弧插补方向；

B22、根据起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)及圆弧插补方向确定半径R、起点角度φa、终点角度φb、及终点角度与起点角度之差Δφ，并根据N＝floor(|π*R*Δφ/(180*DfStep)|+1)计算圆弧插补次数N，并根据Dfstep＝Δφ/N计算当前圆弧插补精度Dfstep，根据Xm＝R*cos(φa+m*Dfstep)+Xo确定实际运动轨迹中N个插补点的横坐标，根据Ym＝R*sin(φa+m*Dfstep)+Yo确定实际运动轨迹中N个插补点的纵坐标；其中，floor函数为向下取整函数，m为取值范围为[1,N]的正整数。

3.一种基于SCARA机械手的运动轨迹规划系统，其特征在于，包括：

设置模块，用于上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；

轨迹获取及发送模块，用于所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；

接收及运动模块，用于所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动；

所述设置模块中的所述轨迹规划方式是直线插补轨迹规划、圆弧插补轨迹规划和飞行捕获平滑轨迹规划；

所述轨迹获取及发送模块中当所述轨迹规划方式是直线插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标和插补精度值；当所述轨迹规划方式是圆弧插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向；当所述轨迹规划方式是飞行捕获平滑轨迹规划时，则所述运动参数包括起点坐标、触发点坐标、终点坐标、触发脉宽和平滑度；

所述轨迹获取及发送模块具体包括直线插补单元、圆弧插补单元、飞行捕获平滑单元、转角速度获取单元和发送单元，其中：所述直线插补单元，用于当所述轨迹规划方式是直线插补轨迹规划，则所述运动控制器中的DSP根据直线插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；

所述圆弧插补单元，用于当所述轨迹规划方式是圆弧插补轨迹规划，则所述运动控制器中的DSP根据圆弧插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；

所述飞行捕获平滑单元，用于当所述轨迹规划方式是飞行捕获平滑轨迹规划，则所述运动控制器中的DSP根据飞行捕获平滑算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；

所述转角速度获取单元，用于所述DSP将所述多个插补点的物理坐标转化相对应的SCARA机械手大小臂的转角，并根据预定的梯形曲线加减速方法计算多个插补点的速度；

所述发送单元，用于所述DSP将所述多个插补点对应的SCARA机械手大小臂的转角及速度发送至所述运动控制器中的运动控制芯片，运动控制芯片根据SCARA机械手大小臂的转角及速度控制SCARA机械手运动；

所述直线插补单元具体包括第一获取子单元、第一判断子单元、第一直线插补子单元和第二直线插补子单元，其中：第一获取子单元，用于所述DSP获取所述运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向间距ΔX及纵向间距ΔY；

第一判断子单元，用于判断|ΔX|是否大于或等于|ΔY|，当大于或等于时则启动第一直线插补子单元，当小于时则第二直线插补子单元；

第一直线插补子单元，用于根据n1＝floor(|ΔX|/ΔL+1)计算第一插补次数n1，并根据ΔLN1＝ΔX/n1计算当前第一插补精度ΔLN1，并根据ΔXi＝ΔLN1确定第一横向增量ΔXi，根据ΔYi＝ΔLN1*ΔY/ΔX确定第一纵向增量ΔYi，根据Xi＝Xi-1+ΔXi确定实际运动轨迹中n1个插补点的横坐标，根据Yi＝Yi-1+ΔYi确定实际运动轨迹中n1个插补点的纵坐标；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数；

第二直线插补子单元，用于根据n2＝floor(|ΔY|/ΔL+1)计算第二插补次数n2，并根据ΔLN2＝ΔY/n2计算当前第二插补精度ΔLN2，并根据ΔXj＝ΔLN2*ΔX/ΔY确定第二横向增量ΔXj，根据ΔYj＝ΔLN2确定第二纵向增量ΔYj，根据Xj＝Xj-1+ΔXj确定实际运动轨迹中n2个插补点的横坐标，根据Yj＝Yj-1+ΔYj确定实际运动轨迹中n2个插补点的纵坐标；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数。

说明书

一种基于SCARA机械手的运动轨迹规划方法及系统

技术领域

[0001] 本发明涉及机械手控制技术领域，尤其涉及的是一种基于SCARA机械手的运动轨迹规划方法及系统。

背景技术

[0002] 当前，SCARA机械手广泛应用于电子、汽车、塑料、食品等工业领域，其主要职能是完成搬运和装配工作。随着加工工艺的复杂性与精确性不断提升，SCARA机械手在流水线上经常需要与其他工业设备协同作业，难免会出现与障碍物发生碰撞的危险。因此，对工作范围内存在障碍物的SCARA机械手的运动轨迹进行规划便显得尤为重要。

[0003] 轨迹规划(Path Planning)是指给定环境的障碍物条件，以及起始点和目标点位置，要求选择一条从起始点到目标点的路径，使SCARA机械手能安全、无碰撞地通过所有的障碍物。现有的普通SCARA机械手的轨迹精度平均为0.3mm，可见其轨迹精度并不高，而且轨迹规划的耗时也较长。

[0004] 因此，现有技术还有待于改进和发展。

发明内容

[0005] 本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种基于SCARA机械手的运动轨迹规划方法及系统，可有效解决采用现有技术SCARA机械手的轨迹精度不高，规划时间耗时长的缺陷。

[0006] 本发明解决技术问题所采用的技术方案如下：

[0007] 一种基于SCARA机械手的运动轨迹规划方法，其中，所述方法包括步骤：

[0008] A、上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；

[0009] B、所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；

[0010] C、所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动；

[0011] 所述步骤A中的所述轨迹规划方式是直线插补轨迹规划、圆弧插补轨迹规划和飞行捕获平滑轨迹规划；

[0012] 所述步骤A中当所述轨迹规划方式是直线插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标和插补精度值；当所述轨迹规划方式是圆弧插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向；当所述轨迹规划方式是飞行捕获平滑轨迹规划时，则所述运动参数包括起点坐标、触发点坐标、终点坐标、触发脉宽和平滑度；

[0013] 所述步骤B中具体包括：

[0014] B1、当所述轨迹规划方式是直线插补轨迹规划，则所述运动控制器中的DSP根据直线插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行B4；

[0015] B2、当所述轨迹规划方式是圆弧插补轨迹规划，则所述运动控制器中的DSP根据圆弧插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行B4；

[0016] B3、当所述轨迹规划方式是飞行捕获平滑轨迹规划，则所述运动控制器中的DSP根据飞行捕获平滑算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行B4；

[0017] B4、所述DSP将所述多个插补点的物理坐标转化为相对应的SCARA机械手大小臂的转角，并根据预定的梯形曲线加减速方法计算多个插补点的速度；

[0018] B5、所述DSP将所述多个插补点对应的SCARA机械手大小臂的转角及速度发送至所述运动控制器中的运动控制芯片，运动控制芯片根据所述SCARA机械手大小臂的转角及速度控制SCARA机械手运动；

[0019] 所述步骤B1具体包括：

[0020] B11、所述DSP获取所述运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向间距ΔX及纵向间距ΔY；

[0021] B12、判断|ΔX|是否大于或等于|ΔY|，当大于或等于时则执行步骤B13，当小于时则执行步骤B14；

[0022] B13、根据n1＝floor(|ΔX|/ΔL+1)计算第一插补次数n1，并根据ΔLN1＝ΔX/n1计算当前第一插补精度ΔLN1，并根据ΔXi＝ΔLN1确定第一横向增量ΔXi，根据ΔYi＝ΔLN1*ΔY/ΔX确定第一纵向增量ΔYi，根据Xi＝Xi-1+ΔXi确定实际运动轨迹中n1个插补点的横坐标，根据Yi＝Yi-1+ΔYi确定实际运动轨迹中n1个插补点的纵坐标；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数；

[0023] B14、根据n2＝floor(|ΔY|/ΔL+1)计算第二插补次数n2，并根据ΔLN2＝ΔY/n2计算当前第二插补精度ΔLN2，并根据ΔXj＝ΔLN2*ΔX/ΔY确定第二横向增量ΔXj，根据ΔYj＝ΔLN2确定第二纵向增量ΔYj，根据Xj＝Xj-1+ΔXj确定实际运动轨迹中n2个插补点的横坐标，根据Yj＝Yj-1+ΔYj确定实际运动轨迹中n2个插补点的纵坐标；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数。

[0024] 所述基于SCARA机械手的运动轨迹规划方法，其中，所述步骤B2具体包括：

[0025] B21、所述DSP获取所述运动参数中的起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)、圆弧插补精度值DfStep及圆弧插补方向；

[0026] B22、根据起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)及圆弧插补方向确定半径R、起点角度φa、终点角度φb、及终点角度与起点角度之差Δφ，并根据N＝floor(|π*R*Δφ/(180*DfStep)|+1)计算圆弧插补次数N，并根据Dfstep＝Δφ/N计算当前圆弧插补精度Dfstep，根据Xm＝R*cos(φa+m*Dfstep)+Xo确定实际运动轨迹中N个插补点的横坐标，根据Ym＝R*sin(φa+m*Dfstep)+Yo确定实际运动轨迹中N个插补点的纵坐标；其中，floor函数为向下取整函数，m为取值范围为[1,N]的正整数。

[0027] 一种基于SCARA机械手的运动轨迹规划系统，其中，包括：

[0028] 设置模块，用于上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；

[0029] 轨迹获取及发送模块，用于所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；

[0030] 接收及运动模块，用于所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动；

[0031] 所述设置模块中的所述轨迹规划方式是直线插补轨迹规划、圆弧插补轨迹规划和飞行捕获平滑轨迹规划；

[0032] 所述轨迹获取及发送模块中当所述轨迹规划方式是直线插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标和插补精度值；当所述轨迹规划方式是圆弧插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向；当所述轨迹规划方式是飞行捕获平滑轨迹规划时，则所述运动参数包括起点坐标、触发点坐标、终点坐标、触发脉宽和平滑度；

[0033] 所述轨迹获取及发送模块具体包括直线插补单元、圆弧插补单元、飞行捕获平滑单元、转角速度获取单元和发送单元，其中：

[0034] 所述直线插补单元，用于当所述轨迹规划方式是直线插补轨迹规划，则所述运动控制器中的DSP根据直线插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；

[0035] 所述圆弧插补单元，用于当所述轨迹规划方式是圆弧插补轨迹规划，则所述运动控制器中的DSP根据圆弧插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；

[0036] 所述飞行捕获平滑单元，用于当所述轨迹规划方式是飞行捕获平滑轨迹规划，则所述运动控制器中的DSP根据飞行捕获平滑算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；

[0037] 所述转角速度获取单元，用于所述DSP将所述多个插补点的物理坐标转化相对应的SCARA机械手大小臂的转角，并根据预定的梯形曲线加减速方法计算多个插补点的速度；

[0038] 所述发送单元，用于所述DSP将所述多个插补点对应的SCARA机械手大小臂的转角及速度发送至所述运动控制器中的运动控制芯片，运动控制芯片根据SCARA机械手大小臂的转角及速度控制SCARA机械手运动；

[0039] 所述直线插补单元具体包括第一获取子单元、第一判断子单元、第一直线插补子单元和第二直线插补子单元，其中：

[0040] 第一获取子单元，用于所述DSP获取所述运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向间距ΔX及纵向间距ΔY；

[0041] 第一判断子单元，用于判断|ΔX|是否大于或等于|ΔY|，当大于或等于时则启动第一直线插补子单元，当小于时则第二直线插补子单元；

[0042] 第一直线插补子单元，用于根据n1＝floor(|ΔX|/ΔL+1)计算第一插补次数n1，并根据ΔLN1＝ΔX/n1计算当前第一插补精度ΔLN1，并根据ΔXi＝ΔLN1确定第一横向增量ΔXi，根据ΔYi＝ΔLN1*ΔY/ΔX确定第一纵向增量ΔYi，根据Xi＝Xi-1+ΔXi确定实际运动轨迹中n1个插补点的横坐标，根据Yi＝Yi-1+ΔYi确定实际运动轨迹中n1个插补点的纵坐标；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数；

[0043] 第二直线插补子单元，用于根据n2＝floor(|ΔY|/ΔL+1)计算第二插补次数n2，并根据ΔLN2＝ΔY/n2计算当前第二插补精度ΔLN2，并根据ΔXj＝ΔLN2*ΔX/ΔY确定第二横向增量ΔXj，根据ΔYj＝ΔLN2确定第二纵向增量ΔYj，根据Xj＝Xj-1+ΔXj确定实际运动轨迹中n2个插补点的横坐标，根据Yj＝Yj-1+ΔYj确定实际运动轨迹中n2个插补点的纵坐标；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数。

[0044] 本发明所提供的一种基于SCARA机械手的运动轨迹规划方法及系统，方法包括：上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动。本发明具有直线插补，圆弧插补，飞行捕捉三种不同运动轨迹算法，分别计算出各自的运动轨迹插补点数据，每个插补点数据包含位置数据和速度数据，具有响应快，精度高特点。

附图说明

[0045] 图1是本发明所述基于SCARA机械手的运动轨迹规划方法的较佳实施例的流程图。

[0046] 图2是本发明所述基于SCARA机械手的运动轨迹规划方法中获取实际运动轨迹信息并发送的具体流程图。

[0047] 图3是本发明所述基于SCARA机械手的运动轨迹规划方法中进行直线插补的具体流程的流程图。

[0048] 图4是本发明直线插补示意图。

[0049] 图5是本发明所述基于SCARA机械手的运动轨迹规划方法中进行圆弧插补的具体流程的流程图。

[0050] 图6是本发明圆弧插补示意图。

[0051] 图7a、图7b分别是本发明中所述SCARA机械手的俯视图及侧视图。

[0052] 图8是本发明SCARA机械手物理坐标的大小臂角度的示意图。

[0053] 图9是本发明具体实施例的SCARA两段运动轨迹图。

[0054] 图10是图9中转角局部放大图。

[0055] 图11是本发明具体实施例的SCARA两段运动轨迹优化图。

[0056] 图12是本发明具体实施例的运动轨迹的矢量变化算法图。

[0057] 图13是本发明所述基于SCARA机械手的运动轨迹规划系统较佳实施例的结构框图。

具体实施方式

[0058] 为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

[0059] 请参见图1，图1是本发明所述基于SCARA机械手的运动轨迹规划方法的较佳实施例的流程图。如图1所示，所述基于SCARA机械手的运动轨迹规划方法，包括以下步骤：

[0060] 步骤S100、上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器。

[0061] 本发明的实施例中，用户通过上位机设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将其发送至SCARA机械手的运动控制器，也就是告诉了SCARA机械手应该走到哪里去、以及该以什么方式走到终点。

[0062] 步骤S200、所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手。

[0063] 在步骤S200中，所得到的SCARA机械手的实际运动轨迹信息包括实际运动轨迹中多个插补点的物理坐标、与所述多个插补点的物理坐标一一对应的极坐标及速度。当所述运动控制器获取SCARA机械手的实际运动轨迹信息后，将其发送至SCARA机械手。

[0064] 步骤S300、所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动。

[0065] 本发明的实施例中，SCARA机械手接收所述实际运动轨迹信息后，根据所述实际运动轨迹信息进行运动，确保了SCARA机械手的在每一时刻的精准运动，同时能有效的避开障碍物。

[0066] 本发明的实施例中，需先判断所述轨迹规划方式，当判定所述轨迹规划方式是直线插补轨迹规划时，则通过直线插补算法计算实际运动轨迹的插补点；当判定插所述轨迹规划方式是圆弧插补轨迹规划时，则通过圆弧插补算法计算实际运动轨迹的插补点；当判定所述轨迹规划方式是飞行捕获平滑轨迹规划时，则通过平滑算法计算实际运动轨迹的插补点。

[0067] 进一步地实施例，如图2所示，所述步骤S200中获取实际运动轨迹信息并发送的具体流程包括：

[0068] 步骤S201、当所述轨迹规划方式是直线插补轨迹规划，则所述运动控制器中的DSP根据直线插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行步骤S204。

[0069] 进一步地实施例，如图3所示，所述步骤S201进行直线插补的具体流程包括：

[0070] 步骤S2011、所述DSP获取所述运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向间距ΔX及纵向间距ΔY；

[0071] 步骤S2012、判断|ΔX|是否大于或等于|ΔY|，当大于或等于时则执行步骤S2013，当小于时则执行步骤S2014；

[0072] 步骤S2013、根据n1＝floor(|ΔX|/ΔL+1)计算第一插补次数n1，并根据ΔLN1＝ΔX/n1计算当前第一插补精度ΔLN1，并根据ΔXi＝ΔLN1确定第一横向增量ΔXi，根据ΔYi＝ΔLN1*ΔY/ΔX确定第一纵向增量ΔYi，根据Xi＝Xi-1+ΔXi确定实际运动轨迹中n1个插补点的横坐标，根据Yi＝Yi-1+ΔYi确定实际运动轨迹中n1个插补点的纵坐标；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数；

[0073] 步骤S2014、根据n2＝floor(|ΔY|/ΔL+1)计算第二插补次数n2，并根据ΔLN2＝ΔY/n2计算当前第二插补精度ΔLN2，并根据ΔXj＝ΔLN2*ΔX/ΔY确定第二横向增量ΔXj，根据ΔYj＝ΔLN2确定第二纵向增量ΔYj，根据Xj＝Xj-1+ΔXj确定实际运动轨迹中n2个插补点的横坐标，根据Yj＝Yj-1+ΔYj确定实际运动轨迹中n2个插补点的纵坐标；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数。

[0074] 为了进一步说明上述直线插补的具体流程，现通过一实例来说明。如图4所示，其为本发明直线插补示意图，假设给定直线OA，其中O点坐标为(X0，Y0)，A点坐标为(Xe，Ye)，动点Ni-1(即插补点)的坐标为(Xi-1，Yi-1)，OA距离的横向间距ΔX＝Xe-X0、纵向间距ΔY＝Ye-Y0，位移增量最大分量为ΔL，在此设定ΔL＝0.1mm。

[0075] 当|ΔX|≥|ΔY|时，求第一插补次数n1，n1＝floor(|ΔX|/ΔL+1)，修正ΔLN1＝ΔX/n1，则ΔXi＝ΔLN1，ΔYi＝ΔLN1*ΔY/ΔX；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数。

[0076] 当|ΔX|＜|ΔY|，求第二插补次数n2，n2＝floor(|ΔY|/ΔL+1)，修正ΔLN2＝ΔY/n2，则ΔXj＝ΔLN2*ΔX/ΔY，ΔYj＝ΔLN2；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数。

[0077] 由此可得下一个插补点Ni(Xi，Yi)的坐标值为(Xi-1+ΔX，Yi-1+ΔY)。

[0078] 步骤S202、当所述轨迹规划方式是圆弧插补轨迹规划，则所述运动控制器中的DSP根据圆弧插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行S204。

[0079] 进一步地实施例，如图5所示，所述步骤S202进行圆弧插补的具体流程包括：

[0080] 步骤S2021、所述DSP获取所述运动参数中的起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)、圆弧插补精度值DfStep及圆弧插补方向；

[0081] 步骤S2022、根据起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)及圆弧插补方向确定半径R、起点角度φa、终点角度φb、及终点角度与起点角度之差Δφ，并根据N＝floor(|π*R*Δφ/(180*DfStep)|+1)计算圆弧插补次数N，并根据Dfstep＝Δφ/N计算当前圆弧插补精度Dfstep，根据Xm＝R*cos(φa+m*Dfstep)+Xo确定实际运动轨迹中N个插补点的横坐标，根据Ym＝R*sin(φa+m*Dfstep)+Yo确定实际运动轨迹中N个插补点的纵坐标；其中，floor函数为向下取整函数，m为取值范围为[1,N]的正整数。

[0082] 为了进一步说明上述圆弧插补的具体流程，现通过一实例来说明。如图6所示，其为本发明圆弧插补示意图，假设设有圆弧，且A(Xa,Ya)和B(Xb,Yb)是圆弧上的两个点，已知A点，B点，和圆心O点(Xo,Yo)。从A点到B点以圆弧插补(DfStep＝0.1弧度)的方式运行，需考虑A、B两点在直角坐标系中所处象限，实际计算时分为以下四种情况：

[0083] (1)当A、B位于第一象限时：

[0084] 起始点A点的角度为

[0085] 终点B点的角度为

[0086] (2)当A、B位于第二象限时：

[0087] 起始点A点的角度为

[0088] 终点B点的角度为

[0089] (3)当A、B位于第三象限时：

[0090] 起始点A点的角度为

[0091] 终点B点的角度为

[0092] (4)当A、B位于第四象限时：

[0093] 起始点A点的角度为

[0094] 终点B点的角度为

[0095] 当A、B处于上述象限中的任一象限时，其半径均为：

[0096]

[0097] 当A到B的圆弧为顺时针方向圆弧时，且φb-φa＜0时，则Δφ＝φb-φa；当A到B的圆弧为顺时针方向圆弧时，且φb-φa＞0时，则Δφ＝-(360-φb+φa)；当A到B的圆弧为逆时针方向圆弧时，且φb-φa＞0时，则Δφ＝φb-φa；当A到B的圆弧为逆时针方向圆弧时，且φb-φa＜0时，则Δφ＝360+φb-φa。

[0098] 当获取了起点坐标(Xa，Ya)、终点坐标(Xb,，Yb)、圆心坐标(Xo，Yo)、半径R、起点角度φa、终点角度φb及终点角度与起点角度之差Δφ后，可通过需要的插补次数为N＝floor(|π*R*Δφ/(180*DfStep)|+1)计算圆弧插补次数N。此时需修正Dfstep＝Δφ/N，并根据Xm＝R*cos(φa+m*Dfstep)+Xo确定实际运动轨迹中N个插补点的横坐标，根据Ym＝R*sin(φa+m*Dfstep)+Yo确定实际运动轨迹中N个插补点的纵坐标；其中，floor函数为向下取整函数，m为取值范围为[1,N]的正整数。

[0099] 步骤S203、当所述轨迹规划方式是飞行捕获平滑轨迹规划，则所述运动控制器中的DSP根据飞行捕获平滑算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后执行步骤S204；

[0100] 步骤S204、所述DSP将所述多个插补点的物理坐标转化相对应的SCARA机械手大小臂的转角，并根据预定的梯形曲线加减速方法计算多个插补点的速度。

[0101] 本发明的实施例中，如图7a和图7b所示，其分别为本发明中所述SCARA机械手的俯视图及侧视图，而且图7a是将SCARA置于平面直角坐标系中、并标注了SCARA机械手大臂臂长L1和小臂臂长L2、大臂限位范围(A2，A1)及小臂限位范围(A4，A3)。

[0102] 在步骤S204中需将通过插补算法获取的多个插补点的物理坐标转化为极坐标，也即转化为SCARA机械手大小臂的转角。为了进一步说明上述物理坐标转化为极坐标的具体流程，现通过一实例来说明。

[0103] 如图8所示，其为本发明SCARA机械手物理坐标的大小臂角度的示意图。如图8所示，已知A、B两点，SCARA机械手需要从A点运行到B点，其大臂、小臂的臂长分别为L1、L2，当SCARA的小臂末端处于起点A时，大臂初始转角∠XOI、小臂初始转角为∠FIA，在此定义逆时针为正方向，则在右臂模式下，大臂角度为∠XOJ、小臂角度为∠GJB；在左臂模式下，大臂角度为∠XOK、小臂角度为∠HKB。在图8中B1、B2、B3及B4分别表示终点坐标B在第一象限至第四象限的标识符，同理G1、G2、G3及G4分别表示坐标点G在第一象限至第四象限的标识符，H1、H2、H3及H4分别表示坐标点H在第一象限至第四象限的标识符，J1、J2、J3及J4分别表示坐标点J在第一象限至第四象限的标识符，K1、K2、K3及K4分别表示坐标点K在第一象限至第四象限的标识符。

[0104] 本发明的实施例中，根据物理坐标确定大小臂的转角的第一步是根据∠FIA角度判断，当前处在左臂还是右臂状态,∠FIA角度由位置计数器可以直接读出。当IA线位于FI线右侧时为右臂模式，当IA线位于FI线左侧时为左臂模式。第二步是根据B点位置(Xb，Yb)和新的左右臂模式计算 PI＝3.1415926，在计算式要分为B在第一象限-第四象限四种情况：

[0105] (a)当B点在第一象限时：

[0106] 右臂模式：

[0107]

[0108] 左臂模式：

[0109]

[0110] (b)当B点在第二象限时：

[0111] 右臂模式：

[0112]

[0113] 左臂模式：

[0114]

[0115] (c)当B点在第三象限时：

[0116] 右臂模式：

[0117]

[0118] 左臂模式：

[0119]

[0120] (d)当B点在第四象限时：

[0121] 右臂模式：

[0122]

[0123] 左臂模式：

[0124]

[0125] 其中，在第四象限中的大臂角度为负角度，在第三象限SCARA存在限位(0，A1)，无法由第三象限直接转到第四象限，必须经过第二象限位置，再第一象限位置到达。

[0126] 由上述条件可知，若大臂小臂移动角度为Δα、Δβ，则：

[0127] 右臂模式：

[0128] 左臂模式：

[0129] 即得到相对于A点的旋转角度，如果B点在第一象限则n＝1；第二象限则n＝2；第三象限则n＝3；第四象限则n＝4；Δα，Δβ为正值逆时针旋转，为负值顺时针旋转。

[0130] 在步骤S204中除了将实际运动轨迹中的多个插补点的物理坐标转化为SCARA机械手的大小臂转角，还需获得每一插补点处的速度。本发明的实施例中通过一预定的梯形曲线加减速方法计算多个插补点的速度，此算法基于PCL6045运动控制芯片，使用了芯片上三级缓存功能，每次写入一个插补位置时，可以同时填写一个该插补位移的速度。小线段结构体信息如下：

[0131] typedefstruct

[0132] {

[0133] LgInt xl；//x存放大臂相对位置，已转化为脉冲数值

[0134] LgInt yl；//y存放小臂相对位置，已转化为脉冲数值

[0135] LgInt speed；//速度

[0136] }TRunCode；

[0137] 加减速步数由参数设定(accnum，decnum)，可以计算出每条小线段对应的速度。加速时，每次速度增加值为accvalue＝(MaxVel-StrVel)/accnum；减速时，每次速度减少值为decvalue＝(MaxVel-StrVel)/decnum；其中MaxVel表示最大速度，StrVel表示起始速度。若总的小线段有m条，当前小线段下标为n，则：

[0138] 当n+decnum＞m时，则进行减速，Vn＝Vn-1-decvalue，减速至初始速度；

[0139] 当n+decnum＜＝m时，并且n＜accnum，则进行加速，Vn＝Vn-1+accvalue，加速至最大速度；

[0140] 其他情况下均为匀速，且Vn＝Vn-1。

[0141] 步骤S205、所述DSP将所述多个插补点对应的SCARA机械手大小臂的转角及速度发送至所述运动控制器中的运动控制芯片，运动控制芯片根据所述SCARA机械手大小臂的转角及速度控制SCARA机械手运动。

[0142] 下面以一具体实施例来说明本发明所述基于SCARA机械手的飞行捕获运动轨迹规划方法。

[0143] 假设SCARA机械手的预定运动轨迹为如图9所示的SCARA两段运动轨迹，图10为图9的转角处局部放大图，从图9可以看出两条轨迹AB与BC之间存在转折点，SCARA在运行时会存在大转角，使机器产生震动，对机械构件产生磨损。SCARA从A点经过B触发点然后到C点的轨迹。底层运动控制芯片采用的是插补方式运行，即A到B点运动控制芯片按照直线插补方式运行，但是体现SCARA上为如图9所示轨迹(由于SCARA机械结构，运动控制芯片采用插补模式运行，但是运动轨迹却是曲线)。根据要求，在B点，SCARA不能停止后再运行。如果不进行平滑算法的话，就会出现大转角，在B点会出现机械振动，经过优化后的轨迹如图11所示。

[0144] 请参见图12，其为运动轨迹的矢量变化算法图。将AB、BC直线按照速度规划进行细分成，在此默认分成T＝8段，每段时间为单位时间，增大T能够增加轨迹的平滑度。在A点时，方向速度标量为Vmax， 方向速度标量为0，在C点时， 方向速度标量为Vmax。 方向速度标量逐渐减少为0； 方向速度标量逐渐增加到为Vmax。根据位移公式S＝aT2/2，则可求加速度a＝2×S/T2，则

[0145] SA1A2＝0.5*a*T82-0.5*a*T72；

[0146] SA2A3＝0.5*a*T72-0.5*a*T62；

[0147] SA3A4＝0.5*a*T62-0.5*a*T52；

[0148] SA4A5＝0.5*a*T52-0.5*a*T42；

[0149] SA5A6＝0.5*a*T42-0.5*a*T32；

[0150] SA6A7＝0.5*a*T32-0.5*a*T22；

[0151] SA7A8＝0.5*a*T22-0.5*a*T12；

[0152] SA8A9＝0.5*a*T12，此处加速度方向与位移方向相反；

[0153] SC8C9＝0.5*a*T82-0.5*a*T72；

[0154] SC7C8＝0.5*a*T72-0.5*a*T62；

[0155] SC6C7＝0.5*a*T62-0.5*a*T52；

[0156] SC5C6＝0.5*a*T52-0.5*a*T42；

[0157] SC4C5＝0.5*a*T42-0.5*a*T32；

[0158] SC3C4＝0.5*a*T32-0.5*a*T22；

[0159] SC2C3＝0.5*a*T22-0.5*a*T12；

[0160] SC1C2＝0.5*a*T12，此处加速度方向与位移方向相同；

[0161] 式中Ti＝i ,i∈[1 ,8]，由于 所以坐标点

[0162] 同理：

[0163]

[0164]

[0165]

[0166]

[0167]

[0168]

[0169] 如此可计算Pm点位置，由AB到BC两段轨迹，细分为由P1、P2、P3、P4、P5、P6、P7、P8、P9九个点组成的轨迹。由图12得到的模拟轨迹图可知，能够实现角度的平滑过渡，得到坐标后，可按照本发明中物理坐标到极坐标的转换方法将物理坐标转换为大小臂转角数据，进而发送到运动控制芯片当中。

[0170] 基于上述实施例，本发明还提供一种基于SCARA机械手的运动轨迹规划系统，如图13所示，所述基于SCARA机械手的运动轨迹规划系统，包括：

[0171] 设置模块100，用于上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；具体如上所述。

[0172] 轨迹获取及发送模块200，用于所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；具体如上所述。

[0173] 接收及运动模块300，用于所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动；具体如上所述。

[0174] 具体实施时，设置模块100设置于上位机中；轨迹获取及发送模块200设置在运动控制器中，其中运动控制器主要包括DSP处理器和运动控制芯片PCL6045BL；接收及运动模块300设置在SCARA机械手中。

[0175] 进一步地实施例，在所述基于SCARA机械手的运动轨迹规划系统中，所述设置模块中的所述轨迹规划方式是直线插补轨迹规划、圆弧插补轨迹规划和飞行捕获平滑轨迹规划；具体如上所述。

[0176] 进一步地实施例，在所述基于SCARA机械手的运动轨迹规划系统中，所述轨迹获取及发送模块中当所述轨迹规划方式是直线插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标和插补精度值；当所述轨迹规划方式是圆弧插补轨迹规划时，则所述运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向；当所述轨迹规划方式是飞行捕获平滑轨迹规划时，则所述运动参数包括起点坐标、触发点坐标、终点坐标、触发脉宽和平滑度。

[0177] 进一步地实施例，在所述基于SCARA机械手的运动轨迹规划系统中，所述轨迹获取及发送模块200具体包括直线插补单元、圆弧插补单元、飞行捕获平滑单元、转角速度获取单元和发送单元，其中：

[0178] 所述直线插补单元，用于当所述轨迹规划方式是直线插补轨迹规划，则所述运动控制器中的DSP根据直线插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；具体如上所述。

[0179] 所述圆弧插补单元，用于当所述轨迹规划方式是圆弧插补轨迹规划，则所述运动控制器中的DSP根据圆弧插补算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；具体如上所述。

[0180] 所述飞行捕获平滑单元，用于当所述轨迹规划方式是飞行捕获平滑轨迹规划，则所述运动控制器中的DSP根据飞行捕获平滑算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹中多个插补点的物理坐标，之后启动所述转角速度获取单元；具体如上所述。

[0181] 所述转角速度获取单元，用于所述DSP将所述多个插补点的物理坐标转化相对应的SCARA机械手大小臂的转角，并根据预定的梯形曲线加减速方法计算多个插补点的速度；具体如上所述。

[0182] 所述发送单元，用于所述DSP将所述多个插补点对应的SCARA机械手大小臂的转角及速度发送至所述运动控制器中的运动控制芯片，运动控制芯片根据所述SCARA机械手大小臂的转角及速度控制SCARA机械手运动；具体如上所述。

[0183] 进一步地实施例，在所述基于SCARA机械手的运动轨迹规划系统中，所述直线插补单元具体包括第一获取子单元、第一判断子单元、第一直线插补子单元和第二直线插补子单元，其中：

[0184] 第一获取子单元，用于所述DSP获取所述运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向间距ΔX及纵向间距ΔY；具体如上所述。

[0185] 第一判断子单元，用于判断|ΔX|是否大于或等于|ΔY|，当大于或等于时则启动第一直线插补子单元，当小于时则第二直线插补子单元；具体如上所述。

[0186] 第一直线插补子单元，用于根据n1＝floor(|ΔX|/ΔL+1)计算第一插补次数n1，并根据ΔLN1＝ΔX/n1计算当前第一插补精度ΔLN1，并根据ΔXi＝ΔLN1确定第一横向增量ΔXi，根据ΔYi＝ΔLN1*ΔY/ΔX确定第一纵向增量ΔYi，根据Xi＝Xi-1+ΔXi确定实际运动轨迹中n1个插补点的横坐标，根据Yi＝Yi-1+ΔYi确定实际运动轨迹中n1个插补点的纵坐标；其中，floor函数为向下取整函数，i为取值范围为[1,n1]的正整数；具体如上所述。

[0187] 第二直线插补子单元，用于根据n2＝floor(|ΔY|/ΔL+1)计算第二插补次数n2，并根据ΔLN2＝ΔY/n2计算当前第二插补精度ΔLN2，并根据ΔXj＝ΔLN2*ΔX/ΔY确定第二横向增量ΔXj，根据ΔYj＝ΔLN2确定第二纵向增量ΔYj，根据Xj＝Xj-1+ΔXj确定实际运动轨迹中n2个插补点的横坐标，根据Yj＝Yj-1+ΔYj确定实际运动轨迹中n2个插补点的纵坐标；其中，floor函数为向下取整函数，j为取值范围为[1,n2]的正整数；具体如上所述。

[0188] 进一步地实施例，在所述基于SCARA机械手的运动轨迹规划系统中，所述圆弧插补单元具体包括第二获取子单元和圆弧插补子单元，其中：

[0189] 所述第二获取子单元，用于所述DSP获取所述运动参数中的起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)、圆弧插补精度值DfStep及圆弧插补方向；具体如上所述。

[0190] 所述圆弧插补子单元，用于根据起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)及圆弧插补方向确定半径R、起点角度φa及终点角度φb、及终点角度与起点角度之差Δφ，并根据N＝floor(|π*R*Δφ/(180*DfStep)|+1)计算圆弧插补次数N，并根据Dfstep＝Δφ/N计算当前圆弧插补精度Dfstep，根据Xm＝R*cos(φa+m*Dfstep)+Xo确定实际运动轨迹中N个插补点的横坐标，根据Ym＝R*sin(φa+m*Dfstep)+Yo确定实际运动轨迹中N个插补点的纵坐标；其中，floor函数为向下取整函数，m为取值范围为[1,N]的正整数；具体如上所述。

[0191] 综上所述，本发明所提供的一种基于SCARA机械手的运动轨迹规划方法及系统，方法包括：上位机接收用户的操作指令，设置SCARA机械手的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至SCARA机械手的运动控制器；所述运动控制器根据所述轨迹规划方式对应的算法对SCARA机械手的实际运动轨迹进行运算，得到SCARA机械手的实际运动轨迹信息，并将所述实际运动轨迹信息发送至SCARA机械手；所述SCARA机械手接收所述实际运动轨迹信息，并根据所述实际运动轨迹信息进行运动。本发明具有直线插补，圆弧插补，飞行捕捉三种不同运动轨迹算法，分别计算出各自的运动轨迹插补点数据，每个插补点数据包含位置数据和速度数据，具有响应快，精度高特点。

[0192] 应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。