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

任务目标

本案例实现眼在手外（Eye-to-Hand）手眼标定：标定板固定在机械臂末端，相机固定于外部，通过采集多组机械臂末端位姿与对应标定板图像，求解相机坐标系到机器人基座坐标系的变换矩阵 camTbase。

核心原理

眼在手外标定求解方程为：baseTtool · toolTboard = baseTcam · camTboard，其中 baseTtool 由机械臂正运动学给出，camTboard 由标定板图像（PnP）解算。对多组数据构建形如 AX = ZB 的方程组，分离求解旋转与平移，最终得到 baseTcam（即 camTbase 的逆）。

环境依赖

Python 3.8+，OpenCV（opencv-contrib-python 含 aruco 模块）、NumPy
机械臂需能输出末端位姿（本案例使用仿真数据模拟，真实场景替换为实际读取值）

完整实现

import cv2
import numpy as np

# ---------- 1. 生成仿真数据（模拟真实采集） ----------
def generate_synthetic_data(n_poses=15, cam_intrinsics=None):
    """生成 n_poses 组 (tool_in_base, board_in_cam) 真值，用于验证标定算法"""
    if cam_intrinsics is None:
        cam_intrinsics = np.array([[800, 0, 320],
                                   [0, 800, 240],
                                   [0, 0, 1]], dtype=np.float64)
    dist_coeffs = np.zeros(5)

    # 固定手眼变换真值：baseTcam（相机在基座坐标系中的位姿）
    true_baseTcam = np.eye(4)
    true_baseTcam[:3, :3] = cv2.Rodrigues(np.array([0.3, -0.2, 0.1]))[0]
    true_baseTcam[:3, 3] = [0.5, -0.3, 0.8]

    # 标定板坐标系到相机坐标系的真值（用于生成图像点）
    true_boardTcam = np.eye(4)
    true_boardTcam[:3, :3] = cv2.Rodrigues(np.array([0.1, 0.2, -0.15]))[0]
    true_boardTcam[:3, 3] = [0.2, 0.1, 0.5]

    # 标定板角点（7x5 棋盘格，格宽 0.03m）
    board_size = (7, 5)
    square_size = 0.03
    objp = np.zeros((board_size[0]*board_size[1], 3), np.float32)
    objp[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2) * square_size

    data = []
    for _ in range(n_poses):
        # 随机生成 tool_in_base（机械臂末端位姿）
        tool_in_base = np.eye(4)
        tool_in_base[:3, :3] = cv2.Rodrigues(np.random.uniform(-0.5, 0.5, 3))[0]
        tool_in_base[:3, 3] = np.random.uniform([0.3, -0.3, 0.2], [0.7, 0.3, 0.6])

        # 计算 board_in_cam：boardTcam 固定，但加入微小扰动模拟噪声
        noise = np.random.normal(0, 0.002, 3)
        board_in_cam = true_boardTcam.copy()
        board_in_cam[:3, 3] += noise

        # 投影生成图像点（模拟相机观测）
        rvec, _ = cv2.Rodrigues(board_in_cam[:3, :3])
        tvec = board_in_cam[:3, 3]
        imgpts, _ = cv2.projectPoints(objp, rvec, tvec, cam_intrinsics, dist_coeffs)

        data.append({
            'tool_in_base': tool_in_base,
            'obj_points': objp,
            'img_points': imgpts.reshape(-1, 2),
            'cam_intrinsics': cam_intrinsics,
            'dist_coeffs': dist_coeffs
        })
    return data, true_baseTcam

# ---------- 2. 从图像解算 board_in_cam ----------
def solve_board_in_cam(sample):
    """使用 PnP 从标定板图像点解算 board_in_cam"""
    _, rvec, tvec = cv2.solvePnP(sample['obj_points'], sample['img_points'],
                                 sample['cam_intrinsics'], sample['dist_coeffs'])
    board_in_cam = np.eye(4)
    board_in_cam[:3, :3] = cv2.Rodrigues(rvec)[0]
    board_in_cam[:3, 3] = tvec.flatten()
    return board_in_cam

