4、手眼标定求解算法：Tsai两步法、Park-Martin法、非线性优化法、代码实现与对比

手眼标定的核心，说白了就是解一个形如 AX = XB 的矩阵方程。这个方程看着简单，但实际求解时坑不少。我最早接触这个课题时，以为随便找个库调一下就行，结果在产线上折腾了两天，标定结果始终不对。后来才发现，不同求解算法对噪声的敏感度差异巨大。

这一章，我们就来拆解三种主流求解方法：Tsai两步法、Park-Martin法和非线性优化法。我会结合自己的踩坑经历，帮你理解它们的本质区别和适用场景。

4.1 问题回顾：AX = XB 到底在解什么？

先快速回顾一下。手眼标定有两种常见构型：

• 眼在手外（Eye-to-Hand）：相机固定，机器人末端抓取标定板运动。
• 眼在手上（Eye-in-Hand）：相机装在机器人末端，标定板固定。

无论哪种，最终都归结为求解：

A * X = X * B

其中：

• A：机器人末端从位姿1到位姿2的变换矩阵（由正运动学算出）。
• B：相机从位姿1到位姿2的变换矩阵（由外参标定算出）。
• X：待求的手眼矩阵（相机与机器人末端的固定关系）。

这个方程有4个自由度（旋转3个 + 平移3个），但旋转和平移是耦合的。不同算法的核心差异，就在于如何解耦、如何处理噪声。

4.2 Tsai两步法：经典中的经典

Tsai两步法是我个人最早使用的算法。它的思路很清晰：先解旋转，再解平移。

4.2.1 算法原理

将AX = XB拆成旋转部分和平移部分：

R_A * R_X = R_X * R_B
(R_A - I) * t_X = R_X * t_B - t_A

第一步，用四元数或轴角表示旋转，将旋转方程线性化，通过最小二乘求解R_X。
第二步，将R_X代入平移方程，同样用最小二乘求解t_X。

4.2.2 优点与局限

4.3 Park-Martin法：更鲁棒的旋转求解

Park-Martin法是我后来换用的算法。它用李代数（so3）来表示旋转，把旋转方程转化为一个更稳定的线性系统。

4.3.1 算法原理

核心思想：将旋转矩阵R用李代数向量ω表示，那么旋转方程可以写成：

skew(ω_A + ω_X) = skew(ω_X + ω_B)

通过矩阵变换，最终得到一个关于ω_X的线性方程：

(R_A - I) * ω_X = ω_B - ω_A

这个方程在数值上比Tsai法更稳定，因为李代数在奇异点附近的表现更好。

4.3.2 优点与局限

4.4 非线性优化法：精度上限最高

当精度要求达到亚毫米级时，线性方法往往不够用。这时候就需要上非线性优化了。

4.4.1 算法原理

将AX = XB转化为一个最小化问题：

minimize Σ || A_i * X - X * B_i ||²

用李代数参数化X（6个参数：3旋转 + 3平移），然后使用Levenberg-Marquardt或高斯-牛顿法迭代求解。

4.4.2 优点与局限

4.5 三种算法对比总览

4.6 代码实现与对比

下面我用Python + NumPy实现三种算法的核心逻辑。为了简洁，这里只展示关键步骤。

4.6.1 Tsai两步法核心代码

import numpy as np
from scipy.linalg import logm, expm

def tsai_calibration(A_list, B_list):
    # A_list, B_list: 每组 (4x4) 变换矩阵
    n = len(A_list)
    
    # 第一步：解旋转
    M = np.zeros((3*n, 3))
    for i in range(n):
        R_A = A_list[i][:3, :3]
        R_B = B_list[i][:3, :3]
        # 构造线性方程
        M[3*i:3*i+3, :] = R_A - np.eye(3)
        # 右端项
        # ... (省略具体构造)
    
    # 最小二乘解
    R_X, _, _, _ = np.linalg.lstsq(M, b, rcond=None)
    
    # 第二步：解平移
    # 代入R_X，解线性方程
    # ...
    
    return X  # 4x4 手眼矩阵

4.6.2 Park-Martin法核心代码

def park_martin_calibration(A_list, B_list):
    n = len(A_list)
    
    # 将旋转矩阵转为李代数
    omega_A_list = [logm(A[:3,:3]) for A in A_list]
    omega_B_list = [logm(B[:3,:3]) for B in B_list]
    
    # 构造线性系统
    H = np.zeros((3*n, 3))
    b = np.zeros(3*n)
    for i in range(n):
        R_A = A_list[i][:3, :3]
        H[3*i:3*i+3, :] = R_A - np.eye(3)
        b[3*i:3*i+3] = omega_B_list[i] - omega_A_list[i]
    
    # 最小二乘解
    omega_X, _, _, _ = np.linalg.lstsq(H, b, rcond=None)
    R_X = expm(omega_X)
    
    # 解平移（与Tsai法类似）
    # ...
    
    return X

4.6.3 非线性优化法核心代码

from scipy.optimize import least_squares

def nonlinear_calibration(A_list, B_list, X_init):
    # 将X_init转为6维向量（李代数）
    x0 = se3_to_vector(X_init)
    
    def cost_func(x):
        X = vector_to_se3(x)
        residuals = []
        for A, B in zip(A_list, B_list):
            err = A @ X - X @ B
            residuals.append(err.flatten())
        return np.concatenate(residuals)
    
    result = least_squares(cost_func, x0, method='lm')
    X_opt = vector_to_se3(result.x)
    return X_opt

4.6.4 对比实验

我用仿真数据测试了三种算法。噪声水平设为平移0.5mm、旋转0.1度。

4.7 本章知识体系

下面这张图总结了三种算法的核心逻辑和适用场景：

4.8 避坑指南

• 数据采集要均匀：不要只在一个小范围内采集，旋转角要覆盖30度以上，平移要覆盖机器人工作空间的不同区域。
• 剔除坏点：我习惯在采集后先做一次粗标定，然后计算每组数据的重投影误差，误差超过阈值的直接剔除。
• 注意坐标系约定：不同机器人厂商的坐标系定义可能不同（比如工具坐标系Z轴方向），务必确认后再标定。
• 验证环节不能省：标定完成后，用一组独立数据验证精度。我见过太多标定结果在训练集上很好，测试集上一塌糊涂的情况。