三轴移动机构多点移动控制方法与流程

1.本发明属于三轴联动控制技术，具体涉及三轴移动机构多点移动控制方法。


背景技术：

2.直线插补三轴联动测量设备或者需用到三轴直线插补的场合，其插补行程量过往都是通过人工去测量，因而不可避免的会产生人为误差，导致测量的结果不准确，且测量速度慢，限制了该手段只适用于测量小型产品。


技术实现要素：

3.本发明提出一种三轴移动机构多点移动控制方法，应用于需要走直线插补三轴联动测量设备或其它需用到三轴直线插补的控制场合，其具体通过以下技术手段实现：
4.三轴移动机构多点移动控制方法包括以下步骤：
5.s1，获取待作业的各坐标点数据；
6.s2，设定坐标点间的移动速度为v；
7.s3，根据坐标点数据和移动速度v来计算由当前坐标点e1(x1,y2,z1)移至下一从标点e2(x2,y2,z2)过程中各轴伺服电机的转速，并控制各轴伺服电机按对应转速执行；该转速的计算包含以下步骤：
8.s31，根据空间直角坐标系的两点距离公式计算当前坐标点e1至下一坐标点e2的距离d，
9.s32，计算当前坐标点e1运动至下一坐标点e2的时间t，t＝d/v；
10.s34，计算各轴伺服电机的运行速度vx、vy和vz；vx＝|x2-x1|/t，vy＝|y2-y1|/t，vz＝|z2-z1|/t；
11.s35，计算各轴伺服电机的运行方向；
12.当x2-x1》0时，x轴伺服电机正向运行；当x2-x1《0时，x轴伺服电机反向运行；x2-x1＝0时，x轴伺服电机待机；
13.当y2-y1》0时，y轴伺服电机正向运行；当y2-y1《0时，y轴伺服电机反向运行；y2-y1＝0时，y轴伺服电机待机；
14.当z2-z1》0时，z轴伺服电机正向运行；当z2-z1《0时，z轴伺服电机反向运行；z2-z1＝0时，z轴伺服电机待机；
15.s36，由上述计算所得的运行速度和运行方向控制各轴伺服电机，实现由当前坐标点e1运动向下一坐标点e2的位移；
16.s4，重复上述步骤s3依序遍历各坐标点。
17.本发明的有益效果是：应该本发明方法的三轴移动机构系统性能稳定、处理效率高、精确度高等，利用算法实现类似数控三轴直线插补的功能，与传统数控或者具备插补功能的plc相比，大大降低设备成本，且无需编写g代码实现插补控制，降低对操作人员的要求，且提高测量效率，并且不会产生人为误差，对测量产品的大小没有限制。
附图说明
18.图1为基于空间直角坐标系的位移计算原理图。
19.图2为三轴移动机构自动作业的流程图。
20.图3为三轴移动机构自动作业的流程图(续图2)。
21.图4为三轴移动机构自动作业的流程图(续图3)。
22.图5为三轴移动机构自动作业的流程图(续图4)。
具体实施方式
23.如下对本技术方案作进一步描述：
24.实施例1
25.本发明的三轴移动机构多点移动控制方法应用于需要走直线插补三轴联动测量设备或其它需用到三轴直线插补的控制场合，其包括以下步骤：
26.s1，获取待作业的各坐标点数据，该坐标点数据中标记有超始坐标点、终止坐标点以及在二者之间的各坐标点顺序；
27.s2，设定坐标点间的移动速度为v；
28.s3，根据坐标点数据和移动速度v来计算由当前坐标点e1(x1,y2,z1)移至下一从标点e2(x2,y2,z2)过程中各轴伺服电机的转速，并控制各轴伺服电机按对应转速执行；该转速的计算包含以下步骤：
29.s31，根据空间直角坐标系的两点距离公式计算当前坐标点e1至下一坐标点e2的距离d，
30.s32，计算当前坐标点e1运动至下一坐标点e2的时间t，t＝d/v；
31.s34，计算各轴伺服电机的运行速度vx、vy和vz；vx＝|x2-x1|/t，vy＝|y2-y1|/t，vz＝|z2-z1|/t；
32.s35，计算各轴伺服电机的运行方向；
33.当x2-x1》0时，x轴伺服电机正向运行；当x2-x1《0时，x轴伺服电机反向运行；x2-x1＝0时，x轴伺服电机待机；
34.当y2-y1》0时，y轴伺服电机正向运行；当y2-y1《0时，y轴伺服电机反向运行；y2-y1＝0时，y轴伺服电机待机；
35.当z2-z1》0时，z轴伺服电机正向运行；当z2-z1《0时，z轴伺服电机反向运行；z2-z1＝0时，z轴伺服电机待机；
36.s36，由上述计算所得的运行速度和运行方向控制各轴伺服电机，实现由当前坐标点e1运动向下一坐标点e2的位移；
37.s4，重复上述步骤s3依序遍历各坐标点。
38.通过系统界面将移动速度值设定为v(浮点型)，x/y/z三轴对应当前位置分别e/f/g，目标坐标点为(x,y,z)，即|x-e|＝ox、|y-f|＝oy、|z-g|＝ob。
39.1)当ox》0、oy》0、ob》0时，可得两点间的位移距离为oc，因为ob分别垂直与oy和ox、ox//ay、ox＝ay、oy//xa、oy＝xa可得
△
oay为直角三角形，直角三角形的勾股定理可得：由上可知
△
oac为直角三角形，再次使用勾股定理可得：
求出oc距离后，使用速度公式t＝oc/v可求出时间t，三个伺服电机的实际速度根据速度公式可得：vx＝ox/t、vy＝oy/t、vz＝ob/t。
40.2)当ox＝0、oy》0、ob》0时，因为ox＝0所以点落在坐标轴y跟z形成的面上，由图1可知这个点行走的直线距离为yb，因为bo垂直oy，所以
△
yob为直角三角形，使用直角三角形勾股定理可得：求出by距离后，可求得时间：t＝by/v，y轴、z轴伺服电机的实际速度根据速度公式可得：vy＝oy/t、vz＝ob/t。
41.3)当ox》0、oy＝0、ob》0时，因为oy＝0所以点落在坐标轴x跟z形成得面上，由图1可知这个点行走的直线距离为xb，因为ob垂直ox,所以
△
xob为直角三角形，使用直角三角形勾股定理可得：求出bx距离后，可求得时间：t＝bx/v，x轴、z轴伺服电机的实际速度根据速度公式可得：vx＝ox/t、vz＝ob/t。
42.4)当ox》0、oy》0、ob＝0时，因为ob＝0所以点落在坐标轴x跟y形成得面上，由图1可知这个点行走得直线距离为oa，因为ox垂直oy、ox//ya，所以ay垂直oy且
△
oya为直角三角形，使用直角三角形勾股定理可得：求出oa距离后，可求得时间：t＝oa/v，x轴、y轴伺服电机得实际速度根据速度公式可得：vx＝ay/t、vy＝oy/t。
43.5)当ox/oy/ob任意两个距离等于零而有一个不为零时，不为零的轴向速度vx/vy/vz＝v。
44.作为本实施例的优选方案，若在作业过程中变更设置移动速度v，则由当前坐标点开始重新执行步骤s3以获得当前坐标点移至下一坐标点所需的各轴伺服电机转速。
45.作为本实施例的优选方案，设定为在当前坐标点上的操作完毕后实时获取下一坐标点的坐标值或/和移动速度v，继而计算向下一坐标点位移所需的各轴伺服电机转速。
46.实施例2
47.在实施例1的基础上，考虑到产品形状/轮廓变化较大的加工场合，在当前坐标点向下一坐标点移位的路径上可能存在阻挡物，直接位置容易损坏产品以及z轴上的加工或检测器件，故本实施例设计了z轴复位的方式，在每次作业完毕后先让z轴复位至预设位置，x轴和y轴坐标不变，具体控制步骤如下：
48.在当前坐标点上的操作完毕后先进行z轴向的复位，x轴和y轴向坐标不变，即由当前坐标点e1(x1,y1,z1)复位至坐标点e10(x1,y1,z0)，原则上z0设定为小于任意坐标点的z轴坐标值；再由坐标点e10(x1,y1,z0)计算至下一从标点e2(x2,y2,z2)过程中各轴伺服电机的转速；其中包含以下步骤：
49.s311，根据空间直角坐标系的两点距离公式计算当前坐标点e10至下一坐标点e2的距离d，
50.s321，计算当前坐标点e1运动至下一坐标点e2的时间t，t＝d/v；
51.s341，计算各轴伺服电机的运行速度vx、vy和vz；vx＝|x2-x1|/t，vy＝|y2-y1|/t，vz＝|z2-z0|/t；
52.s351，计算各轴伺服电机的运行方向；
53.当x2-x1》0时，x轴伺服电机正向运行；当x2-x1《0时，x轴伺服电机反向运行；x2-x1
＝0时，x轴伺服电机待机；
54.当y2-y1》0时，y轴伺服电机正向运行；当y2-y1《0时，y轴伺服电机反向运行；y2-y1＝0时，y轴伺服电机待机；
55.由于复位后z轴坐标小于任意一个坐标点的z向坐标，故z轴伺服电机正向运行。
56.一般情况下优先将z0设置为零，即在当前坐标点上的操作完毕后先进行z轴向归零，由e10(x1,y1,0)计算至下一从标点e2(x2,y2,z2)过程中各轴伺服电机的转速；其包含以下步骤：
57.s312，根据空间直角坐标系的两点距离公式计算当前坐标点e10至下一坐标点e2的距离d，
58.s322，计算当前坐标点e1运动至下一坐标点e2的时间t，t＝d/v；
59.s342，计算各轴伺服电机的运行速度vx、vy和vz；vx＝|x2-x1|/t，vy＝|y2-y1|/t，vz＝|z2|/t；
60.s352，计算各轴伺服电机的运行方向；
61.当x2-x1》0时，x轴伺服电机正向运行；当x2-x1《0时，x轴伺服电机反向运行；x2-x1＝0时，x轴伺服电机待机；
62.当y2-y1》0时，y轴伺服电机正向运行；当y2-y1《0时，y轴伺服电机反向运行；y2-y1＝0时，y轴伺服电机待机；
63.z轴伺服电机正向运行。
64.实施例3
65.本发明的三轴移动机构多点移动控制方法应用于多点电阻在线测量控制系统，该多点电阻在线测量控制系统用于测量镀膜后玻璃上各点位的电阻值，以评价镀膜性能及品质，在三轴移动机构的z轴上装设有一个用于测量电阻的探头，针对这个测量电阻的探头价格比较高且易于碰撞损坏，所以在当前坐标点的测量完成之后z轴先归零，x、y轴保持不动，等待z轴归零完成之后，再三轴一起联动到下一个坐标点；多点电阻在线测量控制系统具体执行以下步骤：
66.1)设备上电自检；
67.2)寻参，获取坐标点数据；
68.3)在寻参完成后进行参数设定，其中包括移动速度v的设置；
69.4)于系统操作界面上选择自动模式并点击自动开始；
70.5)三轴移动机构驱使探头前往坐标点1，在至达坐标点1后开始测量电阻值；
71.6)等待测量完成后，x轴和y轴不动，z轴抬升归零；
72.7)当z轴到达零点后，计算由当前位置位移至坐标点2的x\y\z三轴伺服电机的转速(包括运行速度和运行方向)；即执行：
73.根据空间直角坐标系的两点距离公式计算当前坐标点至坐标点2的距离d，继而计算当前坐标点e1运动至下一坐标点e2的时间t，t＝d/v；根据时间t计算各轴伺服电机的运行速度vx、vy和vz；vx＝|x2-x1|/t，vy＝|y2-y1|/t，vz＝|z2|/t；
74.此时，x轴伺服电机的运行方向：当x2-x1》0时，x轴伺服电机正向运行；当x2-x1《0
时，x轴伺服电机反向运行；x2-x1＝0时，x轴伺服电机待机；
75.此时，y轴伺服电机的运行方向：当y2-y1》0时，y轴伺服电机正向运行；当y2-y1《0时，y轴伺服电机反向运行；y2-y1＝0时，y轴伺服电机待机；
76.此时，z轴伺服电机的运行方向：z轴伺服电机正向运行。
77.8)由上述计算所得的运行速度和运行方向控制各轴伺服电机，驱使探头前往坐标点2进行测量；
78.9)坐标点2等待测量完成后z轴抬升归零；
79.10)当z轴到达零点，计算位移至下一坐标点的x\y\z三轴伺服电机的转速，参见附图2至5，如此遍历所有20个坐标点后，x\y\z三轴伺服电机的转速复位于原点，测试结束。
80.作为本发明的一种应用延伸，三轴移动机构的z轴还可装设其它测量仪器、加工刀具、焊枪等以作对应用途，应用于需要走直线插补三轴联动测量设备或其它需用到三轴直线插补的控制场合。应该本发明方法的三轴移动机构系统性能稳定、处理效率高、精确度高等，利用算法实现类似数控三轴直线插补的功能，与传统数控或者具备插补功能的plc相比，大大降低设备成本，且无需编写g代码实现插补控制，降低对操作人员的要求，且提高测量效率，并且不会产生人为误差，对测量产品的大小没有限制。
81.上述优选实施方式应视为本技术方案实施方式的举例说明，凡与本技术方案雷同、近似或以此为基础作出的技术推演、替换、改进等，均应视为本专利的保护范围。