SVD 手眼标定法：从 AX=XB 方程到 Python 实现，误差＜ 0.5mm

jardownload

381人浏览 · 2026-07-04 11:34:32

jardownload · 2026-07-04 11:34:32 发布

SVD 手眼标定法：从 AX=XB 方程到 Python 实现，误差 < 0.5mm

1. 手眼标定的数学本质

在机器人视觉系统中，手眼标定（Hand-Eye Calibration）是建立相机坐标系与机械臂末端坐标系之间转换关系的关键步骤。其核心数学问题可表述为求解矩阵方程 AX=XB，其中：

A 表示机械臂末端执行器在不同位姿间的相对运动变换
B 表示相机观测到的相同运动在图像坐标系中的相对变换
X 即为待求的手眼变换矩阵

该方程的解揭示了三维空间中的刚体运动关系。当采用**眼在手上（Eye-in-Hand） 配置时，X 表示相机到末端执行器的变换；而 眼在手外（Eye-to-Hand）**配置时，X 表示相机到机器人基坐标系的变换。

关键性质：方程 AX=XB 存在唯一解的条件是机械臂运动包含至少两个非平行的旋转轴

2. SVD 求解旋转矩阵的完整推导

2.1 问题建模

给定 n 组运动观测数据 {(A₁,B₁),...,(Aₙ,Bₙ)}，我们需要求解旋转矩阵 R 和平移向量 t 使得：

R_A * R = R * R_B
R_A * t + t_A = R * t_B + t

其中每个 A 和 B 可分解为：

A = [R_A | t_A]   # 3x3旋转矩阵 + 3x1平移向量
     [0   | 1 ]
B = [R_B | t_B]
     [0   | 1 ]

2.2 旋转矩阵求解

通过最小化目标函数 Σ||R_AiR - RR_Bi||²，可推导出：

构建矩阵 H：

H = Σ(R_Bi^T ⊗ R_Ai)  # Kronecker积展开

对 H 进行奇异值分解（SVD）：

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

最优旋转矩阵解为：

R = Vt.T @ U.T

反射矩阵检测（确保 det(R)=1）：

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

2.3 平移向量求解

在求得 R 后，平移向量 t 可通过解线性方程组得到：

(I - R_Ai) * t = R * t_Bi - t_Ai

使用最小二乘法求解超定方程组：

A = np.vstack([I - R_Ai for R_Ai in R_As])
b = np.hstack([R @ t_Bi - t_Ai for t_Ai in t_As])
t = np.linalg.lstsq(A, b, rcond=None)[0]

3. Python 实现与误差分析

3.1 完整代码实现

import numpy as np
from scipy.linalg import svd

def hand_eye_calibration(A_list, B_list):
    """SVD-based hand-eye calibration implementation
    
    Args:
        A_list: List of 4x4 homogeneous matrices (robot motions)
        B_list: List of 4x4 homogeneous matrices (camera observations)
    
    Returns:
        X: 4x4 homogeneous transformation matrix
        error: Average residual error
    """
    # Extract rotation and translation components
    R_A = [A[:3,:3] for A in A_list]
    t_A = [A[:3,3] for A in A_list]
    R_B = [B[:3,:3] for B in B_list]
    t_B = [B[:3,3] for B in B_list]
    
    # Step 1: Solve for rotation matrix R
    K = np.zeros((3,3))
    for Ra, Rb in zip(R_A, R_B):
        K += Rb.T @ Ra
    U, S, Vt = 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 vector t
    A = np.zeros((3*len(R_A), 3))
    b = np.zeros(3*len(R_A))
    for i in range(len(R_A)):
        A[3*i:3*i+3] = np.eye(3) - R_A[i]
        b[3*i:3*i+3] = R @ t_B[i] - t_A[i]
    t = np.linalg.lstsq(A, b, rcond=None)[0]
    
    # Construct homogeneous transformation
    X = np.eye(4)
    X[:3,:3] = R
    X[:3,3] = t
    
    # Calculate average error
    errors = []
    for A, B in zip(A_list, B_list):
        residual = A @ X - X @ B
        errors.append(np.linalg.norm(residual))
    avg_error = np.mean(errors)
    
    return X, avg_error

3.2 仿真数据生成

