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

摘要

本发明提供了一种机械手精确快速回原的方法,触块匀速从正向或者负向通过原点开关，在检测到原点开关信号时，触块继续运行一段距离dp1，到达反馈位置P1；触块继续匀速前行，待触块边沿离开原点开关后，继续运行一段距离dp2，到达反馈位置P2；触块反向以相同速度匀速通过原点开关，得到相应的反馈位置P3、P4；记录触块最后停止时的反馈位置P5及Z偏差Z5；以P1、P2、P3、P4的平均值P0作为原点参考值；根据整圈Z脉冲数C，反馈位置P5及Z偏差Z5，计算P0处的Z偏差Z0；计算反馈位置P5到真实原点P00的位移，机械手根据此位移返回原点。本发明进一步提高了机械手回原的精准度、重复定位精度及找回速度。

主权项

一种机械手精确快速回原的方法，其特征在于，包括如下步骤：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作为原点参考值，P0=P1+P2+P3+P44=P00+(dP1+dP2)-(dP3+dP4)4;]]>S7：根据整圈Z脉冲数C，反馈位置P5以及Z偏差Z5，计算P0处的Z偏差Z0；S8：计算反馈位置P5到真实原点P00的位移，机械手根据此位移返回原点。