3. 手眼标定（眼在手外）

任务目标

在眼在手外（Eye-to-Hand）构型下，相机固定安装于机器人工作空间外，标定目标是求解相机坐标系到机器人基座坐标系的齐次变换矩阵 Tcamera_base。标定完成后，可通过该矩阵将相机检测到的物体位姿转换到机器人基座坐标系下，供后续抓取使用。

标定原理与输入数据

眼在手外标定利用机器人末端（工具）在基座坐标系下的位姿 Tbase_tool 与标定板在相机坐标系下的位姿 Tcamera_board 之间的约束关系。当机器人带动标定板运动到多个不同位姿时，存在以下不变关系：

Tbase_tool * Ttool_board = Tcamera_base⁻¹ * Tcamera_board

其中 Ttool_board 为标定板在工具坐标系下的固定位姿（未知但恒定）。通过多组观测，可求解 Tcamera_base。本案例使用 OpenCV 的 cv2.calibrateHandEye() 函数，输入为多组 Tbase_tool 与 Tcamera_board，输出即为 Tcamera_base。

为聚焦标定核心流程，本案例使用模拟数据：生成已知的 Tcamera_base 真值，并据此构造多组带噪声的观测数据，验证求解精度。

依赖环境

Python 3.8+
NumPy
OpenCV-Python（pip install opencv-python）

关键完整代码

import numpy as np
import cv2

def euler_to_rot(rx, ry, rz):
    """欧拉角(弧度)转旋转矩阵，按Z-Y-X顺序"""
    Rx = np.array([[1,0,0],[0,np.cos(rx),-np.sin(rx)],[0,np.sin(rx),np.cos(rx)]])
    Ry = np.array([[np.cos(ry),0,np.sin(ry)],[0,1,0],[-np.sin(ry),0,np.cos(ry)]])
    Rz = np.array([[np.cos(rz),-np.sin(rz),0],[np.sin(rz),np.cos(rz),0],[0,0,1]])
    return Rz @ Ry @ Rx

def make_T(R, t):
    T = np.eye(4)
    T[:3,:3] = R
    T[:3,3] = t
    return T

def add_noise(T, rot_noise_deg=0.5, trans_noise=2.0):
    """对位姿添加高斯噪声，模拟实际测量误差"""
    R = T[:3,:3]
    t = T[:3,3]
    # 旋转噪声：绕随机轴旋转小角度
    axis = np.random.randn(3)
    axis = axis / np.linalg.norm(axis)
    angle = np.deg2rad(rot_noise_deg)
    K = np.array([[0,-axis[2],axis[1]],[axis[2],0,-axis[0]],[-axis[1],axis[0],0]])
    dR = np.eye(3) + np.sin(angle)*K + (1-np.cos(angle))*(K@K)
    R_noisy = R @ dR
    t_noisy = t + np.random.randn(3) * trans_noise
    return make_T(R_noisy, t_noisy)

# ========== 1. 生成模拟数据 ==========
np.random.seed(42)

# 真值：相机到机器人基座的变换（待求解目标）
Tcamera_base_true = make_T(
    euler_to_rot(0.15, -0.25, 0.35),
    np.array([0.5, -0.3, 0.8])
)

# 工具到标定板的固定变换（未知但恒定）
Ttool_board = make_T(
    euler_to_rot(0.1, 0.2, -0.1),
    np.array([0.05, 0.02, 0.15])
)

num_poses = 15
Tbase_tool_list = []   # 机器人末端在基座下的位姿
Tcamera_board_list = [] # 标定板在相机下的位姿

