3. 手眼标定（眼在手上）
任务目标
本案例实现眼在手上（Eye-in-Hand）手眼标定：相机固定安装于机器人末端法兰，通过采集多组标定板位姿与对应机器人末端位姿，求解相机坐标系到机器人末端坐标系的变换矩阵 X，满足 AX = XB 方程。
核心原理与输入说明
眼在手上系统中，标定板固定于外部世界。移动机器人末端至不同位姿，记录：
- A：相邻两次机器人末端位姿变换矩阵（由正运动学获得）
- B：相邻两次相机观测到的标定板位姿变换矩阵（由相机外参获得）
满足 A·X = X·B，其中 X 即相机到末端的固定变换。本案例使用 Tsai-Lenz 经典解法，输入为若干组 (A, B) 矩阵对，输出 X 的旋转矩阵与平移向量。
依赖环境
- Python 3.8+
- NumPy（矩阵运算）
- OpenCV（可选，用于实际图像标定；本案例核心求解仅需 NumPy）
关键完整代码
import numpy as np
def skew(v):
"""向量 v 的反对称矩阵"""
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
def hand_eye_tsai(A_list, B_list):
"""
眼在手上手眼标定（Tsai-Lenz 方法）
参数:
A_list: 机器人末端位姿变换矩阵列表，每个为 4x4，相邻两两构成 A
B_list: 相机观测标定板位姿矩阵列表，每个为 4x4，相邻两两构成 B
返回:
X: 4x4 相机到机器人末端的变换矩阵
"""
assert len(A_list) >= 3 and len(A_list) == len(B_list), "至少需要3组位姿"
# 构造线性方程组求解旋转轴
P = []
Q = []
for i in range(len(A_list) - 1):
A_i = A_list[i]
A_j = A_list[i + 1]
B_i = B_list[i]
B_j = B_list[i + 1]
# A = A_j @ inv(A_i), B = B_j @ inv(B_i)
A = A_j @ np.linalg.inv(A_i)
B = B_j @ np.linalg.inv(B_i)
Ra, ta = A[:3, :3], A[:3, 3]
Rb, tb = B[:3, :3], B[:3, 3]
# 旋转轴（罗德里格斯）
def rot_to_axis(R):
theta = np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1))
if theta < 1e-8:
return np.zeros(3), 0
r = np.array([R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]]) / (2 * np.sin(theta))
return r, theta
ra, _ = rot_to_axis(Ra)
rb, _ = rot_to_axis(Rb)
# 旋转部分方程: skew(ra + rb) * Pcg = rb - ra
P.append(skew(ra + rb))
Q.append(rb - ra)
P_mat = np.vstack(P)
Q_vec = np.hstack(Q)
# 最小二乘求解旋转轴 Pcg
Pcg, _, _, _ = np.linalg.lstsq(P_mat, Q_vec, rcond=None)
norm_pcg = np.linalg.norm(Pcg)
if norm_pcg < 1e-8:
raise ValueError("无法求解旋转轴，请检查输入位姿是否充分变化")
Pcg = Pcg / norm_pcg
theta = 2 * np.arctan2(norm_pcg, 1) # 简化：实际需从Pcg模长恢复角度
# 由轴角构造旋转矩阵
K = skew(Pcg)
Rx = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)
# 平移部分：最小二乘
C = []
D = []
for i in range(len(A_list) - 1):
A_i = A_list[i]
A_j = A_list[i + 1]
B_i = B_list[i]
B_j = B_list[i + 1]
A = A_j @ np.linalg.inv(A_i)
B = B_j @ np.linalg.inv(B_i)
Ra, ta = A[:3, :3], A[:3, 3]
Rb, tb = B[:3, :3], B[:3, 3]
C.append(np.eye(3) - Ra)
D.append(ta - Rx @ tb)
C_mat = np.vstack(C)
D_vec = np.hstack(D)
tx, _, _, _ = np.linalg.lstsq(C_mat, D_vec, rcond=None)
X = np.eye(4)
X[:3, :3] = Rx
X[:3, 3] = tx
return X
# ========== 最小验证示例 ==========
if __name__ == "__main__":
# 构造已知真值 X_true，生成模拟位姿数据验证求解正确性
theta_true = 0.4
axis_true = np.array([0.2, 0.5, 0.3])
axis_true = axis_true / np.linalg.norm(axis_true)
K_true = skew(axis_true)
R_true = np.eye(3) + np.sin(theta_true) * K_true + (1 - np.cos(theta_true)) * (K_true @ K_true)
t_true = np.array([0.05, -0.02, 0.1])
X_true = np.eye(4)
X_true[:3, :3] = R_true
X_true[:3, 3] = t_true
# 生成 5 组随机末端位姿
np.random.seed(42)
A_list = []
B_list = []
for _ in range(5):
# 随机末端旋转
angle = np.random.uniform(0.1, 0.5)
axis = np.random.randn(3)
axis = axis / np.linalg.norm(axis)
K_axis = skew(axis)
R_end = np.eye(3) + np.sin(angle) * K_axis + (1 - np.cos(angle)) * (K_axis @ K_axis)
t_end = np.random.randn(3) * 0.1
A_end = np.eye(4)
A_end[:3, :3] = R_end
A_end[:3, 3] = t_end
# 标定板在相机坐标系下的位姿（由 X_true 反推）
# 标定板在世界系固定，取世界系下标定板位姿为 T_board_world
T_board_world = np.eye(4)
T_board_world[:3, 3] = np.array([0.3, 0.0, 0.5])
# 相机位姿 = inv(X_true) @ inv(A_end) @ T_board_world
T_cam_board = np.linalg.inv(X_true) @ np.linalg.inv(A_end) @ T_board_world
A_list.append(A_end)
B_list.append(T_cam_board)
X_est = hand_eye_tsai(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("估计旋转矩阵误差 (Frobenius):", err_rot)
print("估计平移向量误差:", err_trans)
print("估计 X:\n", X_est)
print("真值 X:\n", X_true)运行与验证
- 运行方式：将上述代码保存为
hand_eye_calib.py，在终端执行python hand_eye_calib.py。 - 验证逻辑：代码内置模拟数据生成器，构造已知真值
X_true，反向生成满足AX = XB的位姿序列，再调用求解函数估计X。 - 通过条件：程序输出旋转矩阵误差
err_rot < 1e-6且平移误差err_trans < 1e-6，即求解结果与真值一致，证明算法实现正确。
实际使用说明
在实际机器人系统中，A_list 由机器人控制器读取末端位姿（需转换为齐次矩阵），B_list 由相机拍摄标定板并求解外参获得（如使用 OpenCV 的 solvePnP 或 calibrateCamera）。采集时需保证相邻位姿间有足够的旋转变化（建议角度差大于 10°），且至少采集 5 组以上不同姿态，以保证方程数值稳定。求解得到的 X 即为相机到机器人末端的固定变换，可用于后续视觉引导抓取中的坐标转换。