其他分享
首页 > 其他分享> > ABB机械臂手眼协作--空间点的坐标转换

ABB机械臂手眼协作--空间点的坐标转换

作者:互联网

最近在做钢筋捆扎的机器人,涉及到如何将相机中空间点的坐标转换到机械臂base坐标系下实现捆扎。

场景描述

我们采用eyeInhand的手眼方式,设置ABB机械臂的base坐标系与大地坐标系重合,钢筋捆扎操作面(竖直放置)与机械臂base坐标系的YOZ平面平行。机械臂工作场景示意图如下图

待求问题详细描述

已知空间中一点P,点P在3D点云相机坐标系中的坐标为Pc,拍照点机械臂的位姿为Tbg(x,y,z,q1,q2,q3,q4),手眼标定的结果(相机在机械臂末端法兰坐标系下的位姿)Tgc(x,y,z,q1,q2,q3,q4),会用到中间变量P在机械臂末端法兰坐标系中的坐标记为Pg,求P在机械臂base坐标系下的坐标Pb(x,y,z)。

求解过程中关键点记录

答:xyz表示机械臂末端法兰中心在机械臂base坐标系下的位置;q1,q2,q3,q4为四元数的形式,表示机械臂base到机械臂末端法兰中心的旋转关系;手眼标定结果同理

答:机械臂位姿表示中的四元数遵循右手旋转法则为逆时针旋转(空间坐标系的旋转一般都遵循右手法则),手眼标定结果中的四元数也遵循右手法则。

答:参考博客https://blog.csdn.net/weixin_44010117/article/details/107207731,和《摄影测量学基础》中的内容如下图,详细还是要去补数学中空间坐标系的旋转空间点的坐标变化,以及四元数,欧拉角,旋转矩阵,坐标变换之间的复杂关系。。。总之有点复杂,,在此暂不一一叙述。。。

问:将ABB机械臂位姿,手眼标定结果转换为变换矩阵并进行坐标变换(Pc-->Pb)python代码实现?

答:如下代码实现将位姿(x,y,z,q1,q2,q3,q4)转为变换矩阵,并将Pc-->Pb

from scipy.spatial.transform import Rotation as R

#四元数转旋转矩阵
def get_matrix_quate(x, y, z,q1,q2,q3,q4):
    Rq = [q2,q3,q4,q1]
    Rm = R.from_quat(Rq)
    Rm = Rm.as_matrix()
    Rm2 = tfs.affines.compose(np.squeeze(np.asarray((x, y, z))), Rm, [1, 1, 1])
    return Rm2,Rm

Pc=[72.323,171.863,1022.452]
Tgb =[850.08,-64.82,760.68,0.48883,-0.50049,0.46997,-0.53822]
Tcg = [184.324,-261.431,182.170,0.998517,0.012349,-0.038843,0.036090]
#四元数-->旋转矩阵(scipy)
qm_cg,Rmcg = get_matrix_quate(Tcg[0], Tcg[1], Tcg[2], Tcg[3], Tcg[4], Tcg[5], Tcg[6])
qm_gb,Rmgb = get_matrix_quate(Tgb[0],Tgb[1],Tgb[2],Tgb[3],Tgb[4],Tgb[5],Tgb[6])
print('位姿四元数转旋转矩阵:\n',qm_gb)
Pc = Pc+[1]
Pc = np.array(Pc)
Pg = np.dot(qm_cg,Pc)
Pb = np.dot(qm_gb,Pg)
print('Pb:',Pb)

 

标签:四元,--,旋转,Tgb,Pc,BB机,Tcg,坐标系,手眼
来源: https://blog.csdn.net/Bolly_He/article/details/115394805