CN104298244B - 一种工业机器人三维实时高精度定位装置的定位方法 - Google Patents

Abstract

本发明公开了一种工业机器人三维实时高精度定位装置及方法。该装置包括工业机器人系统、工业计算机和相机单元，工业计算机通过第一千兆以太网与工业机器人控制器相连接后接入工业机器人系统，工业计算机通过第二千兆以太网与相机单元相连，其中相机单元为主动相机单元或者多相机单元。该方法如下：根据特征点与实际目标点的位置，设置相机单元的位置状态；建立机器人空间中三维坐标到相机空间中二维坐标的映射关系；获取目标点在三维机器人空间中的位置，工业机器人控制器发送命令控制机器人向目标点运动，将机器人定位到目标点上；通过判断最终定位到实际目标点上。本发明不需要进行标定，且具有定位精度高、视野范围广、实时性好的优点。

Description

一种工业机器人三维实时高精度定位装置的定位方法

技术领域

本发明涉及工业机器人视觉伺服技术领域，特别是一种工业机器人三维实时高精度定位装置及方法。

背景技术

随着工业机器人的应用范围的扩大和多种复杂场合的需要，工业机器人视觉伺服显得越来越重要。在目前的工业机器人实际运用中，由于定位精度、实时性、需要标定等问题，只有极少场合运用了机器人视觉伺服定位，随着工业机器人行业的发展，工业机器人拥有视觉伺服能力将是必然趋势。实时性与定位精度是验证一个机器人视觉定位系统有效性与可实用性的最重要的两个指标，也是能否被投入到实际工业生产中的关键性因素。

基于视觉的机器人控制分为两类：基于位置的视觉伺服和基于图像的视觉伺服。基于位置的视觉伺服机器人定位过程中需要对机器人进行标定，标定中存在的任何误差都会导致机器人在三维空间中的定位产生一定的误差(P.I.Corke and S.Hutchinson2001)。

在基于图像的视觉伺服中，视觉系统中微小的误差将会对机器人定位产生很大的影响，故需要对摄像机以及机器人进行标定，而对摄像机和机器人的标定是一个极其复杂的问题。图像处理与控制机器人运动之间同步实时性是一个必须面临的难题，因此，由于图像处理的延时，高精度与高实时性的基于图像的视觉伺服定位是一个难以完成的任务。Itsushi Kinbara提出了一种新的视觉伺服机器人控制系统，此系统采用对图像特征进行估计的方法，但是这种方法始终摆脱不了需要对相机进行标定的弊端(Itsushi Kinbara,satoshi Komadda,and JunjiHirai 2006)。

绝大部分的视觉伺服定位方法都是对相机的参数进行标定，但是S.B.Skaar首先提出了一种方法名为camera-space manipulation(CSM)，这种方法是建立机器人工装机构上的特征点在相机空间的坐标与相对应的机器人关节空间的关系(S.B.Skaar,W.H.Brockman,and R.Hanson 1987)，基于这种方法的机器人定位不需要对相机进行标定，也不需要对工业机器人进行标定。Juan Manuel Rendon-Mancha提出了一种改进的CSM方法，采用了运用线性的模型代替经典CSM方法中的非线性参数模型(Juan Manuel Rendon-Mancha,Antonio Cardenas,MarcoA.Garcia 2010)。CSM方法被运用到很 多场合，例如太空探索，以及移动机器人等多个领域，但是并没有被深入的运用到实际工业生产当中去。

综上所述，目前工业生产中的机器人三维定位方法具有以下缺点：定位过程中需要对系统进行标定，标定误差大、操作复杂、定位精度低，影响了工业机器人的工作效率，阻碍了工业机器人的发展。

发明内容

本发明的目的在于提供一种高精度的工业机器人三维实时定位装置及方法，具有操作简单、工作效率高、实时性好的优点。

实现本发明目的的技术解决方案为：一种工业机器人三维实时高精度定位装置，包括：工业机器人系统、工业计算机和相机单元，其中工业计算机包括变权重相机空间定位单元、图像处理单元和机器人运动控制单元，机器人运动控制单元包括机器人正运动学单元和机器人逆运动学单元两部分，其中图像处理单元通过变权重相机空间定位单元与机器人运动控制单元相连；工业计算机通过第一千兆以太网与工业机器人控制器相连接，工业机器人控制器的控制端接入工业机器人系统，工业计算机通过第二千兆以太网与相机单元相连；

所述工业机器人系统包括六自由度工业机器人、激光发射装置和工装机构，其中工装机构的初始端刚性固定在六自由度工业机器人的末端，且与六自由度工业机器人的前臂共轴；工装机构的末端设置有特征点和工具，特征点位于工装机构顶端与其轴线成30°～60°夹角的上斜面，工具刚性固定在工装机构的下方；激光发射装置设置在六自由度工业机器人前臂，使激光发射装置发射出的激光线投射到工作平台上；

