2. 相机标定进阶

任务定义

在激光焊接视觉定位系统中，相机固定安装于焊接工作区上方（眼在手外构型），需要建立相机坐标系与机器人基坐标系之间的变换关系，使视觉系统检测到的焊缝位置能够转换为机器人可执行的运动指令。本案例完成眼在手外手眼标定的完整实现：采集标定板在机器人不同位姿下的图像与对应机器人位姿，求解相机到机器人基座的变换矩阵，并输出验证误差。

手眼标定原理（眼在手外）

眼在手外构型下，标定板固定在机器人末端，相机固定于外部。对第 i 次观测，标定板在相机坐标系下的位姿为 Pci（由图像求解），机器人末端在基坐标系下的位姿为 Pbi（由机器人控制器读取）。标定板在机器人基坐标系下的位姿 Pobj 固定不变，因此满足：

Pobj = X · Pbi · Pci-1

其中 X 为待求的相机到机器人基座的变换矩阵。对任意两次观测 i、j，消去 Pobj 可得经典手眼方程：

Aij · X = X · Bij

其中 Aij = Pbj · (Pbi)-1（机器人运动），Bij = Pcj · (Pci)-1（相机观测运动）。本案例采用 Tsai-Lenz 两步法求解：先由旋转方程 Rot(A)·Rot(X) = Rot(X)·Rot(B) 求旋转，再代入平移方程求平移。

机器人 基座 末端 标定板 相机 T_b^e (机器人读数) T_c^o (图像求解) 待求: T_b^c (X) X Y Z 眼在手外：相机固定，标定板随机器人末端运动

代码实现

以下 Python 代码实现完整的眼在手外标定流程。核心函数 hand_eye_calibration 接收两组位姿列表（机器人末端位姿 robot_poses 与标定板在相机下的位姿 cam_poses），返回变换矩阵 X（相机→机器人基座）。代码包含 Tsai-Lenz 求解、平移求解与验证误差计算。

import numpy as np
from scipy.linalg import logm, expm

def pose_to_matrix(pose):
    """将 [x, y, z, rx, ry, rz] (mm, rad) 转为 4x4 齐次矩阵"""
    x, y, z, rx, ry, rz = pose
    # 简化：使用欧拉角 ZYX 顺序构造旋转矩阵
    cx, sx = np.cos(rx), np.sin(rx)
    cy, sy = np.cos(ry), np.sin(ry)
    cz, sz = np.cos(rz), np.sin(rz)
    Rx = np.array([[1,0,0],[0,cx,-sx],[0,sx,cx]])
    Ry = np.array([[cy,0,sy],[0,1,0],[-sy,0,cy]])
    Rz = np.array([[cz,-sz,0],[sz,cz,0],[0,0,1]])
    R = Rz @ Ry @ Rx
    T = np.eye(4)
    T[:3,:3] = R
    T[:3,3] = [x, y, z]
    return T

def matrix_to_pose(T):
    """将 4x4 矩阵转为 [x, y, z, rx, ry, rz]"""
    R = T[:3,:3]
    x, y, z = T[:3,3]
    sy = np.clip(-R[2,0], -1.0, 1.0)
    ry = np.arcsin(sy)
    if np.abs(np.cos(ry)) > 1e-6:
        rx = np.arctan2(R[2,1], R[2,2])
        rz = np.arctan2(R[1,0], R[0,0])
    else:
        rx = np.arctan2(-R[1,2], R[1,1])
        rz = 0.0
    return [x, y, z, rx, ry, rz]