为验证算法性能，我们生成含噪声的仿真数据：

def generate_synthetic_data(X_true, n=10, rot_noise=0.01, trans_noise=0.1):
    """Generate synthetic calibration data with noise
    
    Args:
        X_true: Ground truth transformation (4x4)
        n: Number of data pairs
        rot_noise: Rotation noise level (radians)
        trans_noise: Translation noise level (mm)
    """
    A_list = []
    B_list = []
    
    for _ in range(n):
        # Generate random rigid body motion
        theta = np.random.uniform(0, np.pi/2)
        axis = np.random.randn(3)
        axis /= np.linalg.norm(axis)
        
        # Ground truth A and B
        R_A = rotation_matrix(axis, theta)
        t_A = np.random.randn(3) * 10
        A = np.eye(4)
        A[:3,:3] = R_A
        A[:3,3] = t_A
        
        # Compute corresponding B
        B = np.linalg.inv(X_true) @ A @ X_true
        
        # Add noise
        A_noisy = add_noise(A, rot_noise, trans_noise)
        B_noisy = add_noise(B, rot_noise, trans_noise)
        
        A_list.append(A_noisy)
        B_list.append(B_noisy)
    
    return A_list, B_list

def rotation_matrix(axis, angle):
    """Generate rotation matrix from axis-angle representation"""
    axis = axis / np.linalg.norm(axis)
    a = np.cos(angle/2)
    b, c, d = -axis * np.sin(angle/2)
    return np.array([
        [a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
        [2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
        [2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]
    ])

def add_noise(T, rot_noise, trans_noise):
    """Add noise to homogeneous transformation matrix"""
    R = T[:3,:3]
    t = T[:3,3]
    
    # Add rotation noise
    noise_axis = np.random.randn(3)
    noise_axis /= np.linalg.norm(noise_axis)
    noise_angle = np.random.normal(0, rot_noise)
    R_noise = rotation_matrix(noise_axis, noise_angle)
    R_noisy = R_noise @ R
    
    # Add translation noise
    t_noisy = t + np.random.normal(0, trans_noise, 3)
    
    T_noisy = np.eye(4)
    T_noisy[:3,:3] = R_noisy
    T_noisy[:3,3] = t_noisy
    
    return T_noisy

3.3 误差评估方法

为量化标定精度，我们采用以下评估指标：

旋转误差 ：计算估计旋转与真实旋转之间的角度差

def rotation_error(R_true, R_est):
    return np.arccos((np.trace(R_true.T @ R_est) - 1) / 2)

平移误差 ：欧氏距离度量

def translation_error(t_true, t_est):
    return np.linalg.norm(t_true - t_est)

重投影误差 ：验证标定结果在实际数据上的表现

4. 实际应用中的优化技巧

4.1 数据采集策略

参数	推荐值	说明
运动次数	15-20次	确保充分覆盖工作空间
旋转角度	>30°	每次运动包含显著旋转
平移距离	>100mm	避免微小运动
运动多样性	三轴混合	避免共面运动

4.2 噪声抑制方法

数据预处理 ：

def preprocess_motions(A_list, B_list, min_angle=0.2):
    """Filter out small motions"""
    filtered_A, filtered_B = [], []
    for A, B in zip(A_list, B_list):
        R_A = A[:3,:3]
        angle = np.arccos((np.trace(R_A) - 1)/2)
        if angle > min_angle:
            filtered_A.append(A)
            filtered_B.append(B)
    return filtered_A, filtered_B

加权SVD求解 ：

# 根据运动幅度分配权重
weights = [np.linalg.norm(t_A) * angle for R_A, t_A in zip(R_As, t_As)]
K = sum(w * Rb.T @ Ra for w, Ra, Rb in zip(weights, R_As, R_Bs))

4.3 标定验证流程

棋盘格验证法 ：
- 在机械臂末端固定棋盘格
- 使用标定后的X矩阵预测棋盘格位置
- 与实际观测位置对比

重复运动测试 ：

def repeatability_test(X, test_motions):
    errors = []
    for A, B in test_motions:
        pred_B = np.linalg.inv(X) @ A @ X
        errors.append(np.linalg.norm(pred_B - B))
    return np.mean(errors)