相机单元将实时采集的图像输入工业计算机中的图像处理单元进行处理，并将处理结果发送给变权重相机空间定位单元；机器人正运动学单元将当前六自由度工业机器人的状态信息进行处理后发送给变权重相机空间定位单元；变权重相机空间定位单元对接收到的信息进行综合处理后发送给机器人逆运动学单元；机器人逆运动学单元与工业机器人控制器进行网络通信发送命令控制六自由度工业机器人运动。

所述相机单元为主动相机单元或多相机单元，其中主动相机单元包括第一相机、云台本体、多自由度电机运动单元及电机驱动模块，其中第一相机固定在云台本体上，多自由度电机运动单元固定连接在云台本体上，电机驱动模块通过I/O口接入多自由度电机运动单元；工业计算机通过第二千兆以太网与相机单元中第一相机相连，工业计算机 通过总线与相机单元中电机驱动模块进行通信，电机驱动模块根据接收到的命令控制多自由度电机运动单元运动从而带动云台本体及第一相机移动；多相机单元包括多个第二相机以及与第二相机一一对应的支架，第二相机固定设置在对应的支架上，且每个第二相机均通过第二千兆以太网与工业计算机相连。

一种工业机器人三维实时高精度定位装置的定位方法，其中相机单元为主动相机单元，包括以下步骤：

第1步，第一相机获取实际目标点及特征点，并根据特征点与实际目标点之间的位置关系设置虚拟目标点，目标点包括实际目标点和虚拟目标点；

第2步，设置相机单元，调整第一相机的位置状态，使该位置状态对应的目标点与特征点处于第一相机的视野范围内；

第3步，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系：机器人正运动学单元通过第一千兆以太网获取当前六自由度工业机器人的状态信息，并确定机器人末端识别装置上的特征点在机器人空间中的三维坐标位置；通过采样获取特征点在相机空间中的二维坐标及在机器人空间中的三维坐标位置，利用变权重相机空间定位单元建立机器人空间中三维坐标到相机空间中二维坐标的映射关系；

第4步，获取目标点在三维机器人空间中的位置：根据第3步得到的映射关系，变权重相机空间定位单元将目标点在相机空间中的二维坐标转换为机器人空间中的三维坐标；

第5步，机器人逆运动学单元根据第4步所得的目标点三维坐标，确定相对应的六自由度工业机器人关节角度值并发送给工业机器人控制器，工业机器人控制器发送命令控制六自由度工业机器人向目标点运动；

第6步，判断六自由度工业机器人上设置的工具是否定位至目标点：

①如果未定位至目标点，则返回至第3步，将当前位置特征点在机器人空间中的三维空间坐标以及在相机空间中的二维坐标做为采样点重新估计映射参数，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，并按照第4～6步重新定位；

②如果定位至虚拟目标点，则返回至第2步，调整第一相机到下一位置状态，按照同样的方法将工具定位至下一目标点；

③如果定位至实际目标点，则完成工业机器人的三维实时定位。

一种工业机器人三维实时高精度定位装置的定位方法，其中相机单元为多相机单元，包括以下步骤：

第1步，选取两部第二相机，使机器人末端工装机构的特征点与实际目标点都处于第二相机视野范围内；

第2步，获取实际目标点，激光发射装置发出的激光投射到实际目标点，第二相机实时采集实际目标点的图像并传送至工业计算机中的图像处理单元，图像处理单元对采集到的图像进行处理得到实际目标点在相机空间中的二维坐标位置；

第3步，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系：机器人正运动学单元通过第一千兆以太网获取当前六自由度工业机器人的状态信息，并确定机器人末端识别装置上的特征点在机器人空间中的三维坐标位置；通过采样获取特征点在相机空间中的二维坐标及在机器人空间中的三维坐标位置，利用变权重相机空间定位单元建立机器人空间中三维坐标到相机空间中二维坐标的映射关系；

第4步，获取实际目标点在三维机器人空间中的位置：根据第3步得到的映射关系，变权重相机空间定位单元将目标点在相机空间中的二维坐标转换为机器人空间中的三维坐标；

第5步，机器人逆运动学单元根据第4步所得的实际目标点三维坐标，确定相对应的六自由度工业机器人关节角度值并发送给工业机器人控制器，工业机器人控制器发送命令控制六自由度工业机器人向目标点运动；

第6步，判断六自由度工业机器人上设置的工具是否定位至目标点：

①如果未定位至实际目标点，则返回至第3步，将当前位置特征点在机器人空间中的三维空间坐标以及在相机空间中的二维坐标做为采样点重新估计映射参数，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，并按照第4～6步重新定位；