def hand_eye_calibration(robot_poses, cam_poses):
    """
    眼在手外手眼标定 (Tsai-Lenz)
    robot_poses: 机器人末端在基座下的位姿列表 (N x 6)
    cam_poses:   标定板在相机下的位姿列表 (N x 6)
    返回: X (4x4), 相机坐标系 -> 机器人基座坐标系
    """
    assert len(robot_poses) == len(cam_poses) and len(robot_poses) >= 3, "至少需要3组位姿"
    N = len(robot_poses)
    T_be = [pose_to_matrix(p) for p in robot_poses]  # 末端->基座
    T_co = [pose_to_matrix(p) for p in cam_poses]    # 标定板->相机

    # ---- 步骤1: 构造旋转方程并求解 ----
    P = []  # 旋转方程系数矩阵
    Q = []  # 旋转方程右侧向量
    for i in range(N-1):
        for j in range(i+1, N):
            A = np.linalg.inv(T_be[i]) @ T_be[j]   # A_ij
            B = np.linalg.inv(T_co[i]) @ T_co[j]   # B_ij
            Ra, Rb = A[:3,:3], B[:3,:3]
            # 旋转轴-角度表示
            la = logm(Ra); lb = logm(Rb)
            pa = np.array([la[2,1], la[0,2], la[1,0]])  # 旋转向量
            pb = np.array([lb[2,1], lb[0,2], lb[1,0]])
            # 叉积构造线性方程: (pa + pb) x rx = pb - pa 的变形
            # 使用 Kronecker 积形式: (Ra ⊗ I - I ⊗ Rb^T) vec(Rx) = 0
            K = np.kron(Ra, np.eye(3)) - np.kron(np.eye(3), Rb.T)
            P.append(K[:9,:])  # 取前9行（实际9个方程中选独立行）
            Q.append(np.zeros(9))
    P = np.vstack(P)
    # 对 P 做 SVD 求零空间
    _, _, Vt = np.linalg.svd(P)
    Rx_vec = Vt[-1, :]  # 最小奇异值对应向量
    Rx = Rx_vec.reshape(3,3)
    # 投影到 SO(3)
    U, _, Vt2 = np.linalg.svd(Rx)
    Rx = U @ Vt2
    if np.linalg.det(Rx) < 0:
        U[:, -1] *= -1
        Rx = U @ Vt2

    # ---- 步骤2: 求解平移 ----
    C = []
    D = []
    for i in range(N-1):
        for j in range(i+1, N):
            A = np.linalg.inv(T_be[i]) @ T_be[j]
            B = np.linalg.inv(T_co[i]) @ T_co[j]
            Ra, ta = A[:3,:3], A[:3,3]
            Rb, tb = B[:3,:3], B[:3,3]
            # (Ra - I) * tx = Rx * tb - ta
            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

def compute_error(X, robot_poses, cam_poses):
    """计算标定验证误差: 标定板在基座下位置的重投影误差 (mm)"""
    errors = []
    for rp, cp in zip(robot_poses, cam_poses):
        T_be = pose_to_matrix(rp)
        T_co = pose_to_matrix(cp)
        T_obj = X @ T_be @ np.linalg.inv(T_co)  # 标定板在基座下位姿
        errors.append(T_obj[:3,3])
    errors = np.array(errors)
    # 计算所有位置到均值的距离标准差
    mean_pos = errors.mean(axis=0)
    dists = np.linalg.norm(errors - mean_pos, axis=1)
    return dists.mean(), dists.max()

# ============ 示例数据（模拟采集8个位姿） ============
# 真实 X: 相机在基座前方 500mm, 高 300mm, 无旋转
X_true = np.eye(4)
X_true[:3,3] = [500, 0, 300]

# 生成模拟观测数据
np.random.seed(42)
robot_poses = []
cam_poses = []
for k in range(8):
    # 机器人末端在基座下的位姿（绕基座Z轴旋转 + 平移变化）
    ang = k * np.pi / 4
    x = 200 * np.cos(ang)
    y = 200 * np.sin(ang)
    z = 100 + 20 * np.sin(k)
    rp = [x, y, z, 0, 0, ang]
    # 标定板在末端下的固定偏移
    T_eo = pose_to_matrix([50, 0, 0, 0, 0, 0])
    T_be = pose_to_matrix(rp)
    T_co = np.linalg.inv(X_true) @ T_be @ T_eo  # 标定板在相机下
    cp = matrix_to_pose(T_co)
    robot_poses.append(rp)
    cam_poses.append(cp)

# 执行标定
X_est = hand_eye_calibration(robot_poses, cam_poses)
mean_err, max_err = compute_error(X_est, robot_poses, cam_poses)

print("标定结果 X (相机->机器人基座):")
print(np.round(X_est, 4))
print(f"\n验证误差: 平均 {mean_err:.4f} mm, 最大 {max_err:.4f} mm")
print("预期输出: 平均误差接近 0.0000 mm (无噪声模拟数据)")

运行与验证

依赖安装：

pip install numpy scipy

运行方式：将上述代码保存为 hand_eye_calib.py，执行 python hand_eye_calib.py。

验证判定标准：

  1. 程序正常输出 4×4 变换矩阵 X_est，其平移部分应接近 [500, 0, 300]，旋转部分接近单位阵；
  2. 输出的平均验证误差应接近 0（模拟数据无噪声时理论为 0，实际受浮点精度影响通常 < 1e-6 mm）；
  3. 将 X_est 与真实 X_true 逐元素比较，最大偏差应小于 1e-3。

实际应用说明：在真实系统中，robot_poses 从机器人控制器读取（单位：mm 与 rad），cam_poses 通过拍摄标定板图像并利用相机内参求解（如 OpenCV 的 solvePnP 或 calibrateCamera）。采集时需保证机器人运动包含充分的旋转变化（至少绕两个不同轴旋转），否则旋转方程可能退化导致求解不稳定。本案例代码直接使用模拟数据验证算法正确性，真实数据采集与预处理需结合相机内参标定结果另行接入。

← 上一章 📖 返回目录 下一章 →