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

任务定义

在“眼在手外”布局中，相机固定于机械手外部，标定板安装于机械手末端法兰。本案例目标是：采集多组标定板图像及对应的机械手末端位姿，通过OpenCV的calibrateHandEye函数求解相机坐标系到机械手基座坐标系的变换矩阵camTbase（即X），使得后续视觉引导时可将相机检测到的目标点转换到机械手基座坐标系下进行抓取。

实现原理与输入输出

眼在手外标定满足方程：baseTtool × toolTobj = baseTcam × camTobj。其中baseTtool由机械手控制器读出（末端位姿），camTobj由标定板图像通过PnP求解得到。通过多组位姿变化，可构造AX = XB形式的方程组，OpenCV的calibrateHandEye直接求解该方程得到X = baseTcam。

本案例提供完整可运行的Python脚本，输入为：标定板图像目录、每张图像对应的机械手末端位姿（平移mm + 欧拉角ZYX度），输出为4×4齐次变换矩阵camTbase，并保存为.npy文件供后续抓取流程使用。

代码实现

import cv2
import numpy as np
import glob
import os

# ========== 配置参数 ==========
CHESSBOARD_SIZE = (9, 6)          # 内角点数
SQUARE_SIZE = 25.0                # 方格边长(mm)
IMAGE_DIR = "./calib_images/"     # 标定板图像目录
POSE_FILE = "./poses.txt"         # 机械手末端位姿文件
OUTPUT_FILE = "./camTbase.npy"    # 输出变换矩阵

# ========== 读取机械手末端位姿 ==========
# 文件格式: 每行 tx ty tz rx ry rz (平移mm, 欧拉角ZYX度)
def read_poses(filepath):
    poses = []
    with open(filepath, 'r') as f:
        for line in f:
            line = line.strip()
            if not line:
                continue
            parts = line.split()
            if len(parts) != 6:
                continue
            tx, ty, tz, rx, ry, rz = map(float, parts)
            # 欧拉角(度)转旋转矩阵 (ZYX内旋)
            rx_r, ry_r, rz_r = map(np.radians, [rx, ry, rz])
            Rx = np.array([[1,0,0],[0,np.cos(rx_r),-np.sin(rx_r)],[0,np.sin(rx_r),np.cos(rx_r)]])
            Ry = np.array([[np.cos(ry_r),0,np.sin(ry_r)],[0,1,0],[-np.sin(ry_r),0,np.cos(ry_r)]])
            Rz = np.array([[np.cos(rz_r),-np.sin(rz_r),0],[np.sin(rz_r),np.cos(rz_r),0],[0,0,1]])
            R = Rz @ Ry @ Rx
            T = np.eye(4)
            T[:3,:3] = R
            T[:3,3] = [tx, ty, tz]
            poses.append(T)
    return poses

# ========== 相机内参(需提前标定) ==========
# 示例内参, 实际使用时替换为相机标定结果
camera_matrix = np.array([[1400, 0, 960],
                          [0, 1400, 540],
                          [0, 0, 1]], dtype=np.float64)
dist_coeffs = np.zeros((4, 1))  # 假设无畸变

# ========== 从标定板图像计算 camTobj ==========
def compute_camTobj(image_path):
    img = cv2.imread(image_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, CHESSBOARD_SIZE, None)
    if not ret:
        return None
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    corners_sub = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
    # 标定板坐标系: 原点在第一个内角点, X向右, Y向下, Z垂直板面
    objp = np.zeros((CHESSBOARD_SIZE[0]*CHESSBOARD_SIZE[1], 3), np.float32)
    objp[:,:2] = np.mgrid[0:CHESSBOARD_SIZE[0], 0:CHESSBOARD_SIZE[1]].T.reshape(-1,2) * SQUARE_SIZE
    ret, rvec, tvec = cv2.solvePnP(objp, corners_sub, camera_matrix, dist_coeffs)
    if not ret:
        return None
    R, _ = cv2.Rodrigues(rvec)
    T = np.eye(4)
    T[:3,:3] = R
    T[:3,3] = tvec.flatten()
    return T

