SVD 手眼标定法：从 9 组数据求解 AX=XB 的 5 步核心推导
SVD 手眼标定法：从 9 组数据求解 AX=XB 的 5 步核心推导
在机器人视觉系统中，手眼标定是连接视觉感知与机械臂运动的关键环节。SVD（奇异值分解）法作为一种数学工具，能够高效求解手眼标定方程 AX=XB。本文将深入剖析其数学原理与实现步骤，并提供一个完整的 Python 实现。
1. 手眼标定的数学基础
手眼标定的核心问题是求解刚体变换矩阵 X，使得对于所有机械臂运动 A 和相机观测 B，满足 AX=XB。这个方程可以分解为旋转和平移两部分：
- 旋转部分：R_A * R_X = R_X * R_B
- 平移部分：R_A * t_X + t_A = R_X * t_B + t_X
其中 R 表示旋转矩阵，t 表示平移向量。当收集到多组 (A,B) 数据对时，我们可以构建超定方程组来求解 X。
关键点：至少需要 2 组非平行旋转运动数据才能唯一确定旋转矩阵 R_X。这是因为单一旋转轴无法提供足够的约束条件。
2. SVD 求解旋转矩阵的 5 步推导
2.1 数据预处理
首先对收集的 9 组 (A,B) 数据进行中心化处理：
def center_data(A, B): # 计算均值 mu_A = np.mean(A, axis=0) mu_B = np.mean(B, axis=0) # 中心化 AA = A - mu_A BB = B - mu_B return AA, BB, mu_A, mu_B2.2 构建协方差矩阵
计算两组中心化数据的协方差矩阵 H：
H = np.transpose(AA) @ BB2.3 奇异值分解
对 H 进行 SVD 分解：
U, S, Vt = np.linalg.svd(H)2.4 计算旋转矩阵
通过 SVD 结果计算旋转矩阵：
R = Vt.T @ U.T2.5 反射矩阵检测
检查行列式是否为负，处理反射情况：
if np.linalg.det(R) < 0: Vt[2,:] *= -1 R = Vt.T @ U.T3. 平移向量求解
在获得旋转矩阵后，平移向量可以通过最小二乘法求解：
t = -R @ mu_A.T + mu_B.T4. 数值稳定性分析
SVD 方法具有优秀的数值稳定性，主要体现在：
- 中心化处理：减少数值误差累积
- 正交性保证：SVD 自动保证旋转矩阵的正交性
- 秩缺失处理：当数据存在噪声时仍能提供最优解
下表对比了不同求解方法的数值特性：
| 方法 | 数值稳定性 | 计算复杂度 | 对噪声敏感性 |
|---|---|---|---|
| SVD | 高 | O(n³) | 低 |
| 四元数法 | 中 | O(n²) | 中 |
| 李代数法 | 低 | O(n³) | 高 |
5. Python 完整实现
以下是整合了上述步骤的完整实现：
import numpy as np def svd_hand_eye_calibration(A, B): """ SVD-based hand-eye calibration solving AX=XB Parameters: A: List of 4x4 homogeneous matrices (robot motions) B: List of 4x4 homogeneous matrices (camera observations) Returns: X: 4x4 homogeneous transformation matrix """ # Convert to numpy arrays and extract rotations A = [np.array(a) for a in A] B = [np.array(b) for b in B] R_A = [a[:3,:3] for a in A] t_A = [a[:3,3] for a in A] R_B = [b[:3,:3] for b in B] t_B = [b[:3,3] for b in B] # Step 1: Solve for rotation K = np.zeros((3,3)) for ra, rb in zip(R_A, R_B): K += rb.T @ ra # SVD for rotation U, S, Vt = np.linalg.svd(K) R = Vt.T @ U.T # Handle reflection case if np.linalg.det(R) < 0: Vt[2,:] *= -1 R = Vt.T @ U.T # Step 2: Solve for translation A_mat = [] b_vec = [] for ra, ta, rb, tb in zip(R_A, t_A, R_B, t_B): A_mat.append(ra - np.eye(3)) b_vec.append(tb - R @ ta) A_mat = np.vstack(A_mat) b_vec = np.hstack(b_vec) t = np.linalg.lstsq(A_mat, b_vec, rcond=None)[0] # Construct homogeneous transformation X = np.eye(4) X[:3,:3] = R X[:3,3] = t return X6. 实际应用建议
数据采集：
- 确保机械臂运动包含充分的旋转变化
- 建议采集15-20组数据以提高精度
- 运动范围应覆盖工作空间的主要区域
验证方法：
def verify_calibration(X, A, B): errors = [] for a, b in zip(A, B): ax = a @ X xb = X @ b error = np.linalg.norm(ax - xb) errors.append(error) return np.mean(errors)- 精度提升技巧：
- 使用RANSAC剔除异常数据
- 采用非线性优化进行后处理
- 结合九点标定法提供初始解