CN110009689A - 一种用于协作机器人位姿估计的图像数据集快速构建方法 - Google Patents

Abstract

本发明公开了一种用于协作机器人位姿估计的图像数据集快速构建方法，包含步骤：在机器人基座与相机相对固定的情况下，保证机器人的运动范围处于相机的视野范围内，将标定板与机器人的末端固定连接，保证标定板的坐标系的唯一性，同时保存每张标定板的图片对应的机器人的末端的工具中心点位姿数据；根据所得到的多组标定板的图片与对应的机器人的末端的工具中心点位姿数据，设计标定算法以求解机器人的基座坐标系到相机的坐标系的第一变换矩阵及相机内参，根据机器人的DH参数及构型完成机器人的关键点图像像素点的映射，并进一步生成相关的数据信息。该种方法具有自动化程度高、速度快、准确率高、信息丰富等优势，保证了后续工作的可靠性。

Description

一种用于协作机器人位姿估计的图像数据集快速构建方法

技术领域

本发明涉及机器人领域，尤其涉及一种用于协作机器人位姿估计的图像数据集快速构建方法。

背景技术

近年来，随着深度学习等新型机器学习算法在图像领域取得重大突破，如何将深度学习等新型算法迁移到机器人领域也逐渐成为该领域的研究热点，目前机器人领域结合深度学习算法最为成功的案例是Bin-Picking，即散乱物体的位姿识别与抓取，其中使用到的通常是二维图像或三维点云的数据集，虽然在其他任务中诸如机器人避障运动规划中不一定使用图像等视觉信息，并取得了一定的进展，但通常都是在实验环境或仿真环境下实现，难以应用于实际生产工作中，因此，采用视觉信息构建数据集是深度学习算法的主流方向。

深度学习等机器学习算法往往要求大规模的数据集才能取得较为理想的结果，如果使用图像领域中的手工标注数据集的方法过于费时费力，难以短时间构建规模较大的数据集。机器人领域使用图像信息作为数据集的优势在于，机器人可以使用本体的DH等关键参数进行精确建模，获得机器人本身任意位置的详细信息，同时结合张正友提出的一种简单的相机标定方法，通过拍摄多张标定板图像，经过图像处理，可以获得精确的相机内外参数，再结合机器人末端位姿数据，可以标定出机器人基座和相机坐标系的变换关系，由此完成机器人基座坐标系到相机坐标系再到图像像素坐标系的精确映射。

协作机器人是近年来机器人领域的热点，为了保证它的本质安全性，它在设计、结构与传感器的使用上区别于传统工业机器人，由此可以实现同一工作区中的人机协作任务。但是现阶段由于机器人内部传感器数据的单一性，例如往往只能根据检测所受的外力矩大小来判断是否碰撞，难以保证整个任务的安全性，因此在真正的人机协作场景中需要加入外部传感器结合安全检测算法来实时监测协作机器人的位姿，作为整个***安全的另一层保障。

因此，本领域的技术人员致力于开发一种用于协作机器人位姿估计的图像数据集快速构建方法，该种方法相对于图像领域的手工标注方法具有速度快、准确率高、信息丰富等优势，对使用的协作机器人种类没有特定要求，可以快速应用于不同的协作机器人场景。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是如何解决使用机器学习算法进行协作机器人位姿估计的数据集采集问题，可以大幅度的提高数据集采集效率，并确保数据集的准确性和稳定性。

为实现上述目的，本发明提供了一种用于协作机器人位姿估计的图像数据集快速构建方法，包含如下步骤：

步骤1：在机器人基座与相机相对固定的情况下，保证所述机器人的运动范围处于所述相机的视野范围内，将标定板与所述机器人的末端固定连接，保证所述标定板的坐标系的唯一性，同时保存每张所述标定板的图片对应的所述机器人的末端的工具中心点位姿数据；

