睿尔曼RM65-B机械臂与RealsenseD435i相机的坐标系转换

睿尔曼机械臂RM65-B与Realsense D435i相机的手眼标定_睿尔曼机械臂手眼标定-CSDN博客上述链接是之前写的“手眼标定”的文章在我后续学习过程中我想要根据机械臂上位机提供的机械臂基坐标系下机械臂末端的六个位姿参数构建机械臂基坐标系到机械臂末端坐标系的位姿齐次变换矩阵base_to_hand以及机械臂基坐标系到相机坐标系的位姿齐次变换矩阵base_to_camera。补充一个可以相互转换四元数、欧拉角与位姿矩阵的链接很方便使用https://staff.aist.go.jp/k.koide/workspace/matrix_converter/matrix_converter.html1. 写在前面首先如下图所示明确一下机器人运动学中依据“右手定则”对旋转角度正负的判定防止后续坐标系转换过程中犯迷糊。其次如下图所示明确一下“睿尔曼RM65-B机械臂末端的坐标系、RealsenseD435i相机的坐标系方便后续矩阵转换计算时验证计算得到旋转角度与平移向量对不对。最后机械臂上位机手眼标定代码也一样记录的一直都是“机械臂基座坐标系下机械臂末端原点的三维坐标xyz以及机械臂末端坐标系相对于机械臂基座坐标系的三轴转角rxryrz”这等价于将“机械臂基座坐标系沿自身的三轴先平移Txyz再绕自身的三轴依次旋转rxryrz”。此外平移的顺序满足互换性而旋转矩阵由RR(rz)×R(ry)×R(rx)构建。特别注意对于旋转顺序我们可以根据计算得到的rxryrz自己转一下采用XYZ或者ZYX的旋转顺序均可只要转完基坐标系可以与机械臂末端坐标系重合即可但是在做位姿齐次变换计算时旋转矩阵一定是通过RR(rz)×R(ry)×R(rx)构建的。特别注意由RM65-B机械臂上位机可得“机械臂基座坐标系下机械臂末端原点的三维坐标xyz以及机械臂末端坐标系相对于机械臂基座坐标系的三轴转角rxryrz”可以“先平移旋转”的方式构建基坐标系到末端坐标系矩阵(base_to_hand)。其中欧拉角转旋转矩阵-RR(rz)×R(ry)×R(rx)构建的代码如下import numpy as np def euler_angles_to_rotation_matrix(euler_angles): 参数: euler_angles: 包含三个欧拉角的数组单位为角度。 返回: 3x3 的旋转矩阵。 rx, ry, rz euler_angles # 计算每个轴的旋转矩阵 Rx np.array([ [1, 0, 0], [0, np.cos(rx), -np.sin(rx)], [0, np.sin(rx), np.cos(rx)] ]) # 绕 X 轴旋转 Ry np.array([ [np.cos(ry), 0, np.sin(ry)], [0, 1, 0], [-np.sin(ry), 0, np.cos(ry)] ]) # 绕 Y 轴旋转 Rz np.array([ [np.cos(rz), -np.sin(rz), 0], [np.sin(rz), np.cos(rz), 0], [0, 0, 1] ]) # 绕 Z 轴旋转 # 组合旋转矩阵 R Rz Ry Rx #注意欧拉角转化为旋转矩阵时一定要按照R Rz Ry Rx来计算 return R # 填入欧拉角顺序是rxryrz euler_angles [2.601, -1.554, -2.528] # 转换为旋转矩阵 rotation_matrix euler_angles_to_rotation_matrix(euler_angles) print(旋转矩阵:) print(rotation_matrix) ################################################## #注意欧拉角转化为旋转矩阵时一定要按照R Rz Ry Rx来计算根据前述[rx2.601 rad, ry-1.554 rad, rz-2.528 rad]从而由上述代码可得基坐标系到末端坐标系的旋转矩阵R_base_to_hand为R_base_to_hand[[-0.01373177, -0.07299453, -0.9972378],[-0.00967101, 0.99729488, -0.07286554],[ 0.99985895, 0.00864373, -0.01440056]]根据前述[dx-305.312 mm, dy-36.685 mm, dz672.409 mm]可得基坐标系到末端坐标系的平移向量T_base_to_hand为T_base_to_hand[[-0.305312], [-0.036685], [0.672409]]最终组合R_base_to_hand与T_base_to_hand并以“先平移后旋转”方式构建基坐标系到末端坐标系的位姿齐次变换矩阵base_to_handbase_to_hand[[R_base_to_hand, T_base_to_hand], [ 0, 1]][[-0.01373177, -0.07299453, -0.9972378, -0.305312],[-0.00967101, 0.99729488, -0.07286554, -0.036685],[0.99985895, 0.00864373, -0.01440056, 0.672409],[0, 0, 0, 1]]之前官方给出的“eyeinhand_d435i_realman”手眼标定的代码得到的手眼标定矩阵即机械臂末端坐标系到相机坐标系的矩阵hand_to_camera也是以“先平移后旋转”构建的平移向量单位为m。hand_to_camera[[-0.01534766, -0.99941224, -0.03065323, 0.11390328],[0.99978918, -0.01575722, 0.01316452, -0.04429387],[-0.0136398, -0.03044473, 0.99944338, 0.15155564],[0, 0, 0, 1]]代入上述的base_to_hand与hand_to_camera即可通过矩阵乘法运算即可得到机械臂基坐标系到相机坐标系的位姿齐次变换矩阵base_to_camerabase_to_camera base_to_hand × hand_to_camera并且base_to_camera中的平移向量即机械臂基坐标系下相机坐标系原点的三维坐标而旋转矩阵转换得到的欧拉角即为机械臂基坐标系下相机的姿态角。旋转矩阵转欧拉角单位弧度、角度的代码如下import numpy as np def rotation_matrix_to_euler_angles(R): # 确保输入的旋转矩阵是3x3的 assert R.shape (3, 3), 输入的旋转矩阵必须是3x3的 # 计算欧拉角 sy np.sqrt(R[0, 0] * R[0, 0] R[1, 0] * R[1, 0]) # sy 是 cos(pitch) singular sy 1e-6 # 判断是否是奇异情况 if not singular: x np.arctan2(R[2, 1], R[2, 2]) # roll y np.arctan2(-R[2, 0], sy) # pitch z np.arctan2(R[1, 0], R[0, 0]) # yaw else: x np.arctan2(-R[1, 2], R[1, 1]) # roll y np.arctan2(-R[2, 0], sy) # pitch z 0 return np.array([x, y, z]) # 返回的欧拉角顺序是 (rx, ry, rz) # 填入以RR(rz)×R(ry)×R(rx)构建的旋转矩阵 rotation_matrix np.array([[-0.99939383, 0.00255799, 0.03471034], [-0.00307955, -0.99988291, -0.01496629], [ 0.03466816, -0.01506468, 0.99928567]]) # 获取欧拉角以弧度为单位 euler_angles_radians rotation_matrix_to_euler_angles(rotation_matrix) # 转换为角度 euler_angles_degrees np.degrees(euler_angles_radians) print(欧拉角 (rx, ry, rz) in degrees:) print(euler_angles_radians, euler_angles_degrees) #注意一定要代入以RR(rz)×R(ry)×R(rx)构建的旋转矩阵才能解算得到正确的rxryrz。