CN103543677A

CN103543677A - 一种配药机器人的回零控制系统及其实现方法

Info

Publication number: CN103543677A
Application number: CN201310464304.9A
Authority: CN
Inventors: 肖德明; 何玉成; 胡颖; 雷鸣; 张宏; 曾晖; 李伟; 丁小容; 段丽芳
Original assignee: Peking University Shenzhen Hospital
Current assignee: Shenzhen Bowei medical robot Co. Ltd.
Priority date: 2013-09-30
Filing date: 2013-09-30
Publication date: 2014-01-29
Anticipated expiration: 2033-09-30
Also published as: CN103543677B

Abstract

本发明公开了一种配药机器人的回零控制系统及其实现方法，通过主控制器根据用户指令将各自由度控制模式设定为绝对位置控制模式，伺服驱动器在绝对位置控制模式下，通过零点设定将自由度当前的位置数据保存在伺服驱动器的EEP-ROM存储器中作为零点位置，经过实验测试，配药机器人在各种工况下都能够实现自动回零，且不会发生碰撞，很好地解决了配药机器人在回零过程中的运动干涉问题，并且缩短了机器人回零时间，提高了机器人的工作效率，并且可以很好地满足机械臂之间协同工作的需求。

Description

一种配药机器人的回零控制系统及其实现方法

技术领域

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

背景技术

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

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

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

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

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

相较于现有技术，本发明提供的配药机器人的回零控制系统及其实现方法，通过主控制器根据用户指令将各自由度控制模式设定为绝对位置控制模式，伺服驱动器在绝对位置控制模式下，通过零点设定将自由度当前的位置数据保存在伺服驱动器的EEP-ROM存储器中作为零点位置，经过实验测试，配药机器人在各种工况下都能够实现自动回零，且不会发生碰撞，很好地解决了配药机器人在回零过程中的运动干涉问题，并且缩短了机器人回零时间，提高了机器人的工作效率，并且可以很好地满足机械臂之间协同工作的需求。

附图说明

图1为配药机器人的机械结构的示意图。

图2为本发明的配药机器人的回零控制系统的结构框图。

图3为设置有本发明的配药机器人的回零控制系统的配药机器人的零点位置及尺寸标定的示意图。

图4为本发明的配药机器人的回零控制的流程图。

具体实施方式

本发明提供一种配药机器人的回零控制系统及其实现方法，为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

请参阅图2，其为本发明的配药机器人的回零控制系统，如图所示，其包括：主控制器、若干个可编程运动控制卡（在本实施例中，所述可编程运动控制卡的个数为两个：可编程运动控制卡1和可编程运动控制卡2）、伺服驱动器及中继端子板；其中，所述主控制器通过PCI总线与可编程运动控制卡通信，所述可编程运动控制卡通过中继端子板与各伺服驱动器连接，每一伺服驱动器均配备有电池（如图所示: 伺服驱动器1、伺服驱动器2到伺服驱动器N）。

具体来说，主控制器根据用户指令将各自由度控制模式设定为绝对位置控制模式，伺服驱动器在绝对位置控制模式下，通过零点设定将自由度当前的位置数据保存在伺服驱动器的EEP-ROM存储器中作为零点位置。在本实施中，所述主控制器为工业控制计算机；可编程运动控制卡包含脉冲发送模块、编码反馈模块及开关量输入/输出模块，并且可通过其提供的库函数读取开关量输入/输出点的电平状态；伺服驱动器在绝对位置控制模式下，通过零点设定可将自由度当前的位置数据保存在驱动器的EEP-ROM存储器中作为零点位置，若配备电池，机器人断电后，亦可保存、记录电机的位置信息。

经过实验测试，配药机器人在各种工况下都能够实现自动回零，且不会发生碰撞，很好地解决了配药机器人在回零过程中的运动干涉问题。

请继续参阅图3，图3的结构配药机器人正视简化图，图中对机器人各个自由度零点位置和一些关键尺寸进行标定。配药机器人机械结构及控制系统硬件平台搭建好之后，已经将各自由度控制模式设定为绝对位置控制模式，并且通过零点设定功能设定好各自由度的零点位置。在本实施例中，自由度1（即第一自由度，下同）为单圈旋转运动，当手爪1上、下面水平并且自由度2处于下方时，即为自由度1的原点位置；自由度2、自由度3及自由度5的零点位置如图3所示；自由度4为单圈旋转运动，手爪3上、下面水平并且手爪的手指在上方时（如图所示）为自由度4的零点位置。药瓶开启装置上顶面与零点5的距离为H1，手爪1的下底面与零点5的距离为H2，为保证手爪3不与药瓶开启装置发生碰撞，设定手爪3中心点与零点3的安全距离为S。S1、S2、S3、S4和S5依次为自由度1、自由度2、自由度3、自由度4、自由度5的位置数据。丝杠传动中，靠近电机的运动方向为正，远离电机的运动方向为负。

上述的配药机器人的回零控制系统的实现方法如下：

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

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

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

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

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

概括来说，如图4所示，具体实施方案如下所述：

步骤1：配药机器人上电后，初始化系统。

步骤2：获取自由度1、自由度2、自由度3、自由度4、自由度5的位置数据S1、S2、S3、S4和S5。

步骤3：若S5>H1,则自由度1运动致丝杠1处于水平位置，并且自由度2处于外侧；自由度3运动到S位置处；然后自由度5、自由度3、自由度4、自由度1、自由度2依次回到零点位置。否则，则执行步骤4。

步骤4：若S5>- H2,则自由度1运动致丝杠1处理水平位置，并且自由度2处于外侧；自由度3、自由度4、自由度5、自由度1、自由度2依次回到时零点位置。否则，则执行步骤5。

步骤5：自由度1、自由度2、自由度3、自由度4、自由度5依次回到零点位置。

步骤6：结束。

本发明可以轻易地避免配药机器人在回零点过程中出现的运动干涉问题，避免机器人发生碰撞，并且缩短了机器人回零时间，提高了机器人的工作效率，并且可以很好地满足机械臂之间协同工作的需求。

进一步地，本发明的配药机器人的回零控制系统所用到的主控制器除了工业控制计算机，也可以用其他控制器，如普通台式机等；发明中的可编程运动控制卡也可以用运动控制卡与数据采集卡替代等；发明中的伺服驱动器也可以为其他可配备电池的伺服驱动器。本发明还可以用在一些上电后，条件不允许进行回零操作的多关节串联机器人上，如点焊机器人、弧焊机器人等，这里就不一一展开描述了。

本发明的配药机器人的回零控制系统及其实现方法，通过主控制器根据用户指令将各自由度控制模式设定为绝对位置控制模式，伺服驱动器在绝对位置控制模式下，通过零点设定将自由度当前的位置数据保存在伺服驱动器的EEP-ROM存储器中作为零点位置，经过实验测试，配药机器人在各种工况下都能够实现自动回零，且不会发生碰撞，很好地解决了配药机器人在回零过程中的运动干涉问题，并且缩短了机器人回零时间，提高了机器人的工作效率，并且可以很好地满足机械臂之间协同工作的需求。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。

Claims

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

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

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

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

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

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

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

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