[发明专利]一种机器人机械臂末端姿态的调整方法

权利要求书

1.一种机器人机械臂末端姿态的调整方法，其特征在于，本调整方法涉及的系统包括机器人、电气柜和校正板，其中，所述电气柜操作面上装有按钮，将按钮的圆心记为点A；所述校正板为矩形，粘贴在电气柜操作面的上方，该校正板上设有一个黑色的直角三角形和一个黑色的校正板定位圆，将校正板定位圆的圆心记为点C4，直角三角形的三个顶点分别记为顶点C₁、顶点C₂和顶点C₃，其中顶点C₁为直角三角形直角处的顶点；校正板粘贴完成后，顶点C₁位于校正板的左上角，顶点C₁和顶点C₂的连线为水平线，顶点C₁和顶点C₃的连线与顶点C₁和顶点C₂的连线垂直，点C4和顶点C₁的连线垂直于顶点C₂和顶点C₃的连线，且校正板定位圆位于直角三角形的右下方；所述校正板定位圆用于相机识别定位校正板和确定三个顶点的位置；

所述机器人包括AGV小车、6自由度机械臂、末端执行器和深度相机，所述6自由度机械臂包括6个旋转关节和机械臂基座，所述6个旋转关节由机械臂基座开始依次为旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六，6个旋转关节依次分别绕第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴旋转，其中，第四关节轴和第五关节轴相互垂直，第五关节轴和第六关节轴相互垂直；所述末端执行器和深度相机均安装在6自由度机械臂的末端上，且深度相机的光轴与第六关节轴平行，深度相机跟随6自由度机械臂运动；

所述调整方法包括以下步骤：

步骤1，定义像素坐标系、图像坐标系、相机坐标系、电气柜操作面坐标系、机械臂末端坐标系和机械臂基座坐标系，并使用深度相机进行相机标定；

所述像素坐标系以图像左上角为原点建立的以像素为单位的直角坐标系，包括互相垂直的像素U轴和像素V轴，像素U轴上的横坐标为像素在其图像中的列数，方向为平行于图像面向右，像素V轴上的纵坐标为像素在其图像中的行数，方向为垂直于像素U轴向下；

所述图像坐标系以相机光轴与图像平面的交点为原点建立的坐标系，包括互相垂直的图像X轴和图像Y轴，图像X轴与像素U轴平行且方向一致，图像Y轴与像素V轴平行且方向一致；

所述相机坐标系是以相机光心为原点建立的坐标系，包括相机Xc轴、相机Yc轴和相机Zc轴，其中，相机Zc轴为相机光轴且取摄影方向为正方向，相机Xc轴与像素U轴平行且方向一致，相机Yc轴与像素V轴平行且方向一致；

所述电气柜操作面坐标系是以点C4为原点建立的坐标系，包括操作面Xt轴、操作面Yt轴、操作面Zt轴，其中，操作面Zt轴与操作面法线平行，方向由操作面外指向操作面内，操作面Yt轴与顶点C₁和顶点C₃所在直角边平行，方向为顶点C₁指向顶点C₃，操作面Xt轴与顶点C₁和顶点C₂所在直角边平行，方向为顶点C₁指向顶点C₂；

所述机械臂末端坐标系以旋转关节六中心为原点建立的坐标系，包括机械臂末端Xe轴、机械臂末端Ye轴和机械臂末端Ze轴，其中，机械臂末端Ze轴为第六关节轴，即械臂末端Ze轴与相机光轴平行且方向一致，机械臂末端Xe轴与相机Xc轴平行且方向一致，机械臂末端Ye轴与相机Yc轴平行且方向一致；

机械臂基座坐标系以机械臂基座中心点为原点建立的坐标系，包括机械臂基座X_b轴、机械臂基座Y_b轴和机械臂基座Z_b轴，其中，机械臂基座Z_b轴和第一关节轴平行，机械臂基座X_b轴以AGV小车正前方为正方向，机械臂基座Y_b轴由坐标系右手法则确定；

使用相机标定算法对深度相机进行相机标定，标定出的结果为深度相机的内参矩阵M，其表达式为：

式中，

f_x为图像坐标系中图像X轴上的归一化焦距，f_x＝f/d_x，其中，f为深度相机的焦距，单位为mm，d_x为像素坐标系中的每个像素在图像X轴上的物理尺寸，单位为mm/pixel；

f_y为图像坐标系中图像Y轴上的归一化焦距，f_y＝f/d_y，d_y为像素坐标系中的每个像素在图像Y轴上的物理尺寸，单位为mm/pixel；

u₀为图像坐标系的原点在像素坐标系中的横坐标，v₀为图像坐标系的原点在像素坐标系中的纵坐标，即图像坐标系的原点在像素坐标系中的坐标为(u₀，v₀)；

步骤2，机器人移动到目标电气柜操作面板板前，驱动6自由度机械臂使深度相机到达校正板拍摄位置进行拍照，然后对拍摄得到的图像做图像处理，得到顶点C1、顶点C2和顶点C3的像素坐标和深度值，分别为：顶点C1的像素坐标(u₁，v₁)和深度值d₁，顶点C₂的像素坐标(u₂，v₂)和深度值d₂，顶点C₃的像素坐标(u₃，v₃)和深度值d₃；

步骤3，根据步骤1标定出的内参矩阵M和顶点坐标变换公式一分别计算出顶点C₁在相机坐标系下的相机坐标(x_c1，y_c1，z_c1)、顶点C₂在相机坐标系下的相机坐标(x_c2，y_c2，z_c2)，顶点C₃在相机坐标系下的相机坐标(x_c3，y_c3，z_c3)，所述顶点坐标变换公式一的表达式为：

