CN113246145B - 核工业抓取设备的位姿补偿方法、***和电子装置 - Google Patents

Abstract

本申请涉及一种核工业抓取设备的位姿补偿方法、***、电子装置和存储介质。其中，该基于机器视觉的姿态补偿方法包括：通过抗辐射单目相机获取末端结构的第一图像；采用三维位姿估计模型处理第一图像，得到末端结构的预测三维包围盒；基于预测三维包围盒和真实三维包围盒，以及内参数，确定末端结构在抗辐射单目相机坐标系下的第一实际位姿；基于外参数和末端结构在抗辐射单目相机坐标系下的第一实际位姿，确定末端结构在全局坐标系下的第二实际位姿；基于核工业抓取设备的各电机转角，估计末端结构在全局坐标系下的第一估计位姿；根据第一估计位姿和第二实际位姿，对末端结构的位姿进行补偿。通过本申请，降低了姿态补偿设备的成本。

Description

核工业抓取设备的位姿补偿方法、***和电子装置

技术领域

本申请涉及工控领域，特别是涉及核工业抓取设备的位姿补偿方法、***、电子装置和存储介质。

背景技术

核工业高放射环境中机器人的动力均是电机通过全齿轮与关节进行传递，而该方式在电机通过全齿轮与关节进行传递之间时，会存在较大的误差，且核工业屏蔽室中常常充满辐射和酸雾，普通的电子设备无法在该环境中应用。

现有技术中的通常采用以下两种方式进行姿态补偿：一种是通过普通的视觉姿态补偿方式，而该方式需要结合机器人上传感器的数据反馈来实现闭环，而在高放射环境中，传感器会受到高放射的影响，容易导致采用传感器的视觉姿态补偿方式存在一定的补偿误差，另一种是采用双目相机来获得目标姿态，而一般的相机无法满足高放射环境的应用，而采用抗辐射的双目相机来实现视觉姿态补偿，由于抗辐射的双目相机其本身价格昂贵，因此采用抗辐射的双目相机来实现视觉姿态补偿会导致成本高的问题。

针对相关技术中存在姿态补偿设备的成本高的问题，目前还没有提出有效的解决方案。

发明内容

在本实施例中提供了一种基于机器视觉的姿态补偿方法、***、电子装置和存储介质，以解决相关技术中存在姿态补偿设备的成本高的问题。

第一个方面，在本实施例中提供了一种核工业抓取设备的位姿补偿方法，所述核工业抓取设备采用多个电机经全齿轮传动多级同心轴驱动，包括：

通过抗辐射单目相机获取核工业抓取设备的末端结构的第一图像，其中，所述抗辐射单目相机的内参数和外参数均已标定；

采用经所述末端结构的图像训练得到的三维位姿估计模型处理所述第一图像，得到所述末端结构在图像坐标系下的具有位姿信息的预测三维包围盒；

基于所述预测三维包围盒和预先测量的所述末端结构的真实三维包围盒，以及所述抗辐射单目相机的内参数，确定所述末端结构在所述抗辐射单目相机坐标系下的第一实际位姿；

基于所述抗辐射单目相机的外参数和所述末端结构在所述抗辐射单目相机坐标系下的第一实际位姿，确定所述末端结构在全局坐标系下的第二实际位姿；

基于所述核工业抓取设备的各电机转角，估计所述末端结构在全局坐标系下的第一估计位姿；

根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端结构的位姿进行补偿。

在其中的一些实施例中，根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端结构的位姿进行补偿包括：

确定所述第一估计位姿和所述第二实际位姿的第一位姿差异；

判断所述第一位姿差异是否大于第一预设位姿差异；

在判断到所述第一位姿差异大于所述第一预设位姿差异的情况下，对所述核工业抓取设备的末端结构的位姿进行补偿。

在其中的一些实施例中，所述方法还包括：

在判断到所述第一位姿差异不大于所述第一预设位姿差异的情况下，结束对所述核工业抓取设备的末端结构的位姿补偿。

