3. 相机标定进阶
任务定义
在激光焊接视觉定位系统中，相机安装于机械臂末端（眼在手上），需建立像素坐标到机器人基座坐标的完整映射。本案例完成手眼标定中的核心矩阵求解：给定一组机械臂末端位姿与对应的标定板角点像素坐标，求解手眼变换矩阵 X（相机到末端），使系统能够通过当前末端位姿将图像坐标转换为基座坐标。
手眼标定原理与数据约定
眼在手上标定满足方程 AX = XB，其中 A 为两次拍摄间机械臂末端的相对运动（4×4齐次矩阵），B 为对应两次拍摄间标定板在相机坐标系下的相对运动。求解 X 后，像素坐标经相机内参转为相机坐标，再经 X 变换到末端坐标系，最后通过当前末端位姿变换到基座坐标系。
本案例提供6组已预处理数据（机械臂末端位姿 T_ee 与标定板角点在相机坐标系下的位姿 T_obj_cam），每组均为4×4齐次矩阵。代码实现Tsai-Lenz两步法：先由旋转方程 RA·RX = RX·RB 求解旋转部分，再通过平移方程求解平移部分。
Python实现（Tsai-Lenz法）
import numpy as np
from scipy.linalg import logm, expm
def hand_eye_calibration(T_ee_list, T_obj_cam_list):
"""
眼在手上手眼标定：求解 AX = XB
参数:
T_ee_list: 机械臂末端位姿列表 (N, 4, 4)
T_obj_cam_list: 标定板在相机坐标系下的位姿列表 (N, 4, 4)
返回:
X: 手眼变换矩阵 (4, 4)，相机到末端的变换
"""
n = len(T_ee_list)
assert n >= 3, "至少需要3组数据"
# 构造旋转方程的最小二乘矩阵
M = np.zeros((3 * n * (n-1) // 2, 9))
row = 0
for i in range(n):
for j in range(i+1, n):
# A = T_ee_i^-1 * T_ee_j
A = np.linalg.inv(T_ee_list[i]) @ T_ee_list[j]
# B = T_obj_cam_i * T_obj_cam_j^-1
B = T_obj_cam_list[i] @ np.linalg.inv(T_obj_cam_list[j])
RA = A[:3, :3]
RB = B[:3, :3]
# 旋转轴-角度表示
logA = logm(RA)
logB = logm(RB)
pa = np.array([logA[2,1], logA[0,2], logA[1,0]])
pb = np.array([logB[2,1], logB[0,2], logB[1,0]])
# 叉积矩阵形式: [pa]_x * rx = pb => (I - [pb]_x) * rx = pa
# 使用 Kronecker 积构造: (pa^T ⊗ I - I ⊗ pb^T) vec(RX) = 0
K = np.kron(pa.reshape(1,3), np.eye(3)) - np.kron(np.eye(3), pb.reshape(1,3))
M[row:row+3, :] = K
row += 3
# SVD求解旋转部分
_, _, Vt = np.linalg.svd(M)
rx = Vt[-1, :]
RX = rx.reshape(3, 3)
# 投影到SO(3)
U, _, Vt = np.linalg.svd(RX)
RX = U @ Vt
if np.linalg.det(RX) < 0:
Vt[-1, :] *= -1
RX = U @ Vt
# 求解平移部分
C = []
d = []
for i in range(n):
for j in range(i+1, n):
A = np.linalg.inv(T_ee_list[i]) @ T_ee_list[j]
B = T_obj_cam_list[i] @ np.linalg.inv(T_obj_cam_list[j])
RA = A[:3, :3]
tA = A[:3, 3]
tB = B[:3, 3]
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
# ========== 样例数据（6组实测预处理位姿） ==========
# 每组: 4x4齐次矩阵 [R t; 0 1]
T_ee_data = [
np.array([[0.998, -0.052, 0.031, 0.512],
[0.053, 0.998, -0.021, -0.134],
[-0.030, 0.023, 0.999, 0.876],
[0, 0, 0, 1]]),
np.array([[0.985, -0.168, 0.041, 0.498],
[0.169, 0.985, -0.028, -0.121],
[-0.036, 0.035, 0.999, 0.892],
[0, 0, 0, 1]]),
np.array([[0.972, 0.234, -0.022, 0.523],
[-0.233, 0.972, 0.031, -0.148],
[0.029, -0.025, 0.999, 0.861],
[0, 0, 0, 1]]),
np.array([[0.991, 0.132, 0.018, 0.505],
[-0.131, 0.991, 0.035, -0.139],
[-0.023, -0.032, 0.999, 0.884],
[0, 0, 0, 1]]),
np.array([[0.979, -0.205, -0.015, 0.517],
[0.204, 0.979, 0.026, -0.127],
[0.020, -0.022, 0.999, 0.869],
[0, 0, 0, 1]]),
np.array([[0.995, 0.098, -0.012, 0.508],
[-0.097, 0.995, 0.029, -0.142],
[0.015, -0.028, 0.999, 0.881],
[0, 0, 0, 1]])
]
T_obj_cam_data = [
np.array([[0.999, 0.012, -0.038, 0.082],
[-0.011, 0.999, 0.031, -0.045],
[0.038, -0.031, 0.999, 0.612],
[0, 0, 0, 1]]),
np.array([[0.982, 0.185, -0.042, 0.095],
[-0.184, 0.982, 0.035, -0.052],
[0.048, -0.027, 0.999, 0.598],
[0, 0, 0, 1]]),
np.array([[0.965, -0.258, 0.033, 0.071],
[0.259, 0.965, -0.028, -0.038],
[-0.025, 0.036, 0.999, 0.625],
[0, 0, 0, 1]]),
np.array([[0.988, -0.152, 0.021, 0.088],
[0.153, 0.988, -0.030, -0.041],
[-0.016, 0.033, 0.999, 0.605],
[0, 0, 0, 1]]),
np.array([[0.976, 0.218, -0.018, 0.079],
[-0.217, 0.976, 0.034, -0.049],
[0.025, -0.029, 0.999, 0.618],
[0, 0, 0, 1]]),
np.array([[0.993, -0.118, 0.015, 0.086],
[0.119, 0.993, -0.026, -0.043],
[-0.012, 0.028, 0.999, 0.609],
[0, 0, 0, 1]])
]
# 执行标定
X_result = hand_eye_calibration(T_ee_data, T_obj_cam_data)
print("手眼变换矩阵 X (相机→末端):")
print(np.round(X_result, 4))
验证方法
运行上述代码，观察输出的4×4矩阵。判定标准：
- 程序正常执行无异常，输出矩阵的旋转部分行列式接近1（误差<1e-3），满足SO(3)约束；
- 将任意两组数据代入
A @ X_result与X_result @ B，两者差值范数应小于0.05，验证方程AX = XB成立； - 平移部分数值在0.05～0.15量级，与相机安装偏移的物理尺寸相符。
标定完成后，实际使用时通过 P_base = T_ee_current @ X_result @ P_cam 将相机坐标系下的点变换到基座坐标系，其中 P_cam 由像素坐标经相机内参反投影得到。