其中，Z是顶点在相机坐标系下测出来的深度值，即顶点到深度相机平面的距离，u为顶点在像素坐标系下的横坐标，v为顶点在像素坐标系下的纵坐标，(x_c，y_c，z_c)为顶点在相机坐标系下的相机坐标，所述深度相机平面为相机Xc轴、相机Yc轴构成的平面；

步骤4，根据顶点C1、顶点C2和顶点C3的坐标关系，求深度相机的相机Xc轴与操作面Xt轴的倾斜角、相机Yc轴与操作面Yt轴的倾斜角、相机Zc轴与操作面Zt轴的倾斜角，所述求解过程如下：

步骤4.1，过顶点C1做一条平行于相机Xc轴的直线，过顶点C2做该直线的垂线，并将其交点记为交点Q₁，连接顶点C1、顶点C2、交点Q₁构成一个直角三角形，∠Q₁C₁C₂即为深度相机的相机Xc轴与操作面Xt轴的倾斜角，其表达式为：

步骤4.2，沿着深度相机光轴的方向，将顶点C3投影到过顶点C1且平行于相机Xc-相机Yc轴的平面上，记为点Q₂，连接顶点C3、顶点C1、点Q₂构成一个直角三角形，∠Q₂C₁C₃即为深度相机的相机Yc轴与操作面Yt轴的倾斜角，其表达式为：

步骤4.3，沿着深度相机光轴的方向，将顶点C1投影到过顶点C2且平行于相机Xc-相机Yc轴的平面上，记为点Q₃，连接顶点C1、顶点C2、点Q₃构成一个直角三角形C₁ Q₃ C₂，∠Q₃C₂C₁即为深度相机的相机Zc轴与操作面Zt轴的倾斜角，其表达式为：

步骤5，定义第一旋转矩阵所述第一旋转矩阵用于描述电器柜操作面坐标系相对于机械臂末端坐标系的当前姿态，其表达式为：

式中，

步骤6，定义第三旋转矩阵所述第三旋转矩阵用于描述电气柜操作面坐标系相对机械臂基座坐标系的当前姿态，其转换式如下：

式中，为6自由度机械臂设定的第二旋转矩阵，用于描述为机械臂末端坐标系相对于机械臂基座坐标系的当前姿态，r₁₁为操作面Xt轴在机械臂基座Xb轴上投影的分量，r₂₁为操作面Xt轴在机械臂基座Yb轴上投影的分量，r₃₁为操作面Xt轴在机械臂基座Zb轴上投影的分量，r₁₂为操作面Yt轴在机械臂基座Xb轴上投影的分量，r₂₂为操作面Yt轴在机械臂基座Yb轴上投影的分量，r₃₂为操作面Yt轴在机械臂基座Zb轴上投影的分量，r₁₃为操作面Zt轴在机械臂基座Xb轴上投影的分量，r₂₃为操作面Zt轴在机械臂基座Yb轴上投影的分量，r₃₃为操作面Zt轴在机械臂基座Zb轴上投影的分量；

定义第一欧拉角所述第一欧拉角用于描述电气柜操作面坐标系相对于机械臂基座坐标系的当前姿态，第一欧拉角的反正切表达式如下：

当时，

当时，

当

式中，Atan2()是双变量反正切函数；

定义第二欧拉角所述第二欧拉角用于描述机械臂末端坐标系相对于机械臂基座坐标系的当前姿态，第二欧拉角的计算式为：

步骤7，驱动6自由度机械臂使机械臂末端坐标系运动到第二欧拉角所描述的姿态下；

步骤8，按照步骤2的方法再次获取顶点C₁、顶点C₂、顶点C₃的像素坐标和深度值，并分别记为调整后的顶点C₁的像素坐标(u_1n，v_1n)和深度值d_1n、调整后的顶点C₂的像素坐标(u_2n，v_2n)和深度值d_2n、调整后的顶点C₃的像素坐标(u_3n，v_3n)和深度值d_3n，其中，n为调整次数，n≥1；

定义第一差值δ1、第二差值δ2、第三差值δ3、第四差值η1和第五差值η2，其中，δ1＝|d_1n-d_2n|，δ2＝|d_1n-d_3n|，δ3＝|d_2n-d_3n|，η1＝|v_1n-v_2n|，η2＝|u_1n-u_3n|；给定第一预设值δ和第二预设值η，并进行如下判断：

若满足δ1＜δ且δ2＜δ且δ3＜δ且η1＜η且η2＜η，则6自由度机械臂末端姿态的调整结束，进入步骤9；

若不满足δ1＜δ且δ2＜δ且δ3＜δ且η1＜η且η2＜η，返回步骤2，进行下一轮调整；

步骤9，在6自由度机械臂末端完成姿态调整后，驱动6自由度机械臂使深度相机到达按钮拍照位姿进行拍照，然后对拍摄得到的图像做图像处理，得到点A的像素坐标(u_a，v_a)和深度值d_a；

步骤10，首先利用步骤3中的顶点坐标变换公式一，将步骤9得到的点A的像素坐标(u_a，v_a)转换成点A在相机坐标系下的相机坐标(x_ca、y_ca、z_ca)；然后利用顶点坐标变换公式二将点A的相机坐标(x_ca、y_ca、z_ca)转换成机械臂基座坐标系下的机械臂基座坐标(x_ba、y_ba、z_ba)，所述顶点坐标变换公式二的表达式为：

其中，是机械臂手眼标定出的相机外参矩阵，是由机械臂系统得出的齐次变换矩阵；

步骤11，驱动6自由度机械臂使其末端执行器到达(x_ba、y_ba、z_ba)位姿，随后末端执行器按下按钮，完成操作；

步骤12，6自由度机械臂恢复到不操作时的位姿，等待下次操作的指令。