步骤2：根据步骤1中所得到的多组所述标定板的图片与对应的所述机器人的末端的工具中心点位姿数据，基于张正友棋盘格标定法标定相机内外参数，设计标定算法以求解所述机器人的基座坐标系到所述相机的坐标系的第一变换矩阵；

步骤3：根据所述机器人的关节-轴参数及构型解算出所述机器人的关键点在机器人基座坐标系下的位置，同时结合步骤2中得到的所述第一变换矩阵与所述相机内参矩阵，完成所述机器人的关键点在图像像素点的映射。

进一步地，步骤1中的所述标定板的两边棋盘格数目分别为奇数和偶数，以保证棋盘格坐标系原点的不变性和采集过程中标定板坐标系的稳定性。

进一步地，步骤1中的机器人的所述工具中心点位姿被设置为在六个自由度上均发生变化，以获得多组所述标定板的图片与对应的所述机器人的末端的工具中心点位姿数据。

进一步地，获取与多组所述标定板的图片对应的工具中心点位姿数据包括步骤：

步骤1.1：记机器人末端工具中心点初始位姿表示为(x,y,z,rx,ry,rz)，其中x,y,z是工具中心点初始坐标数据，rx,ry,rz为工具中心点的旋转向量表示；

步骤1.2：将步骤1.1的所述工具中心点位姿转换为旋转矩阵形式，如式(1)：

式(1)中,θ＝norm(r)，I为对角阵；

步骤1.3：将步骤1.2中所得的旋转矩阵，转换为齐次矩阵T₀:

其中，

步骤1.4：设第二变换矩阵为其中r₁,r₂,r₃,t分别是3×1的列向量，计算经过变换后的工具中心点位姿公式如式(2)：

T_n＝T_fT₀ (2)

步骤1.5：依次改变r₁,r₂,r₃,t向量的值，得到工具中心点在六个自由度发生变化后的位姿。

进一步地，步骤2中第一变换矩阵的求解过程如下：

步骤2.1：构建等式：T_G2T＝T_G2CT_C2BT_B2T；

其中，T_G2C为相机外参，即所述标定板的坐标系到所述相机的坐标系的变换矩阵；T_G2T为固定不变的所述标定板的坐标系到工具中心点坐标系的变换矩阵；T_T2B为所述机器人的位姿数据，即所述工具中心点坐标系到所述机器人的基座的坐标系的变换矩阵；T_B2C为所述机器人的基座的坐标系到所述相机的坐标系的变换矩阵；T_C2B,T_B2T分别是T_B2C,T_T2B的逆阵；

步骤2.2：记T_G2C＝D，T_B2T＝E，T_C2B＝X，构建矩阵A和矩阵B，满足式(3)：

其中，矩阵下标n表示第n次的采集数据；

步骤2.3：求解步骤2.2中所述矩阵X，X的逆矩阵即为第一变换矩阵T_B2C。

进一步地，步骤2中的所述相机内参矩阵是相机坐标系到图像坐标系的变换矩阵，记为I_I，其中，f_x,f_y,u,v是标定参数。

进一步地，步骤3中计算所述机器人的关键点在机器人基座坐标系下的位置，具体包括以下步骤：

步骤3.1：选择相对机器人基座不动的关键点作为关键点基准原点，并记为

步骤3.2：以步骤3.1的p₁为出发点，以机器人其他关键点离基准原点动作自由度远近，按照DH方法依次计算各关键点在机器人基座坐标系下的位置，并将坐标记为记为p_i(i＝2,3,4…)；p_i为其他关键点的坐标向量。

进一步地，步骤3中求所述机器人的关键点至图像像素点的映射包括以下步骤：

步骤8.1：构建所述齐次坐标其中p_i是关键点i(i＝1,2,3,4…)在机器人基座坐标系下的空间坐标；

步骤8.2：记

其中x,y是实际像素位置乘以尺度因子的结果，z为该尺度因子；T为第一变换矩阵；

