4. 手眼标定（眼在手上）基础

任务目标

在“眼在手上”（Eye-in-Hand）构型中，相机固定于机械臂末端法兰，随机械臂一起运动。本案例使用棋盘格标定板（固定于世界/基座坐标系中），通过采集多组机械臂末端位姿与对应的相机图像，求解相机坐标系到机械臂末端坐标系的齐次变换矩阵 X，满足 AX = XB 手眼方程。

原理与求解路径

设机械臂末端到基座的变换为 A（由机械臂运动学正解给出），标定板（世界）到相机的变换为 B（由棋盘格PnP求解）。对于两次不同位姿的采集，有：

A1 * X * B1 = A2 * X * B2
=> (A2⁻¹ * A1) * X = X * (B2 * B1⁻¹)

令 A = A2⁻¹ * A1，B = B2 * B1⁻¹，则问题化为 AX = XB。采用Tsai-Lenz两步法：先由旋转方程 Ra * Rx = Rx * Rb 求解旋转部分 Rx，再代入平移方程求解 tx。

环境与依赖

Python 3.8+，OpenCV（opencv-contrib-python），NumPy
机械臂需能输出末端位姿（本案例使用仿真数据验证算法正确性）
棋盘格标定板（本例使用 9×6 内角点，方格边长 25mm）

完整实现代码

import cv2
import numpy as np

# ========== 1. 数据采集（仿真生成，实际使用时替换为真实数据） ==========
def generate_synthetic_data(n_poses=15):
    """生成仿真数据：随机末端位姿 + 固定标定板，返回末端矩阵列表和对应图像角点"""
    # 真实手眼矩阵（用于验证）
    X_true = np.array([[0, -1, 0, 0.05],
                       [1,  0, 0, 0.02],
                       [0,  0, 1, 0.03],
                       [0,  0, 0, 1]])
    # 标定板到相机的外参（固定，因为相机随末端运动，但板固定）
    # 实际中每张图需PnP求解，这里直接生成B
    B_fixed = np.array([[1, 0, 0, 0],
                        [0, 1, 0, 0],
                        [0, 0, 1, 0.3],
                        [0, 0, 0, 1]])
    # 相机内参
    K = np.array([[800, 0, 320],
                  [0, 800, 240],
                  [0, 0, 1]], dtype=np.float64)
    dist = np.zeros(5)
    objp = np.zeros((9*6, 3), np.float32)
    objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) * 0.025

    A_list, imgpoints_list = [], []
    for i in range(n_poses):
        # 随机末端位姿（绕x,y,z小角度旋转 + 平移）
        rx, ry, rz = np.random.uniform(-0.3, 0.3, 3)
        tx, ty, tz = np.random.uniform(0.4, 0.7, 3)
        R, _ = cv2.Rodrigues(np.array([rx, ry, rz]))
        A = np.eye(4)
        A[:3, :3] = R
        A[:3, 3] = [tx, ty, tz]
        # 相机外参：cam_to_end * end_to_base = cam_to_base（板坐标系）
        # 板坐标系到相机 = inv(B_fixed) 的逆？这里简化：直接构造
        # 实际中：B = 板到相机 = inv(相机到板)
        # 我们设定：板到相机 = B_fixed（固定），则相机到板 = inv(B_fixed)
        # 但相机随末端动，板固定，所以每张图的B应不同。
        # 正确构造：板到基座固定，末端到基座A已知，相机到末端X已知
        # 则板到相机 = inv(A @ X) @ 板到基座
        # 这里反推：给定A, X_true, 板到基座固定，计算B
        board_to_base = np.array([[1, 0, 0, 0.1],
                                  [0, 1, 0, 0.1],
                                  [0, 0, 1, 0],
                                  [0, 0, 0, 1]])
        cam_to_board = np.linalg.inv(A @ X_true) @ board_to_base
        B = np.linalg.inv(cam_to_board)  # 板到相机
        # 投影生成图像角点
        rvec, _ = cv2.Rodrigues(B[:3, :3])
        tvec = B[:3, 3]
        imgpts, _ = cv2.projectPoints(objp, rvec, tvec, K, dist)
        imgpoints_list.append(imgpts.reshape(-1, 2))
        A_list.append(A)
    return A_list, imgpoints_list, X_true

