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

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

场景描述

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

待求问题详细描述

已知空间中一点P，点P在3D点云相机坐标系中的坐标为Pc，拍照点机械臂的位姿为Tbg（ｘ，ｙ，ｚ，ｑ１，ｑ２，ｑ３，ｑ４），手眼标定的结果（相机在机械臂末端法兰坐标系下的位姿）Ｔｇｃ（ｘ，ｙ，ｚ，ｑ１，ｑ２，ｑ３，ｑ４），会用到中间变量P在机械臂末端法兰坐标系中的坐标记为Pｇ，求P在机械臂ｂａｓｅ坐标系下的坐标Ｐｂ（ｘ，ｙ，ｚ）。

求解过程中关键点记录

• 问：ABB机械臂位姿（ｘ，ｙ，ｚ，ｑ１，ｑ２，ｑ３，ｑ４）这７个数的含义？

答：ｘｙｚ表示机械臂末端法兰中心在机械臂ｂａｓｅ坐标系下的位置；ｑ１，ｑ２，ｑ３，ｑ４为四元数的形式，表示机械臂ｂａｓｅ到机械臂末端法兰中心的旋转关系；手眼标定结果同理

• 问：机械臂位姿中四元数描述的旋转是遵循左手旋转法则顺时针旋转还是遵循右手旋转法则逆时针旋转？（因为旋转方向同坐标旋转矩阵不同，坐标变换的公式不同）

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

• 问：四元数表示的坐标系的旋转为顺时针旋转和逆时针旋转时，旋转矩阵和坐标变换分别是什么？有何不同？

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

问：将ABB机械臂位姿，手眼标定结果转换为变换矩阵并进行坐标变换（Pｃ－－＞Ｐｂ）python代码实现？

答：如下代码实现将位姿（ｘ，ｙ，ｚ，ｑ１，ｑ２，ｑ３，ｑ４）转为变换矩阵，并将Pｃ－－＞Ｐｂ

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)