买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：合肥工业大学

摘要：本发明公开了一种基于SVD分解法的机械臂末端姿态调整方法，属于机器人技术领域。所述调整方法包括相机标定、拍摄得到RBG三通道图像并提取像素点得到点集矩阵K、并通过SVD分解得到平面单位法向量F、引入欧拉角、机械臂姿态调整、机械臂末端姿态调整。本发明所述调整方法能够使机械臂根据电气柜待操作面进行调平以及待操作点进行定位，进而精准按下按钮，解决了由于AGV小车定位误差和电气柜做工限制带给机器人机械臂末端姿态的影响，且过程简单快速，易于实施。

主权项：1.一种基于SVD分解法的机械臂末端姿态调整方法，本调整方法涉及的系统包括机器人、AGV小车和电气柜；所述机器人包括6自由度机械臂、末端执行器和深度相机，所述6自由度机械臂包括6个旋转关节和机械臂基座，所述机械臂基座安装在AGV小车上，所述6个旋转关节由机械臂基座开始依次为旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六，且该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，其表达式为： 式中，fx为图像坐标系中图像X轴上的归一化焦距，fx＝fdx，f为深度相机的焦距，单位为mm，dx为像素坐标系中的每个像素在图像X轴上的物理尺寸，单位为mmpixel；fy为图像坐标系中图像Y轴上的归一化焦距，fy＝fdy，dy为像素坐标系中的每个像素在图像Y轴上的物理尺寸，单位为mmpixel；u0为图像坐标系的原点在像素坐标系中的横坐标，v0为图像坐标系的原点在像素坐标系中的纵坐标，即图像坐标系的原点在像素坐标系中的坐标为u0,v0；步骤2，机器人移动到目标电气柜操作面板前，驱动6轴机械臂展开，通过深度相机对电气柜操作面进行拍摄，得到一张RBG三通道图像，以该RBG三通道图像中心点为中心做边长为n个像素点的正方形区域，得到n2个像素点，将其中任意一个记为像素点i，像素点i的像素坐标为ui,vi、深度值为di,i＝1,2,...,n2；通过坐标变换得到该像素点i在相机坐标系下的相机坐标xci,yci,zci，并将n2个像素点i在相机坐标系下的相机坐标xci,yci,zci组成大小为n2×3的点集矩阵K，点集矩阵K的表达式如下： 步骤3，将n2个像素点i拟合出一个平面Q，并定义该平面Q的平面单位法向量为F,该平面单位法向量F在相机坐标系下的坐标为xfc,yfc,zfc；对点集矩阵K进行中心化处理后进行SVD分解，得到平面单位法向量F在相机坐标系下的坐标xfc,yfc,zfc；步骤4，定义3×3的第一旋转矩阵用于描述机械臂末端坐标系相对于机械臂末端执行器坐标系的坐标旋转，定义3×1的第一平移向量用于描述机械臂末端坐标系相对于机械臂末端执行器坐标系的坐标平移，定义机械臂末端坐标系相对于机械臂末端执行器坐标系的位姿变换矩阵，并记为第一位姿变换矩阵其表达式为： 定义3×3的第二旋转矩阵用于描述机械臂末端执行器坐标系相对于机械臂基座坐标系的坐标旋转，定义3×1的第二平移向量用于描述机械臂末端执行器坐标系相对于机械臂基座坐标系的坐标平移，定义机械臂末端执行器相对于机械臂基座坐标系的位姿变换矩阵，并记为第二位姿变换矩阵其表达式为： 定义3×3的手眼标定旋转矩阵用于描述相机坐标系相对于机械臂末端坐标系的坐标旋转，定义3×1的手眼标定平移向量用于描述相机坐标系相对于机械臂末端坐标系的坐标平移，定义相机坐标系相对于机械臂末端坐标系的位姿变换矩阵，并记为手眼标定矩阵其表达式为： 定义3×3的齐次变换旋转矩阵用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标旋转，定义3×1的齐次变换平移向量用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标平移，定义机械臂末端坐标系相对于机械臂基座坐标系的位姿变换矩阵，并记为齐次变换矩阵其表达式为： 步骤5，根据步骤3求出的平面单位法向量F在相机坐标系下的坐标xfc,yfc,zfc、步骤4定义的齐次变换旋转矩阵手眼标定旋转矩阵得到平面单位法向量F在机械臂基座坐标系下的坐标xfb,yfb,zfb，其表达式为： 步骤6，引入第一欧拉角α、第二欧拉角β和第三欧拉角γ，令第三欧拉角γ＝0，给出机械臂末端姿态矩阵RZYZα,β,γ，其表达式为： 在机械臂末端Ze轴方向上取一末端单位向量G，根据末端姿态矩阵RZYZα,β,γ得到末端单位向量G在机械臂基座坐标系下的坐标xZYZ，yZYZ，zZYZ，其表达式为：xZYZ，yZYZ，zZYZT＝RZYZα，β，γ[0，0，1]T＝[cosαsinβsinαsinβcosβ]T步骤7，令步骤5所求平面单位法向量F在机械臂基座坐标系下的坐标xfb，yfb，zfb与末端单位向量G在机械臂基座坐标系下坐标xZYZ,yZYZ,zZYZ相等，得到第一欧拉角α和第二欧拉角β的值，表达式如下：xfb，yfb，zfbT＝xZYZ，yZYZ，zZYZ，zZYZT＝[cosαsinβsinαsinβcosβ]Tβ＝arccoszfbα＝atan2yfb，xfb式中，atan2是双变量反正切函数，arccos是反余弦函数；驱动6自由度机械臂使机械臂将姿态调整到第一欧拉角α和第二欧拉角β所描述的姿态下；步骤8，通过深度相机获取电气柜中的指示灯1和指示灯2的几何中心的像素坐标，并分别记为ul，vl和ur，vr，根据两点的像素坐标差求出相机Xc轴相对于电气柜水平线的偏移角度angle，表达式如下：angle＝arctan|vl-vrul-ur|式中，arctan是反正切函数；再对机械臂末端调整偏移角度angle，使得两个指示灯在图像坐标系中处于同一水平线，则机械臂末端姿态调整结束；步骤9，在步骤8机械臂末端姿态调整完毕后，通过深度相机获取电气柜中待操作点P的像素坐标uP，vP和深度值dP，并通过坐标变换公式计算出待操作点P在相机坐标系下的相机坐标xPc,yPc,zPc；步骤10，根据待操作点P在相机坐标系下的相机坐标xPc,yPc,zPc、手眼标定旋转矩阵第一旋转矩阵第二旋转矩阵计算出待操作点P相对于机械臂基座坐标系的位置坐标xPb,yPb,zPb，表达式如下： 步骤11，驱动6自由度机械臂使其末端执行器到达待操作点P相对于机械臂基座坐标系的位置坐标xPb,yPb,zPb处，随后末端执行器按下按钮，完成操作；步骤12，6自由度机械臂恢复到不操作时的位姿，等待下次操作的指令；步骤2对所述像素点i进行坐标变换得到该像素点i在相机坐标系下的相机坐标xci,yci,zci，其坐标变换的公式为： 步骤9所述通过坐标变换公式计算出待操作点P在相机坐标系下的相机坐标xPc,yPc,zPc，其坐标变换公式为： 步骤3的实现过程如下：步骤3.1，将n2个像素点i拟合出一个平面Q，定义该平面Q的平面单位法向量为F，且该平面单位法向量F在相机坐标系下的坐标为xfc,yfc,zfc；步骤3.2，定义一个点集中心点J，并将点集中心点J在相机坐标系中的坐标记为其表达式如下： 对点集矩阵K进行中心化处理，得到一个中心化数据矩阵B,其表达式为： 步骤3.3，设平面Q过点集中心点J，则点集中心J和其他n2个像素点i形成的向量与平面Q的平面单位法向量F正交，其正交表达式BFT如下： 对正交表达式BFT进行求解，即对中心化数据矩阵B进行SVD分解，表达式如下：B＝KDWT其中，D是n2×3的对角矩阵，主对角线上元素为奇异值，K是n2×n2的第一酉矩阵，由方阵BBT的所有特征向量组成，W是3×3的第二酉矩阵，由方阵BTB的所有特征向量组成；获取第二酉矩阵W的最后一列为特征向量且特征向量的坐标表示为求解出平面单位法向量F在相机坐标系下的坐标xfc,yfc,zfc的具体数值，其表达式分别为：

全文数据：

权利要求：

百度查询： 合肥工业大学 一种基于SVD分解法的机械臂末端姿态调整方法