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_B

2.2 构建协方差矩阵

计算两组中心化数据的协方差矩阵 H：

H = np.transpose(AA) @ BB

2.3 奇异值分解

对 H 进行 SVD 分解：

U, S, Vt = np.linalg.svd(H)

2.4 计算旋转矩阵

通过 SVD 结果计算旋转矩阵：

R = Vt.T @ U.T

2.5 反射矩阵检测

检查行列式是否为负，处理反射情况：

if np.linalg.det(R) < 0: Vt[2,:] *= -1 R = Vt.T @ U.T

3. 平移向量求解

在获得旋转矩阵后，平移向量可以通过最小二乘法求解：

t = -R @ mu_A.T + mu_B.T

4. 数值稳定性分析

SVD 方法具有优秀的数值稳定性，主要体现在：

1. 中心化处理：减少数值误差累积
2. 正交性保证：SVD 自动保证旋转矩阵的正交性
3. 秩缺失处理：当数据存在噪声时仍能提供最优解

下表对比了不同求解方法的数值特性：

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 X

6. 实际应用建议

1. 数据采集：

   • 确保机械臂运动包含充分的旋转变化
   • 建议采集15-20组数据以提高精度
   • 运动范围应覆盖工作空间的主要区域

2. 验证方法：

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)

3. 精度提升技巧：
   • 使用RANSAC剔除异常数据
   • 采用非线性优化进行后处理
   • 结合九点标定法提供初始解