②如果定位至实际目标点，则完成工业机器人的三维实时定位。

本发明与现有技术相比，其显著优点是：(1)在完成机器人伺服定位过程中无需对工业机器人和摄像机进行标定，操作简便，且消除了标定误差；(2)本系统采用了具有多自由度的主动相机单元，通过调节相机单元的位置和姿态可将机器人末端定位到视觉之外的目标点，扩大了定位视野范围；(3)基于3自由度的主动相机单元，可将目标点处于相机视野的中心区域，可以提机器人高定位精度；(4)本系统及方法对视觉采集处理无需很高的实时性，但是整个定位过程所需时间短、实时性高且定位精度高。

附图说明

图1是本发明工业机器人三维实时高精度定位装置的结构示意图。

图2是本发明工业机器人三维实时高精度定位装置中工装机构的结构示意图。

图3是本发明工业机器人三维实时高精度定位装置中主动相机单元示意图。

图4是本发明工业机器人三维实时高精度定位装置中多相机单元示意图。

图5是本发明工业机器人三维实时高精度定位装置中主动相机单元的横向平移视场变化图。

图6是本发明工业机器人三维实时高精度定位装置中主动相机单元的横向旋转视场变化图。

图7是本发明工业机器人三维实时高精度定位装置中主动相机单元的竖向偏转视场变化图。

图8是本发明工业机器人三维实时高精度定位装置中相机视场相交示意图。

图9是本发明工业机器人三维实时高精度定位方法的流程图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

一、结合图1～2，本发明工业机器人三维实时高精度定位装置，包括：工业机器人系统1、工业计算机2和相机单元3，其中工业计算机2包括变权重相机空间定位单元2-1、图像处理单元2-2和机器人运动控制单元2-3，机器人运动控制单元2-3包括机器人正运动学单元2-3-1和机器人逆运动学单元2-3-2两部分，其中图像处理单元2-2通过变权重相机空间定位单元2-1与机器人运动控制单元2-3相连；工业计算机2通过第一千兆以太网5与工业机器人控制器6相连接，工业机器人控制器6的控制端接入工业机器人系统1，工业计算机2通过第二千兆以太网4与相机单元3相连；

所述工业机器人系统1包括六自由度工业机器人1-0、激光发射装置1-1和工装机构1-2，其中工装机构1-2的初始端刚性固定在六自由度工业机器人1-0的末端，且与六自由度工业机器人1-0的前臂共轴；工装机构1-2的末端设置有特征点1-6和工具1-5，特征点1-6位于工装机构1-2顶端与其轴线成30°～60°夹角的上斜面，工具1-5刚性固定在工装机构1-2的下方；激光发射装置1-1设置在六自由度工业机器人1-0前臂，使激光发射装置1-1发射出的激光线投射到工作平台1-3上；

相机单元3将实时采集的图像输入工业计算机2中的图像处理单元2-2进行处理，并将处理结果发送给变权重相机空间定位单元2-1；机器人正运动学单元2-3-1将当前六自由度工业机器人1-0的状态信息进行处理后发送给变权重相机空间定位单元2-1；变权重相机空间定位单元2-1对接收到的信息进行综合处理后发送给机器人逆运动学单元2-3-2；机器人逆运动学单元2-3-2与工业机器人控制器6进行网络通信发送命令控制六自由度工业机器人1-0运动。

相机单元3为主动相机单元或者多相机单元，分别如下：

结合图3，所述相机单元3为主动相机单元时，包括第一相机3-2、云台本体3-1、多自由度电机运动单元3-3及电机驱动模块3-4，其中第一相机3-2固定在云台本体3-1上，多自由度电机运动单元3-3固定连接在云台本体3-1上，电机驱动模块3-4通过I/O口接入多自由度电机运动单元3-3；工业计算机2通过第二千兆以太网4与相机单元3中第一相机3-2相连，工业计算机2通过总线与相机单元3中电机驱动模块3-4进行通信，电机驱动模块3-4根据接收到的命令控制多自由度电机运动单元3-3运动从而带动云台本体3-1及第一相机3-2移动。所述云台本体3-1有1～6个自由度，且每个云台本体3-1上设置两个以上第一相机3-2。

结合图4，所述相机单元3为多相机单元，包括多个第二相机3-5以及与第二相机3-5一一对应的支架3-6，第二相机3-5固定设置在对应的支架3-6上，且每个第二相机3-5均通过第二千兆以太网4与工业计算机2相连。

结合图5～7，相机单元3为主动相机单元时，第一相机3-2的多个自由度运动，分别是图5横向平移、图6横向旋转、图7竖向旋转，通过图5可以看出，通过横向平移可以实现主动相机单元视场的远近调节，结合图8第一相机3-2视场相交示意图，视场中心点与云台本体3-1的距离D为：