步骤8.3：关键点i的实际像素位置

进一步地，还包括基于所述关键点的实际像素位置和相机图像分辨率大小，生成图像中机器人的包围框，具体步骤如下：

步骤9.1：构建包含所有所述关键点实际像素位置的点集<p_i>；

步骤9.2：对所述点集<p_i>的元素坐标，分别按照x,y方向进行最大最小排列获得

p_min(x_min,y_min),p_max(x_max,y_max)，为所述点集中横纵坐标最小和最大的点；

步骤9.3：在所述步骤9.2的基础上，根据实际相机图像分辨率大小生成图像中机器人的包围框，其中，所述包围框的左上角和右下角点坐标分别为：

p_box1＝(x_min-0.05h,y_min-0.05h)

p_box2＝(x_max+0.05h,y_max+0.05h)

其中，h为图像的像素高度，p_box1为所述包围框左上角点，p_box2为所述包围框右下角点。

进一步地，步骤3之后还包括保存步骤，保存内容包括所述相机采集的图像、各关键点在图像像素所在的横纵坐标、各关键点在相机坐标系下的三维坐标。

与现有技术相比，本发明针对目前图像数据集使用人工标注的缺点，结合机器人自身的构型特点，提出了一种用于协作机器人位姿估计的图像数据集快速构建方法，该种方法具有自动化程度高、速度快、准确率高、信息丰富等优势，保证了后续工作的可靠性。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的一个较佳实施例的用于协作机器人位姿估计的图像数据集快速构建***流程图；

图2是UR5机器人上6个关节位置分布的示意图

图3是本发明的一个较佳实施例的实施例的UR5机器人的9个关键点位置示意图；

图4是图2所示实施例的UR5机器人上DH参数示意图；

图5是图2所示实施例的第一变换矩阵和相机内参矩阵计算结果图；

图6是图2所示实施例的实际数据集具体保存形式的示意图；

图7是图2所示实施例的RGB相机在不同视角下中生成的图像中UR5机器人的包围框示意图。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。

实施例一

图1所示为本实施例用于协作机器人位姿估计的图像数据集快速构建***的具体过程：

步骤一：搭建硬件平台，将机器人基座与相机保持相对固定，保证机器人运动范围处于相机视野范围内；

步骤二：调整机器人初始位姿，在机器人末端固结标定板，然后在六个自由度上叠加变化，改变机器人末端工具中心点(简称TCP)位姿，收集标定数据，保存每张标定板图片对应的机器人末端TCP位姿数据；

步骤三：设计相应的标定算法，使用步骤二中采集的多组标定板图片与对应的机器人TCP位姿数据，解算出机器人基座坐标系到相机坐标系之间的第一变换矩阵，便于将机器人空间的TCP位姿数据统一到相机坐标系下；

步骤四：通过机器人建模与构型分析，获取机器人DH参数，同时分析机器人的具体构型，解算出机器人关键点在机器人坐标系下的位置，同时结合步骤三中得到的第一变换矩阵与相机内参矩阵，即可完成空间中的机器人关键点到图像像素点的映射，并进一步生成一系列相关的数据；

步骤五：以文本文件的形式保存处理后的图像数据集数据。

为保证标定板坐标系的稳定性，标定板的两边棋盘格数目分别为奇数和偶数。

本实施例中，采用示教器调整机器人的初始姿态。

本实施例中，相应的标定算法，是指使用张正友棋盘格标定法标定相机内外参数，并将对应的机器人TCP位姿数据转换为齐次矩阵形式，结合标定板的外部参数，计算出机器人基座坐标系到相机坐标系的第一变换矩阵。

实施例二

本实施例中，采用UR5机器人和RGB相机。

下面结合本实施例采用的UR5机器人和RGB相机，详细说明本发明的图像数据集快速构建方法。