在其中的一些实施例中，在根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端结构的位姿进行补偿之后，所述方法还包括：

在进行位姿补偿后，将所述核工业抓取设备运动到另一位姿，再次获取在所述另一位姿下的所述核工业抓取设备的第三实际位姿、第二估计位姿；

确定所述第二估计位姿和所述第三实际位姿的第二位姿差异；

判断所述第二位姿差异是否大于第二预设位姿差异；

在判断到所述第二位姿差异不大于所述第二预设位姿差异的情况下，结束对所述核工业抓取设备的末端结构的位姿补偿。

在其中的一些实施例中，在根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端结构的位姿进行补偿之后，所述方法还包括：

在进行位姿补偿后，将所述核工业抓取设备运动到特定位姿，并获取在所述特定位姿下的所述末端结构的第二图像以及所述特定位姿下的所述末端结构的模板图像；

匹配所述第二图像与所述模板图像；

在所述第二图像与所述模板图像的匹配度大于预设匹配度的情况下，结束对所述核工业抓取设备的末端结构的位姿补偿。

在其中的一些实施例中，所述三维位姿估计模型的训练包括：

获取初始YOLO-6D模型和训练样本集，其中，所述训练样本集包括：包括末端结构位姿的图像、不包括末端关节位姿的图像；

以所述不包括末端关节位姿的图像作为输入，以所述包括末端结构位姿的图像作为监督，训练所述初始YOLO-6D模型，直至所述初始YOLO-6D模型参数收敛，得到所述三维位姿估计模型。

在其中的一些实施例中，基于所述预测三维包围盒和预先测量的所述末端结构的真实三维包围盒，以及所述抗辐射单目相机的内参数，确定所述末端结构在所述抗辐射单目相机坐标系下的第一实际位姿包括：

调用预设OpenCV数据库中的SlovePnP算法模型；

将所述SlovePnP算法模型切换成EPnP计算方式；

将所述预测三维包围盒和预先测量的所述末端结构的真实三维包围盒，以及所述抗辐射单目相机的内参数输入到切换成所述EPnP计算方式的所述SlovePnP算法模型中，得到所述末端结构在所述抗辐射单目相机坐标系下的第一实际位姿。

第二个方面，本申请提供了一种核工业抓取设备的位姿补偿***，在其中的一些实施例中，包括：核工业抓取设备、抗辐射单目相机和计算设备，

所述抗辐射单目相机用于获取核工业抓取设备的末端结构的第一图像，其中，所述抗辐射单目相机的内参数和外参数均已标定；

所述计算设备用于采用经所述末端结构的图像训练得到的三维位姿估计模型处理所述第一图像，得到所述末端结构在图像坐标系下的具有位姿信息的预测三维包围盒；

所述计算设备用于基于所述预测三维包围盒和预先测量的所述末端结构的真实三维包围盒，以及所述抗辐射单目相机的内参数，确定所述末端结构在所述抗辐射单目相机坐标系下的第一实际位姿；

所述计算设备用于基于所述抗辐射单目相机的外参数和所述末端结构在所述抗辐射单目相机坐标系下的第一实际位姿，确定所述末端结构在全局坐标系下的第二实际位姿；

所述计算设备用于基于所述核工业抓取设备的各电机转角，估计所述末端结构在全局坐标系下的第一估计位姿；

所述计算设备用于根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端结构的位姿进行补偿。

第三个方面，在本实施例中提供了一种电子装置，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一个方面所述的核工业抓取设备的位姿补偿方法。

第四个方面，在本实施例中提供了一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述第一个方面所述的核工业抓取设备的位姿补偿方法。