# ========== 主流程 ==========
def main():
    # 1. 读取所有标定板图像
    image_files = sorted(glob.glob(os.path.join(IMAGE_DIR, "*.png")))
    if len(image_files) < 3:
        print("错误: 至少需要3张标定板图像")
        return

    # 2. 读取机械手位姿
    tool_poses = read_poses(POSE_FILE)
    if len(tool_poses) != len(image_files):
        print(f"错误: 图像数量({len(image_files)})与位姿数量({len(tool_poses)})不一致")
        return

    # 3. 计算每张图像对应的 camTobj
    camTobj_list = []
    valid_pairs = []
    for i, img_file in enumerate(image_files):
        camTobj = compute_camTobj(img_file)
        if camTobj is not None:
            camTobj_list.append(camTobj)
            valid_pairs.append(i)

    if len(camTobj_list) < 3:
        print("错误: 有效标定图像不足3张")
        return

    # 4. 构造手眼标定输入
    # 眼在手外: R_gripper2base = baseTtool的旋转, t_gripper2base = baseTtool的平移
    #           R_target2cam = camTobj的旋转,   t_target2cam = camTobj的平移
    R_gripper2base = []
    t_gripper2base = []
    R_target2cam = []
    t_target2cam = []

    for i in valid_pairs:
        baseTtool = tool_poses[i]
        camTobj = camTobj_list[len(R_gripper2base)]  # 按有效顺序对应

        R_gripper2base.append(baseTtool[:3,:3])
        t_gripper2base.append(baseTtool[:3,3])
        R_target2cam.append(camTobj[:3,:3])
        t_target2cam.append(camTobj[:3,3])

    # 5. 求解手眼标定
    R_cam2base, t_cam2base = cv2.calibrateHandEye(
        R_gripper2base, t_gripper2base,
        R_target2cam, t_target2cam,
        method=cv2.CALIB_HAND_EYE_TSAI
    )

    # 6. 组装并保存结果
    camTbase = np.eye(4)
    camTbase[:3,:3] = R_cam2base
    camTbase[:3,3] = t_cam2base.flatten()
    np.save(OUTPUT_FILE, camTbase)
    print("标定完成, 结果已保存至:", OUTPUT_FILE)
    print("camTbase =")
    print(camTbase)

if __name__ == "__main__":
    main()

输入数据准备

标定板图像：将棋盘格标定板（9×6内角点，方格25mm）固定在机械手末端，控制机械手运动到10～15个不同位姿（包含平移和旋转变化），每到一个位姿用固定相机拍摄一张图像，保存至calib_images/目录，命名为img_01.png、img_02.png等。

位姿文件：在poses.txt中按图像顺序记录每个位姿下机械手末端在基座坐标系中的位姿，格式为每行6个数：tx ty tz rx ry rz（平移单位mm，旋转为ZYX欧拉角单位度）。示例前3行：

350.2 120.5 80.3 15.2 -10.5 5.8
420.8 95.2 110.6 -8.3 12.7 -15.4
280.5 180.3 65.2 20.1 5.6 10.2

运行与验证

运行环境：Python 3.8+，安装opencv-python、numpy。执行命令：

python handeye_calib.py

验证方法：程序运行后观察终端输出。若输出4×4矩阵且无报错，说明标定流程执行成功。进一步验证精度：取一组未参与标定的测试位姿，拍摄标定板图像，利用标定结果将标定板原点从相机坐标转换到基座坐标，与机械手示教器读取的实际位置比较，误差小于5mm即判定标定合格。

预期输出示例（仅示意格式，非实测值）：

camTbase =
[[ 9.98e-01 -5.20e-02  3.10e-02  1.20e+03]
 [ 5.40e-02  9.97e-01 -4.80e-02 -2.50e+02]
 [-2.80e-02  5.00e-02  9.98e-01  8.50e+02]
 [ 0.00e+00  0.00e+00  0.00e+00  1.00e+00]]

📚 课程目录