其中，θ₁是第一个相机轴线z₁与云台本体3-1之间的夹角，θ₂是第二个相机轴线z₂与云台本体3-1之间的夹角，s是两相机之间的距离。由公式可以得出当相机轴线与云台本体3-1之间的角度固定的情况下，云台本体3-1与视觉空间的D随着s的增大而增大，通过图6可以看出，通过横向旋转可以实现主动相机单元视场的横向移动，通过图7可以看出，通过竖向旋转可以实现主动相机单元视场的竖向移动。

二、本发明工业机器人三维实时高精度定位方法，当机器人末端与实际目标点相距较远时，将机器人高精度定位到目标点，根据相机单元的选择有两种方法可以完成此任务：

(1)、第一种方法是利用上面提出的主动相机单元，通过对第一相机3-2进行移动从而使得相机视野覆盖整个工业机器人工作空间，完成机器人定位；第二种方法是利用多相机单元，将多个第二相机3-5分布在不同位置，使得任意两个第二相机3-5组合后的视野能够覆盖所需要的工业机器人工作空间，利用其中两个视野最好的第二相机3-5实现机器人三维定位；结合图9，本发明工业机器人三维实时高精度定位方法，相机单元为主动相机单元时，步骤如下：

第1步，第一相机3-2获取实际目标点1-4及特征点1-6，并根据特征点1-6与实际目标点1-4之间的位置关系设置虚拟目标点，目标点包括实际目标点1-4和虚拟目标点，实际目标点1-4获取方法如下：激光发射装置1-1发出的激光投射到实际目标点，第一相机3-2实时采集目标点的图像并传送至工业计算机2中的图像处理单元2-2，图像处理单元2-2对采集到的图像进行处理得到实际目标点在相机空间中的二维坐标位置；

在机器人视觉伺服定位到实际目标点1-4的过程中，需要保证机器人工装机构1-2上的特征点1-6与目标点始终在第一相机3-2的视觉范围内，且六自由度工业机器人1-0运动定位过程中第一相机3-2不能够移动；当处于初始状态时，如果实际目标点1-4与机器人末端距离较远不能够同时位于第一相机3-2的视觉范围内，则需要设置虚拟目标点，不断将机器人末端定位到虚拟目标点，同时改变主动相机单元的位置状态，直至将机器人末端定位到实际目标点。如果实际目标点1-4与机器人末端同时位于第一相机3-2的视觉范围内当同时，则直接将机器人末端定位到实际目标点上。基于相机空间工业机器人高精度定位的前提条件，尽量保持目标点处于相机空间的中心区域，所以主动相机单元3的移动显得尤为重要，设置虚拟目标点的具体步骤如下：

步骤1.1，当工装机构1-2的特征点1-6处于第一相机3-2的视野中心区域时，记录此时第一相机3-2的位置状态，即第一相机3-2的横向平移距离、横向旋转角度、竖向旋转角度；

步骤1.2，激光发射装置1-1发出激光投射到实际目标点1-4，通过移动第一相机3-2寻找实际目标点1-4，当实际目标点1-4处于第一相机3-2的视野中心区域时，记录此时第一相机3-2的位置状态；

步骤1.3，根据步骤1.1与步骤1.2中记录的第一相机3-2的位置状态差，对状态差进行等分，确定虚拟目标点的个数，以及定位过程中与各虚拟目标点对应的第一相机3-2位置状态。

第2步，设置主动相机单元，调整第一相机3-2的位置状态，使该位置状态对应的目标点与特征点1-6处于第一相机3-2的视野范围内，在将机器人定位到此目标点的过程中保持主动相机单元状态不发生改变；

第3步，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系：机器人正运动学单元2-3-1通过第一千兆以太网5获取当前六自由度工业机器人1-0的状态信息，并确定机器人末端工装机构1-2上的特征点1-6在机器人空间中的三维坐标位置；通过采样获取特征点1-6在相机空间中的二维坐标及在机器人空间中的三维坐标位置，利用变权重相机空间定位单元2-1建立机器人空间中三维坐标到相机空间中二维坐标的映射关系；所述建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，具体如下：

步骤3.1，利用变权重相机空间定位单元2-1建立特征点1-6在在机器人空间中三维坐标及相机空间中二维坐标的映射关系，映射关系如下：

其中E＝[E₁,E₂,E₃,E₄,E₅,E₆]是映射参数，P＝[P_x,P_y,P_z]是特征点1-6在机器人空间中的三维坐标，(x_c,y_c)是特征点1-6在相机空间中的二维坐标；

将(2)式简化为：

