2. 手眼标定（眼在手外）
任务目标
在“眼在手外”布局中，相机固定安装于机器人工作空间上方，不随机械臂运动。本案例通过采集至少3组（推荐9~12组）标定板位姿及对应的机器人末端位姿，求解相机坐标系到机器人基座坐标系的齐次变换矩阵 X，满足 AX = XB 形式的矩阵方程。
核心原理与输入数据
眼在手外标定中，A 为机器人末端到基座的变换矩阵（由正运动学获得），B 为标定板到相机的变换矩阵（由PnP求解）。两组不同位姿下消去标定板与末端之间的固定变换，得到 AijX = XBij。本案例使用 Tsai-Lenz 两步法：先求解旋转分量，再线性求解平移分量。
输入数据为两组位姿序列（每组至少3个，本案例以9组为例）：
robot_poses：机器人末端位姿（位置+欧拉角，ZYX顺序），单位 mm 和 degcamera_poses：标定板在相机坐标系下的位姿（位置+欧拉角，ZYX顺序），单位 mm 和 deg
完整实现代码
import numpy as np
from scipy.linalg import logm, expm
def euler_to_rot(roll, pitch, yaw):
"""ZYX欧拉角转旋转矩阵"""
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
Rx = np.array([[1,0,0],[0,cr,-sr],[0,sr,cr]])
Ry = np.array([[cp,0,sp],[0,1,0],[-sp,0,cp]])
Rz = np.array([[cy,-sy,0],[sy,cy,0],[0,0,1]])
return Rz @ Ry @ Rx
def pose_to_matrix(pos, euler):
"""位置+欧拉角 → 4x4齐次矩阵"""
T = np.eye(4)
T[:3,:3] = euler_to_rot(*euler)
T[:3,3] = pos
return T
def hand_eye_calibrate(robot_poses, camera_poses):
"""
眼在手外标定：AX = XB
robot_poses: 末端在基座下的位姿 [(pos, euler), ...]
camera_poses: 标定板在相机下的位姿 [(pos, euler), ...]
返回: X (4x4), 相机→基座变换
"""
n = len(robot_poses)
assert n == len(camera_poses) and n >= 3, "至少需要3组位姿"
# 构建A和B矩阵列表
A_list, B_list = [], []
for i in range(n):
for j in range(i+1, n):
A = np.linalg.inv(pose_to_matrix(*robot_poses[i])) @ pose_to_matrix(*robot_poses[j])
B = np.linalg.inv(pose_to_matrix(*camera_poses[i])) @ pose_to_matrix(*camera_poses[j])
A_list.append(A)
B_list.append(B)
# 第一步：求解旋转分量（Tsai-Lenz）
M = np.zeros((3, 3))
for A, B in zip(A_list, B_list):
Ra, Rb = A[:3,:3], B[:3,:3]
alpha = logm(Ra)
beta = logm(Rb)
a = np.array([alpha[2,1], alpha[0,2], alpha[1,0]])
b = np.array([beta[2,1], beta[0,2], beta[1,0]])
M += np.outer(b, a)
U, _, Vt = np.linalg.svd(M)
R = U @ np.diag([1, 1, np.linalg.det(U @ Vt)]) @ Vt
# 第二步：求解平移分量
C = np.zeros((3, 3))
d = np.zeros(3)
for A, B in zip(A_list, B_list):
Ra, ta = A[:3,:3], A[:3,3]
Rb, tb = B[:3,:3], B[:3,3]
C += (Ra - np.eye(3))
d += ta - R @ tb
t = np.linalg.lstsq(C, d, rcond=None)[0]
X = np.eye(4)
X[:3,:3] = R
X[:3,3] = t
return X
# ========== 示例数据（9组模拟位姿） ==========
# 实际使用时替换为真实采集数据
robot_data = [
([300, 0, 200], [0, 0, 0]),
([350, 50, 220], [5, 3, 10]),
([280, 80, 210], [-3, 5, -8]),
([320, -40, 230], [8, -2, 15]),
([260, 30, 190], [-5, 8, 5]),
([370, -20, 200], [2, -6, -12]),
([290, 60, 240], [6, 4, 20]),
([340, 90, 180], [-8, -3, 7]),
([310, -60, 215], [4, 7, -15]),
]
camera_data = [
([50, -30, 500], [10, 5, 30]),
([80, 20, 480], [15, 8, 40]),
([30, 50, 510], [7, 10, 22]),
([60, -10, 490], [18, 3, 45]),
([20, 40, 520], [5, 13, 18]),
([90, -50, 470], [12, -1, 35]),
([45, 70, 505], [16, 12, 50]),
([75, 30, 485], [2, 7, 27]),
([55, -20, 495], [14, 9, 38]),
]
X = hand_eye_calibrate(robot_data, camera_data)
print("相机→基座变换矩阵 X：")
print(np.round(X, 4))
# ========== 验证：重投影误差 ==========
def compute_residual(X, robot_poses, camera_poses):
"""计算所有位姿对的残差：||A X - X B||_F"""
residuals = []
for i in range(len(robot_poses)):
for j in range(i+1, len(robot_poses)):
A = np.linalg.inv(pose_to_matrix(*robot_poses[i])) @ pose_to_matrix(*robot_poses[j])
B = np.linalg.inv(pose_to_matrix(*camera_poses[i])) @ pose_to_matrix(*camera_poses[j])
res = np.linalg.norm(A @ X - X @ B, 'fro')
residuals.append(res)
return np.mean(residuals), np.max(residuals)
mean_res, max_res = compute_residual(X, robot_data, camera_data)
print(f"\n验证残差（Frobenius范数）:")
print(f" 平均残差: {mean_res:.6f}")
print(f" 最大残差: {max_res:.6f}")
print(f"\n判定标准: 平均残差 < 1e-3 且最大残差 < 1e-2 时标定结果可靠")运行与验证
环境依赖：Python 3.8+，NumPy，SciPy（仅用于 logm/expm）。安装命令：pip install numpy scipy
运行方式：将上述代码保存为 hand_eye_calibration.py，直接执行 python hand_eye_calibration.py。
验证方法：代码内置残差计算，输出平均和最大 Frobenius 范数残差。判定通过条件：平均残差小于 1e-3 且最大残差小于 1e-2。残差越小，说明求解的 X 越满足 AX = XB 约束。
真实数据接入说明
实际工程中，robot_poses 从机器人控制器读取末端位姿（或通过正运动学计算），camera_poses 通过拍摄标定板并利用PnP算法（如OpenCV的 solvePnP）获得。将采集数据按 (位置mm, 欧拉角deg) 格式填入即可，无需修改标定函数本体。