4. Tsai-Lenz 两步法：旋转矩阵解耦、平移向量求解、算法流程与代码实现、精度分析
手眼标定里，Tsai-Lenz 算法是我个人最常用的一种。为什么？因为它把问题拆成了两步——先算旋转，再算平移。说白了，就是把一个复杂的非线性问题，变成了两个相对简单的线性问题。我在项目里踩过不少坑，最后发现，老老实实用 Tsai-Lenz，反而最稳。
4.1 核心思想：为什么是两步？
你想想看，手眼标定的方程是 AX = XB。这里的 X 是我们要找的手眼矩阵，包含旋转 R 和平移 t。如果一口气把 R 和 t 都求出来，那是个非线性问题，求解起来很麻烦。
Tsai-Lenz 的聪明之处在于：先忽略平移，只解旋转。等旋转矩阵定下来了，再回头解平移。这样每一步都是线性的，有闭式解，稳定可靠。
关键洞察： 旋转矩阵 R 只影响方向，不影响位置。所以我们可以先通过多组姿态数据，把 R 单独解出来。R 定了，t 就是线性方程组的事。
4.2 旋转矩阵解耦：从 AX=XB 到 RAR = RRB
我们先看旋转部分。方程 AX = XB 展开后，旋转部分就是：
RA · R = R · RB
这里 RA 是机器人末端从位姿 1 到位姿 2 的旋转变化，RB 是相机从位姿 1 到位姿 2 的旋转变化。我们要找的 R 就是手眼旋转矩阵。
这个方程怎么解？Tsai-Lenz 用了旋转向量（轴角）表示法。把每个旋转矩阵转成旋转向量，方程就变成了：
PA = R · PB
其中 PA 和 PB 分别是 RA 和 RB 对应的旋转向量。这下就简单了——变成了一个典型的旋转向量对齐问题。
我记得第一次手动推导这一步时，卡了很久。后来发现，其实可以用罗德里格斯公式把旋转矩阵和旋转向量来回转换，代码实现起来并不复杂。
4.3 平移向量求解：旋转定了，平移就是线性
旋转矩阵 R 求出来后，回到原方程 AX = XB 的平移部分：
(RA - I) · t = R · tB - tA
这里 tA 和 tB 分别是机器人末端和相机的平移变化量。I 是单位矩阵。
你看，现在未知数只有 t 了。而且这是个线性方程。多组数据联立，用最小二乘法就能解出 t。
我的经验： 解平移时，数据质量比数量更重要。如果某组数据的旋转变化很小（比如小于 5 度），(RA - I) 会接近零矩阵，导致方程病态。我一般会过滤掉旋转角小于 10 度的数据对。
4.4 算法流程：一步一步来
整个 Tsai-Lenz 两步法的流程，我整理成了下面这张图：
4.5 代码实现：Python 版 Tsai-Lenz
下面是我在实际项目中用过的 Python 实现。核心函数就两个：一个解旋转，一个解平移。
import numpy as np
from scipy.linalg import svd
def tsai_lenz_rotation(R_A_list, R_B_list):
"""
两步法第一步：求解旋转矩阵 R
R_A_list: 机器人末端旋转变化矩阵列表 [R_A1, R_A2, ...]
R_B_list: 相机旋转变化矩阵列表 [R_B1, R_B2, ...]
返回: 手眼旋转矩阵 R
"""
n = len(R_A_list)
# 构建旋转向量对齐方程组
P = np.zeros((3 * n, 3))
Q = np.zeros(3 * n)
for i in range(n):
# 旋转矩阵 → 旋转向量
r_A = rotation_matrix_to_vector(R_A_list[i])
r_B = rotation_matrix_to_vector(R_B_list[i])
# 构建方程: P_A = R * P_B
P[3*i:3*i+3, :] = skew_symmetric(r_A + r_B)
Q[3*i:3*i+3] = r_B - r_A
# SVD 求解
U, S, Vt = svd(P, full_matrices=False)
x = Vt.T @ np.linalg.inv(np.diag(S)) @ U.T @ Q
# 旋转向量 → 旋转矩阵
R = vector_to_rotation_matrix(x)
return R
def tsai_lenz_translation(R, R_A_list, t_A_list, R_B_list, t_B_list):
"""
两步法第二步：求解平移向量 t
R: 第一步求出的手眼旋转矩阵
"""
n = len(R_A_list)
A = np.zeros((3 * n, 3))
b = np.zeros(3 * n)
for i in range(n):
# 构建方程: (R_A - I) * t = R * t_B - t_A
A[3*i:3*i+3, :] = R_A_list[i] - np.eye(3)
b[3*i:3*i+3] = R @ t_B_list[i] - t_A_list[i]
# 最小二乘求解
t, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
return t
def rotation_matrix_to_vector(R):
"""罗德里格斯公式：旋转矩阵 → 旋转向量"""
theta = np.arccos((np.trace(R) - 1) / 2)
if theta < 1e-10:
return np.zeros(3)
rx = (R[2,1] - R[1,2]) / (2 * np.sin(theta))
ry = (R[0,2] - R[2,0]) / (2 * np.sin(theta))
rz = (R[1,0] - R[0,1]) / (2 * np.sin(theta))
return np.array([rx, ry, rz]) * theta
def vector_to_rotation_matrix(v):
"""罗德里格斯公式：旋转向量 → 旋转矩阵"""
theta = np.linalg.norm(v)
if theta < 1e-10:
return np.eye(3)
k = v / theta
K = skew_symmetric(k)
R = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)
return R
def skew_symmetric(v):
"""向量的反对称矩阵"""
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])使用建议： 实际调用时，建议至少用 5-8 组数据。数据太少，SVD 解不稳定。我一般会采集 10-15 组，然后剔除残差最大的 2-3 组。
4.6 精度分析：影响标定结果的关键因素
Tsai-Lenz 算法本身是精确的，但实际标定精度受很多因素影响。我整理了一个表格，方便你对照排查：
| 影响因素 | 影响程度 | 说明 | 改善方法 |
|---|---|---|---|
| 机器人定位精度 | 高 | 机器人运动学参数误差会直接传递到手眼矩阵 | 使用高精度机器人，或先做机器人标定 |
| 相机标定精度 | 高 | 相机内参和畸变参数不准，位姿估计就偏 | 用棋盘格多角度标定，重投影误差 < 0.1px |
| 数据对数量 | 中 | 太少数据解不稳定，太多数据引入噪声 | 8-15 组为宜，覆盖不同姿态 |
| 姿态变化幅度 | 高 | 旋转变化太小会导致方程病态 | 相邻位姿旋转角 > 15°，平移 > 50mm |
| 标定板检测精度 | 中 | 角点检测误差会传递到位姿估计 | 使用高分辨率相机，亚像素角点检测 |
| 数据筛选策略 | 中 | 异常数据对会拉低整体精度 | 计算残差，剔除残差最大的 20% 数据 |
我曾经踩过的坑： 有一次标定结果反复跳动，查了两天才发现是机器人末端工具重量导致机器人实际位姿和理论位姿有偏差。后来我在机器人负载下重新做了 TCP 标定，问题才解决。记住：机器人带负载和不带负载，运动学参数是有差异的。
4.7 精度验证：标定得好不好？
标定完怎么知道准不准？我一般用两个指标：
- 重投影误差： 用标定结果把相机坐标系下的点投影到机器人基坐标系，再和实际测量值对比。误差在 1-2mm 以内算合格。
- 闭环误差： 让机器人走一个闭环路径，回到起点后，看手眼矩阵计算出的位置和实际位置的偏差。
嗯，这里要注意：重投影误差小，不代表标定一定准。如果所有数据都在同一个姿态附近采集，重投影误差可能很小，但外推到大范围运动时误差会很大。所以验证时一定要用和标定数据不同的姿态。
我个人习惯是：标定完成后，单独采集 5 组验证数据，计算平均重投影误差。如果超过 3mm，我会重新检查数据质量，或者增加数据量再标一次。