步骤3.2，确定映射参数E＝[E₁,E₂,E₃,E₄,E₅,E₆]：通过同时采样特征点1-6在相机空间中的二维坐标及在机器人空间中的三维坐标，用采样点进行估计，通过最小化J(E)得到映射参数E，公式如下：

其中是第i组采样特征点1-6在相机空间中的二维坐标，表示第i组采样特征点1-6在机器人空间中的三维坐标，ω_i为第i组采样特征点1-6的权重， 四种权重值分别为：

其中k是常系数且1＜k＜10，d_i是第i组采样特征点1-6在相机空间中的位置与目标点在相机空间中的位置之间的欧氏距离。

采用四种不同权重值分别进行工业机器人定位任务，得到定位时间以及定位精度如下表1所示，由结果可以看出权重可以明显提高定位的精度，同时定位时间也明显减少，特别是权重时，定位精度与定位时间最佳。

表1

步骤3.3，对采样点在相机空间中的坐标进行平化：由于从物理空间往相机空间映射过程中存在着透视效应的问题，即离镜头近的物体看起来比同样大小的离镜头远的物体要大，因此需要对模型做平化处理，对采样点在相机空间中的坐标进行平化处理的公式如下：

其中(X_c0,Y_c0)是指被平化过后的采样点在相机空间中的坐标，(X_c,Y_c)是指未被平化 时的采样点在相机空间中的坐标。Z_r是相机空间原点到采样点三维空间中坐标系原点之间的距离，Z_i可以通过下列公式获取：

Z_i＝Z_r+ΔZ_i (7)

ΔZ_i＝e₃₁P_x ⁱ+e₃₂P_y ⁱ+e₃₃P_z ⁱ (8)

其中(P_x ⁱ,P_y ⁱ,P_z ⁱ)第i组机器人特征点1-6在机器人空间中的三维坐标，使用以上公式(6)～(10)可以对采样特征点1-6在相机空间中的二维坐标进行一次平化，而通过平化的采样点，可以产生一组新的映射参数E，再一次进行平化，直至映射参数E不再发生变化，至此完成了映射参数E的估计，得到相机的空间运动学方程。

第4步，获取目标点在三维机器人空间中的位置：根据第3步的映射关系，变权重相机空间定位单元2-1将目标点在相机空间中的二维坐标转换为机器人空间中的三维坐标；

在实际应用中，主动相机单元要求至少两台第一相机3-2，相机空间的目标点采样可以采用激光点的方式在两个相机中进行采样，通过第3步得到的映射参数并结合目标点在相机空间中的坐标，可以通过以下公式(11)获得目标点在机器人空间中的三维坐标P＝[P_x,P_y,P_z]：

其中是目标点在第一个相机空间中的坐标，是目标点在第二个相机空间中的坐标，E₁是第一个相机的映射参数，E₂是第二个相机的映射参数。

第5步，机器人逆运动学单元2-3-2根据第4步所得的目标点三维坐标，确定相对应的六自由度工业机器人1-0关节角度值并发送给工业机器人控制器6，工业机器人控制器6发送命令控制六自由度工业机器人1-0向目标点运动；

第6步，判断六自由度工业机器人1-0上设置的工具1-5是否定位至目标点：

①如果未定位至目标点，则返回至第3步，将当前位置特征点1-6在机器人空间中的三维空间坐标以及在相机空间中的二维坐标做为采样点重新估计映射参数，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，并按照第4～6步重新定位；

②如果定位至虚拟目标点，则返回至第2步，顺序调整第一相机3-2到下一位置状态，按照同样的方法将工具1-5定位至下一目标点；

③如果定位至实际目标点1-4，则完成工业机器人的三维实时定位。

结合图4，相机单元3为多相机单元时，相机单元3由多个相机3-5与支架3-6组成，相机3-5固定连载在支架3-6上，支架3-6固定放置在工作平台1-3上方，相机通过千兆以太网4与工业计算机2相连；

(2)、第二种方法是相机单元为多相机单元时，相机个数的制定原则是：任何两个第二相机3-5的组合后的视场可以覆盖所需要的机器人工作空间。定位过程中存在两种情况：当机器人末端与实际目标点1-4距离较远时，首先选取一组视野范围宽广且能够覆盖机器人末端工装机构1-2上特征点1-6与实际目标点1-4的相机进行初步定位，然后重新选取视野范围较小但是能够覆盖特征点与目标点的一组相机进行高精度定位；当机器人末端与实际目标点1-4较近时，则直接将机器人定位到实际目标点1-4上，因此本发明工业机器人三维实时高精度定位装置的定位方法，当相机单元为多相机单元时，步骤如下：

第1步，选取两部相机，使机器人末端工装机构1-2的特征点1-6与实际目标点1-4都处于第二相机3-5视野范围内；

