一种基于SVD分解法的机械臂末端姿态调整方法

专利主权项内容

1.一种基于SVD分解法的机械臂末端姿态调整方法，本调整方法涉及的系统包括机器人、AGV小车和电气柜；所述机器人包括6自由度机械臂、末端执行器和深度相机，所述6自由度机械臂包括6个旋转关节和机械臂基座，所述机械臂基座安装在AGV小车上，所述6个旋转关节由机械臂基座开始依次为旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六，且该六个旋转关节依次分别绕第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴旋转，其中，第四关节轴和第五关节轴相互垂直，第五关节轴和第六关节轴相互垂直；所述末端执行器和深度相机均安装在6自由度机械臂的末端上，深度相机的光轴与第六关节轴平行且方向一致，深度相机跟随6自由度机械臂运动；所述电气柜的操作面板上有待操作的按钮，将该按钮的几何中心记为待操作点P，在待操作点P的上方设有两个指示灯，该两个指示灯处于同一水平线上，且分别记为指示灯1和指示灯2；其特征在于，所述调整方法包括以下步骤：步骤1，定义像素坐标系、图像坐标系、相机坐标系、机械臂末端坐标系、机械臂末端执行器坐标系和机械臂基座坐标系，并使用深度相机进行相机标定；所述像素坐标系以图像左上角为原点建立的以像素为单位的直角坐标系，包括互相垂直的像素U轴和像素V轴，像素U轴上的横坐标为像素在其图像中的列数，方向为平行于图像面向右，像素V轴上的纵坐标为像素在其图像中的行数，方向为垂直于像素U轴向下；所述图像坐标系以相机光轴与图像平面的交点为原点建立的坐标系，包括互相垂直的图像X轴和图像Y轴，图像X轴与像素U轴平行且方向一致，图像Y轴与像素V轴平行且方向一致；所述相机坐标系是以相机光心为原点建立的坐标系，包括相机Xc轴、相机Yc轴和相机Zc轴，其中，相机Zc轴为相机光轴且取摄影方向为正方向，相机Xc轴与像素U轴平行且方向一致，相机Yc轴与像素V轴平行且方向一致；所述机械臂末端坐标系以旋转关节六末端端面中心为原点建立的坐标系，包括机械臂末端Xe轴、机械臂末端Ye轴和机械臂末端Ze轴，其中，机械臂末端Ze轴与相机光轴平行且方向一致，机械臂末端Xe轴与相机Xc轴平行且方向一致，机械臂末端Ye轴与相机Yc轴平行且方向一致；所述机械臂末端执行器坐标系以执行器末端端面中心为原点建立坐标系，包括末端执行器Xt轴、末端执行器Yt轴和末端执行器Zt轴，其中，末端执行器Zt轴与机械臂末端Ze轴平行且方向一致，末端执行器Xt轴与机械臂末端Xe轴平行且方向一致，末端执行器Yt轴与机械臂末端Ye轴平行且方向一致；机械臂基座坐标系以机械臂基座中心点为原点建立的坐标系，包括机械臂基座Xb轴、机械臂基座Yb轴和机械臂基座Zb轴，其中，机械臂基座Zb轴和第一关节轴平行且方向一致，机械臂基座Xb轴以AGV小车正前方为正方向，机械臂基座Yb轴由坐标系右手法则确定；使用相机标定算法对深度相机进行相机标定，标定出的结果为深度相机的内参矩阵M，其表达式为：式中，f为图像坐标系中图像X轴上的归一化焦距，f＝f/d，f为深度相机的焦距，单位为mm，d为像素坐标系中的每个像素在图像X轴上的物理尺寸，单位为mm/pixel；f为图像坐标系中图像Y轴上的归一化焦距，f＝f/d，d为像素坐标系中的每个像素在图像Y轴上的物理尺寸，单位为mm/pixel；u为图像坐标系的原点在像素坐标系中的横坐标，v为图像坐标系的原点在像素坐标系中的纵坐标，即图像坐标系的原点在像素坐标系中的坐标为(u，v)；xxxxyyyy0000步骤2，机器人移动到目标电气柜操作面板前，驱动6轴机械臂展开，通过深度相机对电气柜操作面进行拍摄，得到一张RBG三通道图像，以该RBG三通道图像中心点为中心做边长为n个像素点的正方形区域，得到n个像素点，将其中任意一个记为像素点i，像素点i的像素坐标为(u，v)、深度值为d，i＝1，2，...，n；2iii2通过坐标变换得到该像素点i在相机坐标系下的相机坐标(x，y，z)，并将n个像素点i在相机坐标系下的相机坐标(x，y，z)组成大小为n×3的点集矩阵K，点集矩阵K的表达式如下：cicici2cicici2步骤3，将n个像素点i拟合出一个平面Q，并定义该平面Q的平面单位法向量为F，该平面单位法向量F在相机坐标系下的坐标为(x，y，z)；对点集矩阵K进行中心化处理后进行SVD分解，得到平面单位法向量F在相机坐标系下的坐标(x，y，z)；2fcfcfcfcfcfc步骤4，定义3×3的第一旋转矩阵用于描述机械臂末端坐标系相对于机械臂末端执行器坐标系的坐标旋转，定义3×1的第一平移向量/>用于描述机械臂末端坐标系相对于机械臂末端执行器坐标系的坐标平移，定义机械臂末端坐标系相对于机械臂末端执行器坐标系的位姿变换矩阵，并记为第一位姿变换矩阵/>其表达式为：定义3×3的第二旋转矩阵用于描述机械臂末端执行器坐标系相对于机械臂基座坐标系的坐标旋转，定义3×1的第二平移向量/>用于描述机械臂末端执行器坐标系相对于机械臂基座坐标系的坐标平移，定义机械臂末端执行器相对于机械臂基座坐标系的位姿变换矩阵，并记为第二位姿变换矩阵/>其表达式为：定义3×3的手眼标定旋转矩阵用于描述相机坐标系相对于机械臂末端坐标系的坐标旋转，定义3×1的手眼标定平移向量/>用于描述相机坐标系相对于机械臂末端坐标系的坐标平移，定义相机坐标系相对于机械臂末端坐标系的位姿变换矩阵，并记为手眼标定矩阵/>其表达式为：定义3×3的齐次变换旋转矩阵用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标旋转，定义3×1的齐次变换平移向量/>用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标平移，定义机械臂末端坐标系相对于机械臂基座坐标系的位姿变换矩阵，并记为齐次变换矩阵/>其表达式为：步骤5，根据步骤3求出的平面单位法向量F在相机坐标系下的坐标(x，y，z)、步骤4定义的齐次变换旋转矩阵手眼标定旋转矩阵/>得到平面单位法向量F在机械臂基座坐标系下的坐标(x，y，z)，其表达式为：fcfcfcfbfbfb步骤6，引入第一欧拉角α、第二欧拉角β和第三欧拉角γ，令第三欧拉角γ＝0，给出机械臂末端姿态矩阵R(α，β，γ)，其表达式为：ZYZ在机械臂末端Ze轴方向上取一末端单位向量G，根据末端姿态矩阵R(α，β，γ)得到末端单位向量G在机械臂基座坐标系下的坐标(x，y，Z)，其表达式为：ZYZZYZZYZZYZ(x，y，z)＝R(α，β，γ)[0，0，1]＝[cosαsinβ sinαsinβ cosβ]ZYZZYZZYZTZYZTT步骤7，令步骤5所求平面单位法向量F在机械臂基座坐标系下的坐标(x，y，z)与末端单位向量G在机械臂基座坐标系下坐标(x，y，z)相等，得到第一欧拉角α和第二欧拉角β的值，表达式如下：fbfbfbZYZZYZZYZ(x，y，z)＝(x，y，z)＝[cosαsinβ sinαsinβ cosβ]fbfbfbTZYZZYZZYZTTβ＝arccos(z)fbα＝atan2(y，x)fbfb式中，atan2()是双变量反正切函数，arccos()是反余弦函数；驱动6自由度机械臂使机械臂将姿态调整到第一欧拉角α和第二欧拉角β所描述的姿态下；步骤8，通过深度相机获取电气柜中的指示灯1和指示灯2的几何中心的像素坐标，并分别记为(u，v)和(u，v)，根据两点的像素坐标差求出相机Xc轴相对于电气柜水平线的偏移角度angle，表达式如下：llrrangle＝arctan|(v-v)/(u-u)|lrlr式中，arctan()是反正切函数；再对机械臂末端调整偏移角度angle，使得两个指示灯在图像坐标系中处于同一水平线，则机械臂末端姿态调整结束；步骤9，在步骤8机械臂末端姿态调整完毕后，通过深度相机获取电气柜中待操作点P的像素坐标(u，v)和深度值d，并通过坐标变换公式计算出待操作点P在相机坐标系下的相机坐标(x，y，z)；PPPPcPcPc步骤10，根据待操作点P在相机坐标系下的相机坐标(x，y，z)、手眼标定旋转矩阵第一旋转矩阵/>第二旋转矩阵/>计算出待操作点P相对于机械臂基座坐标系的位置坐标(x，y，z)，表达式如下：PcPcPcPbPbPb步骤11，驱动6自由度机械臂使其末端执行器到达待操作点P相对于机械臂基座坐标系的位置坐标(x，y，z)处，随后末端执行器按下按钮，完成操作；PbPbPb步骤12，6自由度机械臂恢复到不操作时的位姿，等待下次操作的指令。 数据由马 克 数 据整理