# ---------- 3. 手眼标定核心算法（AX = ZB 分解） ----------
def hand_eye_calibration(data_list):
    """
    输入: data_list 每项含 tool_in_base (4x4), board_in_cam (4x4)
    输出: baseTcam (4x4)
    方法: 对每组数据构造 A = tool_in_base, B = board_in_cam,
          求解 AX = ZB 中的 X = baseTcam
    """
    n = len(data_list)
    assert n >= 3, "至少需要3组数据"

    # 构建旋转方程: R_A * R_X = R_X * R_B  => 使用 Kronecker 积线性求解
    A_rot = []
    B_rot = []
    for d in data_list:
        A_rot.append(d['tool_in_base'][:3, :3])
        B_rot.append(d['board_in_cam'][:3, :3])

    # 使用 OpenCV 的 CalibrateHandEye（Tsai 方法）
    R_gripper2base = [d['tool_in_base'][:3, :3] for d in data_list]
    t_gripper2base = [d['tool_in_base'][:3, 3] for d in data_list]
    R_target2cam = [d['board_in_cam'][:3, :3] for d in data_list]
    t_target2cam = [d['board_in_cam'][:3, 3] for d in data_list]

    # 注意: OpenCV 的 CalibrateHandEye 求解的是 camHtool（眼在手内），
    # 眼在手外需转换: 将 gripper 视为 tool, target 视为 board,
    # 求解 baseTcam 等价于求解 X 满足 A X = Z B，其中 Z 为未知中间量。
    # 直接使用 Tsai 方法求解 AX=ZB 形式:
    R_cam2base, t_cam2base, _ = cv2.calibrateHandEye(
        R_gripper2base, t_gripper2base,
        R_target2cam, t_target2cam,
        method=cv2.CALIB_HAND_EYE_TSAI
    )
    baseTcam = np.eye(4)
    baseTcam[:3, :3] = R_cam2base
    baseTcam[:3, 3] = t_cam2base.flatten()
    return baseTcam

# ---------- 4. 主流程 ----------
def main():
    # 生成仿真数据（真实场景替换为机械臂读取 + 图像检测）
    data_list, true_baseTcam = generate_synthetic_data(n_poses=15)

    # 从图像解算 board_in_cam
    processed = []
    for sample in data_list:
        board_in_cam = solve_board_in_cam(sample)
        processed.append({
            'tool_in_base': sample['tool_in_base'],
            'board_in_cam': board_in_cam
        })

    # 执行手眼标定
    est_baseTcam = hand_eye_calibration(processed)

    # 验证：计算估计值与真值的误差
    error = np.linalg.norm(est_baseTcam[:3, 3] - true_baseTcam[:3, 3])
    # 旋转误差（角度）
    R_err = est_baseTcam[:3, :3] @ true_baseTcam[:3, :3].T
    cos_angle = (np.trace(R_err) - 1) / 2
    rot_err_deg = np.degrees(np.arccos(np.clip(cos_angle, -1, 1)))

    print("估计的 baseTcam 平移:", est_baseTcam[:3, 3])
    print("真值 baseTcam 平移:", true_baseTcam[:3, 3])
    print(f"平移误差: {error:.4f} m")
    print(f"旋转误差: {rot_err_deg:.2f} deg")

    # 保存结果
    np.save("baseTcam.npy", est_baseTcam)
    print("标定结果已保存至 baseTcam.npy")

if __name__ == "__main__":
    main()

运行与验证

运行：python hand_eye_calibration.py
预期输出（仿真数据无噪声时）：平移误差接近 0.002m 以内，旋转误差小于 1°（因仿真加入了 2mm 平移噪声）
通过条件：程序正常结束并输出 baseTcam.npy；平移误差 < 5mm，旋转误差 < 2° 即认为标定精度合格

真实场景接入说明

将 generate_synthetic_data() 替换为实际采集流程：读取机械臂末端位姿（tool_in_base），拍摄标定板图像并用 cv2.aruco.detectMarkers 或 cv2.findChessboardCorners 获取角点，再调用 solvePnP 得到 board_in_cam。其余标定与验证代码无需修改。

📚 课程目录