第2步，获取实际目标点1-4，激光发射装置1-1发出的激光投射到实际目标点1-4，第二相机3-5实时采集实际目标点1-4的图像并传送至工业计算机2中的图像处理单元2-2，图像处理单元2-2对采集到的图像进行处理得到实际目标点1-4在相机空间中的二维坐标位置；

第3步，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系：机器人正运动学单元2-3-1通过第一千兆以太网5获取当前六自由度工业机器人1-0的状态信息，并确定机器人末端工装机构1-2上的特征点1-6在机器人空间中的三维坐标位置；通过采样获取特征点1-6在相机空间中的二维坐标及在机器人空间中的三维坐标位置，利用变权重相机空间定位单元2-1建立机器人空间中三维坐标到相机空间中二维坐标的映射 关系；

第4步，获取实际目标点1-4在三维机器人空间中的位置：根据第3步得到的映射关系，变权重相机空间定位单元2-1将目标点在相机空间中的二维坐标转换为机器人空间中的三维坐标；

第5步，机器人逆运动学单元2-3-2根据第4步所得的实际目标点1-4三维坐标，确定相对应的六自由度工业机器人1-0关节角度值并发送给工业机器人控制器6，工业机器人控制器6发送命令控制六自由度工业机器人1-0向目标点运动；

第6步，判断六自由度工业机器人1-0上设置的工具1-5是否定位至目标点：

①如果未定位至实际目标点1-4，则返回至第3步，将当前位置特征点1-6在机器人空间中的三维空间坐标以及在相机空间中的二维坐标做为采样点重新估计映射参数，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，并按照第4～6步重新定位；

②如果定位至实际目标点1-4，则完成工业机器人的三维实时定位。

实施例1

采用基于主动相机单元的工业机器人三维实时高精度定位方法，在本发明装置下进行了21次重复工业机器人三维实时定位实验，定位实验结果如表2所示，通过表2可以看出本系统及方法具有很高的定位精度，平均误差为0.2989mm。

表2

综上所述，本发明工业机器人三维实时高精度定位装置及方法，在机器人定位过程中不需要对机器人以及相机进行标定，操作简便，且消除了标定误差；采用了具有3自由度的主动相机单元，与固定相机单元的定位系统相比，通过调节相机单元的位置和姿态，可实现目标点不在视觉范围内情况下的机器人定位任务；且具有定位精度高，实时性好的优点。

Claims (4)

1.一种工业机器人三维实时高精度定位装置的定位方法，其特征在于，所述工业机器人三维实时高精度定位装置，包括：工业机器人系统(1)、工业计算机(2)和相机单元(3)，其中工业计算机(2)包括变权重相机空间定位单元(2-1)、图像处理单元(2-2)和机器人运动控制单元(2-3)，机器人运动控制单元(2-3)包括机器人正运动学单元(2-3-1)和机器人逆运动学单元(2-3-2)两部分，其中图像处理单元(2-2)通过变权重相机空间定位单元(2-1)与机器人运动控制单元(2-3)相连；工业计算机(2)通过第一千兆以太网(5)与工业机器人控制器(6)相连接，工业机器人控制器(6)的控制端接入工业机器人系统(1)，工业计算机(2)通过第二千兆以太网(4)与相机单元(3)相连；所述工业机器人系统(1)包括六自由度工业机器人(1-0)、激光发射装置(1-1)和工装机构(1-2)，其中工装机构(1-2)的初始端刚性固定在六自由度工业机器人(1-0)的末端，且与六自由度工业机器人(1-0)的前臂共轴；工装机构(1-2)的末端设置有特征点(1-6)和工具(1-5)，特征点(1-6)位于工装机构(1-2)顶端与其轴线成30°～60°夹角的上斜面，工具(1-5)刚性固定在工装机构(1-2)的下方；激光发射装置(1-1)设置在六自由度工业机器人(1-0)前臂，使激光发射装置(1-1)发射出的激光线投射到工作平台(1-3)上；所述相机单元(3)为主动相机单元，包括第一相机(3-2)、云台本体(3-1)、多自由度电机运动单元(3-3)及电机驱动模块(3-4)，其中第一相机(3-2)固定在云台本体(3-1)上，多自由度电机运动单元(3-3)固定连接在云台本体(3-1)上，电机驱动模块(3-4)通过I/O口接入多自由度电机运动单元(3-3)；工业计算机(2)通过第二千兆以太网(4)与相机单元(3)中第一相机(3-2)相连，工业计算机(2)通过总线与相机单元(3)中电机驱动模块(3-4)进行通信，电机驱动模块(3-4)根据接收到的命令控制多自由度电机运动单元(3-3)运动从而带动云台本体(3-1)及第一相机(3-2)移动，所述定位方法包括以下步骤：

