📚 课程目录第1章相机标定与手眼标定基础

1. 相机标定与手眼标定基础

任务目标

本案例完成两项标定：相机内参标定（获取焦距、主点及畸变系数）与眼在手上手眼标定（获取相机到机械臂末端的变换矩阵，并结合末端到基座的已知变换，最终得到相机到机械臂基座的变换矩阵）。

环境与依赖

Python 3.8+，安装 opencv-python、numpy
棋盘格标定板（如 9×6 内角点，方格边长 25mm）
机械臂可输出末端位姿（平移 mm + 欧拉角或旋转矩阵），本案例使用旋转矩阵

实现方案

内参标定使用 OpenCV 的 findChessboardCorners 与 calibrateCamera。手眼标定采用 眼在手上 构型，采集多组机械臂末端位姿与对应棋盘格图像，利用 OpenCV 的 calibrateHandEye（Tsai 方法）求解 camHend（相机到末端）。最终通过机械臂正运动学提供的 endHbase，计算 camHbase = endHbase⁻¹ · camHend⁻¹ 的逆变换关系，得到相机到基座的变换。

关键完整代码

import cv2
import numpy as np
import glob

# ========== 1. 相机内参标定 ==========
chessboard_size = (9, 6)
square_size = 25.0  # mm

# 准备对象点
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)
objp *= square_size

obj_points = []  # 3D 点
img_points = []  # 2D 点
images = glob.glob('calib_images/*.jpg')  # 内参标定图片

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    if ret:
        obj_points.append(objp)
        corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1),
                                    (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
        img_points.append(corners2)

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, gray.shape[::-1], None, None)
print("内参矩阵:\n", mtx)
print("畸变系数:", dist.ravel())

# ========== 2. 眼在手上手眼标定 ==========
# 采集数据：每张图像对应一个末端位姿（旋转矩阵 + 平移向量）
# 文件命名: handeye_XX.jpg 与 handeye_XX.npy (npy 中保存 4x4 末端到基座矩阵 endHbase)
R_gripper2base_list = []
t_gripper2base_list = []
R_target2cam_list = []
t_target2cam_list = []

for fname in sorted(glob.glob('handeye_images/*.jpg')):
    idx = fname.split('_')[-1].split('.')[0]
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    if not ret:
        continue
    # 使用已标定的内参计算棋盘格在相机坐标下的位姿
    ret, rvec, tvec = cv2.solvePnP(objp, corners, mtx, dist)
    R_target2cam = cv2.Rodrigues(rvec)[0]
    t_target2cam = tvec.reshape(3)

    # 读取对应的机械臂末端位姿
    pose_data = np.load(f'handeye_poses/{idx}.npy')  # 4x4 矩阵 endHbase
    R_gripper2base = pose_data[:3, :3]
    t_gripper2base = pose_data[:3, 3]

    R_gripper2base_list.append(R_gripper2base)
    t_gripper2base_list.append(t_gripper2base)
    R_target2cam_list.append(R_target2cam)
    t_target2cam_list.append(t_target2cam)

# 求解 camHend（相机到末端）
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base_list, t_gripper2base_list,
    R_target2cam_list, t_target2cam_list,
    method=cv2.CALIB_HANDEYE_TSAI)

camHend = np.eye(4)
camHend[:3, :3] = R_cam2gripper
camHend[:3, 3] = t_cam2gripper.reshape(3)
print("相机到末端变换 camHend:\n", camHend)

# ========== 3. 计算相机到基座变换 ==========
# 取任意一组末端位姿（例如第一组）计算机器人基座到末端的逆
endHbase = np.eye(4)
endHbase[:3, :3] = R_gripper2base_list[0]
endHbase[:3, 3] = t_gripper2base_list[0]

baseHend = np.linalg.inv(endHbase)
# 相机到基座 = 末端到基座的逆 × 相机到末端的逆
camHbase = baseHend @ np.linalg.inv(camHend)
print("相机到基座变换 camHbase:\n", camHbase)

运行方式

准备两个文件夹：calib_images/（15~20 张不同姿态棋盘格照片）与 handeye_images/（15~20 张机械臂带动相机在不同位姿拍摄的棋盘格照片）。
为每张 handeye_images/ 中的图像，在 handeye_poses/ 下保存同名 .npy 文件，内容为对应时刻机械臂末端到基座的 4×4 齐次变换矩阵。
运行脚本，观察终端输出的内参矩阵、畸变系数、camHend 与 camHbase。

验证方法

内参验证：重投影误差应小于 0.5 像素（calibrateCamera 返回的 ret 值）。
手眼验证：取一组未参与标定的测试数据，将棋盘格角点通过 camHbase 变换到基座坐标系，与机械臂示教器读取的该点基座坐标比较，误差小于 5mm 即通过。

常见问题

标定失败：确保棋盘格照片清晰、角点完整，且拍摄角度覆盖 30°~60° 俯仰变化。
手眼标定精度差：机械臂末端位姿与图像必须严格同步采集；末端运动范围应覆盖多个姿态，避免共面运动。