2. 手眼标定（眼在手上）
任务目标
在“眼在手上”（Eye-in-Hand）构型中，相机固定于机械臂末端法兰，随机械臂一起运动。本案例通过采集多组机械臂末端位姿与标定板在相机坐标系下的位姿，求解相机坐标系到机械臂末端坐标系的变换矩阵 camTend，为后续视觉引导提供坐标变换基础。
核心原理
眼在手上的手眼标定求解方程：
endTbase⁻¹ · endTbase' · camTend = camTend · boardTcam⁻¹ · boardTcam'化简为经典形式 AX = XB，其中：
A = endTbase⁻¹ · endTbase'（机械臂末端运动）B = boardTcam⁻¹ · boardTcam'（标定板在相机系下的运动）X = camTend（待求的手眼变换矩阵）
环境依赖
pip install numpy opencv-python完整实现代码
import numpy as np
import cv2
def skew(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方法求解AX=XB，返回4x4变换矩阵X"""
# 构建线性方程组
P = []
Q = []
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]
# 旋转部分：RA - I
P.append(np.eye(3) - RA)
Q.append(tA - RA @ tB)
P = np.vstack(P)
Q = np.hstack(Q)
# 最小二乘求解平移
tX, _, _, _ = np.linalg.lstsq(P, Q, rcond=None)
# 求解旋转部分（利用罗德里格斯变换）
M = []
for A, B in zip(A_list, B_list):
RA = A[:3, :3]
RB = B[:3, :3]
rA = cv2.Rodrigues(RA)[0].flatten()
rB = cv2.Rodrigues(RB)[0].flatten()
M.append(np.outer(rB, rA))
M = np.sum(M, axis=0)
# 通过SVD分解得到旋转矩阵
U, _, Vt = np.linalg.svd(M)
RX = Vt.T @ U.T
if np.linalg.det(RX) < 0:
Vt[-1, :] *= -1
RX = Vt.T @ U.T
X = np.eye(4)
X[:3, :3] = RX
X[:3, 3] = tX
return X
def generate_synthetic_data(num_poses=10):
"""生成仿真数据：随机末端位姿与标定板位姿"""
# 真实手眼变换（用于验证）
true_camTend = np.eye(4)
true_camTend[:3, :3] = cv2.Rodrigues(np.array([0.1, 0.2, 0.3]))[0]
true_camTend[:3, 3] = [0.05, -0.02, 0.1]
A_list, B_list = [], []
for _ in range(num_poses):
# 随机末端位姿
endTbase = np.eye(4)
endTbase[:3, :3] = cv2.Rodrigues(np.random.randn(3) * 0.3)[0]
endTbase[:3, 3] = np.random.randn(3) * 0.2
# 标定板在相机系下的位姿（由真实手眼变换生成）
boardTcam = np.eye(4)
boardTcam[:3, :3] = cv2.Rodrigues(np.random.randn(3) * 0.2)[0]
boardTcam[:3, 3] = np.random.randn(3) * 0.15
# 计算对应的末端运动与标定板运动
A_list.append(endTbase)
B_list.append(boardTcam)
# 构造相邻位姿间的运动
A_motion = [np.linalg.inv(A_list[i]) @ A_list[i+1] for i in range(num_poses-1)]
B_motion = [np.linalg.inv(B_list[i]) @ B_list[i+1] for i in range(num_poses-1)]
return true_camTend, A_motion, B_motion
if __name__ == "__main__":
# 生成仿真数据
true_X, A_motion, B_motion = generate_synthetic_data(12)
# 求解手眼变换
X_est = hand_eye_tsai(A_motion, B_motion)
# 验证：计算误差
error = np.linalg.norm(X_est[:3, :3] - true_X[:3, :3]) + \
np.linalg.norm(X_est[:3, 3] - true_X[:3, 3])
print(f"估计的手眼变换矩阵:\n{X_est}")
print(f"真实手眼变换矩阵:\n{true_X}")
print(f"总误差: {error:.6f}")
# 判定标准：误差小于0.1视为标定成功
if error < 0.1:
print("标定验证通过")
else:
print("标定验证失败")运行与验证
- 直接运行脚本：
python hand_eye_calibration.py - 程序使用仿真数据自动验证，输出估计的
camTend矩阵与真实值对比 - 判定标准：旋转矩阵误差与平移向量误差之和小于0.1，输出“标定验证通过”
真实场景接入说明
在实际机器人系统中，A_list 来自机械臂控制器读取的末端位姿（通常为4×4齐次变换矩阵），B_list 通过拍摄标定板并利用 cv2.solvePnP 获得标定板在相机坐标系下的位姿。将上述仿真数据替换为实际采集数据即可完成真实标定。
常见问题
- 标定结果精度低：增加采集位姿数量（建议15组以上），并确保机械臂运动包含充分的旋转变化，避免共面运动。
- 矩阵奇异：当机械臂运动为纯平移时，旋转部分无约束，需保证相邻位姿间有足够的角度变化。
- 实际应用：标定完成后，视觉引导时通过
camTend @ boardTcam将目标点从相机坐标系变换到末端坐标系，再结合机械臂正运动学转换到基座坐标系。