一种用于无人实验室离心机的机器人自主作业控制方法

本发明涉及无人实验室领域，具体地说是一种用于无人实验室离心机的机器人自主作业控制方法。

背景技术：

1、生物安全实验室是研究致病性病原微生物的重要平台，在医疗、疾控、微生物检测、分子生物学等研究中有着广泛应用，但由于生物安全实验室操作通常也具有危险性和很多要求，因此目前生物安全实验室发展趋势是自动化和无人化操作。

2、离心机操作是所有生化实验中最基本的操作，但目前想要通过机器人实现自主操作离心机存在以下问题：1、离心机装置内部空间受限，并且存在障碍物干扰，机器人只能在狭小区域操作，如公开号为cn111366436a的中国发明专利中公开了一种全自动精液处理装置，其内部设置有离心机、第一机械臂和第二机械臂，第一机械臂顶部设有吸管头，第二机械臂下部设有管夹、上部设有能够旋转的管盖抓手，该装置虽然可以实现处理过程的无人操作，但由于内部空间有限，其处理试管数量较少，并且机械臂控制路径较短。2、对于图1所示的离心机而言，其内部设有转子座，并且所述转子座上沿着圆周方向均布有多个试管槽，可大大提高试管处理数量，但同时也会由于转子座旋转造成离心机内部试管槽不固定，进而导致试管不固定，机器人难以通过直接示教完成定位实现取放，并且该离心机内部无法再安装机械臂等装置，只能通过设于外部的机器人装置实现试管取放，同时所述试管槽又是与竖直方向呈一定角度倾斜设置，这些都增加了机械人自主控制的难度。

技术实现思路

1、本发明的目的在于提供一种用于无人实验室离心机的机器人自主作业控制方法，整个离心机操作过程均通过机器人装置实现，满足无人实验室的使用要求。

2、本发明的目的是通过以下技术方案来实现的：

3、一种用于无人实验室离心机的机器人自主作业控制方法，包括机器人装置和离心机，所述机器人装置包括移动小车、机械臂和试管夹爪，移动小车上设有机械臂基座和试管托盘，且机械臂设于所述机械臂基座上，所述机械臂末端设有视觉相机、力传感器、试管夹爪和旋转关节，其中试管夹爪通过所述旋转关节驱动旋转，所述离心机内部设有转子座，所述转子座上沿着圆周方向均布有多个试管槽，并且所述试管槽倾斜设置，包括如下步骤：

4、步骤一：设置第一特征码和第二特征码，其中第一特征码设于离心机外部，第二特征码设于离心机转子座中心处并建立坐标系o；

5、步骤二：机器人装置通过移动小车驱动移动至指定位置，机械臂末端的试管夹爪从试管托盘上夹取试管并移动至初始姿态点a，然后控制系统利用视觉相机和第一特征码获得校正后的坐标系o相对于机械臂基座坐标系(base)的矩阵

6、步骤三：视觉相机对第二特征码进行识别并记录第二特征码此时的姿态q1，然后根据校正后的矩阵求出机械臂末端旋转形成的旋转虚拟圆s1上任一点坐标系i相对于机械臂基座坐标系(base)的矩阵；

7、步骤四：根据矩阵控制机械臂末端调整至放试管姿态，然后驱动试管夹爪沿着机械臂末端轴向向下移动，若下降过程中机械臂末端的力传感器检测受力，试管夹爪退回，机械臂末端的旋转关节绕坐标系o的z轴旋转设定角度，然后试管夹爪继续沿着机械臂末端轴向向下移动试探，直到试管下降设定距离过程中没有受力，此时第一个试管插入对应的试管槽中，控制系统记录此时机械臂末端姿态角度，然后根据360°/n(n为试管槽数量)计算获得其他试管槽位置的转动角度并完成其余各个试管的放置；

8、步骤五：所有试管放入后，机械臂末端控制离心机关盖；

9、步骤六：离心机关盖后，机械臂末端控制离心机启动；

10、步骤七：离心机工作完毕后，机械臂末端控制离心机开盖；

11、步骤八：机械臂末端调整至和步骤四放入第一个试管时相同的姿态，然后视觉相机获得第二特征码此时的姿态q2，根据q1和q2确定旋转初始角度θ′，然后机械臂末端绕第二特征码的z轴(即坐标系o的z轴)旋转θ′至取第一个试管的姿态位置，第一个试管取完后，控制系统根据360°/n(n为试管槽数量)计算获得其他试管槽位置的转动角度并完成其余各个试管的取出。