for i in range(num_poses):
    # 随机生成机器人末端位姿（基座坐标系下）
    Tbase_tool = make_T(
        euler_to_rot(np.random.uniform(-1.0,1.0),
                     np.random.uniform(-1.0,1.0),
                     np.random.uniform(-1.0,1.0)),
        np.random.uniform(-0.3, 0.3, 3) + np.array([0.3, 0.2, 0.1])
    )
    # 由不变关系计算标定板在相机坐标系下的理论位姿
    Tcamera_board = Tcamera_base_true @ Tbase_tool @ Ttool_board
    # 添加测量噪声
    Tbase_tool_noisy = add_noise(Tbase_tool, rot_noise_deg=0.3, trans_noise=1.0)
    Tcamera_board_noisy = add_noise(Tcamera_board, rot_noise_deg=0.3, trans_noise=1.0)
    Tbase_tool_list.append(Tbase_tool_noisy)
    Tcamera_board_list.append(Tcamera_board_noisy)

# ========== 2. 手眼标定求解 ==========
# 提取旋转矩阵和平移向量
R_base_tool = [T[:3,:3] for T in Tbase_tool_list]
t_base_tool = [T[:3,3] for T in Tbase_tool_list]
R_cam_board = [T[:3,:3] for T in Tcamera_board_list]
t_cam_board = [T[:3,3] for T in Tcamera_board_list]

# calibrateHandEye: 输入末端位姿(R_base_tool,t_base_tool)和标定板位姿(R_cam_board,t_cam_board)
# 输出为相机到基座的变换(R_cam_base, t_cam_base)
R_cam_base, t_cam_base = cv2.calibrateHandEye(
    R_base_tool, t_base_tool,
    R_cam_board, t_cam_board,
    method=cv2.CALIB_HAND_EYE_TSAI
)

Tcamera_base_est = make_T(R_cam_base, t_cam_base.flatten())

# ========== 3. 精度验证 ==========
# 计算旋转误差（度）
R_err = Tcamera_base_est[:3,:3] @ Tcamera_base_true[:3,:3].T
cos_angle = (np.trace(R_err) - 1) / 2
cos_angle = np.clip(cos_angle, -1.0, 1.0)
rot_err_deg = np.degrees(np.arccos(cos_angle))

# 计算平移误差（mm）
trans_err_mm = np.linalg.norm(Tcamera_base_est[:3,3] - Tcamera_base_true[:3,3]) * 1000

print("=== 标定结果 ===")
print("估计 Tcamera_base:")
print(np.round(Tcamera_base_est, 4))
print("\n真值 Tcamera_base:")
print(np.round(Tcamera_base_true, 4))
print(f"\n旋转误差: {rot_err_deg:.4f}°")
print(f"平移误差: {trans_err_mm:.2f} mm")

# ========== 4. 端到端验证：将相机坐标转换到基座坐标 ==========
# 模拟一个在相机坐标系下检测到的物体点
p_cam = np.array([0.2, -0.1, 0.5, 1.0])
p_base_est = Tcamera_base_est @ p_cam
p_base_true = Tcamera_base_true @ p_cam
point_err_mm = np.linalg.norm(p_base_est[:3] - p_base_true[:3]) * 1000
print(f"\n点转换误差: {point_err_mm:.2f} mm")

运行方式

python hand_eye_calibration.py

验证方法与通过标准

标定精度验证：程序输出估计值与真值的旋转误差和平移误差。由于模拟数据添加了约0.3°旋转噪声和1mm平移噪声，标定结果通常满足：旋转误差 < 1°，平移误差 < 5mm，即认为标定求解正确。
端到端一致性验证：将相机坐标系下的一个点通过估计的 Tcamera_base 转换到基座坐标系，与真值变换结果对比，点位置误差应小于10mm。该步骤验证了标定矩阵的实际可用性。

若误差远大于上述阈值，说明数据构造或求解流程有误，需检查位姿输入顺序是否与 calibrateHandEye 接口要求一致。

真实场景接入说明

在实际机器人系统中，Tbase_tool 可从机器人控制器读取当前末端位姿（通常为平移向量+欧拉角/四元数，需转换为4×4矩阵）；Tcamera_board 通过拍摄标定板图像并用 cv2.solvePnP() 或 ArUco 检测获得。将本案例中的模拟数据替换为实际采集的多组位姿对即可完成真实标定。

📚 课程目录