针对求解机器人基座到相机坐标系的固定变换矩阵问题，设计相应的标定算法，记机器人末端工具中心点初始位姿表示为(x,y,z,rx,ry,rz)，其中x,y,z是工具中心点坐标数据,rx,ry,rz为位姿的旋转向量表示，转换为旋转矩阵如式(1)：

式(1)中，θ＝norm(r)，I为对角阵；

将式(1)的旋转矩阵，转换为齐次矩阵形式，具体公式如下：

其中，

记第二变换矩阵为其中r₁,r₂,r₃,t分别是3×1的列向量，依次改变r₁,r₂,r₃,t向量的值，即可实现TCP在六个自由度发生变化，经过变换后的TCP位姿坐标如式(2)：

T_n＝T_fT₀ (2)

为求解机器人基座到相机坐标系的固定变换矩阵问题，构建数据集A,B，采用经典的AX＝XB求解算法，解算出机器人基座坐标系到相机坐标系之间的第一变换矩阵；相机-机器人标定的本质即为在已知多组AX＝XB中的X,其中A和B的构建方式如下：

由相机标定的原理可知，相机内参是相机坐标系到图像坐标系的变换矩阵，记为I_I；相机外参表示的是标定板坐标系到相机坐标系的变换矩阵，记为T_G2C；机器人位姿数据表示工具中心点坐标系到机器人基座坐标系的变换矩阵，记为T_T2B；由于标定板是固结于机器人末端，因此，标定板坐标系到工具中心点坐标系的变换矩阵是固定不变的，记为T_G2T；机器人基座的坐标系到相机坐标系的变换，记为T_B2C。

由以上的变换关系可知，棋盘格坐标系到工具中心点坐标系的变换满足等式：

T_G2T＝T_G2CT_G2BT_B2T，其中T_C2B,T_B2T分别是T_B2C,T_T2B的逆阵

上式中，采集得到的T_G2C与T_B2T分别记为D,E，固定不变的T_C2B,T_G2T记为X,C，则上式记为C＝D₁XE₁＝D₂XE₂＝...＝D_nXE_n，其中下标表示的是第n次的采集数据。

则有令则有AX＝XB即完成数据A与B的构建，此时求得的结果是相机坐标系到机器人基座坐标系的变换矩阵，需要求逆之后才是机器人基座坐标系到相机坐标系的第一变换矩阵T_B2C。

进一步地，相机内参矩阵记为I_I，如式(4)：

式(4)中，f_x,f_y,u,v是标定参数；

为求解机器人关键点在相机实际像素下的位置，采用DH法对机器人进行运动学建模，结合机器人的具体构型，求解出所有关键点的空间位置表达式，实现机器人关键点的准确定位，并根据第一变换矩阵和相机内参矩阵，精确映射到图像像素上，进一步生成一系列相关的数据信息。应当注意，采用DH方法建模时，实际机器人关键点的空间位置并非和DH坐标系的原点完全一致，这时需要结合机器人的具体构型进行分析。

图2所示为本实施例UR5的关键点示意图，图3所示为本实施例UR5的DH关键参数示意图。图4是所示实施例的UR5机器人上DH参数示意图。根据本实施例的UR5资料，得知机器人的DH参数，具体参数如下表：

机器人各关键点在机器人基座坐标系的坐标计算过程如下：

1)记机器人基座(关键点1)的坐标为

2)由DH参数易知关键点2的坐标为

3)考虑到关节1的运动会导致关键点3的坐标发生变化，因此关键点3的坐标为其中，delta₁是DH参数以外的实际测量值，为一常数。

4)由于关节2、3、4组成一平面机械臂，关键点4可以认为在关键点3的基础上叠加由关节2运动产生的影响，因此有其中abs(x)表示取实数x的绝对值。

因此，

5)同理，关键点5可以认为与关键点4的差异在于关节1运动时的转动半径不一样，经测量，关键点5的转动半径为delta₂，与delta₁性质相同。

