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

权利要求书

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：根据步骤1中所得到的多组所述标定板的图片与对应的所述机器人的末端的工具中心点位姿数据，基于张正友棋盘格标定法标定相机内外参数，求解所述机器人的基座坐标系到所述相机的坐标系的第一变换矩阵；相机内参矩阵是相机坐标系到图像坐标系的变换矩阵，记为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为其他关键点的坐标向量；

同时结合步骤2中得到的所述第一变换矩阵与所述相机内参矩阵，完成所述机器人的关键点在图像像素点的映射，具体包括以下步骤：

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

步骤8.2：记

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

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

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