# ========== 2. 从图像角点解算B（PnP） ==========
def solve_B_from_image(objp, imgpts, K, dist):
    """给定棋盘格3D点和图像2D点，返回板到相机变换矩阵B"""
    ret, rvec, tvec = cv2.solvePnP(objp, imgpts, K, dist)
    R, _ = cv2.Rodrigues(rvec)
    B = np.eye(4)
    B[:3, :3] = R
    B[:3, 3] = tvec.flatten()
    return B

# ========== 3. Tsai-Lenz手眼标定核心 ==========
def tsai_lenz_hand_eye(A_list, B_list):
    """输入：A_list（末端到基座）, B_list（板到相机），输出：相机到末端X"""
    n = len(A_list)
    assert n >= 3, "至少需要3组数据"
    # 构建旋转方程的最小二乘
    P = []  # 3x3 矩阵堆叠
    Q = []  # 3x1 向量
    for i in range(n):
        for j in range(i+1, n):
            Aij = np.linalg.inv(A_list[j]) @ A_list[i]
            Bij = B_list[j] @ np.linalg.inv(B_list[i])
            Ra, Rb = Aij[:3, :3], Bij[:3, :3]
            # 旋转轴-角度表示
            la, _ = cv2.Rodrigues(Ra)
            lb, _ = cv2.Rodrigues(Rb)
            la, lb = la.flatten(), lb.flatten()
            # skew matrix
            sk = lambda v: np.array([[0, -v[2], v[1]],
                                     [v[2], 0, -v[0]],
                                     [-v[1], v[0], 0]])
            P.append(sk(la + lb))
            Q.append(lb - la)
    P = np.vstack(P)
    Q = np.hstack(Q)
    # 最小二乘解旋转向量
    rx, _, _, _ = np.linalg.lstsq(P, Q, rcond=None)
    Rx, _ = cv2.Rodrigues(rx)
    # 求解平移
    C, D = [], []
    for i in range(n):
        for j in range(i+1, n):
            Aij = np.linalg.inv(A_list[j]) @ A_list[i]
            Bij = B_list[j] @ np.linalg.inv(B_list[i])
            Ra, Rb = Aij[:3, :3], Bij[:3, :3]
            ta, tb = Aij[:3, 3], Bij[:3, 3]
            C.append(Ra - np.eye(3))
            D.append(Rx @ tb - ta)
    C = np.vstack(C)
    D = np.hstack(D)
    tx, _, _, _ = np.linalg.lstsq(C, D, rcond=None)
    X = np.eye(4)
    X[:3, :3] = Rx
    X[:3, 3] = tx
    return X

# ========== 4. 主流程 ==========
if __name__ == "__main__":
    # 生成仿真数据
    A_list, imgpoints, X_true = generate_synthetic_data(15)
    # 棋盘格3D点
    objp = np.zeros((9*6, 3), np.float32)
    objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) * 0.025
    K = np.array([[800, 0, 320], [0, 800, 240], [0, 0, 1]], dtype=np.float64)
    dist = np.zeros(5)
    # 从图像解算B
    B_list = [solve_B_from_image(objp, pts, K, dist) for pts in imgpoints]
    # 手眼标定
    X_est = tsai_lenz_hand_eye(A_list, B_list)
    # 验证：计算误差
    err_rot = np.linalg.norm(X_est[:3, :3] - X_true[:3, :3])
    err_trans = np.linalg.norm(X_est[:3, 3] - X_true[:3, 3])
    print("估计的手眼矩阵 X_est:")
    print(X_est)
    print(f"\n旋转部分误差: {err_rot:.6f}")
    print(f"平移部分误差: {err_trans:.6f} (米)")
    print(f"\n真实手眼矩阵 X_true:")
    print(X_true)

运行方式

python hand_eye_calibration.py

验证方法与通过标准

数据要求：至少采集3组不同机械臂位姿（本案例仿真生成15组），位姿差异应足够大以保证方程良态。
判定标准：程序输出估计的 X_est 与真实 X_true 的误差。旋转矩阵误差 < 1e-3，平移误差 < 1e-3 米（即亚毫米级），视为标定成功。
实际应用：将 generate_synthetic_data() 替换为真实采集流程——记录机械臂末端位姿（读取控制器或运动学正解），同时拍摄棋盘格图像并用 cv2.findChessboardCorners 提取角点。

常见问题

数据组数不足：少于3组时方程欠定，至少采集10组以上不同位姿。
位姿变化过小：机械臂移动幅度太小时，旋转轴向量接近零向量，导致最小二乘奇异。应让末端做多方向、大角度旋转。
棋盘格检测失败：确保标定板平整、光照均匀，图像清晰无运动模糊。

📚 课程目录