与相关技术相比，在本实施例中提供的核工业抓取设备的位姿补偿方法、***、电子装置和存储介质，通过抗辐射单目相机获取核工业抓取设备的末端结构的第一图像，其中，抗辐射单目相机的内参数和外参数均已标定；采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒；基于预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机的内参数，确定末端结构在抗辐射单目相机坐标系下的第一实际位姿；基于抗辐射单目相机的外参数和末端结构在抗辐射单目相机坐标系下的第一实际位姿，确定末端结构在全局坐标系下的第二实际位姿；基于核工业抓取设备的各电机转角，估计末端结构在全局坐标系下的第一估计位姿；根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿的方式，解决了相关技术中姿态补偿设备的成本高的问题，降低了姿态补偿设备的成本。

本申请的一个或多个实施例的细节在以下附图和描述中提出，以使本申请的其他特征、目的和优点更加简明易懂。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是本实施例的核工业抓取设备的位姿补偿的终端的硬件结构框图；

图2是本实施例的核工业抓取设备的位姿补偿方法的流程图；

图3是本优选实施例的核工业抓取设备的位姿补偿方法的流程图；

图4是本实施例的核工业抓取设备的位姿补偿***的结构框图。

具体实施方式

为更清楚地理解本申请的目的、技术方案和优点，下面结合附图和实施例，对本申请进行了描述和说明。

除另作定义外，本申请所涉及的技术术语或者科学术语应具有本申请所属技术领域具备一般技能的人所理解的一般含义。在本申请中的“一”、“一个”、“一种”、“该”、“这些”等类似的词并不表示数量上的限制，它们可以是单数或者复数。在本申请中所涉及的术语“包括”、“包含”、“具有”及其任何变体，其目的是涵盖不排他的包含；例如，包含一系列步骤或模块（单元）的过程、方法和***、产品或设备并未限定于列出的步骤或模块（单元），而可包括未列出的步骤或模块（单元），或者可包括这些过程、方法、产品或设备固有的其他步骤或模块（单元）。在本申请中所涉及的“连接”、“相连”、“耦接”等类似的词语并不限定于物理的或机械连接，而可以包括电气连接，无论是直接连接还是间接连接。在本申请中所涉及的“多个”是指两个或两个以上。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。通常情况下，字符“/”表示前后关联的对象是一种“或”的关系。在本申请中所涉及的术语“第一”、“第二”、“第三”等，只是对相似对象进行区分，并不代表针对对象的特定排序。

在本实施例中提供的方法实施例可以在终端、计算机或者类似的运算装置中执行。比如在终端上运行，图1是本实施例的核工业抓取设备的位姿补偿方法的终端的硬件结构框图。如图1所示，终端可以包括一个或多个（图1中仅示出一个）处理器102和用于存储数据的存储器104，其中，处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置。上述终端还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述终端的结构造成限制。例如，终端还可包括比图1中所示更多或者更少的组件，或者具有与图1所示出的不同配置。

存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如在本实施例中的核工业抓取设备的位姿补偿方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至终端。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输设备106用于经由一个网络接收或者发送数据。上述的网络包括终端的通信供应商提供的无线网络。在一个实例中，传输设备106包括一个网络适配器（NetworkInterface Controller，简称为NIC），其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输设备106可以为射频（Radio Frequency，简称为RF）模块，其用于通过无线方式与互联网进行通讯。

在本实施例中提供了一种核工业抓取设备的位姿补偿方法，其中，核工业抓取设备采用多个电机经全齿轮传动多级同心轴驱动，图2是本实施例的核工业抓取设备的位姿补偿方法的流程图，如图2所示，该流程包括如下步骤：

步骤S201，通过抗辐射单目相机获取核工业抓取设备的末端结构的第一图像，其中，抗辐射单目相机的内参数和外参数均已标定。

在本步骤中，抗辐射单目相机可以是但不限于CID相机，末端结构可以是但不限于核工业中的机械手的末端关节。

需要说明的是，抗辐射单目相机的内参数和外参数都可以是预先标定好的。

在本实施例中，外参数可以包括相机在真实世界中的位姿。

步骤S202，采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒。

在本步骤中，该三维位姿估计模型是预先采用末端结构的图像训练得到的，该训练得到的三维位姿估计模型具有一定准确度的预测三维包围盒的获取能力，通过该三维位姿模型得到的预测三维包围盒，在一定程度上可以提高预测三维包围盒的精确度。