令则关键点5的坐标为

6)关键点6受关节1、2、3的影响，有

故关键点6的坐标为

7)同理，关键点7与关键点6的差异在于关节1运动时的转动半径不一样，测量值为delta₃＝d₄。

因此，关键点7的坐标为：

8)关键点8受关节1、2、3、4的影响，因此有

则关键点8的坐标为：

9)关键点9的坐标为末端法兰盘坐标p₉直接由DH参数建模得到，由此机器人基座坐标系下9个关键点的空间位置全部计算出。

构建关键点的齐次坐标其中p_i是关键点i在机器人基座坐标系下的空间坐标，则有

其中x,y是实际像素位置乘以一尺度因子的结果，z为该尺度因子，故关键点i的实际像素位置为对9个关键点都进行这样的处理后，可以获得包含9个元素的点集<p_i>。

为进一步精确机器人在图像中的范围，对点集<p_i>中的元素分别按照x,y方向进行最大最小排列获得p_min(x_min,y_min),p_max(x_max,y_max)，为点集中横纵坐标最小和最大的点，在此基础上根据实际图像分辨率大小生成图像中机器人的包围框：

p_box1＝(x_min-0.05h,y_min-0.05h)

p_box2＝(x_max+0.05h,y_max+0.05h)

其中，p_box1为所述包围框左上角点，p_box2为所述包围框右下角点，h为图像的像素高度。

为进一步建立深度学习的大规模数据集，本实施例还设置有保存功能，对处理过的协作机器人位姿数据保存为文档形式。

图5所示为本实施例的第一变换矩阵和相机内参矩阵的计算结果；

图6所示为数据集的保存形式，内容包括相机采集的图像和自动标注有图像中机器人关键点位置、包围框角点坐标以及相机坐标系下关键点空间坐标等关键信息的文本文件，具体地，每一行代表一个样本，保存的信息依次是图像在本地的保存命名、包围框的左上角像素坐标与包围框的像素宽度与高度、各关键点在图像上的像素所在的横纵坐标、各关键点在相机坐标系下的三维坐标，各类信息以“/”分隔。

图7所示，为本实施例不同视角下UR5不同姿态下计算得生成的图像中机器人的包围框。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，包含如下步骤：

步骤1：在机器人基座与相机相对固定的情况下，保证所述机器人的运动范围处于所述相机的视野范围内，将标定板与所述机器人的末端固定连接，保证所述标定板的坐标系的唯一性，同时保存每张所述标定板的图片对应的所述机器人的末端的工具中心点位姿数据；

步骤2：根据步骤1中所得到的多组所述标定板的图片与对应的所述机器人的末端的工具中心点位姿数据，基于张正友棋盘格标定法标定相机内外参数，求解所述机器人的基座坐标系到所述相机的坐标系的第一变换矩阵；

步骤3：根据所述机器人的关节-轴参数及构型解算出所述机器人的关键点在机器人基座坐标系下的位置，同时结合步骤2中得到的所述第一变换矩阵与所述相机内参矩阵，完成所述机器人的关键点在图像像素点的映射。

2.如权利要求1所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤1中的所述标定板的两边棋盘格数目分别为奇数和偶数，以保证棋盘格坐标系原点的不变性和采集过程中标定板坐标系的稳定性。

3.如权利要求1所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤1中的机器人的所述工具中心点位姿被设置为在六个自由度上均发生变化，以获得多组所述标定板的图片与对应的所述机器人的末端的工具中心点位姿数据。

4.如权利要求3所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，所述步骤1获取与多组所述标定板的图片对应的工具中心点位姿数据包括以下步骤：

步骤1.1：记机器人末端工具中心点初始位姿表示为(x,y,z,rx,ry,rz)，其中x,y,z是工具中心点初始坐标数据，rx,ry,rz为工具中心点的旋转向量表示；

