3. 手眼标定（眼在手上）
任务目标
在眼在手上（Eye-in-Hand）构型中，相机刚性安装在机械臂末端法兰上，随机械臂一起运动。标定板固定在世界坐标系中，通过机械臂运动到多个位姿拍摄标定板图像，利用已知的机械臂末端位姿与检测到的标定板角点，求解相机坐标系到机械臂末端坐标系的固定变换矩阵 X（即手眼矩阵）。
核心原理与求解方程
对第 i 次拍摄，记：
Ei：机械臂末端到基座的齐次变换矩阵（由机械臂正运动学给出，已知）B：标定板到相机坐标系的齐次变换矩阵（由标定板角点检测与PnP求解，已知）X：相机到机械臂末端的齐次变换矩阵（待求，固定不变）
由坐标变换链 Ei · X · Bi = 常量（标定板在基座坐标系中固定），对两次拍摄 i、j 消去常量可得：
A · X = X · B
其中 A = Ei-1 · Ej，B = Bi · Bj-1。该方程为经典手眼标定 AX = XB 形式，采用 Tsai-Lenz 两步法求解：先由旋转矩阵求旋转轴与旋转角，再线性最小二乘求平移向量。
环境依赖
- Python 3.8+，OpenCV（
opencv-contrib-python≥ 4.5），NumPy - 标定板：7×10 棋盘格（内角点 6×9），方格边长 30 mm
- 机械臂：提供正运动学接口，可输出末端位姿（本案例使用仿真数据验证算法正确性）
关键完整代码
import cv2
import numpy as np
def tsai_lenz_hand_eye(A_list, B_list):
"""求解 AX = XB，A_list/B_list 为 N 组齐次变换矩阵 (4x4)"""
n = len(A_list)
# ---- 旋转部分：构造线性方程组 ----
P = [] # 9 x 3 系数块
Q = [] # 9 x 1 常数块
for i in range(n):
Ra, ta = A_list[i][:3,:3], A_list[i][:3,3]
Rb, tb = B_list[i][:3,:3], B_list[i][:3,3]
# 旋转轴（罗德里格斯）
rvec_a, _ = cv2.Rodrigues(Ra)
rvec_b, _ = cv2.Rodrigues(Rb)
P.append(np.kron(np.eye(3), rvec_a.reshape(-1,1)).T)
Q.append(rvec_b.reshape(-1,1))
P = np.vstack(P)
Q = np.vstack(Q)
# 最小二乘解旋转轴
Pg, _, _, _ = np.linalg.lstsq(P, Q, rcond=None)
Pg = Pg.flatten()
# 由旋转轴构造旋转矩阵
theta = np.linalg.norm(Pg)
if theta < 1e-9:
raise ValueError("旋转角接近零，无法标定")
rvec = Pg / theta * theta # 实际为 Pg 本身
R, _ = cv2.Rodrigues(Pg)
# ---- 平移部分：线性最小二乘 ----
C = []
D = []
for i in range(n):
Ra, ta = A_list[i][:3,:3], A_list[i][:3,3]
Rb, tb = B_list[i][:3,:3], B_list[i][:3,3]
C.append(np.eye(3) - Ra)
D.append(ta - R @ tb)
C = np.vstack(C)
D = np.vstack(D)
t, _, _, _ = np.linalg.lstsq(C, D, rcond=None)
X = np.eye(4)
X[:3,:3] = R
X[:3,3] = t.flatten()
return X
# ============ 仿真数据生成（验证算法正确性） ============
def generate_synthetic_data(num_poses=15):
"""生成仿真手眼标定数据：已知真实 X，反推 A/B"""
# 真实手眼矩阵（相机到末端）
rvec_true = np.array([0.1, -0.2, 0.3])
R_true, _ = cv2.Rodrigues(rvec_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
A_list, B_list = [], []
# 固定标定板在基座中的位姿
board_in_base = np.eye(4)
board_in_base[:3,3] = [0.3, 0.0, 0.2]
for _ in range(num_poses):
# 随机末端位姿（绕不同轴旋转）
rvec_e = np.random.uniform(-0.5, 0.5, 3)
R_e, _ = cv2.Rodrigues(rvec_e)
t_e = np.random.uniform([0.3,-0.2,0.1], [0.6,0.2,0.4])
E = np.eye(4)
E[:3,:3] = R_e
E[:3,3] = t_e
# 相机看到标定板的位姿 B = X^-1 · E^-1 · board_in_base
X_inv = np.linalg.inv(X_true)
E_inv = np.linalg.inv(E)
B = X_inv @ E_inv @ board_in_base
A_list.append(E)
B_list.append(B)
return A_list, B_list, X_true
# ============ 主程序 ============
if __name__ == "__main__":
# 生成仿真数据（实际使用时替换为真实测量值）
A_list, B_list, X_true = generate_synthetic_data(15)
# 求解手眼矩阵
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(f"旋转矩阵误差 (Frobenius范数): {err_rot:.6f}")
print(f"平移向量误差 (L2范数): {err_trans:.6f}")
# 验证 AX = XB 残差
max_residual = 0.0
for i in range(len(A_list)):
lhs = A_list[i] @ X_est
rhs = X_est @ B_list[i]
max_residual = max(max_residual, np.linalg.norm(lhs - rhs))
print(f"AX=XB 最大残差: {max_residual:.6e}")运行方式
- 将上述代码保存为
hand_eye_calib.py - 终端执行：
python hand_eye_calib.py
真实数据替换说明
实际机械臂标定时，将 generate_synthetic_data() 替换为真实采集流程：
- 控制机械臂运动 10~15 个不同位姿（保证旋转轴方向差异足够大，避免退化）
- 每个位姿记录：机械臂末端位姿
Ei（4×4 矩阵）与拍摄图像 - 对每张图像使用
cv2.findChessboardCorners检测角点，结合相机内参用cv2.solvePnP得到标定板到相机的变换Bi - 将
Ei与Bi成对传入tsai_lenz_hand_eye()
验证方法与通过标准
- 仿真验证：运行代码后，旋转矩阵误差应 < 1e-6，平移误差 < 1e-6，AX=XB 残差 < 1e-8，证明算法实现正确
- 真实数据验证：标定完成后，控制机械臂运动到新位姿，拍摄标定板图像，利用标定结果将标定板角点从相机坐标转换到基座坐标，各角点在基座坐标系中的位置应基本重合（重投影误差 < 2~3 mm 即合格）
常见问题
- 旋转轴退化：所有拍摄位姿的旋转轴方向过于接近时，方程病态。应保证各次运动的旋转轴方向差异明显
- 噪声敏感：末端位姿精度对标定结果影响大，建议使用重复定位精度高的机械臂，并尽量增大各拍摄位姿间的角度差
- 内参影响：相机内参需预先单独标定，内参误差会直接传递到手眼标定结果中