需要说明的是，三维包围盒是末端结构对应于抗辐射单目相机坐标系下的3D投影坐标，可以包括中心点坐标和8个角点坐标。

步骤S203，基于预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机的内参数，确定末端结构在抗辐射单目相机坐标系下的第一实际位姿。

在本步骤中，核工业抓取设备在不同的位姿均有与之对应的真实三维包围盒，通过上述方式，实现了末端结构相对应抗辐射单目相机的相对位姿，即第一实际位姿。

步骤S204，基于抗辐射单目相机的外参数和末端结构在抗辐射单目相机坐标系下的第一实际位姿，确定末端结构在全局坐标系下的第二实际位姿。

在本步骤中，根据外参数以及第一实际位姿，可以将末端结构的第一实际位姿转换成全局坐标系下的第二实际位姿。

需要说明的是，全局坐标系是三维空间物体所在的坐标系，模型的顶点坐标就是基于这个坐标系来表达的。

步骤S205，基于核工业抓取设备的各电机转角，估计末端结构在全局坐标系下的第一估计位姿。

在本步骤中，电机的电机转角均有对应的末端结构在全局坐标系下的估计位姿，这个估计位姿可以根据齿轮耦合算法计算得到。

需要说明的是，本实施例中，基于电机的电机转角求解末端结构的位姿可以通过齿轮耦合算法来实现，基于末端结构的位姿求解电机转角可以基于逆解解析算法来实现。

步骤S206，根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿。

基于上述步骤S201至S206，通过采用抗辐射单目相机获取核工业抓取设备的末端结构的第一图像，并基于第一图像获取到的第二实际位姿，以及第一估计位姿实现了对核工业抓取设备末端结构的位姿补偿，解决了相关技术中姿态补偿设备的成本高的问题，降低了姿态补偿设备的成本。

在其中的一些实施例中，根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿包括：确定第一估计位姿和第二实际位姿的第一位姿差异；判断第一位姿差异是否大于第一预设位姿差异；在判断到第一位姿差异大于第一预设位姿差异的情况下，对核工业抓取设备的末端结构的位姿进行补偿。

在本实施例中，通过在判断到第一位姿差异大于第一预设位姿差异的情况下，对核工业抓取设备的末端结构的位姿进行补偿的方式，实现了对末端结构的姿态补偿，避免了末端结构的姿态偏差过大问题。

在其中的一些实施例中，还可以在判断到第一位姿差异不大于第一预设位姿差异的情况下，结束对核工业抓取设备的末端结构的位姿补偿。

在本实施例中，采用对不大于第一预设位姿差异的末端结构进行结束补偿流程的方式，节省了位姿补偿的流程。

在其中的一些实施例中，在根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿之后，还可以在进行位姿补偿后，将核工业抓取设备运动到另一位姿，再次获取在另一位姿下的核工业抓取设备的第三实际位姿、第二估计位姿；确定第二估计位姿和第三实际位姿的第二位姿差异；判断第二位姿差异是否大于第二预设位姿差异；在判断到第二位姿差异不大于第二预设位姿差异的情况下，结束对核工业抓取设备的末端结构的位姿补偿。

在本实施例中，通过将核工业抓取设备运动到另一位姿，并基于另一位姿来对核工业抓取设备的末端结构的位姿补偿进行进一步的验证的方式，提高了位姿补偿的准确性。

在其中的一些实施例中，在根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿之后，还可以在进行位姿补偿后，将核工业抓取设备运动到特定位姿，并获取在特定位姿下的末端结构的第二图像以及特定位姿下的末端结构的模板图像；匹配第二图像与模板图像；在第二图像与模板图像的匹配度大于预设匹配度的情况下，结束对核工业抓取设备的末端结构的位姿补偿。

