睿尔曼机械臂RM65-B与Realsense D435i相机的手眼标定！！_睿尔曼机械臂手眼标定-CSDN博客
上述链接是之前写的“手眼标定”的文章，在我后续学习过程中，我想要根据机械臂上位机提供的机械臂基坐标系下机械臂末端的六个位姿参数，构建机械臂基坐标系到机械臂末端坐标系的位姿齐次变换矩阵（base_to_hand），以及机械臂基坐标系到相机坐标系的位姿齐次变换矩阵（base_to_camera）。
补充一个可以相互转换四元数、欧拉角与位姿矩阵的链接，很方便使用：！！！！！！！！
https://staff.aist.go.jp/k.koide/workspace/matrix_converter/matrix_converter.html
1. 写在前面：
首先，如下图所示，明确一下机器人运动学中，依据“右手定则”对旋转角度正负的判定，防止后续坐标系转换过程中犯迷糊。
其次，如下图所示，明确一下“睿尔曼RM65-B机械臂末端的坐标系、RealsenseD435i相机的坐标系，方便后续矩阵转换计算时，验证计算得到旋转角度与平移向量对不对。
最后，机械臂上位机（手眼标定代码也一样）记录的一直都是“机械臂基座坐标系下机械臂末端原点的三维坐标（x，y，z）以及机械臂末端坐标系相对于机械臂基座坐标系的三轴转角（rx，ry，rz）”，这等价于将“机械臂基座坐标系沿自身的三轴先平移T=（x，y，z），再绕自身的三轴依次旋转（rx，ry，rz）”。此外，平移的顺序满足互换性，而旋转矩阵由R=R(rz)×R(ry)×R(rx)构建。
特别注意：对于旋转顺序，我们可以根据计算得到的（rx，ry，rz）自己转一下，采用XYZ或者ZYX的旋转顺序均可，只要转完基坐标系可以与机械臂末端坐标系重合即可，但是在做位姿齐次变换计算时，旋转矩阵一定是通过R=R(rz)×R(ry)×R(rx)构建的。
特别注意：！！！！！！！！！！！！！！！！！
由RM65-B机械臂上位机可得“机械臂基座坐标系下机械臂末端原点的三维坐标（x，y，z）以及机械臂末端坐标系相对于机械臂基座坐标系的三轴转角（rx，ry，rz）”，可以“先平移旋转”的方式构建基坐标系到末端坐标系矩阵(base_to_hand)。
其中，欧拉角转旋转矩阵-R=R(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 # ！！！！！！！！！填入欧拉角，顺序是rx，ry，rz 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来计算根据前述[rx=2.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, dz=672.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_hand）：
base_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_camera）"：
base_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) # ！！！！！！！！！填入以R=R(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) #注意：一定要代入以R=R(rz)×R(ry)×R(rx)构建的旋转矩阵，才能解算得到正确的rx，ry，rz。
