手眼标定中史陶比尔机器人位姿旋转矩阵转换问题排查

结果异常来自两个核心错误：

1. 泰特-布莱恩角转换时旋转顺序、内外旋模式不匹配：史陶比尔控制器输出的rx,ry,rz为外旋XYZ顺序Tait-Bryan角，你直接调用的transforms3d.taitbryan.euler2mat(rx, ry, rz)默认返回内旋ZYX顺序的旋转矩阵，仅交换rx、rz无法修正顺序和旋转模式的差异。
2. 传入OpenCV标定接口的变换方向错误：cv2.calibrateHandEye要求输入夹爪到基座的变换，你当前传入的是基座到夹爪的变换，方向完全相反。

1. 姿态角转旋转矩阵 #

方法1：调用transforms3d指定轴顺序 #

直接通过axes参数显式指定外旋XYZ顺序即可，无需手动实现矩阵乘法：

import numpy as np
import transforms3d as t3d
import cv2

# 注意：史陶比尔控制器输出角度单位默认为度，需先转为弧度
rx_rad = np.deg2rad(rx)
ry_rad = np.deg2rad(ry)
rz_rad = np.deg2rad(rz)

# sxyz代表静态轴（外旋）、XYZ旋转顺序，完全匹配史陶比尔输出约定
R_base2gripper = t3d.euler.euler2mat(rx_rad, ry_rad, rz_rad, axes='sxyz')
t_base2gripper = np.array([x, y, z]).reshape(3, 1)

方法2：手动合成旋转矩阵（用于校验） #

外旋XYZ顺序的矩阵合成规则为：先绕固定X轴旋转rx，再绕固定Y轴旋转ry，最后绕固定Z轴旋转rz，矩阵相乘顺序从右到左：

def rot_x(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

def rot_y(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

def rot_z(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

R_base2gripper = rot_z(rz_rad) @ rot_y(ry_rad) @ rot_x(rx_rad)

2. 转换变换方向匹配OpenCV接口要求 #

cv2.calibrateHandEye的输入参数定义为夹爪到基座的变换，需要对基座到夹爪的变换求逆：

# 旋转矩阵的逆等于其转置
R_gripper2base = R_base2gripper.T
# 平移向量逆变换公式
t_gripper2base = -R_gripper2base @ t_base2gripper

3. 调用标定接口 #

传入正确方向的变换参数完成标定：

R_cam2base, t_cam2base = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam  # 这部分是你标定得到的标定板到相机的变换，无需修改
)

• 采集标定位姿时，需让固定在夹爪上的标定板覆盖相机视野的四个象限、带30°以内的不同倾斜角度，至少采集15组以上有效位姿，避免位姿共面导致标定漂移。
• 史陶比尔不同型号控制器的角度输出可能存在180°偏置，若修正后仍有单轴反向问题，可将对应轴的角度加np.pi后重新计算旋转矩阵验证。

内容的提问来源于stack exchange，提问作者teun_Q