[发明专利]一种配药机器人的回零控制系统及其实现方法

权利要求书

1.一种配药机器人的回零控制系统，其特征在于，包括：主控制器、若干个可编程运动控制卡、伺服驱动器及中继端子板；

其中，所述主控制器通过PCI总线与可编程运动控制卡通信，所述可编程运动控制卡通过中继端子板与各伺服驱动器连接，每一伺服驱动器均配备有电池；

主控制器根据用户指令将各自由度控制模式设定为绝对位置控制模式，伺服驱动器在绝对位置控制模式下，通过零点设定将自由度当前的位置数据保存在伺服驱动器的EEP-ROM存储器中作为零点位置；

同时，配备有电池的伺服驱动器还用于在断电后保存、记录与伺服驱动器连接的电机的位置信息。

2.根据权利要求1所述的配药机器人的回零控制系统，其特征在于，所述主控制器为工业控制计算机。

3.根据权利要求1所述的配药机器人的回零控制系统，其特征在于，所述可编程运动控制卡采用运动控制卡与数据采集卡替代。

4.根据权利要求1所述的配药机器人的回零控制系统，其特征在于，所述可编程运动控制卡包括依次连接的脉冲发送模块、编码反馈模块及开关量输入/输出模块。

5.一种采用权利要求1所述的配药机器人的回零控制系统的实现方法，其特征在于，所述方法包括：

M1、预先根据用户指令主设定为绝对位置控制模式,并设定好第一、第二、第三、第四和第五自由度的零点位置；且丝杠传动中，靠近电机的运动方向为正，远离电机的运动方向为负；

M2、配药机器人通电，初始化后，获取与第一、第二、第三、第四和第五自由度的分别对应的位置数据：第一位置数据S1、第二位置数据S2、第三位置数据S3、第四位置数据S4和第五位置数据S5； 

M3、获取药瓶开启装置上顶面与第五自由度的零点位置的距离H1后，比较H1与第S5的大小：若S5大于H1，则第一自由度运动致第一丝杠处于水平位置，并且第二自由度处于外侧；同时，第三自由度运动到S位置处，所述S位置为第三手爪的中心点与第三自由度的零点的距离，然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零点位置； 

M4、若S5大于H1，则获取第一手爪的下底面与第五自由度的零点位置的距离H2；并比较H2与S5的大小，若S5大于-H2，则第一自由度运动致第一丝杠处于水平位置，并且第二自由度处于外侧；然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零点位置；

M5、若S5小于-H2，则第一自由度、第二自由度、第三自由度、第四自由度和第五自由度依次回到零点位置。

6.根据权利要求5所述的回零控制系统的实现方法，其特征在于，所述第一自由度为单圈旋转运动，当第一手爪上、下面水平并且第二自由度处于下方时，即为第一自由度的零点位置。

7.根据权利要求5所述的回零控制系统的实现方法，其特征在于，所述第四自由度为单圈旋转运动，第三手爪上、下面水平并且第三手爪的手指在上方时为第四自由度的零点位置。