在本步骤中，特定位姿可以是用户预先设定好的，且在该特定位姿有对应的模板图像，通过将核工业抓取设备运动特定位姿并获取第二图像，以及在第二图像与模板图像的匹配度大于预设匹配度的情况下，结束对核工业抓取设备的末端结构的位姿补偿的方式，实现了核工业抓取设备的末端结构的位姿补偿的进一步的验证；且通过图像匹配的方式，可以无需再次获取位姿，节省了末端结构的位姿补偿的验证流程。

在其中的一些实施例中，三维位姿估计模型的训练包括：获取初始YOLO-6D模型和训练样本集，其中，训练样本集包括：包括末端结构位姿的图像、不包括末端关节位姿的图像；以不包括末端关节位姿的图像作为输入，以包括末端结构位姿的图像作为监督，训练初始YOLO-6D模型，直至初始YOLO-6D模型参数收敛，得到三维位姿估计模型。

需要说明的是，YOLO-6D算法是一种从而图像中获取到三维位姿的一种算法。

在本实施例中，通过以不包括末端关节位姿的图像作为输入，以包括末端结构位姿的图像作为监督，训练初始YOLO-6D模型，直至初始YOLO-6D模型参数收敛，得到三维位姿估计模型的方式，实现了对三维位姿估计模型的训练，同时通过参数收敛的三维位姿估计模型来预测三维包围盒，可以提高获取到的三维包围盒的精确度。

在其中的一些实施例中，基于预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机的内参数，确定末端结构在抗辐射单目相机坐标系下的第一实际位姿包括：调用预设OpenCV数据库中的SlovePnP算法模型；将SlovePnP算法模型切换成EPnP计算方式；将预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机的内参数输入到切换成EPnP计算方式的SlovePnP算法模型中，得到末端结构在抗辐射单目相机坐标系下的第一实际位姿。

需要说明的是，OpenCV是一个基于BSD许可（开源）发行的跨平台计算机视觉和机器学习软件库，可以运行在Linux、Windows、Android和Mac OS操作***上。

在本实施例中，PnP算法的位姿检测的方式可以是包括：

（1）通过抗辐射单目相机获取第一图像。

（2）标定抗辐射单目相机的内参数。

（3）基于YOLO-6D算法训练得到的三维位姿估计模型获取末端关节在第一图像中的预测三维包围盒。

（4）选取预测三维包围盒中的中心点以及八个角点中的任意四个特征点，分别得到每个特征点在成像平面坐标系中的坐标{qi（xi，yi，f），i=1，2，3，4}和特征点之间的距离。

（5）可以在四个特征点所在的两条直线平行的情况下，定义四个特征点相关的距离因子{Qm，m=1，2，3，4}；也可以在四个特征点所在的两条直线相交的情况下，计算出该相交的交点在世界坐标系中的坐标及其在第一图像的成像平面坐标系中的坐标。

（6）根据相交的交点在世界坐标系中的坐标及其在第一图像的成像平面坐标系中的坐标，分别计算出世界坐标系中Xw轴、Yw轴和Zw轴上的向量变换到抗辐射单目相机坐标系中的单位向量。

（7）基于世界坐标系中Xw轴、Yw轴和Zw轴上的向量变换到抗辐射单目相机坐标系中的单位向量，以及基于RT转换矩阵求解出末端关节相对于抗辐射单目相机的初始姿态；根据世界坐标系的原点在抗辐射单目相机中的坐标求解出末端关节相对于抗辐射单目相机的初始位置。

（8）根据末端关节相对于抗辐射单目相机的初始姿态和初始位置，求解末端关节的实际姿态。

由于在核辐射环境下，摄像设备直接获取的图像信号带有较强的污染噪声，为了抑制噪声，获取更加真实的物体信息，在其中的一些实施例中，在采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒之前，还可以对第一图像进行预处理，其中，预处理包括：图像降噪处理。

在本实施例中，通过对第一图像进行图像降噪处理的方式，可以去除第一图像的噪声，以使得获取到的第一实际位姿更精确，进而避免补偿位姿的误差较大的问题。