第1步，第一相机(3-2)获取实际目标点(1-4)及特征点(1-6)，并根据特征点(1-6)与实际目标点(1-4)之间的位置关系设置虚拟目标点，目标点包括实际目标点(1-4)和虚拟目标点；

第2步，设置相机单元(3)，调整第一相机(3-2)的位置状态，使该位置状态对应的目标点与特征点(1-6)处于第一相机(3-2)的视野范围内；

第3步，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系：机器人正运动学单元(2-3-1)通过第一千兆以太网(5)获取当前六自由度工业机器人(1-0)的状态信息，并确定机器人末端识别装置(1-2)上的特征点(1-6)在机器人空间中的三维坐标位置；通过采样获取特征点(1-6)在相机空间中的二维坐标及在机器人空间中的三维坐标位置，利用变权重相机空间定位单元(2-1)建立机器人空间中三维坐标到相机空间中二维坐标的映射关系；

第4步，获取目标点在三维机器人空间中的位置：根据第3步得到的映射关系，变权重相机空间定位单元(2-1)将目标点在相机空间中的二维坐标转换为机器人空间中的三维坐标；

第5步，机器人逆运动学单元(2-3-2)根据第4步所得的目标点三维坐标，确定相对应的六自由度工业机器人(1-0)关节角度值并发送给工业机器人控制器(6)，工业机器人控制器(6)发送命令控制六自由度工业机器人(1-0)向目标点运动；

第6步，判断六自由度工业机器人(1-0)上设置的工具(1-5)是否定位至目标点：

①如果未定位至目标点，则返回至第3步，将当前位置特征点(1-6)在机器人空间中的三维空间坐标以及在相机空间中的二维坐标做为采样点重新估计映射参数，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，并按照第4～6步重新定位；

②如果定位至虚拟目标点，则返回至第2步，调整第一相机(3-2)到下一位置状态，按照同样的方法将工具(1-5)定位至下一目标点；

③如果定位至实际目标点(1-4)，则完成工业机器人的三维实时定位。

2.根据权利要求1所述的工业机器人三维实时高精度定位方法，其特征在于，第1步中设置虚拟目标点的步骤如下：

步骤1.1，当工装机构(1-2)的特征点(1-6)处于第一相机(3-2)的视野中心区域时，记录此时第一相机(3-2)的位置状态，即第一相机(3-2)的横向平移距离、横向旋转角度、竖向旋转角度；

步骤1.2，激光发射装置(1-1)发出激光投射到实际目标点(1-4)，通过移动第一相机(3-2)寻找实际目标点(1-4)，当实际目标点(1-4)处于第一相机(3-2)的视野中心区域时，记录此时第一相机(3-2)的位置状态；

步骤1.3，根据步骤1.1与步骤1.2中记录的第一相机(3-2)的位置状态差，确定虚拟目标点的个数，以及定位过程中与各虚拟目标点对应的第一相机(3-2)位置状态。

3.根据权利要求1或2所述的工业机器人三维实时高精度定位方法，其特征在于，第3步中所述建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，具体如下：

步骤3.1，建立特征点(1-6)在在机器人空间中三维坐标及相机空间中二维坐标的映射关系，映射关系如下：

$\begin{array}{c}{X}_c=(E_1^2+E_2^2-E_3^2-E_4^2)P_x+2(E_2{E}_3+E_1{E}_4)P_y+2(E_2{E}_4-E_1{E}_3)P_z+E_5\\ Y_c=2(E_2{E}_3-E_1{E}_4)P_x+(E_1^2-E_2^2+E_3^2-E_4^2)P_y+2(E_3{E}_4+E_1{E}_2)P_z+E_6\end{array}---\left(1\right)$

其中E＝[E₁,E₂,E₃,E₄,E₅,E₆]是映射参数，P＝[P_x,P_y,P_z]是特征点(1-6)在机器人空间中的三维坐标，(X_c,Y_c)是特征点(1-6)在相机空间中的二维坐标；

将(1)式简化为：

$\begin{array}{c}{X}_c=X_c(P,E)\\ Y_c=Y_c(P,E)\end{array}---\left(2\right)$

步骤3.2，确定映射参数E＝[E₁,E₂,E₃,E₄,E₅,E₆]：变权重相机空间定位单元(2-1)通过同时采样特征点(1-6)在相机空间中的二维坐标及在机器人空间中的三维坐标，用采样点进行估计，通过最小化J(E)得到映射参数E，公式如下：

$J\left(E\right)={\mathrm{\Σ}}_i({\mathrm{\ω}}_i{\left(x_c^i-X_c\left(P^i,E\right)\right)}^2+{\mathrm{\ω}}_i{\left(y_c^i-Y_c\left(P^i,E\right)\right)}^2)---\left(3\right)$