12、步骤二中：为第一特征码(aruco)相对于机械臂基座坐标系(base)的位置矩阵，为坐标系o相对于第一特征码的位置矩阵。

13、步骤三中，为机械臂末端初始点坐标系a相对于机械臂基座坐标系(base)的矩阵，为坐标系o相对于初始点坐标系a的矩阵，有：

14、

15、上式(1)中，通过机械臂常规示教获得，为步骤二中校正后获得，上式(1)变换后得：

16、

17、上式(2)中，axo，ayo，azo为坐标系a的原点在坐标系o下的坐标，rxa，rya，rza为坐标系a的原点在坐标系o下的欧拉角，其中axo，ayo满足：

18、axo2+ayo2＝rs2            (3)；

19、上式(3)中，rs为旋转虚拟圆s1的半径；

20、s1上任一点坐标系i在坐标系o下的姿态为：

21、[ixo，iyo，izo，rxi，ryi，rzi]；

22、根据上式(2)，设坐标系a的原点相对于坐标系o的起始角为θao＝arctan(ayo/axo)，坐标系i相对于a的原点的旋转角度为θai，得到：

23、ixo＝rs*cos(θao+θai)     (4.1)；

24、iyo＝rs*sin(θao+θai)     (4.2)；

25、rxi＝rxa                (4.3)；

26、ryi＝rya                (4.4)；

27、rzi＝rza+θai            (4.5)；

28、根据上式(4.1)-(4.5)求得坐标系i相对于坐标系o的矩阵[ixo，iyo，izo，rxi，ryi，rzi]；

29、而：

30、根据上式(5)求得

31、所述离心机上设有开盖按钮和启动按钮，步骤五中，机械臂末端按照校正后的示教关盖轨迹移动实现离心机关盖；步骤六中，机械臂末端按照校正后的示教启动轨迹移动至离心机上的启动按钮位置，并按下所述启动按钮实现离心机启动；步骤七中，机械臂末端按照校正后的示教开盖轨迹移动至离心机上的开盖按钮位置，并按下所述开盖按钮实现离心机开盖。

32、所述示教关盖轨迹、示教启动轨迹、示教开盖轨迹均包括多个轨迹点，且各个轨迹点pi均通过下式(6)校正:

33、

34、上式(6)中，为第一特征码(aruco)相对于机械臂基座坐标系(base)的矩阵，为pi点相对于第一特征码的矩阵。

35、所述试管夹爪末端的设有用于按压按钮的推柱。

36、步骤八中，设：

37、q1＝[qx1，qy1，qz1，qrx1，qxy1，qrz1]    (7.1)；

38、q2＝[qx2，qy2，qz2，qrx2，qry2，qrz2]    (7.2)；

39、θ＝qz2-qz1，θ∈[-π，π]                (7.3)；

40、上式中，qx1，qy1，qz1，qx2，qy2，qz2为第二特征码在机械臂基座坐标系下的坐标，qrx1，qry1，qrz1，qrx2，qry2，qrz2为第二特征码在机械臂基座坐标系下的欧拉角，θ为第二特征码绕自身z轴(也即坐标系o的z轴)的旋转角度，当上式(7.3)获得的θ﹥0时，代表转子座顺时针旋转θ，有：

41、θ′＝θ％60；θ′∈[0，60)           (8)；

42、此时机械臂末端顺时针旋转初始角度θ′；

43、当上式(7.3)获得的θ＜0时，代表转子座逆时针旋转θ，有：

44、θ′＝abs(θ)％60；θ′∈[0，60)      (9)；

45、此时机械臂末端逆时针旋转初始角度θ′。

46、本发明的优点与积极效果为：

47、1、本发明整个离心机操作过程均通过机器人装置实现，满足无人实验室的使用要求。

48、2、本发明针对的离心机，其转子座上沿着圆周方向均布设有试管槽，并且所述试管槽与竖直方向呈一定夹角倾斜设置，本发明方法通过第一特征码、第二特征码和视觉相机配合能够实现各个试管倾斜状态的准确取放，满足该种类型离心机的使用要求。

49、3、考虑到每次机器人装置移动至离心机附近的位置都会有偏差，本发明通过视觉相机和第一特征码辅助校正放试管、关盖、开盖等轨迹，从而避免了机器人装置位置变化产生的影响。

50、4、考虑到离心机工作完毕后，离心机转子座上的各个试管位置完全随机，并且利用第二特征码识别出来的相对于机械臂末端的绝对位姿绝对精度太低，无法满足取试管要求，本发明根据第二特征码在离心前后旋转的姿态变化实现相对定位，既可以保证取试管精度，同时也简化了计算。