为了避免导致输出位姿的取值范围差别很大，在其中的一些实施例中，在采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒之前，还可以对第一图像进行预处理，其中，预处理包括：归一化处理。

在本实施例中，通过对图像进行归一化处理的方式，使得根据第一图像中的预测三维包围盒确定末端关节的第一实际位姿的取值范围在0-1之间，避免了输出范围差别很大，而导致后续基于预测三维包围盒获取到的第一实际位姿存在误差较大的问题。

在一些实施例中，从二维图像获取三维信息，可能会存在一定的误差性。由于离线示教过程的确定性，还可以对第一图像建立匹配特征，通过图像分割、形态学处理、特征区域提取、边缘检测等方式，来减少从图像中获取到的三维信息的误差。

下面通过优选实施例对本实施例进行描述和说明。

图3是本优选实施例的基于机器视觉的姿态补偿方法的流程图，如图3所示，该基于机器视觉的姿态补偿方法包括如下步骤：

步骤S301，通过抗辐射单目相机获取核工业抓取设备的末端结构的第一图像，其中，抗辐射单目相机的内参数和外参数均已标定。

步骤S302，采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒。

步骤S303，基于预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机的内参数，确定末端结构在抗辐射单目相机坐标系下的第一实际位姿。

步骤S304，基于抗辐射单目相机的外参数和末端结构在抗辐射单目相机坐标系下的第一实际位姿，确定末端结构在全局坐标系下的第二实际位姿。

步骤S305，基于核工业抓取设备的各电机转角，估计末端结构在全局坐标系下的第一估计位姿。

步骤S306，根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿。

步骤S307，在进行位姿补偿后，将核工业抓取设备运动到特定位姿，并获取在特定位姿下的末端结构的第二图像以及特定位姿下的末端结构的模板图像。

步骤S308，匹配第二图像与模板图像。

步骤S309，在第二图像与模板图像的匹配度大于预设匹配度的情况下，结束对核工业抓取设备的末端结构的位姿补偿。

通过上述步骤，实现了核工业高放射环境中离线示教过程中的精确姿态补偿，解决了相关技术中姿态补偿设备的成本高的问题，降低了姿态补偿设备的成本，且该补偿过程不受无法使用传感器的影响。

在本实施例中还提供了一种核工业抓取设备的位姿补偿***，该***用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。以下所使用的术语“模块”、“单元”、“子单元”等可以实现预定功能的软件和/或硬件的组合。尽管在以下实施例中所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图4是本实施例的核工业抓取设备的位姿补偿***的结构框图，如图4所示，该装置包括：核工业抓取设备41、抗辐射单目相机42和计算设备43，

抗辐射单目相机42用于获取核工业抓取42设备的末端结构的第一图像，其中，抗辐射单目相机42的内参数和外参数均已标定；

计算设备41用于采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒；

计算设备41用于基于预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机42的内参数，确定末端结构在抗辐射单目相机42坐标系下的第一实际位姿；

计算设备41用于基于抗辐射单目相机42的外参数和末端结构在抗辐射单目相机42坐标系下的第一实际位姿，确定末端结构在全局坐标系下的第二实际位姿；

计算设备41用于基于核工业抓取设备42的各电机转角，估计末端结构在全局坐标系下的第一估计位姿；

计算设备41用于根据第一估计位姿和第二实际位姿，对核工业抓取设备42的末端结构的位姿进行补偿。

在一些实施例中，计算设备41还可以用于实现上述实施例中的核工业抓取设备的位姿补偿方法。

需要说明的是，上述各个模块可以是功能模块也可以是程序模块，既可以通过软件来实现，也可以通过硬件来实现。对于通过硬件来实现的模块而言，上述各个模块可以位于同一处理器中；或者上述各个模块还可以按照任意组合的形式分别位于不同的处理器中。

在本实施例中还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。

可选地，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。

可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：

