📚 课程目录第3章手眼标定（眼在手上）

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

任务定义

将相机刚性安装在机械手末端（Eye-in-Hand），采集标定板图像并同步记录机械手末端位姿，求解固定变换矩阵 camTend（相机坐标系→末端坐标系）。该矩阵用于后续视觉引导抓取时，将相机检测到的目标坐标转换到机械手基坐标系。

原理与求解方程

Eye-in-Hand 手眼标定求解 AX = XB 问题。设机械手末端到基座的变换为 endTbase，标定板到相机的变换为 camTcal（由图像外参获得），相机到末端的固定变换为 camTend。对两次不同位姿 i、j 有：

endTbase[i] · camTend · camTcal[i] = endTbase[j] · camTend · camTcal[j]

整理得：

A · X = X · B
A = inv(endTbase[j]) · endTbase[i]
B = camTcal[j] · inv(camTcal[i])
X = camTend

采集至少 3 组（建议 8~15 组）不同姿态数据，使用 Tsai-Lenz 或 Daniilidis 方法求解。本案例采用 OpenCV 的 cv2.calibrateHandEye 实现。

实现代码

import cv2
import numpy as np
import glob

# ========== 配置 ==========
CALIB_IMAGES = "calib_images/*.jpg"   # 标定板图像路径
BOARD_SIZE = (9, 6)                    # 内角点数
SQUARE_SIZE = 0.025                    # 方格边长（米）
GRIPPER_TO_BASE_FILE = "gripper_poses.npy"  # 机械手末端位姿文件

# ========== 1. 读取机械手末端位姿（4x4齐次矩阵列表） ==========
gripper_poses = np.load(GRIPPER_TO_BASE_FILE)  # shape: (N, 4, 4)

# ========== 2. 提取标定板角点并计算相机外参 ==========
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
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

obj_points = []  # 世界系（标定板系）3D点
img_points = []  # 图像2D点
image_files = sorted(glob.glob(CALIB_IMAGES))

# 先做相机内参标定（或加载已有内参）
# 此处假设已有内参矩阵 mtx 和畸变系数 dist
mtx = np.load("camera_mtx.npy")
dist = np.load("camera_dist.npy")

for fname in image_files:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, BOARD_SIZE, None)
    if ret:
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        obj_points.append(objp)
        img_points.append(corners2)

assert len(obj_points) >= 3, "至少需要3张有效标定板图像"
assert len(obj_points) == len(gripper_poses), "图像数量与位姿数量不一致"

# ========== 3. 计算每张图像对应的标定板到相机变换 ==========
cal_to_cam_list = []  # camTcal
for i in range(len(obj_points)):
    ret, rvec, tvec = cv2.solvePnP(obj_points[i], img_points[i], mtx, dist)
    R, _ = cv2.Rodrigues(rvec)
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = tvec.flatten()
    cal_to_cam_list.append(T)

# ========== 4. 求解手眼矩阵 X = camTend ==========
R_gripper2base = []
t_gripper2base = []
R_target2cam = []
t_target2cam = []

for i in range(len(gripper_poses)):
    R_gripper2base.append(gripper_poses[i][:3, :3])
    t_gripper2base.append(gripper_poses[i][:3, 3])
    R_target2cam.append(cal_to_cam_list[i][:3, :3])
    t_target2cam.append(cal_to_cam_list[i][:3, 3])

R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam,
    method=cv2.CALIB_HAND_EYE_TSAI_LENZ
)

camTend = np.eye(4)
camTend[:3, :3] = R_cam2gripper
camTend[:3, 3] = t_cam2gripper.flatten()

# ========== 5. 保存结果 ==========
np.save("camTend.npy", camTend)
print("相机到末端变换矩阵 camTend 已保存至 camTend.npy")
print(camTend)

数据准备与运行

依赖：OpenCV（含 contrib）、NumPy。

输入文件：

calib_images/：包含 8~15 张不同姿态的标定板照片（JPG），标定板需完全在视野内且姿态差异明显；
gripper_poses.npy：与图像一一对应的机械手末端位姿，每张图像拍摄时记录一次末端 4×4 齐次矩阵，保存为 (N,4,4) 数组；
camera_mtx.npy、camera_dist.npy：相机内参矩阵与畸变系数（需预先标定）。

运行：将上述文件放置于代码同目录，执行 python hand_eye_calib.py。

验证方法

重投影验证：取一组未参与标定的测试图像，利用标定结果将标定板角点从标定板坐标系转换到相机坐标系，再投影到图像平面，计算重投影误差。若平均误差小于 0.5 像素，则标定结果可靠。

坐标转换验证：在机械手可达范围内放置一个固定尖点，分别从两个差异较大的机械手姿态拍摄该尖点，利用 camTend 将尖点从相机坐标转换到基座坐标，两次转换结果在基座系中的距离应小于 5 mm。

判定标准：重投影误差 < 0.5 像素，且两次转换位置偏差 < 5 mm，即判定标定通过。

预期输出示例

相机到末端变换矩阵 camTend 已保存至 camTend.npy
[[ 9.998e-01 -1.200e-02  1.500e-02  0.042]
 [-1.100e-02 -9.990e-01  1.800e-02 -0.018]
 [ 1.600e-02 -1.700e-02 -9.990e-01  0.035]
 [ 0.000e+00  0.000e+00  0.000e+00  1.000e+00]]

以上为基于代码逻辑的示例输出，实际数值取决于采集数据。