本发明属于自动化验机械臂技术领域，涉及一种基于视觉标定的自动化验机械臂装置、控制系统及其控制方法。
背景技术：
目前在医院、科研等单位使用的化验分析仪器中使用的进样方式多为手工操作模式，如试管插放式和整排推入式等，既耗费人力，又因操作人员的不同而带来后期处理的差异性。伴随着科技的进步，使得自动化验机械臂的出现成为可能。若使机械臂定位到目标试管，首先就要确定目标试管的位置。视觉系统作为整个系统的“眼睛”，用于目标试管的识别和定位，能否准确、快速地识别出目标试管，直接影响着自动化验机械臂的可靠性和稳定性。
论文“一种用于智慧工厂化学检测的机械臂设计”(《智慧工厂》，2017年第7期)，公开了一种嵌入式系统驱动机械臂，其采样装置固定在机械臂末端由气泵驱动，试管架相对于机械臂固定的放置在操作平面。该技术的不足主要表现为：
(1)工作过程中处于开环控制状态，在机械臂运动的过程中没有任何反馈信息，无法确定机械臂运动的实际状况。
(2)不能自主地消除外界干扰。机械臂的底座固定在操作平面，而试管架是经常需要人为放置的，包括化验前的添加样品，化验结束后的取走反应结果。人为的放置或多或少都会有误差，而上述系统没有反馈装置，不能反馈误差。误差小则不能精确定位，误差过大就会导致系统不能正常工作。
(3)不能满足快速视觉伺服控制的要求。自动化验机械臂的工作环境相对复杂，自动化验机械臂视觉系统的精准、快速识别试管架上的目标试管，仍是研究的难点和重点。为了提高自动化验机械臂识别目标试管的位置的精度，满足自动化验机械臂精确性的需求，需加入优化识别算法。
(4)定位准确率不高。外界和自身产生的干扰会使得后续试管的识别定位准确率下降，并且无法采取有效措施修正，这些干扰累积下来将会使整个系统定位试管的准确率大大下降。
技术实现要素：
本发明的目的在于针对现有技术的不足之处，提供一种基于视觉标定的自动化验机械臂装置、控制系统及其控制方法，可以实现自动化验，系统自动识别并定位试管架以及试管位置，自动消除偏差抗干扰性强、提高操作的准确率、增强自动化水平，具有较好的抗干扰性，精度高、重复性好、效率高。
为解决上述技术问题，本发明采用以下技术方案。
本发明的一种基于视觉标定的自动化验机械臂装置，其特征在于，包括控制系统、机械臂主体、末端执行器、气泵、试管架；
机械臂主体包括固定在支撑平台的第一伺服电机、与第一伺服电机相邻的第二伺服电机、与第二伺服电机相连的大臂，与大臂相连的第三伺服电机、与第三伺服电机相连的小臂；所述的第一伺服电机控制机械臂的左右转动，第二伺服电机控制机械臂的前后转动，第三伺服电机控制末端执行器的上下运动，该三个伺服电机整体联动下实现末端执行器在操作空间中的移动；
机械臂主体通过L型工件与末端执行器相连；
末端执行器包括注射器和摄像头；所述的摄像头的轴线垂直于试管架的上平面。
气泵位于机械臂主体后方，注射器通过软管与气泵相连；气泵用于驱动注射器吸入或者排出液体；
试管架设置于机械臂主体前方，多个规格大小一致的试管矩阵式排列在试管架上，并按照从左至右、从上至下的顺序进行标号。
优选地，所述的摄像头为线性摄像头。
本发明的一种基于视觉标定的自动化验机械臂装置控制系统，其特征在于，包括视觉系统、机械臂控制系统、气泵控制系统和上位机控制系统；
视觉系统用于实现目标试管的识别和定位；
机械臂控制系统用于带动设置于机械臂主体末端的末端执行器在空间内移动；通过第一伺服电机控制机械臂左右转动，第二伺服电机控制机械臂前后转动，第三伺服电机控制末端执行器上下运动，整体联动实现末端执行器在工作空间中移动；末端执行器是化验机械臂的关键执行部件，是实现吸取和排除化验试剂的装置；
气泵控制系统用于驱动注射器吸取和排出化验试剂的装置；
上位机控制系统用于分析图像信息并结合用户的命令控制机械臂控制系统和气泵控制系统。
本发明的一种基于视觉标定的自动化验机械臂装置控制方法，其特征在于，首先建立自动化验机械臂装置以及试管架在空间中的三维模型，然后，进行以下步骤：
步骤1.判断机械臂是否处于抬升高度，如果是则进行下一步，如果不是则使机械臂抬升至抬升高度再进行下一步；
步骤2.通过视觉处理确定目标试管位置；所述视觉处理包含：目标试管图像坐标的获取与处理、目标试管图像坐标变换为机械臂的动作坐标；
步骤3.机械臂带动注射器到达目标试管口；
步骤4.机械臂再带动注射器下降进入目标试管；
步骤5.气泵驱动注射器对目标试管进行操作；
步骤6.结束后机械臂抬升返回抬升高度。
所述步骤2包括：
2.1.将图像灰度化：即是将彩色图像转化为黑白图；
2.2.采用阈值判别法将图像二值化；
2.3.试管架边缘的检测、轮廓的提取以及图像轮廓中心点的提取；
2.4.将像素坐标转换为物理坐标；
2.5.计算试管架相对于摄像头的旋转角θ；
2.6.计算试管架相对于摄像头的偏移量；
2.7.计算目标试管在试管架平面坐标的位置；
2.8.计算目标试管在摄像头坐标系的位置。
与传统设备相比具有以下优点和有益效果：
①应用视觉标定控制方法，能够识别不同情况下各试管的编号以及位置，抗干扰性更强。
②提高了试管架及试管识别定位的准确性和实时性，获取目标试管位置的误差大幅减少。
③改进了视觉伺服控制方法，视觉系统的控制更加快速准确。
④自动消除偏差、抗干扰性强、提高操作的准确率以及作业的效率。
附图说明
图1是本发明的基于视觉标定的自动化验机械臂装置的一种实施例的结构示意图。
图2是本发明的基于视觉标定的自动化验机械臂装置控制系统的一种实施例的关系图。
图3是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的工作流程图。
图4是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的摄像头坐标系与世界坐标系示意图。
图5是本发明的基于视觉标定的自动化验机械臂装置的一种实施例的试管架平面示意图。
图6是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的直角坐标系uv和XO1Y的关系图。
图7是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的摄像头采集图像二值化示意图。
图8是本发明的基于视觉标定的自动化验机械臂装置的一种实施例的标号后的试管架图像。
图9是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的目标试管坐标由试管架平面坐标系变换到摄像头坐标系的示意图。
图10是本发明的一个实施例的实验示意图。
其中，1机械臂主体，2末端执行器，3气泵，4试管架，5第一伺服电机，6第二伺服电机，7大臂，8第三伺服电机，9小臂，10L型工件，11注射器，12摄像头。
具体实施方式
本发明的一种新型基于视觉标定的自动化验机械臂装置、控制系统及其控制方法，通过视觉系统自动识别并定位试管以及试管架位置，进而实现自动化验，有助于减少误差、提高化验效率、增强自动化水平。采用单目视觉标定，首先，建立了自动化验机械臂以及试管架在空间中的三维模型。然后，通过视觉识别试管架的位置并定位排布在试管架上的各个试管。进而，控制机械臂的在操作空间内的运动，使机械臂带动挂载在机械臂末端执行器的注射器移动到目标试管的试管口。最后由气泵驱动注射器实现对目标试管中样品的吸取或者排出。
本发明的自动化验机械臂装置包括控制系统、机械臂主体1、末端执行器2、气泵3、试管架4。
控制系统包括视觉系统、机械臂控制系统、气泵控制系统和上位机控制系统。视觉系统采集视觉信息传给上位机控制系统，上位机控制系统分析图像信息并结合用户的命令控制机械臂控制系统和气泵控制系统。
其中视觉系统用于实现目标试管的识别和定位；
机械臂控制系统用于控制整个控制系统的运动，其带动挂载在机械臂末端的末端执行器在空间内移动；其中第一伺服电机控制机械臂左右转动，第二伺服电机控制机械臂前后转动，第三伺服电机控制末端执行器上下运动，整体联动实现末端执行器在工作空间中移动；末端执行器是化验机械臂的关键执行部件，是实现吸取和排除化验试剂的装置；
气泵控制系统是自动化验机械臂的关键执行部件，是驱动注射器吸取和排出化验试剂的装置；
上位机控制系统用于对整个系统的控制；整个机械臂装置的化验速度以及效率与上述各部分工作性能密切相关。本发明采用标准化、模块化硬件构成自动化验机械臂的控制系统，可适应开放式控制系统的要求。
下面结合附图对本发明做进一步详细说明。
图l是本发明的基于视觉标定的自动化验机械臂装置的一种实施例的结构示意图。如图1所示，该实施例装置包括控制系统、机械臂主体1，挂载在机械臂末端的末端执行器2，与末端执行器通过软管相连的气泵3，位于机械臂前方操作空间的试管架4。该操作空间是指机械臂在三维空间中的工作范围。
机械臂主体1包括固定在支撑平台的第一伺服电机5、与第一伺服电机5相邻的第二伺服电机6、与第二伺服电机6相连的大臂7，与大臂7相连的第三伺服电机8、与第三伺服电机8相连的小臂9，其中第一伺服电机控制机械臂的左右转动，第二伺服电机控制机械臂前后转动，第三伺服电机控制末端执行器上下运动，整体联动实现末端执行器在工作空间中移动。
末端执行器2与机械臂主体1通过L型工件10相连，末端执行器2包括注射器11和摄像头12末端执行器2具有视觉感知能力，能够处理目标试管位置不确定的物理特性。视觉传感器采用Eye-in-hand安装方式，摄像头12自上而下俯视整个试管架4，采集试管架4的图像。
气泵3位于机械臂后方，注射器11通过软管与气泵3相连。气泵3的吸气或者出气，驱动注射器11吸入或者排出液体。若试管吸入或排出液体的体积为V，气泵工作时长为t，则满足：V＝k*t，k表示固定系数。
试管架4位于机械臂主体1前方的可操作范围内，其上矩阵式地排列有规格大小一致的多个试管，并按照从左至右，从上至下的顺序进行标号。
本发明采用线性摄像头，所采集的图像无畸变。得到目标试管P在摄像头坐标系中的相对位置后，机械臂即可对目标试管P进行处理。
图2是本发明的基于视觉标定的自动化验机械臂装置控制系统的一种实施例的关系图。如图2所示，控制系统包括视觉系统、机械臂控制系统、气泵控制系统和上位机控制系统。视觉系统采集视觉信息传给上位机控制系统，上位机控制系统分析图像信息并结合用户的命令控制机械臂控制系统和气泵控制系统。
其中视觉系统是基础，主要用于实现目标试管的识别和定位；机械臂控制系统是整个系统的运动核心，其带动挂载在末端的执行器在空间内移动；气泵控制系统是自动化验机械臂的关键执行部件，是驱动注射器吸取和排出化验试剂的装置；上位机控制系统主要用于对整个系统的控制；整个机器人的化验速度以及效率与上述各部分工作性能密切相关。本发明采用标准化、模块化硬件构成自动化验机械臂装置的控制系统。
若让机械臂对目标试管进行操作，首先要确定目标试管的位置。单目视觉标定的目的即是为了确定目标物体的位置。为了减少系统处理信息的负担，本发明仅需某几个时间点(在机械臂处于抬升高度寻找下一个目标试管的位置时)处理摄像头返回的视觉信息。
本发明采用规格一致的试管，统一摆放在试管架上，从而使机械臂在带动注射器进入不同位置的试管和抬升离开试管时可以有相同的高度，即深入高度和抬升高度。
本发明的一种基于视觉标定的自动化验机械臂装置的控制方法，通过视觉系统自动识别并定位试管以及试管架位置，进而实现自动化验，有助于减少误差、提高化验效率、增强自动化水平。它采用单目视觉标定，首先，建立了自动化验机械臂以及试管架在空间中的三维模型。然后，通过视觉识别试管架的位置并定位排布在试管架上的各个试管。进而，控制机械臂的在空间内的运动，使机械臂带动挂载在机械臂末端执行器的注射器移动到目标试管的试管口。最后由气泵驱动注射器实现对目标试管中样品的吸取或者排出。
图3是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的工作流程图。如图3所示，本实施例方法，首先，建立自动化验机械臂以及试管架在空间中的三维模型，然后，进行以下步骤：
步骤1.判断机械臂是否处于抬升高度，如果是则进行下一步，如果不是则使机械臂抬升至抬升高度再进行下一步。
步骤2.通过视觉处理确定目标试管位置；
步骤3.机械臂带动注射器到达目标试管口。
步骤4.机械臂再带动注射器下降进入目标试管。
步骤5.气泵驱动注射器对目标试管进行操作。
步骤6.结束后机械臂抬升返回抬升高度。
其中，步骤2又包括以下步骤：
2.1.图像的灰度化：图像的灰度化就是将彩色图像转化为黑白图像。
2.2.图像的二值化：采用阈值判别法将图像二值化。
2.3.边缘的检测、轮廓的提取以及图像轮廓中心点的提取。
2.4.将像素坐标转换为物理坐标。
2.5.计算试管架相对于摄像头的旋转角θ。
2.6.试管架相对于摄像头的偏移量。
2.7.计算目标试管在试管架平面坐标的位置。
2.8.计算目标试管在摄像头坐标系的位置。
图3是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的工作流程图。如图3所示，控制系统在一个工作周期开始时先判断机械臂是否处于抬升高度，如果是则通过视觉处理确定目标试管位置，如果不是则使机械臂抬升至抬升高度再通过视觉处理确定目标试管位置。确定好目标试管的位置后，则使机械臂带动注射器到达目标试管口，接着机械臂再带动注射器下降进入目标试管，然后气泵驱动目标试管进行操作，结束后机械臂再抬升返回抬升高度，整个工作周期结束。
在进入试管和抬升离开试管的过程中机械臂线性竖直下降或者抬升，因此在这个过程中并不需要摄像头的参与，在注射器对相应试管进行吸取或者排出液体的操作时也不需要摄像头的参与，而在注射器完成一个工作周期回到抬升平面寻找下一个操作试管时，视觉系统需要处理来自摄像头的图像信息以便确定下一个试管的位置。
视觉处理包含：目标物体图像坐标的获取与处理和目标物体图像坐标变换为机械臂的动作坐标。图4是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的摄像头坐标系与世界坐标系示意图。如图4所示，所涉及的两个坐标系为世界坐标系和摄像头坐标系。
世界坐标系(XW，YW，ZW)是在外部环境中选择的一个基准坐标系，用来描述目标物体的位置，可以根据描述和计算的方便等原则自由选取，在机器人视觉标定中，将世界坐标系设为机器人的基座标系。其中的任一点都用(xw，yw，zw)表示。这样对于选取线性摄像机模型，选择机器人基坐标系为摄像机标定的世界坐标系可以大大简化数学表达式和简化标定操作。
摄像头坐标系(XC，YC，ZC)是将坐标原点设在摄像头镜头光心上，两个坐标轴XC、YC轴平行于图像平面，另一个轴ZC轴则垂直于图像平面。与图像平面的交点在图像坐标系上的坐标为(u0，v0)即摄像头的主点。摄像头的主点一般位于图像平面的中心处。对于图像中的任一点，就用(u，v)表示。图像坐标系(u，v)则是定义在二维图像上的一个直角坐标系，分为以像素为单位和以物理长度(如毫米)为单位两种表示方式，分别用(u，v)和(x，y)表示，在图像坐标系中，以像素为单位的坐标系的原点定义在图像的左上角，以物理长度为单位的坐标系的原点定义在图像的中心。
通常在机器人系统中，首先要计算出目标P在世界坐标系中的相对位置，再计算摄像头在世界坐标系中的位置，最后得出目标P在摄像头坐标系中的相对位置。
单帧图像的获取是由摄像头采集传输给上位机控制系统，其数字图像在系统中是以二维数组M×N的形式进行存储。M表示图像在水平方向上的像素点个数，N表示图像在垂直方向上的像素点个数。M×N是该单帧图像像素的大小。
图像处理包含以下步骤：图像的灰度化、图像的二值化、图像轮廓的提取以及中心点的生成。图像的灰度化就是将彩色图像转化为黑白图像。因为转化为黑白图像后，方便于对所采集的数字图像进行后续的处理和运算，所以在视觉图像处理中系统先把采集到的彩色图像转化成灰度图像。在实现了图像的灰度化操作之后，系统对图像进行二值化操作。图5是本发明的基于视觉标定的自动化验机械臂装置的一种实施例的试管架平面示意图。如图5所示，为了简化视觉处理的复杂程度，使参照物易于被标定，本发明的试管架周围粘贴一圈矩形的黑色的标线，易于与白色的试管架和透明的试管区分开来。本发明采用阈值判别法将图像二值化。实现了图像的二值化后，就要对图像进行分析运算，进行边缘的检测、轮廓的提取以及图像轮廓中心点的提取。
摄像头的标定，用于确定摄像头的内外参数。摄像头标定的途径就是根据确定的摄像头模型，通过已知若干特征点的图像坐标和该这些特征点的世界坐标这两组坐标值来求解摄像头的内外参数。本发明中试管架平面与安装的摄像头的位置关系是：摄像头的轴心垂直于试管架平面。
摄像头模型是光学成像几何关系的简化，其中最简单的模型是线性模型(或称针孔模型)。线性模型成像关系为线性，是最常用的理想模型。摄像头采集的图像通过以数字信号的形式输入到上位机控制系统，每一个数字图像在上位机控制系统内都是以一个M×N数组的形式存储起来，数组中的每一个元素就代表一个像素，每一个元素的数值就是图像点的亮度，也称为灰度。
图6是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的直角坐标系uv和XO1Y的关系图。如图6所示，在图像上定义了一个以像素为单位的直角坐标系uv，每一像素的坐标(u，v)分别代表该像素在数组中的列数与行数。(u，v)是以象素为单位的图像坐标系的坐标。由于(u，v)只表示象素位于数组中的列数与行数，而无法用物理单位表示出该象素在图像中的位置。因此，再建立一个以物理单位来表示图像点位置的图像坐标系XO1Y。这个坐标系的原点定义在摄像头中心轴与图像平面的交点上，在摄像头的标定过程中，假设坐标系的原点在图像的中心。两个坐标轴x轴、y轴分别与u轴、v轴平行。在图像坐标系中，设O1在u，v坐标系中的坐标为(u0，v0)，每一个像素在x轴与y轴方向上的物理尺寸为dx，dy，则图像中任意一个象素在两个坐标系下的坐标有如下关系:
x＝(u-u0)dx
y＝-(v-v0)dy
或者：
本发明采用平面视觉，涉及平面视觉相关的二维坐标变换的公式和坐标变换的相关特性。在机械臂的移动过程中，摄像头的轴线始终垂直于试管架平面，因此摄像头采集的图像坐标系与世界坐标系相平行，而二维坐标的变换仅有旋转和平移。通过对摄像头采集的二值化后的图像进行处理可以得到试管架相对于摄像头的旋转量和偏移量。
图7是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的摄像头采集图像二值化示意图。如图7所示，A’B’C’D’是摄像头采集图像的外边框，O’是其中点，ABCD是图像二值化后的试管架边界，O是其中点。试管架相对于摄像头的旋转角θ可以通过计算线AB的斜率或者线CD的斜率得出。
设A点的坐标为(xa，ya)，B点的坐标为(xb，yb)，C点的坐标为(xc，yc)，D点的坐标为(xd，yd)，线AB的斜率kAB，线CD的斜率kCD，线AB相对于A’B’的倾斜角θAB，线CD相对于C’D’的倾斜角θCD则:
θAB＝arctankAB
θCD＝arctankCD
若θAB与θCD相差不大，则取：
至此可以求出试管架相对于摄像头的旋转角θ，而试管架相对于摄像头的偏移量即试管架的中点O相对于摄像头中点O’的位置，由于O’点是坐标系XO1Y的原点，因此O点相对于O’点的位置即O点坐标(xo，yo)。
图8是本发明的基于视觉标定的自动化验机械臂装置的一种实施例的标号后的试管架图像。如图8所示，本实施例的试管矩阵式地排列在试管架上，按照从左至右，从上至下的顺序对试管进行标号。若想对某个试管进行操作，则可以通过该试管标号确定该试管相对于试管架的位置，再结合试管架相对于摄像头的位置，则可以确定该试管相对于摄像头的位置。
在试管架的平面图像中以中心的第18号试管的中心点为原点建立直角坐标系，试管间的横向间距和纵向间距都为Δ，设d号试管在试管架上的第a行(最上面一行为第0行)第b列(最左边一列为第0列)，设a，b都为整型，则
b＝d-7*a
由此可得d号试管的位置P：
P＝((b-3)*Δ,(2-a)*Δ)
然后，将该点的坐标由试管架平面坐标系变换到摄像头坐标系。图9是本发明的基于视觉标定的自动化验机械臂装置控制方法的一种实施例的目标试管坐标由试管架平面坐标系变换到摄像头坐标系的示意图。如图9所示，首先由试管架相对于摄像头的旋转角θ引入旋转矩阵R：
将坐标P经过旋转之后得到P’：
然后在加上试管架相对于摄像头的旋转量和偏移量(xo，yo)，得到最终P在摄像头坐标中的坐标P”：
这样机械臂根据目标试管在摄像头坐标系中的位置线性移动就可以到达该试管上方。
本发明的技术方案经过实验验证，其示意图如图10所示，自动化验机械臂的末端执行器位于28号试管上方，机械臂在实际运动过程中会有以下几种情况：
1)系统开机时，末端执行器正好位于第28号试管上方；
2)上一个工作周期结束后末端执行器停止于第28号试管上方；
3)人为改变了试管架的位置，使末端执行器正好位于第28号试管上方。
要求系统对1号试管操作，分别在上述情况下进行实验测试。结果显示本发明的控制系统在达到稳定状态后与1号试管的中心误差极小。可以看出采用视觉标定的控制方法，机械臂较好地实现了视觉定位试管位置，说明本发明抗干扰性能较好。