步骤S201，通过抗辐射单目相机获取核工业抓取设备的末端结构的第一图像，其中，抗辐射单目相机的内参数和外参数均已标定。

步骤S202，采用经末端结构的图像训练得到的三维位姿估计模型处理第一图像，得到末端结构在图像坐标系下的具有位姿信息的预测三维包围盒。

步骤S203，基于预测三维包围盒和预先测量的末端结构的真实三维包围盒，以及抗辐射单目相机的内参数，确定末端结构在抗辐射单目相机坐标系下的第一实际位姿。

步骤S204，基于抗辐射单目相机的外参数和末端结构在抗辐射单目相机坐标系下的第一实际位姿，确定末端结构在全局坐标系下的第二实际位姿。

步骤S205，基于核工业抓取设备的各电机转角，估计末端结构在全局坐标系下的第一估计位姿。

步骤S206，根据第一估计位姿和第二实际位姿，对核工业抓取设备的末端结构的位姿进行补偿。

需要说明的是，在本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，在本实施例中不再赘述。

此外，结合上述实施例中提供的核工业抓取设备的位姿补偿方法，在本实施例中还可以提供一种存储介质来实现。该存储介质上存储有计算机程序；该计算机程序被处理器执行时实现上述实施例中的任意一种核工业抓取设备的位姿补偿方法。

应该明白的是，这里描述的具体实施例只是用来解释这个应用，而不是用来对它进行限定。根据本申请提供的实施例，本领域普通技术人员在不进行创造性劳动的情况下得到的所有其它实施例，均属本申请保护范围。

显然，附图只是本申请的一些例子或实施例，对本领域的普通技术人员来说，也可以根据这些附图将本申请适用于其他类似情况，但无需付出创造性劳动。另外，可以理解的是，尽管在此开发过程中所做的工作可能是复杂和漫长的，但是，对于本领域的普通技术人员来说，根据本申请披露的技术内容进行的某些设计、制造或生产等更改仅是常规的技术手段，不应被视为本申请公开的内容不足。

“实施例”一词在本申请中指的是结合实施例描述的具体特征、结构或特性可以包括在本申请的至少一个实施例中。该短语出现在说明书中的各个位置并不一定意味着相同的实施例，也不意味着与其它实施例相互排斥而具有独立性或可供选择。本领域的普通技术人员能够清楚或隐含地理解的是，本申请中描述的实施例在没有冲突的情况下，可以与其它实施例结合。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对专利保护范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。

Claims (8)

1.一种核工业抓取设备的位姿补偿方法，所述核工业抓取设备采用多个电机经全齿轮传动多级同心轴驱动，其特征在于包括：

通过抗辐射单目相机获取核工业抓取设备的末端关节的第一图像，其中，所述抗辐射单目相机的内参数和外参数均已标定；

采用经所述末端关节的图像训练得到的三维位姿估计模型处理所述第一图像，得到所述末端关节在图像坐标系下的具有位姿信息的预测三维包围盒；

基于所述预测三维包围盒和预先测量的所述末端关节的真实三维包围盒，以及所述抗辐射单目相机的内参数，确定所述末端关节在抗辐射单目相机坐标系下的第一实际位姿；

基于所述抗辐射单目相机的外参数和所述末端关节在所述抗辐射单目相机坐标系下的第一实际位姿，确定所述末端关节在全局坐标系下的第二实际位姿；

基于所述核工业抓取设备的各电机转角，估计所述末端关节在全局坐标系下的第一估计位姿；

根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端关节的位姿进行补偿；

其中，基于所述预测三维包围盒和预先测量的所述末端关节的真实三维包围盒，以及所述抗辐射单目相机的内参数，确定所述末端关节在抗辐射单目相机坐标系下的第一实际位姿包括：

调用预设OpenCV数据库中的SlovePnP算法模型；

将所述SlovePnP算法模型切换成EPnP计算方式；

将所述预测三维包围盒和预先测量的所述末端关节的真实三维包围盒，以及所述抗辐射单目相机的内参数输入到切换成所述EPnP计算方式的所述SlovePnP算法模型中，得到所述末端关节在所述抗辐射单目相机坐标系下的第一实际位姿；