步骤1.2：将步骤1.1的所述工具中心点位姿转换为旋转矩阵形式，如式(1)：

式(1)中,θ＝norm(r)，I为对角阵；

步骤1.3：将步骤1.2中所得的旋转矩阵，转换为齐次矩阵T₀:

其中，

步骤1.4：设第二变换矩阵为其中r₁,r₂,r₃,t分别是3×1的列向量，计算经过变换后的工具中心点位姿公式如式(2)：

T_n＝T_fT₀ (2)

步骤1.5：依次改变r₁,r₂,r₃,t向量的值，得到工具中心点在六个自由度发生变化后的位姿。

5.如权利要求1所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤2中第一变换矩阵的求解过程如下：

步骤2.1：构建等式：T_G2T＝T_G2CT_C2BT_B2T；

其中，T_G2C为相机外参，即所述标定板的坐标系到所述相机的坐标系的变换矩阵；T_G2T为固定不变的所述标定板的坐标系到工具中心点坐标系的变换矩阵；T_T2B为所述机器人的位姿数据，即所述工具中心点坐标系到所述机器人的基座的坐标系的变换矩阵；T_B2C为所述机器人的基座的坐标系到所述相机的坐标系的变换矩阵；T_C2B,T_B2T分别是T_B2C,T_T2B的逆阵；

步骤2.2：记T_G2C＝D，T_B2T＝E，T_C2B＝X，构建矩阵A和矩阵B，满足式(3)：

其中，矩阵下标n表示第n次的采集数据；

步骤2.3：求解步骤2.2中所述矩阵X，X的逆矩阵即为第一变换矩阵T_B2C。

6.如权利要求1所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤2中的所述相机内参矩阵是相机坐标系到图像坐标系的变换矩阵，记为I_I，其中，f_x,f_y,u,v是标定参数。

7.如权利要求6所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤3中计算所述机器人的关键点在机器人基座坐标系下的位置，具体包括以下步骤：

步骤3.1：选择相对机器人基座不动的关键点作为关键点基准原点，并记为

步骤3.2：以步骤3.1的p₁为出发点，以机器人其他关键点离基准原点动作自由度远近，按照DH方法依次计算各关键点在机器人基座坐标系下的位置，并将坐标记为记为p_i(i＝2,3,4…)；p_i为其他关键点的坐标向量。

8.如权利要求7所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤3中求所述机器人的关键点至图像像素点的映射包括以下步骤：

步骤8.1：构建所述齐次坐标其中p_i是关键点i(i＝1,2,3,4…)在机器人基座坐标系下的空间坐标；

步骤8.2：记

其中x,y是实际像素位置乘以尺度因子的结果，z为该尺度因子；T是第一变换矩阵；

步骤8.3：关键点i的实际像素位置

9.如权利要求8所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，还包括基于所述关键点的实际像素位置和相机图像分辨率大小，生成图像中机器人的包围框，具体步骤如下：

步骤9.1：构建包含所有所述关键点实际像素位置的点集<p_i>；

步骤9.2：对所述点集<p_i>的元素坐标，分别按照x,y方向进行最大最小排列获得p_min(x_min,y_min),p_max(x_max,y_max)，为所述点集中横纵坐标最小和最大的点；

步骤9.3：在所述步骤9.2的基础上，根据相机图像实际分辨率大小生成图像中机器人的包围框，其中，所述包围框的左上角和右下角点坐标分别为：

p_box1＝(x_min-0.05h,y_min-0.05h)

p_box2＝(x_max+0.05h,y_max+0.05h)

其中，h为图像的像素高度，p_box1为所述包围框左上角点，p_box2为所述包围框右下角点。

10.如权利要求1所述的用于协作机器人位姿估计的图像数据集快速构建方法，其特征在于，步骤3之后还包括保存步骤，保存内容包括所述相机采集的图像、各关键点在图像像素所在的横纵坐标、各关键点在相机坐标系下的三维坐标。