其中是第i组采样特征点(1-6)在相机空间中的二维坐标，表示第i组采样特征点(1-6)在机器人空间中的三维坐标，ω_i为第i组采样特征点(1-6)的权重，四种权重值分别为：

$\left\{\begin{array}{c}{\mathrm{\ω}}_i^1=k/d_i\\ {\mathrm{\ω}}_i^2=k+1/d_i\\ {\mathrm{\ω}}_i^3=1/{\left(d_i\right)}^k\\ {\mathrm{\ω}}_i^4=k/{\left(d_i\right)}^k\end{array}\right.---\left(4\right)$

其中k是常系数且1＜k＜10，d_i是第i组采样特征点(1-6)在相机空间中的位置与目标点在相机空间中的位置之间的欧氏距离。

4.一种工业机器人三维实时高精度定位装置的定位方法，其特征在于，所述工业机器人三维实时高精度定位装置，包括：工业机器人系统(1)、工业计算机(2)和相机单元(3)，其中工业计算机(2)包括变权重相机空间定位单元(2-1)、图像处理单元(2-2)和机器人运动控制单元(2-3)，机器人运动控制单元(2-3)包括机器人正运动学单元(2-3-1)和机器人逆运动学单元(2-3-2)两部分，其中图像处理单元(2-2)通过变权重相机空间定位单元(2-1)与机器人运动控制单元(2-3)相连；工业计算机(2)通过第一千兆以太网(5)与工业机器人控制器(6)相连接，工业机器人控制器(6)的控制端接入工业机器人系统(1)，工业计算机(2)通过第二千兆以太网(4)与相机单元(3)相连；所述工业机器人系统(1)包括六自由度工业机器人(1-0)、激光发射装置(1-1)和工装机构(1-2)，其中工装机构(1-2)的初始端刚性固定在六自由度工业机器人(1-0)的末端，且与六自由度工业机器人(1-0)的前臂共轴；工装机构(1-2)的末端设置有特征点(1-6)和工具(1-5)，特征点(1-6)位于工装机构(1-2)顶端与其轴线成30°～60°夹角的上斜面，工具(1-5)刚性固定在工装机构(1-2)的下方；激光发射装置(1-1)设置在六自由度工业机器人(1-0)前臂，使激光发射装置(1-1)发射出的激光线投射到工作平台(1-3)上；所述相机单元(3)为多相机单元，包括多个第二相机(3-5)以及与第二相机(3-5)一一对应的支架(3-6)，第二相机(3-5)固定设置在对应的支架(3-6)上，且每个第二相机(3-5)均通过第二千兆以太网(4)与工业计算机(2)相连，所述定位方法包括以下步骤：

第1步，选取两部第二相机(3-5)，使机器人末端工装机构(1-2)的特征点(1-6)与实际目标点(1-4)都处于第二相机(3-5)视野范围内；

第2步，获取实际目标点(1-4)，激光发射装置(1-1)发出的激光投射到实际目标点(1-4)，第二相机(3-5)实时采集实际目标点(1-4)的图像并传送至工业计算机(2)中的图像处理单元(2-2)，图像处理单元(2-2)对采集到的图像进行处理得到实际目标点(1-4)在相机空间中的二维坐标位置；

第3步，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系：机器人正运动学单元(2-3-1)通过第一千兆以太网(5)获取当前六自由度工业机器人(1-0)的状态信息，并确定机器人末端识别装置(1-2)上的特征点(1-6)在机器人空间中的三维坐标位置；通过采样获取特征点(1-6)在相机空间中的二维坐标及在机器人空间中的三维坐标位置，利用变权重相机空间定位单元(2-1)建立机器人空间中三维坐标到相机空间中二维坐标的映射关系；

第4步，获取实际目标点(1-4)在三维机器人空间中的位置：根据第3步得到的映射关系，变权重相机空间定位单元(2-1)将目标点在相机空间中的二维坐标转换为机器人空间中的三维坐标；

第5步，机器人逆运动学单元(2-3-2)根据第4步所得的实际目标点(1-4)三维坐标，确定相对应的六自由度工业机器人(1-0)关节角度值并发送给工业机器人控制器(6)，工业机器人控制器(6)发送命令控制六自由度工业机器人(1-0)向目标点运动；

第6步，判断六自由度工业机器人(1-0)上设置的工具(1-5)是否定位至目标点：

①如果未定位至实际目标点(1-4)，则返回至第3步，将当前位置特征点(1-6)在机器人空间中的三维空间坐标以及在相机空间中的二维坐标做为采样点重新估计映射参数，建立机器人空间中三维坐标到相机空间中二维坐标的映射关系，并按照第4～6步重新定位；

②如果定位至实际目标点(1-4)，则完成工业机器人的三维实时定位。