其中，根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端关节的位姿进行补偿包括：

确定所述第一估计位姿和所述第二实际位姿的第一位姿差异；

判断所述第一位姿差异是否大于第一预设位姿差异；

在判断到所述第一位姿差异大于所述第一预设位姿差异的情况下，对所述核工业抓取设备的末端关节的位姿进行补偿。

2.根据权利要求1所述的核工业抓取设备的位姿补偿方法，其特征在于，所述方法还包括：

在判断到所述第一位姿差异不大于所述第一预设位姿差异的情况下，结束对所述核工业抓取设备的末端关节的位姿补偿。

3.根据权利要求1所述的核工业抓取设备的位姿补偿方法，其特征在于，在根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端关节的位姿进行补偿之后，所述方法还包括：

在进行位姿补偿后，将所述核工业抓取设备运动到另一位姿，再次获取在所述另一位姿下的所述核工业抓取设备的第三实际位姿、第二估计位姿；

确定所述第二估计位姿和所述第三实际位姿的第二位姿差异；

判断所述第二位姿差异是否大于第二预设位姿差异；

在判断到所述第二位姿差异不大于所述第二预设位姿差异的情况下，结束对所述核工业抓取设备的末端关节的位姿补偿。

4.根据权利要求1所述的核工业抓取设备的位姿补偿方法，其特征在于，在根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端关节的位姿进行补偿之后，所述方法还包括：

在进行位姿补偿后，将所述核工业抓取设备运动到特定位姿，并获取在所述特定位姿下的所述末端关节的第二图像以及所述特定位姿下的所述末端关节的模板图像；

匹配所述第二图像与所述模板图像；

在所述第二图像与所述模板图像的匹配度大于预设匹配度的情况下，结束对所述核工业抓取设备的末端关节的位姿补偿。

5.根据权利要求1所述的核工业抓取设备的位姿补偿方法，其特征在于，所述三维位姿估计模型的训练包括：

获取初始YOLO-6D模型和训练样本集，其中，所述训练样本集包括：包括末端关节位姿的图像、不包括末端关节位姿的图像；

以所述不包括末端关节位姿的图像作为输入，以所述包括末端关节位姿的图像作为监督，训练所述初始YOLO-6D模型，直至所述初始YOLO-6D模型参数收敛，得到所述三维位姿估计模型。

6.一种核工业抓取设备的位姿补偿***，其特征在于，包括：核工业抓取设备、抗辐射单目相机和计算设备，

所述抗辐射单目相机用于获取核工业抓取设备的末端关节的第一图像，其中，所述抗辐射单目相机的内参数和外参数均已标定；

所述计算设备用于采用经所述末端关节的图像训练得到的三维位姿估计模型处理所述第一图像，得到所述末端关节在图像坐标系下的具有位姿信息的预测三维包围盒；

所述计算设备用于基于所述预测三维包围盒和预先测量的所述末端关节的真实三维包围盒，以及所述抗辐射单目相机的内参数，确定所述末端关节在抗辐射单目相机坐标系下的第一实际位姿；

所述计算设备用于基于所述抗辐射单目相机的外参数和所述末端关节在所述抗辐射单目相机坐标系下的第一实际位姿，确定所述末端关节在全局坐标系下的第二实际位姿；

所述计算设备用于基于所述核工业抓取设备的各电机转角，估计所述末端关节在全局坐标系下的第一估计位姿；

所述计算设备用于根据所述第一估计位姿和所述第二实际位姿，对所述核工业抓取设备的末端关节的位姿进行补偿；

所述计算设备用于执行权利要求1至5中任一项所述的核工业抓取设备的位姿补偿方法。

7.一种电子装置，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行权利要求1至5中任一项所述的核工业抓取设备的位姿补偿方法。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述的核工业抓取设备的位姿补偿方法的步骤。

Images