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

说明书

技术领域

本发明涉及医用设备技术领域，特别涉及一种配药机器人的回零控制系统及其实现方法。  

背景技术

随着机器人应用领域的不断扩展，机器人工作环境越来越复杂，面临的作业任务难度也越来越大，单臂机器人已经不能满足某些高难度任务需求，需要双臂甚至多臂协同操作才能完成。 

在医院药液配制过程中，对于医用药瓶（主要为安瓿瓶、西林瓶）的操作，如果采用配药机器人进行操作，配药机器人上电后，需要进行回零操作，将机器人各个轴回到初始位置。 

请参阅图1，其为配药机器人的机械结构示意图，如图所示，配药机器人包括机械臂Ⅰ、机械臂Ⅱ、药瓶开启装置和药品架等，其中机械臂Ⅰ包含自由度1（也称第一自由度）、自由度2（也称第二自由度）、手爪1（也称第一手爪）及手爪2（也称第二手爪），手爪1固定在丝杠1导轨末端；机械臂Ⅱ包含自由度3（也称第三自由度）、自由度4（也称第四自由度）、自由度5（也称第五自由度）及手爪3（也称第三手爪）。配药机器人上电后，需要先进行回零操作，使机器人的各个轴回到初始位置。 

现有的配药机器人大多采用位置传感器，通过设置回零开关来实现配药机器人各轴的回零操作，但随着配药机器人机械结构日趋复杂化，各关节间容易出现运动干涉(特别是在按下急停开关及出现故障并修复后)，致使机器人发生碰撞损坏。作为一种双臂机器人，配药机器人在后续的作业过程中，需要双臂协同工作才能有效完成任务，如果协同控制不当，也易产生相互碰撞。 

有鉴于此，现有技术还有待改进和提高。  

发明内容

鉴于上述现有技术的不足之处，本发明的目的在于提供一种配药机器人的回零控制系统及其实现方法，以解决现有技术中配药机器人采用位置传感器进行回零操作，容易导致配药机器人发生碰撞损坏的问题。 

为了达到上述目的，本发明采取了以下技术方案： 

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

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

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

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

所述的配药机器人的回零控制系统，其中，所述主控制器为工业控制计算机。 

所述的配药机器人的回零控制系统，其中，所述可编程运动控制卡采用运动控制卡与数据采集卡替代。 

所述的配药机器人的回零控制系统，其中，所述可编程运动控制卡包括依次连接的脉冲发送模块、编码反馈模块及开关量输入/输出模块。 

一种上述的配药机器人的回零控制系统的实现方法，其中，所述方法包括： 

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，则第一自由度、第二自由度、第三自由度、第四自由度和第五自由度依次回到零点位置。

所述的回零控制系统的实现方法，其中，所述第一自由度为单圈旋转运动，当第一手爪上、下面水平并且第二自由度处于下方时，即为第一自由度的零点位置。