[发明专利]一种机械手精确快速回原的方法

权利要求书

1.一种机械手精确快速回原的方法，其特征在于，包括如下步骤：

S1：设原点开关的真实原点位置为P00，原点开关长度为2P0，触块长度为PK，触块匀速从正向或者负向通过原点开关，在检测到原点开关信号时，触块继续运行一段距离dp1，到达反馈位置P1，此时CPU收到原点开关信号开始处理，P1＝P00-P0+dp1；

S2：触块继续匀速前行，待触块边沿离开原点开关后，继续运行一段距离dp2，到达反馈位置P2，此时CPU收到已检测不到原点开关的信号开始处理，P2＝P00+P0+dp2+PK；

S3：触块反向以相同速度匀速通过原点开关，在检测到原点开关信号时，触块继续运行一段距离dP3，到达反馈位置P3，此时CPU收到原点开关信号开始处理，P3＝P00+P0-dP3；

S4：触块继续匀速前行，待触块边沿离开原点开关后，继续运行一段距离dp4，到达反馈位置P4，此时CPU收到已检测不到原点开关的信号开始处理，P4＝P00-P0-dp4-PK；

S5：记录触块最后停止时的反馈位置P5以及Z偏差Z5；

S6：以P1、P2、P3、P4的平均值P0作为原点参考值，

S7：根据整圈Z脉冲数C，反馈位置P5以及Z偏差Z5，计算P0处的Z偏差Z0，其中，Z偏差Z0的计算方法为：计算反馈位置P5到P0处运动的整圈数余数为K，如果K≤Z5，则Z0＝Z5-K，如果K>Z5，则Z0＝Z5+C-K；

S8：计算反馈位置P5到真实原点P00的位移，机械手根据此位移返回原点，其中，计算反馈位置P5到真实原点P00的位移为P0-P5+DZ0，式中，偏差DZ0的具体计算方法为：将Z0与第一次开机找原点时记录的位置P00做差得到DZ，如果DZ小于等于半圈，则偏差DZ0＝DZ，如果DZ大于半圈，则偏差DZ0＝DZ-C。